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