def test_simulator_integrator_manipulation(self):
# TODO(eric.cousineau): Move this to `analysis_test.py`.
system = ConstantVectorSource([1])
# Create simulator with basic constructor.
simulator = Simulator(system)
simulator.Initialize()
simulator.set_target_realtime_rate(0)
integrator = simulator.get_mutable_integrator()
target_accuracy = 1E-6
integrator.set_target_accuracy(target_accuracy)
self.assertEqual(integrator.get_target_accuracy(), target_accuracy)
maximum_step_size = 0.2
integrator.set_maximum_step_size(maximum_step_size)
self.assertEqual(integrator.get_maximum_step_size(), maximum_step_size)
minimum_step_size = 2E-2
integrator.set_requested_minimum_step_size(minimum_step_size)
self.assertEqual(integrator.get_requested_minimum_step_size(),
minimum_step_size)
integrator.set_throw_on_minimum_step_size_violation(True)
self.assertTrue(integrator.get_throw_on_minimum_step_size_violation())
integrator.set_fixed_step_mode(True)
self.assertTrue(integrator.get_fixed_step_mode())
const_integrator = simulator.get_integrator()
self.assertTrue(const_integrator is integrator)
def test_simulator_integrator_manipulation(self):
# TODO(eric.cousineau): Move this to `analysis_test.py`.
system = ConstantVectorSource([1])
# Create simulator with basic constructor.
simulator = Simulator(system)
simulator.Initialize()
simulator.set_target_realtime_rate(0)
integrator = simulator.get_mutable_integrator()
target_accuracy = 1E-6
integrator.set_target_accuracy(target_accuracy)
self.assertEqual(integrator.get_target_accuracy(), target_accuracy)
maximum_step_size = 0.2
integrator.set_maximum_step_size(maximum_step_size)
self.assertEqual(integrator.get_maximum_step_size(), maximum_step_size)
minimum_step_size = 2E-2
integrator.set_requested_minimum_step_size(minimum_step_size)
self.assertEqual(integrator.get_requested_minimum_step_size(),
minimum_step_size)
integrator.set_throw_on_minimum_step_size_violation(True)
self.assertTrue(integrator.get_throw_on_minimum_step_size_violation())
integrator.set_fixed_step_mode(True)
self.assertTrue(integrator.get_fixed_step_mode())
const_integrator = simulator.get_integrator()
self.assertTrue(const_integrator is integrator)
# Test context-less constructors for
# integrator types.
test_integrator = RungeKutta2Integrator(
system=system, max_step_size=0.01)
test_integrator = RungeKutta3Integrator(system=system)
# Test simulator's reset_integrator, and also the full constructors for
# all integrator types.
rk2 = RungeKutta2Integrator(
system=system,
max_step_size=0.01,
context=simulator.get_mutable_context())
with catch_drake_warnings(expected_count=1):
# TODO(12873) We need an API for this that isn't deprecated.
simulator.reset_integrator(rk2)
rk3 = RungeKutta3Integrator(
system=system,
context=simulator.get_mutable_context())
with catch_drake_warnings(expected_count=1):
# TODO(12873) We need an API for this that isn't deprecated.
simulator.reset_integrator(rk3)
def test_simulator_integrator_manipulation(self):
system = ConstantVectorSource([1])
# Create simulator with basic constructor.
simulator = Simulator(system)
simulator.Initialize()
simulator.set_target_realtime_rate(0)
integrator = simulator.get_mutable_integrator()
target_accuracy = 1E-6
integrator.set_target_accuracy(target_accuracy)
self.assertEqual(integrator.get_target_accuracy(), target_accuracy)
maximum_step_size = 0.2
integrator.set_maximum_step_size(maximum_step_size)
self.assertEqual(integrator.get_maximum_step_size(), maximum_step_size)
minimum_step_size = 2E-2
integrator.set_requested_minimum_step_size(minimum_step_size)
self.assertEqual(integrator.get_requested_minimum_step_size(),
minimum_step_size)
integrator.set_throw_on_minimum_step_size_violation(True)
self.assertTrue(integrator.get_throw_on_minimum_step_size_violation())
integrator.set_fixed_step_mode(True)
self.assertTrue(integrator.get_fixed_step_mode())
const_integrator = simulator.get_integrator()
self.assertTrue(const_integrator is integrator)
# Test context-less constructors for
# integrator types.
test_integrator = RungeKutta2Integrator(
system=system, max_step_size=0.01)
test_integrator = RungeKutta3Integrator(system=system)
# Test simulator's reset_integrator,
# and also the full constructors for
# all integrator types.
simulator.reset_integrator(
RungeKutta2Integrator(
system=system,
max_step_size=0.01,
context=simulator.get_mutable_context()))
simulator.reset_integrator(
RungeKutta3Integrator(
system=system,
context=simulator.get_mutable_context()))
def test_simulator_integrator_manipulation(self):
system = ConstantVectorSource([1])
# Create simulator with basic constructor.
simulator = Simulator(system)
simulator.Initialize()
simulator.set_target_realtime_rate(0)
integrator = simulator.get_mutable_integrator()
target_accuracy = 1E-6
integrator.set_target_accuracy(target_accuracy)
self.assertEqual(integrator.get_target_accuracy(), target_accuracy)
maximum_step_size = 0.2
integrator.set_maximum_step_size(maximum_step_size)
self.assertEqual(integrator.get_maximum_step_size(), maximum_step_size)
minimum_step_size = 2E-2
integrator.set_requested_minimum_step_size(minimum_step_size)
self.assertEqual(integrator.get_requested_minimum_step_size(),
minimum_step_size)
integrator.set_throw_on_minimum_step_size_violation(True)
self.assertTrue(integrator.get_throw_on_minimum_step_size_violation())
integrator.set_fixed_step_mode(True)
self.assertTrue(integrator.get_fixed_step_mode())
const_integrator = simulator.get_integrator()
self.assertTrue(const_integrator is integrator)
# Test context-less constructors for
# integrator types.
test_integrator = RungeKutta2Integrator(
system=system, max_step_size=0.01)
test_integrator = RungeKutta3Integrator(system=system)
# Test simulator's reset_integrator,
# and also the full constructors for
# all integrator types.
simulator.reset_integrator(
RungeKutta2Integrator(
system=system,
max_step_size=0.01,
context=simulator.get_mutable_context()))
simulator.reset_integrator(
RungeKutta3Integrator(
system=system,
context=simulator.get_mutable_context()))
def test_dense_integration(self):
x = Variable("x")
sys = SymbolicVectorSystem(state=[x], dynamics=[-x+x**3])
simulator = Simulator(sys)
integrator = simulator.get_mutable_integrator()
self.assertIsNone(integrator.get_dense_output())
integrator.StartDenseIntegration()
pp = integrator.get_dense_output()
self.assertIsInstance(pp, PiecewisePolynomial)
simulator.AdvanceTo(1.0)
self.assertIs(pp, integrator.StopDenseIntegration())
self.assertEqual(pp.start_time(), 0.0)
self.assertEqual(pp.end_time(), 1.0)
self.assertIsNone(integrator.get_dense_output())
diagram = builder.Build()
# init_state = np.concatenate((rb_tree.getZeroConfiguration(),np.zeros(9)), axis=0)#.reshape((-1,1))
# print(init_state.shape)
# simulation setting
diagram_context = diagram.CreateDefaultContext()
plant_context = diagram.GetMutableSubsystemContext(plant, diagram_context)
#plant.set_state_vector(diagram_context, init_state)
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(1)
simulator.Initialize()
simulator.get_mutable_integrator().set_target_accuracy(1e-3)
simulation_time = 5
t0 = time.time()
lcm.StartReceiveThread()
simulator.StepTo(simulation_time)
lcm.StopReceiveThread()
tf = time.time()
print("t = ", tf - t0)
#drake_visualizer.ReplayCachedSimulation()
# plot graph
plot_system_graphviz(diagram, max_depth=2147483647)
plt.show()
