def test_model_instance_port_access(self): # Create a MultibodyPlant with a kuka arm and a schunk gripper. # the arm is welded to the world, the gripper is welded to the # arm's end effector. wsg50_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "wsg_50_description/sdf/schunk_wsg_50.sdf") iiwa_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/sdf/iiwa14_no_collision.sdf") plant = MultibodyPlant(time_step=2e-3) parser = Parser(plant) iiwa_model = parser.AddModelFromFile(file_name=iiwa_sdf_path, model_name='robot') gripper_model = parser.AddModelFromFile(file_name=wsg50_sdf_path, model_name='gripper') plant.Finalize() # Test that we can get an actuation input port and a continuous state # output port. self.assertIsInstance(plant.get_actuation_input_port(iiwa_model), InputPort) self.assertIsInstance(plant.get_state_output_port(gripper_model), OutputPort) self.assertIsInstance( plant.get_generalized_contact_forces_output_port( model_instance=gripper_model), OutputPort)
def test_scene_graph_queries(self): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) parser = Parser(plant=plant, scene_graph=scene_graph) parser.AddModelFromFile( FindResourceOrThrow( "drake/bindings/pydrake/multibody/test/two_bodies.sdf")) plant.Finalize(scene_graph) diagram = builder.Build() # The model `two_bodies` has two (implicitly) floating bodies that are # placed in the same position. The default state would be for these two # bodies to be coincident, and thus collide. context = diagram.CreateDefaultContext() sg_context = diagram.GetMutableSubsystemContext(scene_graph, context) query_object = scene_graph.get_query_output_port().Eval(sg_context) # Implicitly require that this should be size 1. point_pair, = query_object.ComputePointPairPenetration() self.assertIsInstance(point_pair, PenetrationAsPointPair) signed_distance_pair, = query_object.\ ComputeSignedDistancePairwiseClosestPoints() self.assertIsInstance(signed_distance_pair, SignedDistancePair) inspector = query_object.inspector() bodies = { plant.GetBodyFromFrameId(inspector.GetFrameId(id_)) for id_ in [point_pair.id_A, point_pair.id_B] } self.assertSetEqual( bodies, {plant.GetBodyByName("body1"), plant.GetBodyByName("body2")})
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 test_viewer_applet(self): """Check that _ViewerApplet doesn't crash when receiving messages. Note that many geometry types are not yet covered by this test. """ # Create the device under test. dut = mut.Meldis() meshcat = dut.meshcat lcm = dut._lcm # The path is created by the constructor. self.assertEqual(meshcat.HasPath("/DRAKE_VIEWER"), True) # Enqueue the load + draw messages. sdf_file = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) parser = Parser(plant=plant) parser.AddModelFromFile(sdf_file) plant.Finalize() DrakeVisualizer.AddToBuilder(builder=builder, scene_graph=scene_graph, lcm=lcm) diagram = builder.Build() context = diagram.CreateDefaultContext() diagram.Publish(context) # The geometry isn't registered until the load is processed. link_path = "/DRAKE_VIEWER/2/plant/acrobot/Link2/0" self.assertEqual(meshcat.HasPath(link_path), False) # Process the load + draw; make sure the geometry exists now. lcm.HandleSubscriptions(timeout_millis=0) dut._invoke_subscriptions() self.assertEqual(meshcat.HasPath(link_path), True)
def test_kuka(self): """Kuka IIWA with mesh geometry.""" file_name = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/sdf/" "iiwa14_no_collision.sdf") builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) kuka = builder.AddSystem(MultibodyPlant()) parser = Parser(plant=kuka, scene_graph=scene_graph) parser.AddModelFromFile(file_name) kuka.AddForceElement(UniformGravityFieldElement([0, 0, -9.81])) kuka.Finalize(scene_graph) assert kuka.geometry_source_is_registered() builder.Connect(kuka.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(kuka.get_source_id())) visualizer = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=None, 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.StepTo(.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 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 test_joint_sliders(self): # Load up an acrobot. acrobot_file = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.urdf") builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) parser = Parser(plant) parser.AddModelFromFile(acrobot_file) plant.Finalize() # Construct a sliders system, using every available option. meshcat = Meshcat() dut = JointSliders( meshcat=meshcat, plant=plant, initial_value=[0.1, 0.2], lower_limit=[-3.0, -6.0], upper_limit=[3.0, 6.0], step=[0.25, 0.50], ) # Various methods should not crash. dut.get_output_port() dut.Delete() # The constructor also accepts single values for broadcast (except for # the initial value). dut = JointSliders( meshcat=meshcat, plant=plant, initial_value=[0.1, 0.2], lower_limit=-3.0, upper_limit=3.0, step=0.1, ) dut.Delete() # The constructor also accepts None directly, for optionals. dut = JointSliders( meshcat=meshcat, plant=plant, initial_value=None, lower_limit=None, upper_limit=None, step=None, ) dut.Delete() # The constructor has default values, in any case. dut = JointSliders(meshcat, plant) # The Run function doesn't crash. builder.AddSystem(dut) diagram = builder.Build() dut.Run(diagram=diagram, timeout=1.0) # The SetPositions function doesn't crash (Acrobot has two positions). dut.SetPositions(q=[1, 2])
def test_manipulation_station_add_iiwa_and_wsg_explicitly(self): station = ManipulationStation() parser = Parser(station.get_mutable_multibody_plant(), station.get_mutable_scene_graph()) plant = station.get_mutable_multibody_plant() # Add models for iiwa and wsg iiwa_model_file = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/iiwa7/" "iiwa7_no_collision.sdf") iiwa = parser.AddModelFromFile(iiwa_model_file, "iiwa") X_WI = RigidTransform.Identity() plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0", iiwa), X_WI) wsg_model_file = FindResourceOrThrow( "drake/manipulation/models/wsg_50_description/sdf/" "schunk_wsg_50.sdf") wsg = parser.AddModelFromFile(wsg_model_file, "gripper") X_7G = RigidTransform.Identity() plant.WeldFrames( plant.GetFrameByName("iiwa_link_7", iiwa), plant.GetFrameByName("body", wsg), X_7G) # Register models for the controller. station.RegisterIiwaControllerModel( iiwa_model_file, iiwa, plant.world_frame(), plant.GetFrameByName("iiwa_link_0", iiwa), X_WI) station.RegisterWsgControllerModel( wsg_model_file, wsg, plant.GetFrameByName("iiwa_link_7", iiwa), plant.GetFrameByName("body", wsg), X_7G) # Finalize station.Finalize() self.assertEqual(station.num_iiwa_joints(), 7) # This WSG gripper model has 2 independent dof, and the IIWA model # has 7. self.assertEqual(plant.num_positions(), 9) self.assertEqual(plant.num_velocities(), 9)
def test_contact_applet_hydroelastic(self): """Check that _ContactApplet doesn't crash when receiving hydroelastic messages. """ # Create the device under test. dut = mut.Meldis() meshcat = dut.meshcat lcm = dut._lcm # Enqueue a hydroelastic contact message. sdf_file = FindResourceOrThrow( "drake/multibody/meshcat/test/hydroelastic.sdf") builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.001) parser = Parser(plant=plant) parser.AddModelFromFile(sdf_file) body1 = plant.GetBodyByName("body1") body2 = plant.GetBodyByName("body2") plant.AddJoint( PrismaticJoint(name="body1", frame_on_parent=plant.world_body().body_frame(), frame_on_child=body1.body_frame(), axis=[0, 0, 1])) plant.AddJoint( PrismaticJoint(name="body2", frame_on_parent=plant.world_body().body_frame(), frame_on_child=body2.body_frame(), axis=[1, 0, 0])) plant.Finalize() ConnectContactResultsToDrakeVisualizer(builder=builder, plant=plant, scene_graph=scene_graph, lcm=lcm) diagram = builder.Build() context = diagram.CreateDefaultContext() plant.SetPositions(plant.GetMyMutableContextFromRoot(context), [0.1, 0.3]) diagram.Publish(context) # The geometry isn't registered until the load is processed. hydro_path = "/CONTACT_RESULTS/hydroelastic/" + \ "body1.two_bodies::body1_collision+body2" hydro_path2 = "/CONTACT_RESULTS/hydroelastic/" + \ "body1.two_bodies::body1_collision2+body2" self.assertEqual(meshcat.HasPath(hydro_path), False) self.assertEqual(meshcat.HasPath(hydro_path2), False) # Process the load + draw; contact results should now exist. lcm.HandleSubscriptions(timeout_millis=1) dut._invoke_subscriptions() self.assertEqual(meshcat.HasPath("/CONTACT_RESULTS/hydroelastic"), True) self.assertEqual(meshcat.HasPath(hydro_path), True) self.assertEqual(meshcat.HasPath(hydro_path2), True)
def make_ball_paddle(contact_model, contact_surface_representation, time_step): multibody_plant_config = \ MultibodyPlantConfig( time_step=time_step, contact_model=contact_model, contact_surface_representation=contact_surface_representation) # We pose the paddle, so that its top surface is on World's X-Y plane. # Intuitively we push it down 1 cm because the box is 2 cm thick. p_WPaddle_fixed = RigidTransform(RollPitchYaw(0, 0, 0), np.array([0, 0, -0.01])) builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlant(multibody_plant_config, builder) parser = Parser(plant) paddle_sdf_file_name = \ FindResourceOrThrow("drake/examples/hydroelastic/python_ball_paddle" "/paddle.sdf") paddle = parser.AddModelFromFile(paddle_sdf_file_name, model_name="paddle") plant.WeldFrames(frame_on_parent_F=plant.world_frame(), frame_on_child_M=plant.GetFrameByName("paddle", paddle), X_FM=p_WPaddle_fixed) ball_sdf_file_name = \ FindResourceOrThrow("drake/examples/hydroelastic/python_ball_paddle" "/ball.sdf") parser.AddModelFromFile(ball_sdf_file_name) # TODO(DamrongGuoy): Let users override hydroelastic modulus, dissipation, # and resolution hint from the two SDF files above. plant.Finalize() DrakeVisualizer.AddToBuilder(builder=builder, scene_graph=scene_graph) ConnectContactResultsToDrakeVisualizer(builder=builder, plant=plant, scene_graph=scene_graph) nx = plant.num_positions() + plant.num_velocities() state_logger = builder.AddSystem(VectorLogSink(nx)) builder.Connect(plant.get_state_output_port(), state_logger.get_input_port()) diagram = builder.Build() return diagram, plant, state_logger
def test_multibody_add_joint(self, T): """ Tests joint constructors and `AddJoint`. """ MultibodyPlant = MultibodyPlant_[T] RigidTransform = RigidTransform_[T] RollPitchYaw = RollPitchYaw_[T] instance_file = FindResourceOrThrow( "drake/examples/double_pendulum/models/double_pendulum.sdf") # Add different joints between multiple model instances. # TODO(eric.cousineau): Remove the multiple instances and use # programmatically constructed bodies once this API is exposed in # Python. num_joints = 2 # N.B. `Parser` only supports `MultibodyPlant_[float]`. plant_f = MultibodyPlant_[float]() parser = Parser(plant_f) instances = [] for i in range(num_joints + 1): instance = parser.AddModelFromFile( instance_file, "instance_{}".format(i)) instances.append(instance) proximal_frame = "base" distal_frame = "lower_link" joints_f = [ RevoluteJoint_[float]( name="revolve_things", frame_on_parent=plant_f.GetBodyByName( distal_frame, instances[1]).body_frame(), frame_on_child=plant_f.GetBodyByName( proximal_frame, instances[2]).body_frame(), axis=[0, 0, 1], damping=0.), WeldJoint_[float]( name="weld_things", parent_frame_P=plant_f.GetBodyByName( distal_frame, instances[0]).body_frame(), child_frame_C=plant_f.GetBodyByName( proximal_frame, instances[1]).body_frame(), X_PC=RigidTransform_[float].Identity()), ] for joint_f in joints_f: joint_out = plant_f.AddJoint(joint_f) self.assertIs(joint_f, joint_out) # The model is now complete. plant_f.Finalize() plant = to_type(plant_f, T) for joint_f in joints_f: # Not using `joint` because we converted the `plant_f` to `plant` joint = plant.get_joint(joint_index=joint_f.index()) self._test_joint_api(T, joint)
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 _make_diagram(self, *, sdf_filename, visualizer_params, lcm): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) parser = Parser(plant=plant) parser.AddModelFromFile(FindResourceOrThrow(sdf_filename)) plant.Finalize() DrakeVisualizer.AddToBuilder(builder=builder, scene_graph=scene_graph, params=visualizer_params, lcm=lcm) diagram = builder.Build() return diagram
def test_scene_graph_queries(self, T): # SceneGraph does not support `Expression` type. PenetrationAsPointPair = PenetrationAsPointPair_[T] builder_f = DiagramBuilder_[float]() # N.B. `Parser` only supports `MultibodyPlant_[float]`. plant_f, scene_graph_f = AddMultibodyPlantSceneGraph(builder_f) parser = Parser(plant=plant_f, scene_graph=scene_graph_f) parser.AddModelFromFile( FindResourceOrThrow( "drake/bindings/pydrake/multibody/test/two_bodies.sdf")) plant_f.Finalize() diagram_f = builder_f.Build() # Do conversion. diagram = to_type(diagram_f, T) plant = diagram.GetSubsystemByName(plant_f.get_name()) scene_graph = diagram.GetSubsystemByName(scene_graph_f.get_name()) # The model `two_bodies` has two (implicitly) floating bodies that are # placed in the same position. The default state would be for these two # bodies to be coincident, and thus collide. context = diagram.CreateDefaultContext() sg_context = diagram.GetMutableSubsystemContext(scene_graph, context) query_object = scene_graph.get_query_output_port().Eval(sg_context) # Implicitly require that this should be size 1. point_pair, = query_object.ComputePointPairPenetration() self.assertIsInstance(point_pair, PenetrationAsPointPair_[float]) signed_distance_pair, = query_object.\ ComputeSignedDistancePairwiseClosestPoints() self.assertIsInstance(signed_distance_pair, SignedDistancePair_[T]) signed_distance_to_point = query_object.\ ComputeSignedDistanceToPoint(p_WQ=np.ones(3)) self.assertEqual(len(signed_distance_to_point), 2) self.assertIsInstance(signed_distance_to_point[0], SignedDistanceToPoint_[T]) self.assertIsInstance(signed_distance_to_point[1], SignedDistanceToPoint_[T]) inspector = query_object.inspector() def get_body_from_frame_id(frame_id): # Get body from frame id, and check inverse method. body = plant.GetBodyFromFrameId(frame_id) self.assertEqual( plant.GetBodyFrameIdIfExists(body.index()), frame_id) return body bodies = {get_body_from_frame_id(inspector.GetFrameId(id_)) for id_ in [point_pair.id_A, point_pair.id_B]} self.assertSetEqual( bodies, {plant.GetBodyByName("body1"), plant.GetBodyByName("body2")})
def add_object_from_file(self, object_name, object_path): parser = Parser(self._mbp, self._sg) self._model_ids[object_name] = parser.AddModelFromFile(object_path, object_name) # this should be masked self._model_names_to_mask.append(object_name) def finalize_func(): state_output_port = self._mbp.get_state_output_port(self._model_ids[object_name]) port_state_output_name = object_name + '_state_output' self._port_names.append(port_state_output_name) self._builder.ExportOutput(state_output_port, port_state_output_name) self._finalize_functions.append(finalize_func)
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_contact_applet_point_pair(self): """Check that _ContactApplet doesn't crash when receiving point contact messages. """ # Create the device under test. dut = mut.Meldis() meshcat = dut.meshcat lcm = dut._lcm # Enqueue a point contact result message. sdf_file = FindResourceOrThrow( "drake/examples/manipulation_station/models/sphere.sdf") builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.001) parser = Parser(plant=plant) sphere1_model = parser.AddModelFromFile(sdf_file, "sphere1") sphere2_model = parser.AddModelFromFile(sdf_file, "sphere2") body1 = plant.GetBodyByName("base_link", sphere1_model) body2 = plant.GetBodyByName("base_link", sphere2_model) plant.AddJoint( PrismaticJoint(name="sphere1_x", frame_on_parent=plant.world_body().body_frame(), frame_on_child=body1.body_frame(), axis=[1, 0, 0])) plant.AddJoint( PrismaticJoint(name="sphere2_x", frame_on_parent=plant.world_body().body_frame(), frame_on_child=body2.body_frame(), axis=[1, 0, 0])) plant.Finalize() ConnectContactResultsToDrakeVisualizer(builder=builder, plant=plant, scene_graph=scene_graph, lcm=lcm) diagram = builder.Build() context = diagram.CreateDefaultContext() plant.SetPositions(plant.GetMyMutableContextFromRoot(context), [-0.03, 0.03]) diagram.Publish(context) # The geometry isn't registered until the load is processed. pair_path = "/CONTACT_RESULTS/point/base_link(2)+base_link(3)" self.assertEqual(meshcat.HasPath(pair_path), False) # Process the load + draw; make sure the geometry exists now. lcm.HandleSubscriptions(timeout_millis=0) dut._invoke_subscriptions() self.assertEqual(meshcat.HasPath(pair_path), True)
def test_multibody_add_joint(self): """ Tests joint constructors and `AddJoint`. """ instance_file = FindResourceOrThrow( "drake/examples/double_pendulum/models/double_pendulum.sdf") # Add different joints between multiple model instances. # TODO(eric.cousineau): Remove the multiple instances and use # programmatically constructed bodies once this API is exposed in # Python. num_joints = 2 plant = MultibodyPlant() parser = Parser(plant) instances = [] for i in range(num_joints + 1): instance = parser.AddModelFromFile(instance_file, "instance_{}".format(i)) instances.append(instance) proximal_frame = "base" distal_frame = "lower_link" joints = [ RevoluteJoint( name="revolve_things", frame_on_parent=plant.GetBodyByName(distal_frame, instances[1]).body_frame(), frame_on_child=plant.GetBodyByName(proximal_frame, instances[2]).body_frame(), axis=[0, 0, 1], damping=0.), WeldJoint( name="weld_things", parent_frame_P=plant.GetBodyByName(distal_frame, instances[0]).body_frame(), child_frame_C=plant.GetBodyByName(proximal_frame, instances[1]).body_frame(), X_PC=RigidTransform.Identity()), ] for joint in joints: joint_out = plant.AddJoint(joint) self.assertIs(joint, joint_out) # The model is now complete. plant.Finalize() for joint in joints: self._test_joint_api(joint)
def __init__(self, scene_graph): LeafSystem.__init__(self) assert scene_graph mbp = MultibodyPlant(0.0) parser = Parser(mbp, scene_graph) model_id = parser.AddModelFromFile( FindResource("models/glider/glider.urdf")) mbp.Finalize() self.source_id = mbp.get_source_id() self.body_frame_id = mbp.GetBodyFrameIdOrThrow( mbp.GetBodyIndices(model_id)[0]) self.elevator_frame_id = mbp.GetBodyFrameIdOrThrow( mbp.GetBodyIndices(model_id)[1]) self.DeclareVectorInputPort("state", BasicVector(7)) self.DeclareAbstractOutputPort( "geometry_pose", lambda: AbstractValue.Make(FramePoseVector()), self.OutputGeometryPose)
def CreateIiwaControllerPlant(): # creates plant that includes only the robot, used for controllers. robot_sdf_path = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf" ) sim_timestep = 1e-3 plant_robot = MultibodyPlant(sim_timestep) parser = Parser(plant=plant_robot) parser.AddModelFromFile(robot_sdf_path) plant_robot.WeldFrames(A=plant_robot.world_frame(), B=plant_robot.GetFrameByName("iiwa_link_0")) plant_robot.mutable_gravity_field().set_gravity_vector([0, 0, 0]) plant_robot.Finalize() link_frame_indices = [] for i in range(8): link_frame_indices.append( plant_robot.GetFrameByName("iiwa_link_" + str(i)).index()) return plant_robot, link_frame_indices
def test_cart_pole(self): """Cart-Pole with simple geometry.""" file_name = FindResourceOrThrow( "drake/examples/multibody/cart_pole/cart_pole.sdf") builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) cart_pole = builder.AddSystem(MultibodyPlant()) parser = Parser(plant=cart_pole, scene_graph=scene_graph) parser.AddModelFromFile(file_name) cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81])) cart_pole.Finalize(scene_graph) assert cart_pole.geometry_source_is_registered() builder.Connect( cart_pole.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(cart_pole.get_source_id())) visualizer = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=None, 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_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.StepTo(.1)
def calculate_ee_composite_inertia(gripper_path): plant = MultibodyPlant(0) parser = Parser(plant) parser.AddModelFromFile(gripper_path) plant.Finalize() if gripper_path == schunk_path: gripper_body = plant.GetBodyByName("body") left_finger = plant.GetBodyByName("left_finger") right_finger = plant.GetBodyByName("right_finger") left_joint = plant.GetJointByName("left_finger_sliding_joint") right_joint = plant.GetJointByName("right_finger_sliding_joint") elif gripper_path == franka_hand_path: gripper_body = plant.GetBodyByName("panda_hand") left_finger = plant.GetBodyByName("panda_leftfinger") right_finger = plant.GetBodyByName("panda_rightfinger") left_joint = plant.GetJointByName("panda_finger_joint1") right_joint = plant.GetJointByName("panda_finger_joint2") else: raise ValueError("Gripper %s not known" % gripper_path) X_FLP = left_joint.frame_on_parent().GetFixedPoseInBodyFrame() X_FLC = left_joint.frame_on_child().GetFixedPoseInBodyFrame() X_FL = X_FLP.multiply(X_FLC.inverse()) X_FRP = right_joint.frame_on_parent().GetFixedPoseInBodyFrame() X_FRC = right_joint.frame_on_child().GetFixedPoseInBodyFrame() X_FR = X_FRP.multiply(X_FRC.inverse()) I_base = gripper_body.default_spatial_inertia() I_FL = left_finger.default_spatial_inertia() I_FR = right_finger.default_spatial_inertia() I_ee = I_base I_ee += I_FL.ReExpress(X_FL.rotation()).Shift(-X_FL.translation()) I_ee += I_FR.ReExpress(X_FR.rotation()).Shift(-X_FR.translation()) return I_ee
def test_model_instance_state_access_by_array(self): # Create a MultibodyPlant with a kuka arm and a schunk gripper. # the arm is welded to the world, the gripper is welded to the # arm's end effector. wsg50_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "wsg_50_description/sdf/schunk_wsg_50.sdf") iiwa_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/sdf/iiwa14_no_collision.sdf") timestep = 0.0002 plant = MultibodyPlant(timestep) parser = Parser(plant) iiwa_model = parser.AddModelFromFile(file_name=iiwa_sdf_path, model_name='robot') gripper_model = parser.AddModelFromFile(file_name=wsg50_sdf_path, model_name='gripper') # Weld the base of arm and gripper to reduce the number of states. X_EeGripper = RigidTransform(RollPitchYaw(np.pi / 2, 0, np.pi / 2), [0, 0, 0.081]) plant.WeldFrames(A=plant.world_frame(), B=plant.GetFrameByName("iiwa_link_0", iiwa_model)) plant.WeldFrames(A=plant.GetFrameByName("iiwa_link_7", iiwa_model), B=plant.GetFrameByName("body", gripper_model), X_AB=X_EeGripper) plant.Finalize() # Create a context of the MBP and set the state of the context # to desired values. context = plant.CreateDefaultContext() nq = plant.num_positions() nq_iiwa = plant.num_positions(iiwa_model) nv = plant.num_velocities() nv_iiwa = plant.num_velocities(iiwa_model) q_iiwa_desired = np.linspace(0, 0.3, 7) v_iiwa_desired = q_iiwa_desired + 0.4 q_gripper_desired = [0.4, 0.5] v_gripper_desired = [-1., -2.] x_desired = np.zeros(nq + nv) x_desired[0:7] = q_iiwa_desired x_desired[7:9] = q_gripper_desired x_desired[nq:nq + 7] = v_iiwa_desired x_desired[nq + 7:nq + nv] = v_gripper_desired x = plant.GetMutablePositionsAndVelocities(context=context) x[:] = x_desired q = plant.GetPositions(context=context) v = plant.GetVelocities(context=context) # Get state from context. x = plant.GetPositionsAndVelocities(context=context) x_tmp = plant.GetMutablePositionsAndVelocities(context=context) self.assertTrue(np.allclose(x_desired, x_tmp)) # Get positions and velocities of specific model instances # from the position/velocity vector of the plant. q_iiwa = plant.GetPositions(context=context, model_instance=iiwa_model) q_iiwa_array = plant.GetPositionsFromArray(model_instance=iiwa_model, q=q) self.assertTrue(np.allclose(q_iiwa, q_iiwa_array)) q_gripper = plant.GetPositions(context=context, model_instance=gripper_model) v_iiwa = plant.GetVelocities(context=context, model_instance=iiwa_model) v_iiwa_array = plant.GetVelocitiesFromArray(model_instance=iiwa_model, v=v) self.assertTrue(np.allclose(v_iiwa, v_iiwa_array)) v_gripper = plant.GetVelocities(context=context, model_instance=gripper_model) # Assert that the `GetPositions` and `GetVelocities` return # the desired values set earlier. self.assertTrue(np.allclose(q_iiwa_desired, q_iiwa)) self.assertTrue(np.allclose(v_iiwa_desired, v_iiwa)) self.assertTrue(np.allclose(q_gripper_desired, q_gripper)) self.assertTrue(np.allclose(v_gripper_desired, v_gripper)) # Verify that SetPositionsInArray() and SetVelocitiesInArray() works. plant.SetPositionsInArray(model_instance=iiwa_model, q_instance=np.zeros(nq_iiwa), q=q) self.assertTrue( np.allclose( plant.GetPositionsFromArray(model_instance=iiwa_model, q=q), np.zeros(nq_iiwa))) plant.SetVelocitiesInArray(model_instance=iiwa_model, v_instance=np.zeros(nv_iiwa), v=v) self.assertTrue( np.allclose( plant.GetVelocitiesFromArray(model_instance=iiwa_model, v=v), np.zeros(nv_iiwa))) # Check actuation. nu = plant.num_actuated_dofs() u = np.zeros(nu) u_iiwa = np.arange(nv_iiwa) plant.SetActuationInArray(model_instance=iiwa_model, u_instance=u_iiwa, u=u) self.assertTrue(np.allclose(u[:7], u_iiwa))
def test_model_instance_state_access(self): # Create a MultibodyPlant with a kuka arm and a schunk gripper. # the arm is welded to the world, the gripper is welded to the # arm's end effector. wsg50_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "wsg_50_description/sdf/schunk_wsg_50.sdf") iiwa_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/sdf/iiwa14_no_collision.sdf") plant = MultibodyPlant() parser = Parser(plant) iiwa_model = parser.AddModelFromFile(file_name=iiwa_sdf_path, model_name='robot') gripper_model = parser.AddModelFromFile(file_name=wsg50_sdf_path, model_name='gripper') # Weld the base of arm and gripper to reduce the number of states. X_EeGripper = RigidTransform(RollPitchYaw(np.pi / 2, 0, np.pi / 2), [0, 0, 0.081]) plant.WeldFrames(A=plant.world_frame(), B=plant.GetFrameByName("iiwa_link_0", iiwa_model)) plant.WeldFrames(A=plant.GetFrameByName("iiwa_link_7", iiwa_model), B=plant.GetFrameByName("body", gripper_model), X_AB=X_EeGripper) plant.Finalize() # Create a context of the MBP and set the state of the context # to desired values. context = plant.CreateDefaultContext() nq = plant.num_positions() nv = plant.num_velocities() nq_iiwa = 7 nv_iiwa = 7 nq_gripper = 2 nv_gripper = 2 q_iiwa_desired = np.zeros(nq_iiwa) v_iiwa_desired = np.zeros(nv_iiwa) q_gripper_desired = np.zeros(nq_gripper) v_gripper_desired = np.zeros(nv_gripper) q_iiwa_desired[2] = np.pi / 3 q_gripper_desired[0] = 0.1 v_iiwa_desired[1] = 5.0 q_gripper_desired[0] = -0.3 x_iiwa_desired = np.zeros(nq_iiwa + nv_iiwa) x_iiwa_desired[0:nq_iiwa] = q_iiwa_desired x_iiwa_desired[nq_iiwa:nq_iiwa + nv_iiwa] = v_iiwa_desired x_gripper_desired = np.zeros(nq_gripper + nv_gripper) x_gripper_desired[0:nq_gripper] = q_gripper_desired x_gripper_desired[nq_gripper:nq_gripper + nv_gripper] = v_gripper_desired x_desired = np.zeros(nq + nv) x_desired[0:7] = q_iiwa_desired x_desired[7:9] = q_gripper_desired x_desired[nq:nq + 7] = v_iiwa_desired x_desired[nq + 7:nq + nv] = v_gripper_desired # Check SetPositionsAndVelocities() for each model instance. # Do the iiwa model first. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetPositionsAndVelocities(context, iiwa_model, x_iiwa_desired) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model), x_iiwa_desired)) self.assertTrue( np.allclose( plant.GetPositionsAndVelocities(context, gripper_model), np.zeros(nq_gripper + nv_gripper))) # Do the gripper model. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetPositionsAndVelocities(context, gripper_model, x_gripper_desired) self.assertTrue( np.allclose( plant.GetPositionsAndVelocities(context, gripper_model), x_gripper_desired)) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa))) # Check SetPositions() for each model instance. # Do the iiwa model first. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetPositions(context, iiwa_model, q_iiwa_desired) self.assertTrue( np.allclose(plant.GetPositions(context, iiwa_model), q_iiwa_desired)) self.assertTrue( np.allclose(plant.GetVelocities(context, iiwa_model), np.zeros(nv_iiwa))) self.assertTrue( np.allclose( plant.GetPositionsAndVelocities(context, gripper_model), np.zeros(nq_gripper + nv_gripper))) # Do the gripper model. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetPositions(context, gripper_model, q_gripper_desired) self.assertTrue( np.allclose(plant.GetPositions(context, gripper_model), q_gripper_desired)) self.assertTrue( np.allclose(plant.GetVelocities(context, gripper_model), np.zeros(nq_gripper))) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa))) # Check SetVelocities() for each model instance. # Do the iiwa model first. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetVelocities(context, iiwa_model, v_iiwa_desired) self.assertTrue( np.allclose(plant.GetVelocities(context, iiwa_model), v_iiwa_desired)) self.assertTrue( np.allclose(plant.GetPositions(context, iiwa_model), np.zeros(nq_iiwa))) self.assertTrue( np.allclose( plant.GetPositionsAndVelocities(context, gripper_model), np.zeros(nq_gripper + nv_gripper))) # Do the gripper model. plant.SetPositionsAndVelocities(context, np.zeros(nq + nv)) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context), np.zeros(nq + nv))) plant.SetVelocities(context, gripper_model, v_gripper_desired) self.assertTrue( np.allclose(plant.GetVelocities(context, gripper_model), v_gripper_desired)) self.assertTrue( np.allclose(plant.GetPositions(context, gripper_model), np.zeros(nv_gripper))) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context, iiwa_model), np.zeros(nq_iiwa + nv_iiwa)))
help="Disable opening the gui window for testing.") MeshcatVisualizer.add_argparse_argument(parser) args = parser.parse_args() builder = DiagramBuilder() if args.hardware: station = builder.AddSystem(ManipulationStationHardwareInterface()) station.Connect(wait_for_cameras=False) else: station = builder.AddSystem(ManipulationStation()) station.SetupDefaultStation() parser = Parser(station.get_mutable_multibody_plant(), station.get_mutable_scene_graph()) object = parser.AddModelFromFile( FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf"), "object") 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())
def main(): args_parser = argparse.ArgumentParser( description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) args_parser.add_argument( "filename", type=str, help="Path to an SDF or URDF file.") args_parser.add_argument( "--package_path", type=str, default=None, help="Full path to the root package for reading in SDF resources.") position_group = args_parser.add_mutually_exclusive_group() position_group.add_argument( "--position", type=float, nargs="+", default=[], help="A list of positions which must be the same length as the number " "of positions in the sdf model. Note that most models have a " "floating-base joint by default (unless the sdf explicitly welds " "the base to the world, and so have 7 positions corresponding to " "the quaternion representation of that floating-base position.") position_group.add_argument( "--joint_position", type=float, nargs="+", default=[], help="A list of positions which must be the same length as the number " "of positions ASSOCIATED WITH JOINTS in the sdf model. This " "does not include, e.g., floating-base coordinates, which will " "be assigned a default value.") args_parser.add_argument( "--test", action='store_true', help="Disable opening the gui window for testing.") # TODO(russt): Add option to weld the base to the world pending the # availability of GetUniqueBaseBody requested in #9747. MeshcatVisualizer.add_argparse_argument(args_parser) args = args_parser.parse_args() filename = args.filename if not os.path.isfile(filename): args_parser.error("File does not exist: {}".format(filename)) builder = DiagramBuilder() scene_graph = builder.AddSystem(SceneGraph()) # Construct a MultibodyPlant. plant = MultibodyPlant(0.0) plant.RegisterAsSourceForSceneGraph(scene_graph) # Create the parser. parser = Parser(plant) # Get the package pathname. if args.package_path: # Verify that package.xml is found in the designated path. package_path = os.path.abspath(args.package_path) if not os.path.isfile(os.path.join(package_path, "package.xml")): parser.error("package.xml not found at: {}".format(package_path)) # Get the package map and populate it using the package path. package_map = parser.package_map() package_map.PopulateFromFolder(package_path) # Add the model from the file and finalize the plant. parser.AddModelFromFile(filename) plant.Finalize() # Add sliders to set positions of the joints. sliders = builder.AddSystem(JointSliders(robot=plant)) to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant)) builder.Connect(sliders.get_output_port(0), to_pose.get_input_port()) builder.Connect( to_pose.get_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) # Connect this to drake_visualizer. ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph) # Connect to Meshcat. if args.meshcat is not None: meshcat_viz = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=args.meshcat)) builder.Connect( scene_graph.get_pose_bundle_output_port(), meshcat_viz.get_input_port(0)) if len(args.position): sliders.set_position(args.position) elif len(args.joint_position): sliders.set_joint_position(args.joint_position) # Make the diagram and run it. diagram = builder.Build() simulator = Simulator(diagram) simulator.set_publish_every_time_step(False) if args.test: sliders.window.withdraw() simulator.AdvanceTo(0.1) else: simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(np.inf)
def LittleDogSimulationDiagram2(lcm, rb_tree, dt, drake_visualizer): builder = DiagramBuilder() num_pos = rb_tree.get_num_positions() num_actuators = rb_tree.get_num_actuators() zero_config = np.zeros( (rb_tree.get_num_actuators() * 2, 1)) # just for test # zero_config = np.concatenate((np.array([0, 1, 1, 1, 1.7, 0.5, 0, 0, 0]),np.zeros(9)), axis=0) # zero_config = np.concatenate((rb_tree.getZeroConfiguration(),np.zeros(9)), axis=0) zero_config = np.concatenate( (np.array([1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]), np.zeros(12)), axis=0) # zero_config[0] = 1.7 # zero_config[1] = 1.7 # zero_config[2] = 1.7 # zero_config[3] = 1.7 # 1.SceneGraph system scene_graph = builder.AddSystem(SceneGraph()) scene_graph.RegisterSource('scene_graph_n') plant = builder.AddSystem(MultibodyPlant()) plant_parser = Parser(plant, scene_graph) plant_parser.AddModelFromFile(file_name=model_path, model_name="littledog") plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("body")) # Add gravity to the model. plant.AddForceElement(UniformGravityFieldElement([0, 0, -9.81])) # Model is completed plant.Finalize() builder.Connect(scene_graph.get_query_output_port(), plant.get_geometry_query_input_port()) builder.Connect(plant.get_geometry_poses_output_port(), scene_graph.get_source_pose_port(plant.get_source_id())) ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph) # Robot State Encoder robot_state_encoder = builder.AddSystem( RobotStateEncoder(rb_tree)) # force_sensor_info robot_state_encoder.set_name('robot_state_encoder') builder.Connect(plant.get_continuous_state_output_port(), robot_state_encoder.joint_state_results_input_port()) # Robot command to Plant Converter robot_command_to_rigidbodyplant_converter = builder.AddSystem( RobotCommandToRigidBodyPlantConverter( rb_tree.actuators, motor_gain=None)) # input argu: rigidbody actuators robot_command_to_rigidbodyplant_converter.set_name( 'robot_command_to_rigidbodyplant_converter') builder.Connect( robot_command_to_rigidbodyplant_converter.desired_effort_output_port(), plant.get_actuation_input_port()) # PID controller kp, ki, kd = joints_PID_params(rb_tree) # PIDcontroller = builder.AddSystem(RobotPDAndFeedForwardController(rb_tree, kp, ki, kd)) # PIDcontroller.set_name('PID_controller') rb = RigidBodyTree() robot_frame = RigidBodyFrame("robot_frame", rb_tree.world(), [0, 0, 0], [0, 0, 0]) # insert a robot from urdf files pmap = PackageMap() pmap.PopulateFromFolder(os.path.dirname(model_path)) AddModelInstanceFromUrdfStringSearchingInRosPackages( open(model_path, 'r').read(), pmap, os.path.dirname(model_path), FloatingBaseType.kFixed, # FloatingBaseType.kRollPitchYaw, robot_frame, rb) PIDcontroller = builder.AddSystem( RbtInverseDynamicsController(rb, kp, ki, kd, False)) PIDcontroller.set_name('PID_controller') builder.Connect(robot_state_encoder.joint_state_outport_port(), PIDcontroller.get_input_port(0)) builder.Connect( PIDcontroller.get_output_port(0), robot_command_to_rigidbodyplant_converter.robot_command_input_port()) # Ref trajectory traj_src = builder.AddSystem(ConstantVectorSource(zero_config)) builder.Connect(traj_src.get_output_port(0), PIDcontroller.get_input_port(1)) # Signal logger logger = builder.AddSystem(SignalLogger(num_pos * 2)) builder.Connect(plant.get_continuous_state_output_port(), logger.get_input_port(0)) return builder.Build(), logger, plant
def __init__(self, config=None): if config is None: config_path = os.path.join(os.path.dirname(__file__), "config.yaml") config = yaml.safe_load(open(config_path, 'r')) self._config = config self._step_dt = config["step_dt"] self._model_name = config["model_name"] self._sim_diagram = DrakeSimDiagram(config) mbp = self._sim_diagram.mbp builder = self._sim_diagram.builder # === Add table ===== dims = config["table"]["size"] color = np.array(config["table"]["color"]) friction_params = config["table"]["coulomb_friction"] box_shape = Box(*dims) X_W_T = RigidTransform(p=np.array([0., 0., -dims[2] / 2.])) # This rigid body will be added to the world model instance since # the model instance is not specified. box_body = mbp.AddRigidBody( "table", SpatialInertia(mass=1.0, p_PScm_E=np.array([0., 0., 0.]), G_SP_E=UnitInertia(1.0, 1.0, 1.0))) mbp.WeldFrames(mbp.world_frame(), box_body.body_frame(), X_W_T) mbp.RegisterVisualGeometry(box_body, RigidTransform(), box_shape, "table_vis", color) mbp.RegisterCollisionGeometry(box_body, RigidTransform(), box_shape, "table_collision", CoulombFriction(*friction_params)) # === Add pusher ==== parser = Parser(mbp, self._sim_diagram.sg) self._pusher_model_id = parser.AddModelFromFile( path_util.pusher_peg, "pusher") base_link = mbp.GetBodyByName("base", self._pusher_model_id) mbp.WeldFrames(mbp.world_frame(), base_link.body_frame()) def pusher_port_func(): actuation_input_port = mbp.get_actuation_input_port( self._pusher_model_id) state_output_port = mbp.get_state_output_port( self._pusher_model_id) builder.ExportInput(actuation_input_port, "pusher_actuation_input") builder.ExportOutput(state_output_port, "pusher_state_output") self._sim_diagram.finalize_functions.append(pusher_port_func) # === Add slider ==== parser = Parser(mbp, self._sim_diagram.sg) self._slider_model_id = parser.AddModelFromFile( path_util.model_paths[self._model_name], self._model_name) def slider_port_func(): state_output_port = mbp.get_state_output_port( self._slider_model_id) builder.ExportOutput(state_output_port, "slider_state_output") self._sim_diagram.finalize_functions.append(slider_port_func) if "rgbd_sensors" in config and config["rgbd_sensors"]["enabled"]: self._sim_diagram.add_rgbd_sensors_from_config(config) if "visualization" in config: if not config["visualization"]: pass elif config["visualization"] == "meshcat": self._sim_diagram.connect_to_meshcat() elif config["visualization"] == "drake_viz": self._sim_diagram.connect_to_drake_visualizer() else: raise ValueError("Unknown visualization:", config["visualization"]) self._sim_diagram.finalize() builder = DiagramBuilder() builder.AddSystem(self._sim_diagram) pid = builder.AddSystem( PidController(kp=[0, 0], kd=[1000, 1000], ki=[0, 0])) builder.Connect(self._sim_diagram.GetOutputPort("pusher_state_output"), pid.get_input_port_estimated_state()) builder.Connect( pid.get_output_port_control(), self._sim_diagram.GetInputPort("pusher_actuation_input")) builder.ExportInput(pid.get_input_port_desired_state(), "pid_input_port_desired_state") self._diagram = builder.Build() self._pid_desired_state_port = self._diagram.get_input_port(0) self._simulator = Simulator(self._diagram) self.reset() self.action_space = spaces.Box(low=-1., high=1., shape=(2, ), dtype=np.float32) # TODO: Observation space for images is currently too loose self.observation_space = convert_observation_to_space( self.get_observation())
def test_model_instance_state_access_by_array(self): # Create a MultibodyPlant with a kuka arm and a schunk gripper. # the arm is welded to the world, the gripper is welded to the # arm's end effector. wsg50_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "wsg_50_description/sdf/schunk_wsg_50.sdf") iiwa_sdf_path = FindResourceOrThrow( "drake/manipulation/models/" + "iiwa_description/sdf/iiwa14_no_collision.sdf") timestep = 0.0002 plant = MultibodyPlant(timestep) parser = Parser(plant) iiwa_model = parser.AddModelFromFile(file_name=iiwa_sdf_path, model_name='robot') gripper_model = parser.AddModelFromFile(file_name=wsg50_sdf_path, model_name='gripper') # Weld the base of arm and gripper to reduce the number of states. X_EeGripper = Isometry3.Identity() X_EeGripper.set_translation([0, 0, 0.081]) X_EeGripper.set_rotation( RollPitchYaw(np.pi / 2, 0, np.pi / 2).ToRotationMatrix().matrix()) plant.WeldFrames(A=plant.world_frame(), B=plant.GetFrameByName("iiwa_link_0", iiwa_model)) plant.WeldFrames(A=plant.GetFrameByName("iiwa_link_7", iiwa_model), B=plant.GetFrameByName("body", gripper_model), X_AB=X_EeGripper) plant.Finalize() # Create a context of the MBP and set the state of the context # to desired values. context = plant.CreateDefaultContext() nq = plant.num_positions() nq_iiwa = plant.num_positions(iiwa_model) nv = plant.num_velocities() nv_iiwa = plant.num_velocities(iiwa_model) q_iiwa_desired = np.zeros(7) v_iiwa_desired = np.zeros(7) q_gripper_desired = np.zeros(2) v_gripper_desired = np.zeros(2) q_iiwa_desired[2] = np.pi / 3 q_gripper_desired[0] = 0.1 v_iiwa_desired[1] = 5.0 q_gripper_desired[0] = -0.3 x_plant_desired = np.zeros(nq + nv) x_plant_desired[0:7] = q_iiwa_desired x_plant_desired[7:9] = q_gripper_desired x_plant_desired[nq:nq + 7] = v_iiwa_desired x_plant_desired[nq + 7:nq + nv] = v_gripper_desired x_plant = plant.GetMutablePositionsAndVelocities(context) x_plant[:] = x_plant_desired q_plant = plant.GetPositions(context) v_plant = plant.GetVelocities(context) # Get state from context. x = plant.GetPositionsAndVelocities(context) x_plant_tmp = plant.GetMutablePositionsAndVelocities(context) self.assertTrue(np.allclose(x_plant_desired, x_plant_tmp)) # Get positions and velocities of specific model instances # from the postion/velocity vector of the plant. tree = plant.tree() self.check_old_spelling_exists(tree.GetPositionsFromArray) q_iiwa = plant.GetPositions(context, iiwa_model) q_iiwa_array = plant.GetPositionsFromArray(iiwa_model, q_plant) self.assertTrue(np.allclose(q_iiwa, q_iiwa_array)) q_gripper = plant.GetPositions(context, gripper_model) self.check_old_spelling_exists(tree.GetVelocitiesFromArray) v_iiwa = plant.GetVelocities(context, iiwa_model) v_iiwa_array = plant.GetVelocitiesFromArray(iiwa_model, v_plant) self.assertTrue(np.allclose(v_iiwa, v_iiwa_array)) v_gripper = plant.GetVelocities(context, gripper_model) # Assert that the `GetPositions` and `GetVelocities` return # the desired values set earlier. self.assertTrue(np.allclose(q_iiwa_desired, q_iiwa)) self.assertTrue(np.allclose(v_iiwa_desired, v_iiwa)) self.assertTrue(np.allclose(q_gripper_desired, q_gripper)) self.assertTrue(np.allclose(v_gripper_desired, v_gripper)) # Verify that SetPositionsInArray() and SetVelocitiesInArray() works. plant.SetPositionsInArray(iiwa_model, np.zeros(nq_iiwa), q_plant) self.assertTrue( np.allclose(plant.GetPositionsFromArray(iiwa_model, q_plant), np.zeros(nq_iiwa))) plant.SetVelocitiesInArray(iiwa_model, np.zeros(nv_iiwa), v_plant) self.assertTrue( np.allclose(plant.GetVelocitiesFromArray(iiwa_model, v_plant), np.zeros(nv_iiwa))) # Check actuation. nu = plant.num_actuated_dofs() u_plant = np.zeros(nu) u_iiwa = np.arange(nv_iiwa) plant.SetActuationInArray(iiwa_model, u_iiwa, u_plant) self.assertTrue(np.allclose(u_plant[:7], u_iiwa))