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