def PhasePlot(pendulum):
    phase_plot = plt.figure()
    ax = phase_plot.gca()
    theta_lim = [-math.pi, 3.*math.pi]
    ax.set_xlim(theta_lim)
    ax.set_ylim(-10., 10.)

    params = PendulumParams()
    theta = np.linspace(theta_lim[0], theta_lim[1], 601)  # 4*k + 1
    thetadot = np.zeros(theta.shape)
    E_upright = TotalEnergy(UprightState().CopyToVector(), params)
    E = [E_upright, .1*E_upright, 1.5*E_upright]
    for e in E:
        for i in range(theta.size):
            v = (e + params.mass() * params.gravity() * params.length() *
                 math.cos(theta[i])) / \
                (.5 * params.mass() * params.length() * params.length())
            if (v >= 0):
                thetadot[i] = math.sqrt(v)
            else:
                thetadot[i] = float('nan')
        ax.plot(theta, thetadot, color=[.6, .6, .6])
        ax.plot(theta, -thetadot, color=[.6, .6, .6])

    return ax
def PhasePlot(pendulum):
    phase_plot = plt.figure()
    ax = phase_plot.gca()
    theta_lim = [-math.pi, 3. * math.pi]
    ax.set_xlim(theta_lim)
    ax.set_ylim(-10., 10.)

    params = PendulumParams()
    theta = np.linspace(theta_lim[0], theta_lim[1], 601)  # 4*k + 1
    thetadot = np.zeros(theta.shape)
    E_upright = TotalEnergy(UprightState().CopyToVector(), params)
    E = [E_upright, .1 * E_upright, 1.5 * E_upright]
    for e in E:
        for i in range(theta.size):
            v = ((e + params.mass() * params.gravity() * params.length() *
                  math.cos(theta[i])) /
                 (.5 * params.mass() * params.length() * params.length()))
            if (v >= 0):
                thetadot[i] = math.sqrt(v)
            else:
                thetadot[i] = float("nan")
        ax.plot(theta, thetadot, color=[.6, .6, .6])
        ax.plot(theta, -thetadot, color=[.6, .6, .6])

    return ax
 def test_params(self):
     params = PendulumParams()
     params.set_mass(1.)
     params.set_length(2.)
     params.set_damping(3.)
     params.set_gravity(4.)
     self.assertEqual(params.mass(), 1.)
     self.assertEqual(params.length(), 2.)
     self.assertEqual(params.damping(), 3.)
     self.assertEqual(params.gravity(), 4.)
Example #4
0
 def test_params(self):
     params = PendulumParams()
     params.set_mass(1.)
     params.set_length(2.)
     params.set_damping(3.)
     params.set_gravity(4.)
     self.assertEqual(params.mass(), 1.)
     self.assertEqual(params.length(), 2.)
     self.assertEqual(params.damping(), 3.)
     self.assertEqual(params.gravity(), 4.)
Example #5
0
 def test_params(self):
     params = PendulumParams()
     params.set_mass(mass=1.)
     params.set_length(length=2.)
     params.set_damping(damping=3.)
     params.set_gravity(gravity=4.)
     self.assertEqual(params.mass(), 1.)
     self.assertEqual(params.length(), 2.)
     self.assertEqual(params.damping(), 3.)
     self.assertEqual(params.gravity(), 4.)
     params_mass = params.with_mass(mass=5.)
     params_length = params.with_length(length=6.)
     params_damping = params.with_damping(damping=7.)
     params_gravity = params.with_gravity(gravity=8.)
     self.assertEqual(params_mass.mass(), 5.)
     self.assertEqual(params_length.length(), 6.)
     self.assertEqual(params_damping.damping(), 7.)
     self.assertEqual(params_gravity.gravity(), 8.)
prog = MathematicalProgram()

