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 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 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 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))