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