# Declare the "indeterminates", x.  These are the variables which define the
# polynomials, but are NOT decision variables in the optimization.  We will
# add constraints below that must hold FOR ALL x.
s = prog.NewIndeterminates(1, 's')[0]
c = prog.NewIndeterminates(1, 'c')[0]
thetadot = prog.NewIndeterminates(1, 'thetadot')[0]
# TODO(russt): bind the sugar methods so I can write
#  x = prog.NewIndeterminates(['s','c','thetadot'])
x = np.array([s, c, thetadot])

# Write out the dynamics in terms of sin(theta), cos(theta), and thetadot
p = PendulumParams()
f = [c*thetadot, -s*thetadot,
     (-p.damping()*thetadot - p.mass()*p.gravity()*p.length()*s) /
     (p.mass()*p.length()*p.length())]

# The fixed-point in this coordinate (because cos(0)=1).
x0 = np.array([0, 1, 0])

# Construct a polynomial V that contains all monomials with s,c,thetadot up
# to degree 2.
deg_V = 2
V = prog.NewFreePolynomial(Variables(x), deg_V).ToExpression()

# Add a constraint to enforce that V is strictly positive away from x0.
# (Note that because our coordinate system is sine and cosine, V is also zero
# at theta=2pi, etc).
eps = 1e-4
constraint1 = prog.AddSosConstraint(V - eps*(x-x0).dot(x-x0))
prog = MathematicalProgram()

# Declare the "indeterminates", x.  These are the variables which define the
# polynomials, but are NOT decision variables in the optimization.  We will
# add constraints below that must hold FOR ALL x.
s = prog.NewIndeterminates(1, 's')[0]
c = prog.NewIndeterminates(1, 'c')[0]
thetadot = prog.NewIndeterminates(1, 'thetadot')[0]
# TODO(russt): bind the sugar methods so I can write
#  x = prog.NewIndeterminates(['s','c','thetadot'])
x = np.array([s, c, thetadot])

# Write out the dynamics in terms of sin(theta), cos(theta), and thetadot
p = PendulumParams()
f = [c*thetadot, -s*thetadot,
     (-p.damping()*thetadot - p.mass()*p.gravity()*p.length()*s) /
     (p.mass()*p.length()*p.length())]

# The fixed-point in this coordinate (because cos(0)=1).
x0 = np.array([0, 1, 0])

# Construct a polynomial V that contains all monomials with s,c,thetadot up
# to degree 2.
deg_V = 2
V = prog.NewFreePolynomial(Variables(x), deg_V).ToExpression()

# Add a constraint to enforce that V is strictly positive away from x0.
# (Note that because our coordinate system is sine and cosine, V is also zero
# at theta=2pi, etc).
eps = 1e-4
constraint1 = prog.AddSosConstraint(V - eps*(x-x0).dot(x-x0))
Example #8
0
# Declare the "indeterminates", x.  These are the variables which define the
# polynomials, but are NOT decision variables in the optimization.  We will
# add constraints below that must hold FOR ALL x.
s = prog.NewIndeterminates(1, "s")[0]
c = prog.NewIndeterminates(1, "c")[0]
thetadot = prog.NewIndeterminates(1, "thetadot")[0]
# TODO(russt): bind the sugar methods so I can write
#  x = prog.NewIndeterminates(["s", "c", "thetadot"])
x = np.array([s, c, thetadot])

# Write out the dynamics in terms of sin(theta), cos(theta), and thetadot
p = PendulumParams()
f = [
    c * thetadot, -s * thetadot,
    (-p.damping() * thetadot - p.mass() * p.gravity() * p.length() * s) /
    (p.mass() * p.length() * p.length())
]

# The fixed-point in this coordinate (because cos(0)=1).
x0 = np.array([0, 1, 0])

# Construct a polynomial V that contains all monomials with s,c,thetadot up
# to degree 2.
deg_V = 2
V = prog.NewFreePolynomial(Variables(x), deg_V).ToExpression()

# Add a constraint to enforce that V is strictly positive away from x0.
# (Note that because our coordinate system is sine and cosine, V is also zero
# at theta=2pi, etc).
eps = 1e-4