def test_simulation(self):
# Basic constant-torque acrobot simulation.
acrobot = AcrobotPlant()
# Create the simulator.
simulator = Simulator(acrobot)
context = simulator.get_mutable_context()
# Set an input torque.
input = AcrobotInput()
input.set_tau(1.)
acrobot.GetInputPort("elbow_torque").FixValue(context, input)
# Set the initial state.
state = context.get_mutable_continuous_state_vector()
state.set_theta1(1.)
state.set_theta1dot(0.)
state.set_theta2(0.)
state.set_theta2dot(0.)
self.assertTrue(acrobot.DynamicsBiasTerm(context).shape == (2, ))
self.assertTrue(acrobot.MassMatrix(context).shape == (2, 2))
initial_total_energy = acrobot.EvalPotentialEnergy(context) + \
acrobot.EvalKineticEnergy(context)
# Simulate (and make sure the state actually changes).
initial_state = state.CopyToVector()
simulator.AdvanceTo(1.0)
self.assertLessEqual(
acrobot.EvalPotentialEnergy(context) +
acrobot.EvalKineticEnergy(context), initial_total_energy)
class EnergyShapingControl(LeafSystem):
def __init__(self, k1, k2, k3, lqr_angle_range=1.0):
super().__init__()
self.k1 = k1
self.k2 = k2
self.k3 = k3
self.acrobot_plant = AcrobotPlant()
self.acrobot_context = self.acrobot_plant.CreateDefaultContext()
self.upright_context = self.acrobot_plant.CreateDefaultContext()
self.upright_context.get_mutable_continuous_state()\
.SetFromVector(np.array([np.pi, 0, 0, 0]))
self.lqr_angle_range = lqr_angle_range
self.DeclareInputPort('estimated state', PortDataType.kVectorValued, 4)
self.DeclareVectorOutputPort('control', BasicVector(1),
self.calculate_control)
self.Q = np.diag((10., 10., 1., 1.))
self.R = [10]
self.K = self.calculate_lqr()
self.LQR_on = False
def calculate_lqr(self):
upright_plant = AcrobotPlant()
upright_plant_context = upright_plant.CreateDefaultContext()
upright_plant_context.get_mutable_continuous_state() \
.SetFromVector(np.array([np.pi, 0, 0, 0]))
upright_plant.GetInputPort("elbow_torque") \
.FixValue(upright_plant_context, 0)
lqr = LinearQuadraticRegulator(upright_plant, upright_plant_context,
self.Q, self.R)
return lqr.D()
def calculate_control(self, context, control_vec):
input = self.get_input_port(0).Eval(context)
theta1 = input[0]
theta2 = input[1]
theta1_dot = input[2]
theta2_dot = input[3]
# Current acrobot context based on input.
self.acrobot_context \
.get_mutable_continuous_state() \
.SetFromVector(input)
desired_state = np.array([np.pi, 0, 0, 0])
cost = (desired_state - input).T @ self.Q @ (desired_state - input)
should_use_lqr = cost < 1
if not self.LQR_on:
self.LQR_on = should_use_lqr
if should_use_lqr:
print("LQR on {0}".format(context.get_time()))
if self.LQR_on:
u = -self.K @ (desired_state - input)
control_vec.SetFromVector(u)
else:
# Bias term of the dynamics.
bias = self.acrobot_plant.DynamicsBiasTerm(self.acrobot_context)
# Mass matrix
M = self.acrobot_plant.MassMatrix(self.acrobot_context)
m11 = M[0, 0]
m12 = M[0, 1]
m21 = M[1, 0]
m22 = M[1, 1]
m22_bar = m22 - m21 * m12 / m11
bias_bar = bias[1] - m21 / m11 * bias[0]
# Desired energy, upright configuration.
Ed = self.acrobot_plant.EvalPotentialEnergy(self.upright_context) \
+ self.acrobot_plant.EvalKineticEnergy(self.upright_context)
# Current energy.
Ec = self.acrobot_plant.EvalPotentialEnergy(self.acrobot_context) \
+ self.acrobot_plant.EvalKineticEnergy(self.acrobot_context)
E_error = Ec - Ed
u_bar = E_error * theta1_dot
u = -self.k1 * theta2 - self.k2 * theta2_dot + self.k3 * u_bar
tau = m22_bar * u + bias_bar
control_vec.SetFromVector(np.array([tau]))