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
class SwingUpAndBalanceController(VectorSystem):
def __init__(self):
VectorSystem.__init__(self, 2, 1)
(self.K, self.S) = BalancingLQR()
self.params = PendulumParams() # TODO(russt): Pass these in?
# TODO(russt): Add a witness function to tell the simulator about the
# discontinuity when switching to LQR.
def DoCalcVectorOutput(self, context, pendulum_state, unused, output):
xbar = copy(pendulum_state)
xbar[0] = wrap_to(xbar[0], 0, 2.*math.pi) - math.pi
# If x'Sx <= 2, then use the LQR controller
if (xbar.dot(self.S.dot(xbar)) < 2.):
output[:] = -self.K.dot(xbar)
else:
theta = pendulum_state[0]
thetadot = pendulum_state[1]
total_energy = TotalEnergy(pendulum_state, self.params)
desired_energy = TotalEnergy(UprightState().CopyToVector(),
self.params)
output[:] = self.params.damping() * thetadot - \
.1 * thetadot * (total_energy - desired_energy)
class SwingUpAndBalanceController(VectorSystem):
def __init__(self):
VectorSystem.__init__(self, 2, 1)
(self.K, self.S) = BalancingLQR()
self.params = PendulumParams() # TODO(russt): Pass these in?
# TODO(russt): Add a witness function to tell the simulator about the
# discontinuity when switching to LQR.
def DoCalcVectorOutput(self, context, pendulum_state, unused, output):
xbar = copy(pendulum_state)
xbar[0] = wrap_to(xbar[0], 0, 2. * math.pi) - math.pi
# If x'Sx <= 2, then use the LQR controller
if (xbar.dot(self.S.dot(xbar)) < 2.):
output[:] = -self.K.dot(xbar)
else:
theta = pendulum_state[0]
thetadot = pendulum_state[1]
total_energy = TotalEnergy(pendulum_state, self.params)
desired_energy = TotalEnergy(UprightState().CopyToVector(),
self.params)
output[:] = (self.params.damping() * thetadot - .1 * thetadot *
(total_energy - desired_energy))
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.)
def __init__(self):
VectorSystem.__init__(self, 2, 1)
(self.K, self.S) = BalancingLQR()
self.params = PendulumParams() # TODO(russt): Pass these in?
def __init__(self):
VectorSystem.__init__(self, 2, 1)
(self.K, self.S) = BalancingLQR()
self.params = PendulumParams() # TODO(russt): Pass these in?
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.)
from pydrake.examples.pendulum import PendulumParams
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).
from pydrake.examples.pendulum import PendulumParams
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).
from pydrake.examples.pendulum import PendulumParams
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.
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.)