def show_cloud(pc, use_native=False, **kwargs): # kwargs go to ctor. builder = DiagramBuilder() # Add point cloud visualization. if use_native: viz = meshcat.Visualizer(zmq_url=ZMQ_URL) else: plant, scene_graph = AddMultibodyPlantSceneGraph(builder) plant.Finalize() viz = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=ZMQ_URL, open_browser=False)) builder.Connect(scene_graph.get_pose_bundle_output_port(), viz.get_input_port(0)) pc_viz = builder.AddSystem( MeshcatPointCloudVisualizer(viz, **kwargs)) # Make sure the system runs. diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() context = diagram.GetMutableSubsystemContext( pc_viz, diagram_context) context.FixInputPort( pc_viz.GetInputPort("point_cloud_P").get_index(), AbstractValue.Make(pc)) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.AdvanceTo(sim_time)
def test_cart_pole(self): """Cart-Pole with simple geometry.""" file_name = FindResourceOrThrow( "drake/examples/multibody/cart_pole/cart_pole.sdf") builder = DiagramBuilder() cart_pole, scene_graph = AddMultibodyPlantSceneGraph(builder) Parser(plant=cart_pole).AddModelFromFile(file_name) cart_pole.Finalize() assert cart_pole.geometry_source_is_registered() visualizer = builder.AddSystem(MeshcatVisualizer(scene_graph, zmq_url=ZMQ_URL, open_browser=False)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() cart_pole_context = diagram.GetMutableSubsystemContext( cart_pole, diagram_context) cart_pole.get_actuation_input_port().FixValue(cart_pole_context, 0) cart_slider = cart_pole.GetJointByName("CartSlider") pole_pin = cart_pole.GetJointByName("PolePin") cart_slider.set_translation(context=cart_pole_context, translation=0.) pole_pin.set_angle(context=cart_pole_context, angle=2.) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.AdvanceTo(.1)
def test_texture_override(self): """Draws a textured box to test the texture override pathway.""" object_file_path = FindResourceOrThrow( "drake/systems/sensors/test/models/box_with_mesh.sdf") # Find the texture path just to ensure it exists and # we're testing the code path we want to. FindResourceOrThrow("drake/systems/sensors/test/models/meshes/box.png") builder = DiagramBuilder() plant = MultibodyPlant(0.002) _, scene_graph = AddMultibodyPlantSceneGraph(builder, plant) object_model = Parser(plant=plant).AddModelFromFile(object_file_path) plant.Finalize() # Add meshcat visualizer. viz = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=None, open_browser=False)) builder.Connect(scene_graph.get_pose_bundle_output_port(), viz.get_input_port(0)) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.AdvanceTo(1.0)
def main(): # Simulate with doubles. builder = DiagramBuilder() source = builder.AddSystem(ConstantVectorSource([10.])) adder = builder.AddSystem(SimpleAdder(100.)) builder.Connect(source.get_output_port(0), adder.get_input_port(0)) logger = builder.AddSystem(SignalLogger(1)) builder.Connect(adder.get_output_port(0), logger.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.AdvanceTo(1) x = logger.data() print("Output values: {}".format(x)) assert np.allclose(x, 110.) # Compute outputs with AutoDiff and Symbolic. for T in (AutoDiffXd, Expression): adder_T = SimpleAdder_[T](100.) context = adder_T.CreateDefaultContext() adder_T.get_input_port().FixValue(context, [10.]) output = adder_T.AllocateOutput() adder_T.CalcOutput(context, output) # N.B. At present, you cannot get a reference to existing AutoDiffXd # or Expression numpy arrays, so we will explictly copy the vector: # https://github.com/RobotLocomotion/drake/issues/8116 value, = output.get_vector_data(0).CopyToVector() assert isinstance(value, T) print("Output from {}: {}".format(type(adder_T), repr(value)))
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 _process_event(self, dut): # Use a Simulator to invoke the update event on `dut`. (Wouldn't it be # nice if the Systems API was simple enough that we could apply events # without calling a Simulator!) simulator = Simulator(dut) simulator.AdvanceTo(0.00025) # Arbitrary positive value. return simulator.get_context().Clone()
def visualize(q, dt=None): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, MultibodyPlant(0.001)) load_atlas(plant, add_ground=True) plant_context = plant.CreateDefaultContext() ConnectContactResultsToDrakeVisualizer(builder=builder, plant=plant) ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph) diagram = builder.Build() if len(q.shape) == 1: q = np.reshape(q, (1, -1)) for i in range(q.shape[0]): print(f"knot point: {i}") diagram_context = diagram.CreateDefaultContext() plant_context = diagram.GetMutableSubsystemContext(plant, diagram_context) set_null_input(plant, plant_context) plant.SetPositions(plant_context, q[i]) simulator = Simulator(diagram, diagram_context) simulator.set_target_realtime_rate(0.0) simulator.AdvanceTo(0.0001) if not dt is None: time.sleep(5/(np.sum(dt))*dt[i]) else: time.sleep(0.5)
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 experiment(start_state, force_schedule, sleeptime=None): global experiment_count experiment_count += 1 if experiment_count % 10 == 0: print(f"{experiment_count} experiments run so far...") builder = DiagramBuilder() mbp, sg = make_mbp(builder) mbp.Finalize() DrakeVisualizer.AddToBuilder(builder, sg, lcm=global_lcm_ftw) thrusters = add_thrusters(builder, mbp) breaks = [0] for _, t in force_schedule: breaks.append(breaks[-1] + t) forces = np.array([f for f, t in force_schedule] + [force_schedule[-1][0]]) force_traj = PiecewisePolynomial.ZeroOrderHold(breaks, forces.transpose()) controller = builder.AddSystem(TrajectorySource(force_traj)) builder.Connect(controller.get_output_port(), thrusters.get_command_input_port()) diagram = builder.Build() simulator = Simulator(diagram) mbp_context = diagram.GetSubsystemContext(mbp, simulator.get_context()) mbp.SetPositionsAndVelocities(mbp_context, start_state) for step in range(int(1000 * breaks[-1])): simulator.AdvanceTo(0.001 * step) mbp_context = diagram.GetSubsystemContext(mbp, simulator.get_context()) if sleeptime: time.sleep(sleeptime) return mbp.GetPositionsAndVelocities( diagram.GetSubsystemContext(mbp, simulator.get_context()))
def test_allegro_proximity_geometry(self): """Allegro hand with visual and collision geometry, drawn in proximity-geom mode.""" file_name = FindResourceOrThrow( "drake/manipulation/models/allegro_hand_description/sdf/" "allegro_hand_description_left.sdf") builder = DiagramBuilder() hand, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) Parser(plant=hand).AddModelFromFile(file_name) hand.Finalize() visualizer = builder.AddSystem( MeshcatVisualizer(zmq_url=ZMQ_URL, open_browser=False, role=Role.kProximity)) builder.Connect(scene_graph.get_query_output_port(), visualizer.get_geometry_query_input_port()) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() hand_context = diagram.GetMutableSubsystemContext( hand, diagram_context) hand_actuation_port = hand.get_actuation_input_port() hand_actuation_port.FixValue(hand_context, np.zeros(hand_actuation_port.size())) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.AdvanceTo(.1)
def test_textured_meshes(self): """Draws a solid green box (the texture map is just a green pixel) to test the texture override pathway. You should confirm that you see a green box in the visualizer.""" object_file_path = FindResourceOrThrow( "drake/systems/sensors/test/models/box_with_mesh.sdf") # Find the texture path just to ensure it exists and # we're testing the code path we want to. FindResourceOrThrow("drake/systems/sensors/test/models/meshes/box.png") builder = DiagramBuilder() plant = MultibodyPlant(0.002) _, scene_graph = AddMultibodyPlantSceneGraph(builder, plant) object_model = Parser(plant=plant).AddModelFromFile(object_file_path) plant.Finalize() # Add meshcat visualizer. visualizer = builder.AddSystem( MeshcatVisualizer(zmq_url=ZMQ_URL, open_browser=False)) builder.Connect(scene_graph.get_query_output_port(), visualizer.get_geometry_query_input_port()) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.AdvanceTo(1.0)
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_kuka(self): """Kuka IIWA with mesh geometry.""" file_name = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/sdf/" "iiwa14_no_collision.sdf") builder = DiagramBuilder() kuka, scene_graph = AddMultibodyPlantSceneGraph(builder) Parser(plant=kuka).AddModelFromFile(file_name) kuka.Finalize() visualizer = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=ZMQ_URL, open_browser=False)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() kuka_context = diagram.GetMutableSubsystemContext( kuka, diagram_context) kuka_context.FixInputPort( kuka.get_actuation_input_port().get_index(), np.zeros(kuka.get_actuation_input_port().size())) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.AdvanceTo(.1)
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 test_signal_logger(self): # Log the output of a simple diagram containing a constant # source and an integrator. builder = DiagramBuilder() kValue = 2.4 source = builder.AddSystem(ConstantVectorSource([kValue])) kSize = 1 integrator = builder.AddSystem(Integrator(kSize)) logger_per_step = builder.AddSystem(SignalLogger(kSize)) builder.Connect(source.get_output_port(0), integrator.get_input_port(0)) builder.Connect(integrator.get_output_port(0), logger_per_step.get_input_port(0)) # Add a redundant logger via the helper method. logger_per_step_2 = LogOutput(integrator.get_output_port(0), builder) # Add a periodic logger logger_periodic = builder.AddSystem(SignalLogger(kSize)) kPeriod = 0.1 logger_periodic.set_publish_period(kPeriod) builder.Connect(integrator.get_output_port(0), logger_periodic.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) kTime = 1. simulator.AdvanceTo(kTime) # Verify outputs of the every-step logger t = logger_per_step.sample_times() x = logger_per_step.data() self.assertTrue(t.shape[0] > 2) self.assertTrue(t.shape[0] == x.shape[1]) self.assertAlmostEqual(x[0, -1], t[-1] * kValue, places=2) np.testing.assert_array_equal(x, logger_per_step_2.data()) # Verify outputs of the periodic logger t = logger_periodic.sample_times() x = logger_periodic.data() # Should log exactly once every kPeriod, up to and including kTime. self.assertTrue(t.shape[0] == np.floor(kTime / kPeriod) + 1.) logger_per_step.reset() # Verify that t and x retain their values after systems are deleted. t_copy = t.copy() x_copy = x.copy() del builder del integrator del logger_periodic del logger_per_step del logger_per_step_2 del diagram del simulator del source gc.collect() self.assertTrue((t == t_copy).all()) self.assertTrue((x == x_copy).all())
def test_subgraph_add_to_copying(self): """Ensures that index ordering is generally the same when copying a plant using a MultibodyPlantSubgraph.add_to.""" # TODO(eric.cousineau): Increas number of bodies for viz, once # `create_manual_map` can acommodate it. plant_a, scene_graph_a, _ = self.make_arbitrary_multibody_stuff( num_bodies=1) # Check for general ordering with full subgraph "cloning". builder_b = DiagramBuilder() plant_b, scene_graph_b = AddMultibodyPlantSceneGraph( builder_b, plant_a.time_step()) subgraph_a = mut.MultibodyPlantSubgraph( mut.get_elements_from_plant(plant_a, scene_graph_a)) a_to_b = subgraph_a.add_to(plant_b, scene_graph_b) plant_b.Finalize() util.assert_plant_equals(plant_a, scene_graph_a, plant_b, scene_graph_b) a_to_b_expected = self.create_manual_map(plant_a, scene_graph_a, plant_b, scene_graph_b) self.assertEqual(a_to_b, a_to_b_expected) if VISUALIZE: for model in me.get_model_instances(plant_b): util.build_with_no_control(builder_b, plant_b, model) print("test_subgraph_add_to_copying") DrakeVisualizer.AddToBuilder(builder_b, scene_graph_b) diagram = builder_b.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.) simulator.Initialize() diagram.Publish(simulator.get_context()) simulator.AdvanceTo(1.)
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 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 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(): 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("--simulation_time", type=float, default=10.0, help="Desired duration of the simulation in seconds.") parser.add_argument( "--time_step", type=float, default=0., help="If greater than zero, the plant is modeled as a system with " "discrete updates and period equal to this time_step. " "If 0, the plant is modeled as a continuous system.") args = parser.parse_args() file_name = FindResourceOrThrow( "drake/examples/multibody/cart_pole/cart_pole.sdf") builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) cart_pole = builder.AddSystem(MultibodyPlant(time_step=args.time_step)) cart_pole.RegisterAsSourceForSceneGraph(scene_graph) Parser(plant=cart_pole).AddModelFromFile(file_name) cart_pole.AddForceElement(UniformGravityFieldElement()) cart_pole.Finalize() assert cart_pole.geometry_source_is_registered() builder.Connect(scene_graph.get_query_output_port(), cart_pole.get_geometry_query_input_port()) builder.Connect( cart_pole.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(cart_pole.get_source_id())) ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() cart_pole_context = diagram.GetMutableSubsystemContext( cart_pole, diagram_context) cart_pole_context.FixInputPort( cart_pole.get_actuation_input_port().get_index(), [0]) cart_slider = cart_pole.GetJointByName("CartSlider") pole_pin = cart_pole.GetJointByName("PolePin") cart_slider.set_translation(context=cart_pole_context, translation=0.) pole_pin.set_angle(context=cart_pole_context, angle=2.) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(args.target_realtime_rate) simulator.Initialize() simulator.AdvanceTo(args.simulation_time)
def test_cart_pole(self): """Cart-Pole with simple geometry.""" file_name = FindResourceOrThrow( "drake/examples/multibody/cart_pole/cart_pole.sdf") builder = DiagramBuilder() cart_pole, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) Parser(plant=cart_pole).AddModelFromFile(file_name) cart_pole.Finalize() assert cart_pole.geometry_source_is_registered() # Note: pass window=None argument to confirm kwargs are passed # through to meshcat.Visualizer. visualizer = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=ZMQ_URL, open_browser=False, window=None, delete_prefix_on_load=True)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() cart_pole_context = diagram.GetMutableSubsystemContext( cart_pole, diagram_context) cart_pole.get_actuation_input_port().FixValue(cart_pole_context, 0) cart_slider = cart_pole.GetJointByName("CartSlider") pole_pin = cart_pole.GetJointByName("PolePin") cart_slider.set_translation(context=cart_pole_context, translation=0.) pole_pin.set_angle(context=cart_pole_context, angle=2.) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) visualizer.set_planar_viewpoint(camera_position=[0, -1, 0], camera_focus=[0, 0, 0], xmin=-2, xmax=2, ymin=-1, ymax=2) visualizer.start_recording() simulator.AdvanceTo(.1) visualizer.stop_recording() # Should have animation "clips" for both bodies. # See https://github.com/rdeits/meshcat-python/blob/c4ef22c84336d6a8eaab682f73bb47cfca5d5779/src/meshcat/animation.py#L100 # noqa self.assertEqual(len(visualizer._animation.clips), 2) # After .1 seconds, we should have had 4 publish events. self.assertEqual(visualizer._recording_frame_num, 4) visualizer.publish_recording(play=True, repetitions=1) visualizer.reset_recording() self.assertEqual(len(visualizer._animation.clips), 0) visualizer.delete_prefix()
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 main(): builder = DiagramBuilder() source = builder.AddSystem(ConstantVectorSource([10.])) logger = builder.AddSystem(VectorLogSink(1)) builder.Connect(source.get_output_port(0), logger.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.AdvanceTo(1) x = logger.FindLog(simulator.get_context()).data() print("Output values: {}".format(x)) assert np.allclose(x, 10.)
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())
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_copying(self): """Ensures that index ordering is generally the same when copying a plant using a subgraph.""" time_step = 0.01 builder_a = DiagramBuilder() plant_a, scene_graph_a = AddMultibodyPlantSceneGraph( builder_a, time_step) util.add_arbitrary_multibody_stuff(plant_a) # Ensure that this is "physically" valid. plant_a.Finalize() if VISUALIZE: for model in me.get_model_instances(plant_a): util.no_control(builder_a, plant_a, model) print("test_composition_array_with_scene_graph") ConnectDrakeVisualizer(builder_a, scene_graph_a) diagram = builder_a.Build() simulator = Simulator(diagram) simulator.Initialize() diagram.Publish(simulator.get_context()) simulator.AdvanceTo(1.) # Checking for determinism, making a slight change to trigger an error. for slight_difference in [False, True]: builder_b = DiagramBuilder() plant_b, scene_graph_b = AddMultibodyPlantSceneGraph( builder_b, time_step) util.add_arbitrary_multibody_stuff( plant_b, slight_difference=slight_difference) plant_b.Finalize() if not slight_difference: util.assert_plant_equals(plant_a, scene_graph_a, plant_b, scene_graph_b) else: with self.assertRaises(AssertionError): util.assert_plant_equals(plant_a, scene_graph_a, plant_b, scene_graph_b) # Check for general ordering with full subgraph "cloning". builder_b = DiagramBuilder() plant_b, scene_graph_b = AddMultibodyPlantSceneGraph( builder_b, time_step) subgraph_a = mut.MultibodyPlantSubgraph( mut.get_elements_from_plant(plant_a, scene_graph_a)) subgraph_a.add_to(plant_b, scene_graph_b) plant_b.Finalize() util.assert_plant_equals(plant_a, scene_graph_a, plant_b, scene_graph_b)
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.AdvanceTo(1.0) self.assertFalse((state.CopyToVector() == initial_state).any())
def test_procedural_geometry(self): """ This test ensures we can draw procedurally added primitive geometry that is added to the world model instance (which has a slightly different naming scheme than geometry with a non-default / non-world model instance). """ builder = DiagramBuilder() mbp, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) world_body = mbp.world_body() box_shape = Box(1., 2., 3.) # This rigid body will be added to the world model instance since # the model instance is not specified. box_body = mbp.AddRigidBody( "box", SpatialInertia(mass=1.0, p_PScm_E=np.array([0., 0., 0.]), G_SP_E=UnitInertia(1.0, 1.0, 1.0))) mbp.WeldFrames(world_body.body_frame(), box_body.body_frame(), RigidTransform()) mbp.RegisterVisualGeometry(box_body, RigidTransform.Identity(), box_shape, "ground_vis", np.array([0.5, 0.5, 0.5, 1.])) mbp.RegisterCollisionGeometry(box_body, RigidTransform.Identity(), box_shape, "ground_col", CoulombFriction(0.9, 0.8)) mbp.Finalize() frames_to_draw = {"world": {"box"}} visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() vis_context = diagram.GetMutableSubsystemContext( visualizer, diagram_context) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.AdvanceTo(.1) visualizer.draw(vis_context) self.assertEqual( visualizer.ax.get_title(), "t = 0.1", )
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 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("--simulation_time", type=float, default=10.0, help="Desired duration of the simulation in seconds.") parser.add_argument( "--time_step", type=float, default=0., help="If greater than zero, the plant is modeled as a system with " "discrete updates and period equal to this time_step. " "If 0, the plant is modeled as a continuous system.") args = parser.parse_args() file_name = FindResourceOrThrow( "drake/examples/multibody/cart_pole/cart_pole.sdf") builder = DiagramBuilder() cart_pole, scene_graph = AddMultibodyPlantSceneGraph( builder=builder, time_step=args.time_step) Parser(plant=cart_pole).AddModelFromFile(file_name) cart_pole.Finalize() DrakeVisualizer.AddToBuilder(builder=builder, scene_graph=scene_graph) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() cart_pole_context = cart_pole.GetMyMutableContextFromRoot(diagram_context) cart_pole.get_actuation_input_port().FixValue(cart_pole_context, 0) cart_slider = cart_pole.GetJointByName("CartSlider") pole_pin = cart_pole.GetJointByName("PolePin") cart_slider.set_translation(context=cart_pole_context, translation=0.) pole_pin.set_angle(context=cart_pole_context, angle=2.) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(args.target_realtime_rate) simulator.Initialize() simulator.AdvanceTo(args.simulation_time)