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_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_simulator_ctor(self): # Create simple system. system = ConstantVectorSource([1]) def check_output(context): # Check number of output ports and value for a given context. output = system.AllocateOutput(context) self.assertEquals(output.get_num_ports(), 1) system.CalcOutput(context, output) value = output.get_vector_data(0).get_value() self.assertTrue(np.allclose([1], value)) # Create simulator with basic constructor. simulator = Simulator(system) simulator.Initialize() simulator.set_target_realtime_rate(0) simulator.set_publish_every_time_step(True) self.assertTrue(simulator.get_context() is simulator.get_mutable_context()) check_output(simulator.get_context()) simulator.StepTo(1) # Create simulator specifying context. context = system.CreateDefaultContext() # @note `simulator` now owns `context`. simulator = Simulator(system, context) self.assertTrue(simulator.get_context() is context) check_output(context) simulator.StepTo(1)
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.) context.FixInputPort(0, 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.CalcPotentialEnergy(context) + \ acrobot.CalcKineticEnergy(context) # Simulate (and make sure the state actually changes). initial_state = state.CopyToVector() simulator.StepTo(1.0) self.assertLessEqual(acrobot.CalcPotentialEnergy(context) + acrobot.CalcKineticEnergy(context), initial_total_energy)
def test_simulator_ctor(self): # Create simple system. system = ConstantVectorSource([1]) def check_output(context): # Check number of output ports and value for a given context. output = system.AllocateOutput(context) self.assertEquals(output.get_num_ports(), 1) system.CalcOutput(context, output) value = output.get_vector_data(0).get_value() self.assertTrue(np.allclose([1], value)) # Create simulator with basic constructor. simulator = Simulator(system) simulator.Initialize() simulator.set_target_realtime_rate(0) simulator.set_publish_every_time_step(True) self.assertTrue( simulator.get_context() is simulator.get_mutable_context()) check_output(simulator.get_context()) simulator.StepTo(1) # Create simulator specifying context. context = system.CreateDefaultContext() # @note `simulator` now owns `context`. simulator = Simulator(system, context) self.assertTrue(simulator.get_context() is context) check_output(context) simulator.StepTo(1)
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 run_pendulum_example(args): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) parser = Parser(plant) parser.AddModelFromFile(FindResourceOrThrow( "drake/examples/pendulum/Pendulum.urdf")) plant.Finalize() T_VW = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]) visualizer = ConnectPlanarSceneGraphVisualizer( builder, scene_graph, T_VW=T_VW, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2]) if args.playback: visualizer.start_recording() diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) # Fix the input port to zero. plant_context = diagram.GetMutableSubsystemContext( plant, simulator.get_mutable_context()) plant.get_actuation_input_port().FixValue( plant_context, np.zeros(plant.num_actuators())) plant_context.SetContinuousState([0.5, 0.1]) simulator.AdvanceTo(args.duration) if args.playback: visualizer.stop_recording() ani = visualizer.get_recording_as_animation() # Playback the recording and save the output. ani.save("{}/pend_playback.mp4".format(temp_directory()), fps=30)
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_lcm_interface_system_diagram(self): # First, check the class doc. self.assertIn("only inherits from LeafSystem", mut.LcmInterfaceSystem.__doc__) # Next, construct a diagram and add both the interface system and # a subscriber. builder = DiagramBuilder() lcm = DrakeLcm() lcm_system = builder.AddSystem(mut.LcmInterfaceSystem(lcm=lcm)) # Create subscriber in the diagram. subscriber = builder.AddSystem( mut.LcmSubscriberSystem.Make(channel="TEST_CHANNEL", lcm_type=lcmt_quaternion, lcm=lcm)) diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() # Publish test message. model_message = self._model_message() lcm.Publish("TEST_CHANNEL", model_message.encode()) # Simulate to a non-zero time to ensure the subscriber picks up the # message. eps = np.finfo(float).eps simulator.AdvanceTo(eps) # Ensure that we have what we want. context = subscriber.GetMyContextFromRoot( simulator.get_mutable_context()) actual_message = subscriber.get_output_port(0).Eval(context) self.assert_lcm_equal(actual_message, model_message)
def _make_visualization(self, stop_time): simulator = Simulator(self.builder.Build()) simulator.Initialize() self.meshcat.vis.render_static() # Set simulator context simulator.get_mutable_context().SetTime(0.0) # Start recording and simulate the diagram self.meshcat.reset_recording() self.meshcat.start_recording() simulator.AdvanceTo(stop_time) # Publish the recording self.meshcat.publish_recording() # Render self.meshcat.vis.render_static() input("View visualization. Press <ENTER> to end") print("Finished")
def run_pendulum_example(args): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) parser = Parser(plant) parser.AddModelFromFile( FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf")) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base")) plant.Finalize() pose_bundle_output_port = scene_graph.get_pose_bundle_output_port() Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem( PlanarSceneGraphVisualizer(scene_graph, T_VW=Tview, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2])) builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) # Fix the input port to zero. plant_context = diagram.GetMutableSubsystemContext( plant, simulator.get_mutable_context()) plant_context.FixInputPort(plant.get_actuation_input_port().get_index(), np.zeros(plant.num_actuators())) plant_context.SetContinuousState([0.5, 0.1]) simulator.AdvanceTo(args.duration)
def setUp(self): builder = DiagramBuilder() X_WP_0 = RigidTransform.Identity() X_WP_1 = RigidTransform.Identity() X_WP_1.set_translation([1.0, 0, 0]) id_list = ["0", "1"] self.pc_concat = builder.AddSystem(PointCloudConcatenation(id_list)) self.num_points = 10000 xyzs = np.random.uniform(-0.1, 0.1, (3, self.num_points)) # Only go to 254 to distinguish between point clouds with and without # color. rgbs = np.random.uniform(0., 254.0, (3, self.num_points)) self.pc = PointCloud(self.num_points, Fields(BaseField.kXYZs | BaseField.kRGBs)) self.pc.mutable_xyzs()[:] = xyzs self.pc.mutable_rgbs()[:] = rgbs self.pc_no_rgbs = PointCloud(self.num_points, Fields(BaseField.kXYZs)) self.pc_no_rgbs.mutable_xyzs()[:] = xyzs diagram = builder.Build() simulator = Simulator(diagram) self.context = diagram.GetMutableSubsystemContext( self.pc_concat, simulator.get_mutable_context()) self.pc_concat.GetInputPort("X_FCi_0").FixValue(self.context, X_WP_0) self.pc_concat.GetInputPort("X_FCi_1").FixValue(self.context, X_WP_1)
def test_simulator_context_manipulation(self): system = ConstantVectorSource([1]) # Use default-constructed context. simulator = Simulator(system) self.assertTrue(simulator.has_context()) context_default = simulator.get_mutable_context() # WARNING: Once we call `simulator.reset_context()`, it will delete the # context it currently owns, which is `context_default` in this case. # BE CAREFUL IN SITUATIONS LIKE THIS! # TODO(eric.cousineau): Bind `release_context()`, or migrate context # usage to use `shared_ptr`. context = system.CreateDefaultContext() simulator.reset_context(context) self.assertIs(context, simulator.get_mutable_context()) # WARNING: This will also invalidate `context`. Be careful! simulator.reset_context(None) self.assertFalse(simulator.has_context())
def pendulum_example(duration=1., playback=True, show=True): """ Simulate the pendulum Arguments: duration: Simulation duration (sec) playback: enable pyplot animations """ # To make a visualization, we have to attach a multibody plant, a scene graph, and a visualizer together. In Drake, we can connect all these systems together in a Diagram. builder = DiagramBuilder() # AddMultibodyPlantSceneGraph: Adds a multibody plant and scene graph to the Diagram, and connects their geometry ports. The second input is the timestep for MultibodyPlant plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.) # Now we create the plant model from a file parser = Parser(plant) parser.AddModelFromFile( FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf")) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base")) plant.Finalize() # The SceneGraph port that communicates with the visualizer is the pose bundle output port. A PoseBundle is just a set of poses in SE(3) and a set of frame velocities, expressed in the world frame, used for rendering. pose_bundle_output_port = scene_graph.get_pose_bundle_output_port() # T_VW is the projection matrix from view coordinates to world coordinates T_VW = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]) # Now we add a planar visualizer to the the diagram, so we can see the results visualizer = builder.AddSystem( PlanarSceneGraphVisualizer(scene_graph, T_VW=T_VW, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2], show=show)) # finally, we must connect the scene_graph to the visualizer so they can communicate builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0)) if playback: visualizer.start_recording() # To finalize the diagram, we build it diagram = builder.Build() # We create a simulator of our diagram to step through the diagram in time simulator = Simulator(diagram) # Initialize prepares the simulator for simulation simulator.Initialize() # Slow down the simulator to realtime. Otherwise it could run too fast simulator.set_target_realtime_rate(1.) # To set initial conditions, we modify the mutable simulator context (we could do this before Initialize) plant_context = diagram.GetMutableSubsystemContext( plant, simulator.get_mutable_context()) plant_context.SetContinuousState([0.5, 0.1]) # Now we fix the value of the actuation to get an unactuated simulation plant.get_actuation_input_port().FixValue( plant_context, np.zeros([plant.num_actuators()])) # Run the simulation to the specified duration simulator.AdvanceTo(duration) # Return an animation, if one was made if playback: visualizer.stop_recording() ani = visualizer.get_recording_as_animation() return ani else: return None
def main(): args_parser = argparse.ArgumentParser() args_parser.add_argument("--simulation_sec", type=float, default=np.inf) args_parser.add_argument("--sim_dt", type=float, default=0.1) args_parser.add_argument( "--single_shot", action="store_true", help="Test workflow of visulaization through Simulator.Initialize") args_parser.add_argument("--realtime_rate", type=float, default=1.) args_parser.add_argument("--num_models", type=int, default=3) args = args_parser.parse_args() sdf_file = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/iiwa7/" "iiwa7_no_collision.sdf") builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.01) parser = Parser(plant) models = [] for i in range(args.num_models): model_name = f"iiwa{i}" # TODO(eric.cousineau): This warns about mutating the package path # multiple times :( model = parser.AddModelFromFile(sdf_file, model_name) models.append(model) base_frame = plant.GetFrameByName("iiwa_link_0", model) # Translate along x-axis by 1m to separate. X_WB = RigidTransform([i, 0, 0]) plant.WeldFrames(plant.world_frame(), base_frame, X_WB) plant.Finalize() for model in models: no_control(plant, builder, model) ConnectDrakeVisualizer(builder, scene_graph) ConnectRvizVisualizer(builder, scene_graph) diagram = builder.Build() simulator = Simulator(diagram) context = simulator.get_mutable_context() simulator.set_target_realtime_rate(args.realtime_rate) # Wait for ROS publishers to wake up :( time.sleep(0.3) if args.single_shot: # To see what 'preview' scripts look like. # TODO(eric.cousineau): Make this work *robustly* with Rviz. Markers # still don't always show up :( simulator.Initialize() diagram.Publish(context) else: while context.get_time() < args.simulation_sec: # Use increments to permit Ctrl+C to be caught. simulator.AdvanceTo(context.get_time() + args.sim_dt)
def run_pendulum_example(duration=1.0, playback=True, show=True): """ Runs a simulation of a pendulum Arguments: duration: Simulation duration (sec). playback: Enable pyplot animations to be produced. """ builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) parser = Parser(plant) parser.AddModelFromFile( FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf")) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base")) plant.Finalize() pose_bundle_output_port = scene_graph.get_pose_bundle_output_port() T_VW = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]) visualizer = builder.AddSystem( PlanarSceneGraphVisualizer(scene_graph, T_VW=T_VW, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2], show=show)) builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0)) if playback: visualizer.start_recording() diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) # Fix the input port to zero plant_context = diagram.GetMutableSubsystemContext( plant, simulator.get_mutable_context()) plant.get_actuation_input_port().FixValue(plant_context, np.zeros(plant.num_actuators())) plant_context.SetContinuousState([0.5, 0.1]) simulator.AdvanceTo(duration) if playback: visualizer.stop_recording() ani = visualizer.get_recording_as_animation() return ani else: return None
def test_simulation(self): van_der_pol = VanDerPolOscillator() # Create the simulator. simulator = Simulator(van_der_pol) context = simulator.get_mutable_context() # Set the initial state. state = context.get_mutable_continuous_state_vector() state.SetFromVector([0., 2.]) # Simulate (and make sure the state actually changes). initial_state = state.CopyToVector() simulator.StepTo(1.0) self.assertFalse((state.CopyToVector() == initial_state).any())
def __init__(self, tree): lcm = DrakeLcm() self.tree = tree self.visualizer = DrakeVisualizer(tree=self.tree, lcm=lcm, enable_playback=True) self.x = np.concatenate([ robot.getZeroConfiguration(), np.zeros(tree.get_num_velocities()) ]) # Workaround for the fact that PublishLoadRobot is not public. # Ultimately that should be fixed. sim = Simulator(self.visualizer) context = sim.get_mutable_context() context.FixInputPort(0, BasicVector(self.x)) sim.Initialize()
def run_manipulation_example(args): builder = DiagramBuilder() station = builder.AddSystem(ManipulationStation(time_step=0.005)) station.SetupManipulationClassStation() station.Finalize() plant = station.get_multibody_plant() scene_graph = station.get_scene_graph() pose_bundle_output_port = station.GetOutputPort("pose_bundle") # Side-on view of the station. T_VW = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]) visualizer = builder.AddSystem(PlanarSceneGraphVisualizer( scene_graph, T_VW=T_VW, xlim=[-0.5, 1.0], ylim=[-1.2, 1.2], draw_period=0.1)) builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0)) if args.playback: visualizer.start_recording() diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) # Fix the control inputs to zero. station_context = diagram.GetMutableSubsystemContext( station, simulator.get_mutable_context()) station.GetInputPort("iiwa_position").FixValue( station_context, station.GetIiwaPosition(station_context)) station.GetInputPort("iiwa_feedforward_torque").FixValue( station_context, np.zeros(7)) station.GetInputPort("wsg_position").FixValue( station_context, np.zeros(1)) station.GetInputPort("wsg_force_limit").FixValue( station_context, [40.0]) simulator.AdvanceTo(args.duration) if args.playback: visualizer.stop_recording() ani = visualizer.get_recording_as_animation() # Playback the recording and save the output. ani.save("{}/manip_playback.mp4".format(temp_directory()), fps=30)
def test_simulation(self): # Basic constant-torque rimless_wheel simulation. rimless_wheel = RimlessWheel() # Create the simulator. simulator = Simulator(rimless_wheel) context = simulator.get_mutable_context() context.set_accuracy(1e-8) # Set the initial state. state = context.get_mutable_continuous_state_vector() state.set_theta(0.) state.set_thetadot(4.) # Simulate (and make sure the state actually changes). initial_state = state.CopyToVector() simulator.StepTo(1.0) self.assertFalse((state.CopyToVector() == initial_state).any())
def test_simulation(self): # Basic rimless_wheel simulation. rimless_wheel = RimlessWheel() # Create the simulator. simulator = Simulator(rimless_wheel) context = simulator.get_mutable_context() context.set_accuracy(1e-8) # Set the initial state. state = context.get_mutable_continuous_state_vector() state.set_theta(0.) state.set_thetadot(4.) # Simulate (and make sure the state actually changes). initial_state = state.CopyToVector() simulator.StepTo(1.0) self.assertFalse((state.CopyToVector() == initial_state).any())
def test_simulation(self): # Basic compass_gait simulation. compass_gait = CompassGait() # Create the simulator. simulator = Simulator(compass_gait) context = simulator.get_mutable_context() context.SetAccuracy(1e-8) # Set the initial state. state = context.get_mutable_continuous_state_vector() state.set_stance(0.) state.set_swing(0.) state.set_stancedot(0.4) state.set_swingdot(-2.0) # Simulate (and make sure the state actually changes). initial_state = state.CopyToVector() simulator.AdvanceTo(1.0) self.assertFalse((state.CopyToVector() == initial_state).any())
def test_simulation(self): # Basic constant-torque pendulum simulation. pendulum = PendulumPlant() # Create the simulator. simulator = Simulator(pendulum) context = simulator.get_mutable_context() # Set an input torque. input = PendulumInput() input.set_tau(1.) context.FixInputPort(0, input) # Set the initial state. state = context.get_mutable_continuous_state_vector() state.set_theta(1.) state.set_thetadot(0.) # Simulate (and make sure the state actually changes). initial_state = state.CopyToVector() simulator.StepTo(1.0) self.assertFalse((state.CopyToVector() == initial_state).any())
def test_simulation(self): # Basic constant-torque pendulum simulation. pendulum = PendulumPlant() # Create the simulator. simulator = Simulator(pendulum) context = simulator.get_mutable_context() # Set an input torque. input = PendulumInput() input.set_tau(1.) context.FixInputPort(0, input) # Set the initial state. state = context.get_mutable_continuous_state_vector() state.set_theta(1.) state.set_thetadot(0.) # Simulate (and make sure the state actually changes). initial_state = state.CopyToVector() simulator.AdvanceTo(1.0) self.assertFalse((state.CopyToVector() == initial_state).any())
def run_manipulation_example(args): builder = DiagramBuilder() station = builder.AddSystem(ManipulationStation(time_step=0.005)) station.SetupManipulationClassStation() station.Finalize() plant = station.get_multibody_plant() scene_graph = station.get_scene_graph() pose_bundle_output_port = station.GetOutputPort("pose_bundle") # Side-on view of the station. Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem(PlanarSceneGraphVisualizer( scene_graph, T_VW=Tview, xlim=[-0.5, 1.0], ylim=[-1.2, 1.2], draw_period=0.1)) builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) # Fix the control inputs to zero. station_context = diagram.GetMutableSubsystemContext( station, simulator.get_mutable_context()) station.GetInputPort("iiwa_position").FixValue( station_context, station.GetIiwaPosition(station_context)) station.GetInputPort("iiwa_feedforward_torque").FixValue( station_context, np.zeros(7)) station.GetInputPort("wsg_position").FixValue( station_context, np.zeros(1)) station.GetInputPort("wsg_force_limit").FixValue( station_context, [40.0]) simulator.AdvanceTo(args.duration)
def test_simple_car(self): simple_car = SimpleCar() simulator = Simulator(simple_car) context = simulator.get_mutable_context() output = simple_car.AllocateOutput() # Fix the input. command = DrivingCommand() command.set_steering_angle(0.5) command.set_acceleration(1.) context.FixInputPort(0, command) # Verify the inputs. command_eval = simple_car.EvalVectorInput(context, 0) self.assertTrue(np.allclose( command.get_value(), command_eval.get_value())) # Initialize all the states to zero and take a simulation step. state = context.get_mutable_continuous_state_vector() state.SetFromVector([0.] * state.size()) simulator.StepTo(1.0) # Verify the outputs. simple_car.CalcOutput(context, output) state_index = simple_car.state_output().get_index() state_value = output.get_vector_data(state_index) self.assertIsInstance(state_value, SimpleCarState) self.assertTrue( np.allclose(state.CopyToVector(), state_value.get_value())) pose_index = simple_car.pose_output().get_index() pose_value = output.get_vector_data(pose_index) self.assertIsInstance(pose_value, PoseVector) self.assertTrue(pose_value.get_translation()[0] > 0.) velocity_index = simple_car.velocity_output().get_index() velocity_value = output.get_vector_data(velocity_index) self.assertIsInstance(velocity_value, FrameVelocity) self.assertTrue(velocity_value.get_velocity().translational()[0] > 0.)
def test_simple_car(self): simple_car = SimpleCar() simulator = Simulator(simple_car) context = simulator.get_mutable_context() output = simple_car.AllocateOutput() # Fix the input. command = DrivingCommand() command.set_steering_angle(0.5) command.set_acceleration(1.) context.FixInputPort(0, command) # Verify the inputs. command_eval = simple_car.EvalVectorInput(context, 0) self.assertTrue(np.allclose( command.get_value(), command_eval.get_value())) # Initialize all the states to zero and take a simulation step. state = context.get_mutable_continuous_state_vector() state.SetFromVector([0.] * state.size()) simulator.StepTo(1.0) # Verify the outputs. simple_car.CalcOutput(context, output) state_index = simple_car.state_output().get_index() state_value = output.get_vector_data(state_index) self.assertIsInstance(state_value, SimpleCarState) self.assertTrue( np.allclose(state.CopyToVector(), state_value.get_value())) pose_index = simple_car.pose_output().get_index() pose_value = output.get_vector_data(pose_index) self.assertIsInstance(pose_value, PoseVector) self.assertTrue(pose_value.get_translation()[0] > 0.) velocity_index = simple_car.velocity_output().get_index() velocity_value = output.get_vector_data(velocity_index) self.assertIsInstance(velocity_value, FrameVelocity) self.assertTrue(velocity_value.get_velocity().translational()[0] > 0.)
def test_simulation(self): # Basic constant-torque pendulum simulation. builder = framework.DiagramBuilder() pendulum = builder.AddSystem(PendulumPlant()) source = builder.AddSystem(ConstantVectorSource([1.0])) builder.Connect(source.get_output_port(0), pendulum.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() # TODO(russt): Clean up state vector access below. state = simulator.get_mutable_context().get_mutable_state()\ .get_mutable_continuous_state().get_mutable_vector() initial_state = np.array([1.0, 0.0]) state.SetFromVector(initial_state) simulator.StepTo(1.0) self.assertFalse((state.CopyToVector() == initial_state).all())
def test_leaf_system_overrides(self): test = self class TrivialSystem(LeafSystem): def __init__(self): LeafSystem.__init__(self) self.called_publish = False self.called_feedthrough = False self.called_continuous = False self.called_discrete = False self.called_initialize = False self.called_per_step = False self.called_periodic = False self.called_getwitness = False self.called_witness = False self.called_guard = False self.called_reset = False # Ensure we have desired overloads. self.DeclarePeriodicPublish(1.0) self.DeclarePeriodicPublish(1.0, 0) self.DeclarePeriodicPublish(period_sec=1.0, offset_sec=0.) self.DeclarePeriodicDiscreteUpdate( period_sec=1.0, offset_sec=0.) self.DeclareInitializationEvent( event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=self._on_initialize)) self.DeclarePerStepEvent( event=PublishEvent( trigger_type=TriggerType.kPerStep, callback=self._on_per_step)) self.DeclarePeriodicEvent( period_sec=1.0, offset_sec=0.0, event=PublishEvent( trigger_type=TriggerType.kPeriodic, callback=self._on_periodic)) self.DeclareContinuousState(2) self.DeclareDiscreteState(1) # Ensure that we have inputs / outputs to call direct # feedthrough. self.DeclareInputPort(PortDataType.kVectorValued, 1) self.DeclareVectorInputPort( name="test_input", model_vector=BasicVector(1), random_type=None) self.DeclareVectorOutputPort( "noop", BasicVector(1), noop, prerequisites_of_calc=set([self.nothing_ticket()])) self.witness = self.MakeWitnessFunction( "witness", WitnessFunctionDirection.kCrossesZero, self._witness) self.reset_witness = self.MakeWitnessFunction( "reset", WitnessFunctionDirection.kCrossesZero, self._guard, UnrestrictedUpdateEvent(self._reset)) def DoPublish(self, context, events): # Call base method to ensure we do not get recursion. LeafSystem.DoPublish(self, context, events) # N.B. We do not test for a singular call to `DoPublish` # (checking `assertFalse(self.called_publish)` first) because # the above `_DeclareInitializationEvent` will call both its # callback and this event when invoked via # `Simulator::Initialize` from `call_leaf_system_overrides`, # even when we explicitly say not to publish at initialize. self.called_publish = True def DoHasDirectFeedthrough(self, input_port, output_port): # Test inputs. test.assertIn(input_port, [0, 1]) test.assertEqual(output_port, 0) # Call base method to ensure we do not get recursion. with catch_drake_warnings(expected_count=1): base_return = LeafSystem.DoHasDirectFeedthrough( self, input_port, output_port) test.assertTrue(base_return is None) # Return custom methods. self.called_feedthrough = True return False def DoCalcTimeDerivatives(self, context, derivatives): # Note: Don't call base method here; it would abort because # derivatives.size() != 0. test.assertEqual(derivatives.get_vector().size(), 2) self.called_continuous = True def DoCalcDiscreteVariableUpdates( self, context, events, discrete_state): # Call base method to ensure we do not get recursion. LeafSystem.DoCalcDiscreteVariableUpdates( self, context, events, discrete_state) self.called_discrete = True def DoGetWitnessFunctions(self, context): self.called_getwitness = True return [self.witness, self.reset_witness] def _on_initialize(self, context, event): test.assertIsInstance(context, Context) test.assertIsInstance(event, PublishEvent) test.assertFalse(self.called_initialize) self.called_initialize = True def _on_per_step(self, context, event): test.assertIsInstance(context, Context) test.assertIsInstance(event, PublishEvent) self.called_per_step = True def _on_periodic(self, context, event): test.assertIsInstance(context, Context) test.assertIsInstance(event, PublishEvent) test.assertFalse(self.called_periodic) self.called_periodic = True def _witness(self, context): test.assertIsInstance(context, Context) self.called_witness = True return 1.0 def _guard(self, context): test.assertIsInstance(context, Context) self.called_guard = True return context.get_time() - 0.5 def _reset(self, context, event, state): test.assertIsInstance(context, Context) test.assertIsInstance(event, UnrestrictedUpdateEvent) test.assertIsInstance(state, State) self.called_reset = True system = TrivialSystem() self.assertFalse(system.called_publish) self.assertFalse(system.called_feedthrough) self.assertFalse(system.called_continuous) self.assertFalse(system.called_discrete) self.assertFalse(system.called_initialize) results = call_leaf_system_overrides(system) self.assertTrue(system.called_publish) self.assertTrue(system.called_feedthrough) self.assertFalse(results["has_direct_feedthrough"]) self.assertTrue(system.called_continuous) self.assertTrue(system.called_discrete) self.assertTrue(system.called_initialize) self.assertEqual(results["discrete_next_t"], 1.0) self.assertFalse(system.HasAnyDirectFeedthrough()) self.assertFalse(system.HasDirectFeedthrough(output_port=0)) self.assertFalse( system.HasDirectFeedthrough(input_port=0, output_port=0)) # Test explicit calls. system = TrivialSystem() context = system.CreateDefaultContext() system.Publish(context) self.assertTrue(system.called_publish) context_update = context.Clone() system.CalcTimeDerivatives( context=context, derivatives=context_update.get_mutable_continuous_state()) self.assertTrue(system.called_continuous) witnesses = system.GetWitnessFunctions(context) self.assertEqual(len(witnesses), 2) system.CalcDiscreteVariableUpdates( context=context, discrete_state=context_update.get_mutable_discrete_state()) self.assertTrue(system.called_discrete) # Test per-step, periodic, and witness call backs system = TrivialSystem() simulator = Simulator(system) simulator.get_mutable_context().SetAccuracy(0.1) # Stepping to 0.99 so that we get exactly one periodic event. simulator.AdvanceTo(0.99) self.assertTrue(system.called_per_step) self.assertTrue(system.called_periodic) self.assertTrue(system.called_getwitness) self.assertTrue(system.called_witness) self.assertTrue(system.called_guard) self.assertTrue(system.called_reset)
def test_diagram_simulation(self): # Similar to: //systems/framework:diagram_test, ExampleDiagram size = 3 builder = DiagramBuilder() adder0 = builder.AddSystem(Adder(2, size)) adder0.set_name("adder0") adder1 = builder.AddSystem(Adder(2, size)) adder1.set_name("adder1") integrator = builder.AddSystem(Integrator(size)) integrator.set_name("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)) builder.ExportInput(adder0.get_input_port(0)) builder.ExportInput(adder0.get_input_port(1)) builder.ExportInput(adder1.get_input_port(1)) builder.ExportOutput(integrator.get_output_port(0)) diagram = builder.Build() # 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]) context.FixInputPort(0, input0) input1 = np.array([0.02, 0.03, 0.04]) context.FixInputPort(1, input1) input2 = BasicVector([0.003, 0.004, 0.005]) context.FixInputPort(2, input2) # Test the BasicVector overload. # 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.StepTo(t) # Record snapshot of *entire* context. context_log.append(context.Clone()) 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) print("xc[t = {}] = {}".format(t, xc)) self.assertTrue(np.allclose(xc, xc_expected))
teleop.window.withdraw() # Don't display the window when testing. builder.Connect(teleop.get_output_port(0), station.GetInputPort("iiwa_position")) wsg_buttons = builder.AddSystem(SchunkWsgButtons(teleop.window)) builder.Connect(wsg_buttons.GetOutputPort("position"), station.GetInputPort("wsg_position")) builder.Connect(wsg_buttons.GetOutputPort("force_limit"), station.GetInputPort("wsg_force_limit")) diagram = builder.Build() simulator = Simulator(diagram) station_context = diagram.GetMutableSubsystemContext( station, simulator.get_mutable_context()) station_context.FixInputPort( station.GetInputPort("iiwa_feedforward_torque").get_index(), np.zeros(7)) if not args.hardware: # Set the initial positions of the IIWA to a comfortable configuration # inside the workspace of the station. q0 = [0, 0.6, 0, -1.75, 0, 1.0, 0] station.SetIiwaPosition(q0, station_context) station.SetIiwaVelocity(np.zeros(7), station_context) # Set the initial configuration of the gripper to open. station.SetWsgPosition(0.1, station_context) station.SetWsgVelocity(0, station_context)
def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument( "--target_realtime_rate", type=float, default=1.0, help="Desired rate relative to real time. See documentation for " "Simulator::set_target_realtime_rate() for details.") parser.add_argument("--duration", type=float, default=np.inf, help="Desired duration of the simulation in seconds.") parser.add_argument( "--hardware", action='store_true', help="Use the ManipulationStationHardwareInterface instead of an " "in-process simulation.") parser.add_argument("--test", action='store_true', help="Disable opening the gui window for testing.") parser.add_argument( '--setup', type=str, default='manipulation_class', help="The manipulation station setup to simulate. ", choices=['manipulation_class', 'clutter_clearing', 'planar']) parser.add_argument( "-w", "--open-window", dest="browser_new", action="store_const", const=1, default=None, help="Open the MeshCat display in a new browser window.") args = parser.parse_args() builder = DiagramBuilder() # NOTE: the meshcat instance is always created in order to create the # teleop controls (joint sliders and open/close gripper button). When # args.hardware is True, the meshcat server will *not* display robot # geometry, but it will contain the joint sliders and open/close gripper # button in the "Open Controls" tab in the top-right of the viewing server. meshcat = Meshcat() if args.hardware: # TODO(russt): Replace this hard-coded camera serial number with a # config file. camera_ids = ["805212060544"] station = builder.AddSystem( ManipulationStationHardwareInterface(camera_ids)) station.Connect(wait_for_cameras=False) else: station = builder.AddSystem(ManipulationStation()) # Initializes the chosen station type. if args.setup == 'manipulation_class': station.SetupManipulationClassStation() station.AddManipulandFromFile( "drake/examples/manipulation_station/models/" + "061_foam_brick.sdf", RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0])) elif args.setup == 'clutter_clearing': station.SetupClutterClearingStation() ycb_objects = CreateClutterClearingYcbObjectList() for model_file, X_WObject in ycb_objects: station.AddManipulandFromFile(model_file, X_WObject) elif args.setup == 'planar': station.SetupPlanarIiwaStation() station.AddManipulandFromFile( "drake/examples/manipulation_station/models/" + "061_foam_brick.sdf", RigidTransform(RotationMatrix.Identity(), [0.6, 0, 0])) station.Finalize() geometry_query_port = station.GetOutputPort("geometry_query") DrakeVisualizer.AddToBuilder(builder, geometry_query_port) meshcat_visualizer = MeshcatVisualizerCpp.AddToBuilder( builder=builder, query_object_port=geometry_query_port, meshcat=meshcat) if args.setup == 'planar': meshcat.Set2dRenderMode() pyplot_visualizer = ConnectPlanarSceneGraphVisualizer( builder, station.get_scene_graph(), geometry_query_port) if args.browser_new is not None: url = meshcat.web_url() webbrowser.open(url=url, new=args.browser_new) teleop = builder.AddSystem( JointSliders(meshcat=meshcat, plant=station.get_controller_plant())) num_iiwa_joints = station.num_iiwa_joints() filter = builder.AddSystem( FirstOrderLowPassFilter(time_constant=2.0, size=num_iiwa_joints)) builder.Connect(teleop.get_output_port(0), filter.get_input_port(0)) builder.Connect(filter.get_output_port(0), station.GetInputPort("iiwa_position")) wsg_buttons = builder.AddSystem(SchunkWsgButtons(meshcat=meshcat)) builder.Connect(wsg_buttons.GetOutputPort("position"), station.GetInputPort("wsg_position")) builder.Connect(wsg_buttons.GetOutputPort("force_limit"), station.GetInputPort("wsg_force_limit")) # When in regression test mode, log our joint velocities to later check # that they were sufficiently quiet. if args.test: iiwa_velocities = builder.AddSystem(VectorLogSink(num_iiwa_joints)) builder.Connect(station.GetOutputPort("iiwa_velocity_estimated"), iiwa_velocities.get_input_port(0)) else: iiwa_velocities = None diagram = builder.Build() simulator = Simulator(diagram) # This is important to avoid duplicate publishes to the hardware interface: simulator.set_publish_every_time_step(False) station_context = diagram.GetMutableSubsystemContext( station, simulator.get_mutable_context()) station.GetInputPort("iiwa_feedforward_torque").FixValue( station_context, np.zeros(num_iiwa_joints)) # If the diagram is only the hardware interface, then we must advance it a # little bit so that first LCM messages get processed. A simulated plant is # already publishing correct positions even without advancing, and indeed # we must not advance a simulated plant until the sliders and filters have # been initialized to match the plant. if args.hardware: simulator.AdvanceTo(1e-6) # Eval the output port once to read the initial positions of the IIWA. q0 = station.GetOutputPort("iiwa_position_measured").Eval(station_context) teleop.SetPositions(q0) filter.set_initial_output_value( diagram.GetMutableSubsystemContext(filter, simulator.get_mutable_context()), q0) simulator.set_target_realtime_rate(args.target_realtime_rate) simulator.AdvanceTo(args.duration) # Ensure that our initialization logic was correct, by inspecting our # logged joint velocities. if args.test: iiwa_velocities_log = iiwa_velocities.FindLog(simulator.get_context()) for time, qdot in zip(iiwa_velocities_log.sample_times(), iiwa_velocities_log.data().transpose()): # TODO(jwnimmer-tri) We should be able to do better than a 40 # rad/sec limit, but that's the best we can enforce for now. if qdot.max() > 0.1: print(f"ERROR: large qdot {qdot} at time {time}") sys.exit(1)
FirstOrderLowPassFilter(time_constant=args.filter_time_const, size=6)) builder.Connect(teleop.get_output_port(0), filter.get_input_port(0)) builder.Connect(filter.get_output_port(0), differential_ik.GetInputPort("rpy_xyz_desired")) builder.Connect(teleop.GetOutputPort("position"), station.GetInputPort( "wsg_position")) builder.Connect(teleop.GetOutputPort("force_limit"), station.GetInputPort("wsg_force_limit")) diagram = builder.Build() simulator = Simulator(diagram) station_context = diagram.GetMutableSubsystemContext( station, simulator.get_mutable_context()) station_context.FixInputPort(station.GetInputPort( "iiwa_feedforward_torque").get_index(), np.zeros(7)) q0 = station.GetOutputPort("iiwa_position_measured").Eval(station_context) differential_ik.parameters.set_nominal_joint_position(q0) teleop.SetPose(differential_ik.ForwardKinematics(q0)) filter.set_initial_output_value( diagram.GetMutableSubsystemContext( filter, simulator.get_mutable_context()), teleop.get_output_port(0).Eval(diagram.GetMutableSubsystemContext( teleop, simulator.get_mutable_context()))) differential_ik.SetPositions(diagram.GetMutableSubsystemContext( differential_ik, simulator.get_mutable_context()), q0)
np.array([1, 1, 1, 1]), scene_graph) env_plant.Finalize() plant.RegisterGeometry(scene_graph) builder.Connect(plant.get_geometry_pose_output_port(), scene_graph.get_source_pose_port(plant.source_id())) meshcat = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=None, open_browser=None)) builder.Connect(scene_graph.get_pose_bundle_output_port(), meshcat.get_input_port(0)) # end setup for visualization diagram = builder.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) context = simulator.get_mutable_context() # Run simulation context.set_time(0.) mutable_state_vector = context.get_continuous_state_vector().CopyToVector() mutable_state_vector[:12] = START_STATE context.SetContinuousState(mutable_state_vector) simulator.Initialize() import pdb pdb.set_trace() from time import sleep sleep(3) simulator.StepTo(DURATION) import pdb pdb.set_trace()
builder = DiagramBuilder() plant = builder.AddSystem(QuadrotorPlant()) controller = builder.AddSystem(StabilizingLQRController(plant, [0, 0, 1])) builder.Connect(controller.get_output_port(0), plant.get_input_port(0)) builder.Connect(plant.get_output_port(0), controller.get_input_port(0)) # Set up visualization in MeshCat scene_graph = builder.AddSystem(SceneGraph()) QuadrotorGeometry.AddToBuilder(builder, plant.get_output_port(0), scene_graph) meshcat = builder.AddSystem(MeshcatVisualizer( scene_graph, zmq_url=args.meshcat, open_browser=args.open_browser)) builder.Connect(scene_graph.get_pose_bundle_output_port(), meshcat.get_input_port(0)) # end setup for visualization diagram = builder.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) context = simulator.get_mutable_context() for i in range(args.trials): context.SetTime(0.) context.SetContinuousState(np.random.randn(12,)) simulator.Initialize() simulator.StepTo(args.duration)
options.visualization_callback = draw policy, cost_to_go = FittedValueIteration(simulator, cost_function, state_grid, input_grid, timestep, options) J = np.reshape(cost_to_go, Q.shape) surf = ax.plot_surface(Q, Qdot, J, rstride=1, cstride=1, cmap=cm.jet) Pi = np.reshape(policy.get_output_values(), Q.shape) surf = ax2.plot_surface(Q, Qdot, Pi, rstride=1, cstride=1, cmap=cm.jet) # animate the resulting policy. builder = DiagramBuilder() plant = builder.AddSystem(DoubleIntegrator()) logger = LogOutput(plant.get_output_port(0), builder) vi_policy = builder.AddSystem(policy) builder.Connect(vi_policy.get_output_port(0), plant.get_input_port(0)) builder.Connect(plant.get_output_port(0), vi_policy.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) state = simulator.get_mutable_context().SetContinuousState([-10.0, 0.0]) simulator.AdvanceTo(10.) # Visualize the result as a video. vis = DoubleIntegratorVisualizer() ani = vis.animate(logger, repeat=True) plt.show()
state_grid, input_grid, timestep, options) J = np.reshape(cost_to_go, Q.shape) surf = ax.plot_surface(Q, Qdot, J, rstride=1, cstride=1, cmap=cm.jet) Pi = np.reshape(policy.get_output_values(), Q.shape) surf = ax2.plot_surface(Q, Qdot, Pi, rstride=1, cstride=1, cmap=cm.jet) # animate the resulting policy. builder = DiagramBuilder() plant = builder.AddSystem(DoubleIntegrator()) logger = LogOutput(plant.get_output_port(0), builder) vi_policy = builder.AddSystem(policy) builder.Connect(vi_policy.get_output_port(0), plant.get_input_port(0)) builder.Connect(plant.get_output_port(0), vi_policy.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) state = simulator.get_mutable_context().SetContinuousState([-10.0, 0.0]) simulator.StepTo(10.) # Visualize the result as a video. vis = DoubleIntegratorVisualizer() ani = vis.animate(logger, repeat=True) plt.show()
size=7)) builder.Connect(teleop.get_output_port(0), filter.get_input_port(0)) builder.Connect(filter.get_output_port(0), station.GetInputPort("iiwa_position")) wsg_buttons = builder.AddSystem(SchunkWsgButtons(teleop.window)) builder.Connect(wsg_buttons.GetOutputPort("position"), station.GetInputPort( "wsg_position")) builder.Connect(wsg_buttons.GetOutputPort("force_limit"), station.GetInputPort("wsg_force_limit")) diagram = builder.Build() simulator = Simulator(diagram) station_context = diagram.GetMutableSubsystemContext( station, simulator.get_mutable_context()) station_context.FixInputPort(station.GetInputPort( "iiwa_feedforward_torque").get_index(), np.zeros(7)) # Eval the output port once to read the initial positions of the IIWA. q0 = station.GetOutputPort("iiwa_position_measured").Eval( station_context).get_value() teleop.set_position(q0) filter.set_initial_output_value(diagram.GetMutableSubsystemContext( filter, simulator.get_mutable_context()), q0) # This is important to avoid duplicate publishes to the hardware interface: simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(args.target_realtime_rate)
def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument( "--target_realtime_rate", type=float, default=1.0, help="Desired rate relative to real time. See documentation for " "Simulator::set_target_realtime_rate() for details.") parser.add_argument( "--duration", type=float, default=np.inf, help="Desired duration of the simulation in seconds.") parser.add_argument( "--hardware", action='store_true', help="Use the ManipulationStationHardwareInterface instead of an " "in-process simulation.") parser.add_argument( "--test", action='store_true', help="Disable opening the gui window for testing.") parser.add_argument( "--filter_time_const", type=float, default=0.005, help="Time constant for the first order low pass filter applied to" "the teleop commands") parser.add_argument( "--velocity_limit_factor", type=float, default=1.0, help="This value, typically between 0 and 1, further limits the " "iiwa14 joint velocities. It multiplies each of the seven " "pre-defined joint velocity limits. " "Note: The pre-defined velocity limits are specified by " "iiwa14_velocity_limits, found in this python file.") parser.add_argument( '--setup', type=str, default='default', help="The manipulation station setup to simulate. ", choices=['default', 'clutter_clearing']) MeshcatVisualizer.add_argparse_argument(parser) args = parser.parse_args() if args.test: # Don't grab mouse focus during testing. grab_focus = False # See: https://stackoverflow.com/a/52528832/7829525 os.environ["SDL_VIDEODRIVER"] = "dummy" else: grab_focus = True builder = DiagramBuilder() if args.hardware: station = builder.AddSystem(ManipulationStationHardwareInterface()) station.Connect(wait_for_cameras=False) else: station = builder.AddSystem(ManipulationStation()) # Initializes the chosen station type. if args.setup == 'default': station.SetupDefaultStation() elif args.setup == 'clutter_clearing': station.SetupClutterClearingStation() ycb_objects = CreateDefaultYcbObjectList() for model_file, X_WObject in ycb_objects: station.AddManipulandFromFile(model_file, X_WObject) station.Finalize() ConnectDrakeVisualizer(builder, station.get_scene_graph(), station.GetOutputPort("pose_bundle")) if args.meshcat: meshcat = builder.AddSystem(MeshcatVisualizer( station.get_scene_graph(), zmq_url=args.meshcat)) builder.Connect(station.GetOutputPort("pose_bundle"), meshcat.get_input_port(0)) robot = station.get_controller_plant() params = DifferentialInverseKinematicsParameters(robot.num_positions(), robot.num_velocities()) time_step = 0.005 params.set_timestep(time_step) # True velocity limits for the IIWA14 (in rad, rounded down to the first # decimal) iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3]) # Stay within a small fraction of those limits for this teleop demo. factor = args.velocity_limit_factor params.set_joint_velocity_limits((-factor*iiwa14_velocity_limits, factor*iiwa14_velocity_limits)) differential_ik = builder.AddSystem(DifferentialIK( robot, robot.GetFrameByName("iiwa_link_7"), params, time_step)) builder.Connect(differential_ik.GetOutputPort("joint_position_desired"), station.GetInputPort("iiwa_position")) teleop = builder.AddSystem(MouseKeyboardTeleop(grab_focus=grab_focus)) filter_ = builder.AddSystem( FirstOrderLowPassFilter(time_constant=args.filter_time_const, size=6)) builder.Connect(teleop.get_output_port(0), filter_.get_input_port(0)) builder.Connect(filter_.get_output_port(0), differential_ik.GetInputPort("rpy_xyz_desired")) builder.Connect(teleop.GetOutputPort("position"), station.GetInputPort( "wsg_position")) builder.Connect(teleop.GetOutputPort("force_limit"), station.GetInputPort("wsg_force_limit")) diagram = builder.Build() simulator = Simulator(diagram) # This is important to avoid duplicate publishes to the hardware interface: simulator.set_publish_every_time_step(False) station_context = diagram.GetMutableSubsystemContext( station, simulator.get_mutable_context()) station.GetInputPort("iiwa_feedforward_torque").FixValue( station_context, np.zeros(7)) simulator.AdvanceTo(1e-6) q0 = station.GetOutputPort("iiwa_position_measured").Eval(station_context) differential_ik.parameters.set_nominal_joint_position(q0) teleop.SetPose(differential_ik.ForwardKinematics(q0)) filter_.set_initial_output_value( diagram.GetMutableSubsystemContext( filter_, simulator.get_mutable_context()), teleop.get_output_port(0).Eval(diagram.GetMutableSubsystemContext( teleop, simulator.get_mutable_context()))) differential_ik.SetPositions(diagram.GetMutableSubsystemContext( differential_ik, simulator.get_mutable_context()), q0) simulator.set_target_realtime_rate(args.target_realtime_rate) print_instructions() simulator.AdvanceTo(args.duration)
def test_all_leaf_system_overrides(self): test = self class TrivialSystem(LeafSystem): def __init__(self): LeafSystem.__init__(self) self.called_publish = False self.called_feedthrough = False self.called_continuous = False self.called_discrete = False self.called_initialize = False self.called_per_step = False self.called_periodic = False self.called_getwitness = False self.called_witness = False self.called_guard = False self.called_reset = False # Ensure we have desired overloads. self.DeclarePeriodicPublish(1.0) self.DeclarePeriodicPublish(1.0, 0) self.DeclarePeriodicPublish(period_sec=1.0, offset_sec=0.) self.DeclarePeriodicDiscreteUpdate(period_sec=1.0, offset_sec=0.) self.DeclareInitializationEvent(event=PublishEvent( trigger_type=TriggerType.kInitialization, callback=self._on_initialize)) self.DeclarePerStepEvent( event=PublishEvent(trigger_type=TriggerType.kPerStep, callback=self._on_per_step)) self.DeclarePeriodicEvent( period_sec=1.0, offset_sec=0.0, event=PublishEvent(trigger_type=TriggerType.kPeriodic, callback=self._on_periodic)) self.DeclareContinuousState(2) self.DeclareDiscreteState(1) # Ensure that we have inputs / outputs to call direct # feedthrough. self.DeclareInputPort(PortDataType.kVectorValued, 1) self.DeclareVectorInputPort(name="test_input", model_vector=BasicVector(1), random_type=None) self.DeclareVectorOutputPort("noop", BasicVector(1), noop, prerequisites_of_calc=set( [self.nothing_ticket()])) self.witness = self.MakeWitnessFunction( "witness", WitnessFunctionDirection.kCrossesZero, self._witness) self.reset_witness = self.MakeWitnessFunction( "reset", WitnessFunctionDirection.kCrossesZero, self._guard, UnrestrictedUpdateEvent(self._reset)) def DoPublish(self, context, events): # Call base method to ensure we do not get recursion. LeafSystem.DoPublish(self, context, events) # N.B. We do not test for a singular call to `DoPublish` # (checking `assertFalse(self.called_publish)` first) because # the above `_DeclareInitializationEvent` will call both its # callback and this event when invoked via # `Simulator::Initialize` from `call_leaf_system_overrides`, # even when we explicitly say not to publish at initialize. self.called_publish = True def DoHasDirectFeedthrough(self, input_port, output_port): # Test inputs. test.assertIn(input_port, [0, 1]) test.assertEqual(output_port, 0) # Call base method to ensure we do not get recursion. with catch_drake_warnings(expected_count=1): base_return = LeafSystem.DoHasDirectFeedthrough( self, input_port, output_port) test.assertTrue(base_return is None) # Return custom methods. self.called_feedthrough = True return False def DoCalcTimeDerivatives(self, context, derivatives): # Note: Don't call base method here; it would abort because # derivatives.size() != 0. test.assertEqual(derivatives.get_vector().size(), 2) self.called_continuous = True def DoCalcDiscreteVariableUpdates(self, context, events, discrete_state): # Call base method to ensure we do not get recursion. LeafSystem.DoCalcDiscreteVariableUpdates( self, context, events, discrete_state) self.called_discrete = True def DoGetWitnessFunctions(self, context): self.called_getwitness = True return [self.witness, self.reset_witness] def _on_initialize(self, context, event): test.assertIsInstance(context, Context) test.assertIsInstance(event, PublishEvent) test.assertFalse(self.called_initialize) self.called_initialize = True def _on_per_step(self, context, event): test.assertIsInstance(context, Context) test.assertIsInstance(event, PublishEvent) self.called_per_step = True def _on_periodic(self, context, event): test.assertIsInstance(context, Context) test.assertIsInstance(event, PublishEvent) test.assertFalse(self.called_periodic) self.called_periodic = True def _witness(self, context): test.assertIsInstance(context, Context) self.called_witness = True return 1.0 def _guard(self, context): test.assertIsInstance(context, Context) self.called_guard = True return context.get_time() - 0.5 def _reset(self, context, event, state): test.assertIsInstance(context, Context) test.assertIsInstance(event, UnrestrictedUpdateEvent) test.assertIsInstance(state, State) self.called_reset = True system = TrivialSystem() self.assertFalse(system.called_publish) self.assertFalse(system.called_feedthrough) self.assertFalse(system.called_continuous) self.assertFalse(system.called_discrete) self.assertFalse(system.called_initialize) results = call_leaf_system_overrides(system) self.assertTrue(system.called_publish) self.assertTrue(system.called_feedthrough) self.assertFalse(results["has_direct_feedthrough"]) self.assertTrue(system.called_continuous) self.assertTrue(system.called_discrete) self.assertTrue(system.called_initialize) self.assertEqual(results["discrete_next_t"], 1.0) self.assertFalse(system.HasAnyDirectFeedthrough()) self.assertFalse(system.HasDirectFeedthrough(output_port=0)) self.assertFalse( system.HasDirectFeedthrough(input_port=0, output_port=0)) # Test explicit calls. system = TrivialSystem() context = system.CreateDefaultContext() system.Publish(context) self.assertTrue(system.called_publish) context_update = context.Clone() system.CalcTimeDerivatives( context=context, derivatives=context_update.get_mutable_continuous_state()) self.assertTrue(system.called_continuous) witnesses = system.GetWitnessFunctions(context) self.assertEqual(len(witnesses), 2) system.CalcDiscreteVariableUpdates( context=context, discrete_state=context_update.get_mutable_discrete_state()) self.assertTrue(system.called_discrete) # Test per-step, periodic, and witness call backs system = TrivialSystem() simulator = Simulator(system) simulator.get_mutable_context().SetAccuracy(0.1) # Stepping to 0.99 so that we get exactly one periodic event. simulator.AdvanceTo(0.99) self.assertTrue(system.called_per_step) self.assertTrue(system.called_periodic) self.assertTrue(system.called_getwitness) self.assertTrue(system.called_witness) self.assertTrue(system.called_guard) self.assertTrue(system.called_reset)
def test_diagram_simulation(self): # Similar to: //systems/framework:diagram_test, ExampleDiagram size = 3 builder = DiagramBuilder() adder0 = builder.AddSystem(Adder(2, size)) adder0.set_name("adder0") adder1 = builder.AddSystem(Adder(2, size)) adder1.set_name("adder1") integrator = builder.AddSystem(Integrator(size)) integrator.set_name("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)) builder.ExportInput(adder0.get_input_port(0)) builder.ExportInput(adder0.get_input_port(1)) builder.ExportInput(adder1.get_input_port(1)) builder.ExportOutput(integrator.get_output_port(0)) diagram = builder.Build() # 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 = BasicVector([0.1, 0.2, 0.3]) context.FixInputPort(0, input0) input1 = BasicVector([0.02, 0.03, 0.04]) context.FixInputPort(1, input1) input2 = BasicVector([0.003, 0.004, 0.005]) context.FixInputPort(2, 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.StepTo(t) # Record snapshot of *entire* context. context_log.append(context.Clone()) 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) print("xc[t = {}] = {}".format(t, xc)) self.assertTrue(np.allclose(xc, xc_expected))