def simulate(*, initial_state, controller_params, t_final, tape_period):
"""Simulates an Acrobot + Spong controller from the given initial state and
parameters until the given final time. Returns the state sampled at the
given tape_period.
"""
builder = DiagramBuilder()
plant = builder.AddSystem(AcrobotPlant())
controller = builder.AddSystem(AcrobotSpongController())
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
state_logger = LogOutput(plant.get_output_port(0), builder)
state_logger.set_publish_period(tape_period)
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
plant_context = diagram.GetMutableSubsystemContext(plant, context)
controller_context = diagram.GetMutableSubsystemContext(
controller, context)
plant_context.SetContinuousState(initial_state)
controller_context.get_mutable_numeric_parameter(0).SetFromVector(
controller_params)
simulator.AdvanceTo(t_final)
x_tape = state_logger.data()
return x_tape
def test_default_parameters(self):
controller = AcrobotSpongController()
context = controller.CreateDefaultContext()
expected_parameters = SpongControllerParams()
actual_parameters = controller.get_parameters(context)
self.assertEqual(actual_parameters.k_e(), expected_parameters.k_e())
self.assertEqual(actual_parameters.k_p(), expected_parameters.k_p())
self.assertEqual(actual_parameters.k_d(), expected_parameters.k_d())
self.assertEqual(actual_parameters.balancing_threshold(),
expected_parameters.balancing_threshold())
def test_param_accessors(self):
controller = AcrobotSpongController()
context = controller.CreateDefaultContext()
controller.get_mutable_parameters(context).set_k_e(1.)
actual_parameters = controller.get_parameters(context)
self.assertEqual(actual_parameters.k_e(), 1.)