def simulate_diagram(diagram, ball_paddle_plant, state_logger,
ball_init_position, ball_init_velocity, simulation_time,
target_realtime_rate):
q_init_val = np.array([
1, 0, 0, 0, ball_init_position[0], ball_init_position[1],
ball_init_position[2]
])
v_init_val = np.hstack((np.zeros(3), ball_init_velocity))
qv_init_val = np.concatenate((q_init_val, v_init_val))
simulator_config = SimulatorConfig(
target_realtime_rate=target_realtime_rate,
publish_every_time_step=True)
simulator = Simulator(diagram)
ApplySimulatorConfig(simulator_config, simulator)
plant_context = diagram.GetSubsystemContext(ball_paddle_plant,
simulator.get_context())
ball_paddle_plant.SetPositionsAndVelocities(plant_context, qv_init_val)
simulator.get_mutable_context().SetTime(0)
state_log = state_logger.FindMutableLog(simulator.get_mutable_context())
state_log.Clear()
simulator.Initialize()
simulator.AdvanceTo(boundary_time=simulation_time)
PrintSimulatorStatistics(simulator)
return state_log.sample_times(), state_log.data()
def test_simulator_status(self):
SimulatorStatus.ReturnReason.kReachedBoundaryTime
SimulatorStatus.ReturnReason.kReachedTerminationCondition
SimulatorStatus.ReturnReason.kEventHandlerFailed
system = ConstantVectorSource([1.])
simulator = Simulator(system)
status = simulator.AdvanceTo(1.)
self.assertRegex(status.FormatMessage(),
"^Simulator successfully reached the boundary time")
self.assertTrue(status.succeeded())
self.assertEqual(status.boundary_time(), 1.)
self.assertEqual(status.return_time(), 1.)
self.assertEqual(status.reason(),
SimulatorStatus.ReturnReason.kReachedBoundaryTime)
self.assertIsNone(status.system())
self.assertEqual(status.message(), "")
self.assertTrue(status.IsIdenticalStatus(other=status))
PrintSimulatorStatistics(simulator)
def test_diagram_simulation(self):
# TODO(eric.cousineau): Move this to `analysis_test.py`.
# Similar to: //systems/framework:diagram_test, ExampleDiagram
size = 3
builder = DiagramBuilder()
self.assertTrue(builder.empty())
adder0 = builder.AddSystem(Adder(2, size))
adder0.set_name("adder0")
self.assertFalse(builder.empty())
adder1 = builder.AddSystem(Adder(2, size))
adder1.set_name("adder1")
integrator = builder.AddSystem(Integrator(size))
integrator.set_name("integrator")
self.assertEqual(
builder.GetMutableSystems(),
[adder0, adder1, integrator])
builder.Connect(adder0.get_output_port(0), adder1.get_input_port(0))
builder.Connect(adder1.get_output_port(0),
integrator.get_input_port(0))
# Exercise naming variants.
builder.ExportInput(adder0.get_input_port(0))
builder.ExportInput(adder0.get_input_port(1), kUseDefaultName)
builder.ExportInput(adder1.get_input_port(1), "third_input")
builder.ExportOutput(integrator.get_output_port(0), "result")
diagram = builder.Build()
self.assertEqual(adder0.get_name(), "adder0")
self.assertEqual(diagram.GetSubsystemByName("adder0"), adder0)
self.assertEqual(
diagram.GetSystems(),
[adder0, adder1, integrator])
# TODO(eric.cousineau): Figure out unicode handling if needed.
# See //systems/framework/test/diagram_test.cc:349 (sha: bc84e73)
# for an example name.
diagram.set_name("test_diagram")
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
# Create and attach inputs.
# TODO(eric.cousineau): Not seeing any assertions being printed if no
# inputs are connected. Need to check this behavior.
input0 = np.array([0.1, 0.2, 0.3])
diagram.get_input_port(0).FixValue(context, input0)
input1 = np.array([0.02, 0.03, 0.04])
diagram.get_input_port(1).FixValue(context, input1)
# Test the BasicVector overload.
input2 = BasicVector([0.003, 0.004, 0.005])
diagram.get_input_port(2).FixValue(context, input2)
# Initialize integrator states.
integrator_xc = (
diagram.GetMutableSubsystemState(integrator, context)
.get_mutable_continuous_state().get_vector())
integrator_xc.SetFromVector([0, 1, 2])
simulator.Initialize()
# Simulate briefly, and take full-context snapshots at intermediate
# points.
n = 6
times = np.linspace(0, 1, n)
context_log = []
for t in times:
simulator.AdvanceTo(t)
# Record snapshot of *entire* context.
context_log.append(context.Clone())
# Test binding for PrintSimulatorStatistics
PrintSimulatorStatistics(simulator)
xc_initial = np.array([0, 1, 2])
xc_final = np.array([0.123, 1.234, 2.345])
for i, context_i in enumerate(context_log):
t = times[i]
self.assertEqual(context_i.get_time(), t)
xc = context_i.get_continuous_state_vector().CopyToVector()
xc_expected = (float(i) / (n - 1) * (xc_final - xc_initial)
+ xc_initial)
self.assertTrue(np.allclose(xc, xc_expected))