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_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 MustardExampleSystem(): builder = DiagramBuilder() # Create the physics engine + scene graph. plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0) parser = Parser(plant) parser.package_map().Add("manipulation", FindResource("models")) ProcessModelDirectives( LoadModelDirectives(FindResource("models/mustard_w_cameras.yaml")), plant, parser) plant.Finalize() # Add a visualizer just to help us see the object. use_meshcat = False if use_meshcat: meshcat = builder.AddSystem(MeshcatVisualizer(scene_graph)) builder.Connect(scene_graph.get_pose_bundle_output_port(), meshcat.get_input_port(0)) AddRgbdSensors(builder, plant, scene_graph) diagram = builder.Build() diagram.set_name("depth_camera_demo_system") return diagram
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 test_model_directives(self): model_dir = os.path.dirname( FindResourceOrThrow("drake/multibody/parsing/test/" "process_model_directives_test/package.xml")) plant = MultibodyPlant(time_step=0.01) parser = Parser(plant=plant) parser.package_map().PopulateFromFolder(model_dir) directives_file = model_dir + "/add_scoped_top.yaml" directives = LoadModelDirectives(directives_file) added_models = ProcessModelDirectives(directives=directives, plant=plant, parser=parser) # Check for an instance. model_names = [model.model_name for model in added_models] self.assertIn("extra_model", model_names) plant.GetModelInstanceByName("extra_model") # Test that other bound symbols exist. ModelInstanceInfo.model_name ModelInstanceInfo.model_path ModelInstanceInfo.parent_frame_name ModelInstanceInfo.child_frame_name ModelInstanceInfo.X_PC ModelInstanceInfo.model_instance AddFrame.name AddFrame.X_PF frame = GetScopedFrameByName(plant, "world") self.assertIsNotNone(GetScopedFrameName(plant, frame))
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 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_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 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 add_rope(plant, scene_graph, rope_config, X_W, rope_name="rope"): parser = Parser(plant, scene_graph) rope_sdf = generate_rope_sdf_from_config(rope_config, rope_name) rope_model = parser.AddModelFromString(file_contents=rope_sdf, file_type="sdf", model_name=rope_name) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName(f"{rope_name}_capsule_1"), X_W) return rope_model
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_plant_parser_directives(self): """Returns a tuple (plant, parser, directives) for later testing.""" model_dir = os.path.dirname( FindResourceOrThrow("drake/multibody/parsing/test/" "process_model_directives_test/package.xml")) plant = MultibodyPlant(time_step=0.01) parser = Parser(plant=plant) parser.package_map().PopulateFromFolder(model_dir) directives_file = model_dir + "/add_scoped_top.dmd.yaml" directives = LoadModelDirectives(directives_file) return (plant, parser, directives)
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 test_parser_string(self): """Checks parsing from a string (not file_name).""" sdf_file = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") with open(sdf_file, "r") as f: sdf_contents = f.read() plant = MultibodyPlant(time_step=0.01) parser = Parser(plant=plant) result = parser.AddModelFromString( file_contents=sdf_contents, file_type="sdf") self.assertIsInstance(result, ModelInstanceIndex)
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 _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 make_parser(plant): 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")): args_parser.error(f"package.xml not found at: {package_path}") # Get the package map and populate it using the package path. parser.package_map().PopulateFromFolder(package_path) return parser
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 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 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_model_directives(self): model_dir = os.path.dirname(FindResourceOrThrow( "drake/multibody/parsing/test/" "process_model_directives_test/package.xml")) plant = MultibodyPlant(time_step=0.01) parser = Parser(plant=plant) parser.package_map().PopulateFromFolder(model_dir) directives_file = model_dir + "/add_scoped_top.yaml" directives = LoadModelDirectives(directives_file) added_models = ProcessModelDirectives( directives=directives, plant=plant, parser=parser) # Check for an instance. model_names = [model.model_name for model in added_models] self.assertIn("extra_model", model_names) plant.GetModelInstanceByName("extra_model")
def setUp(self): builder_f = DiagramBuilder() self.plant_f, self.scene_graph_f = AddMultibodyPlantSceneGraph( builder_f, MultibodyPlant(time_step=0.01)) Parser(self.plant_f).AddModelFromFile(FindResourceOrThrow( "drake/bindings/pydrake/multibody/test/two_bodies.sdf")) self.plant_f.Finalize() diagram_f = builder_f.Build() diagram_ad = diagram_f.ToAutoDiffXd() plant_ad = diagram_ad.GetSubsystemByName(self.plant_f.get_name()) TypeVariables = namedtuple( "TypeVariables", ("plant", "plant_context", "body1_frame", "body2_frame")) def make_type_variables(plant_T, diagram_T): diagram_context_T = diagram_T.CreateDefaultContext() return TypeVariables( plant=plant_T, plant_context=diagram_T.GetMutableSubsystemContext( plant_T, diagram_context_T), body1_frame=plant_T.GetBodyByName("body1").body_frame(), body2_frame=plant_T.GetBodyByName("body2").body_frame()) self.variables_f = make_type_variables(self.plant_f, diagram_f) self.variables_ad = make_type_variables(plant_ad, diagram_ad)
def testConnectMeschatVisualizer(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() visualizer = ConnectMeshcatVisualizer(builder=builder, scene_graph=scene_graph, zmq_url=ZMQ_URL, open_browser=False) self.assertIsInstance(visualizer, MeshcatVisualizer) vis2 = ConnectMeshcatVisualizer( builder=builder, scene_graph=scene_graph, output_port=scene_graph.get_pose_bundle_output_port(), zmq_url=ZMQ_URL, open_browser=False) vis2.set_name("vis2") self.assertIsInstance(vis2, MeshcatVisualizer) diagram = builder.Build() context = diagram.CreateDefaultContext() diagram.Publish(context)
def setUp(self): builder = DiagramBuilder() self.plant, self.scene_graph = AddMultibodyPlantSceneGraph( builder, MultibodyPlant(time_step=0.01)) Parser(self.plant).AddModelFromFile(FindResourceOrThrow( "drake/bindings/pydrake/multibody/test/two_bodies.sdf")) self.plant.Finalize() diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() plant_context = diagram.GetMutableSubsystemContext( self.plant, diagram_context) self.body1_frame = self.plant.GetBodyByName("body1").body_frame() self.body2_frame = self.plant.GetBodyByName("body2").body_frame() self.ik_two_bodies = ik.InverseKinematics( plant=self.plant, plant_context=plant_context) # Test non-SceneGraph constructor. ik.InverseKinematics(plant=self.plant) self.prog = self.ik_two_bodies.get_mutable_prog() self.q = self.ik_two_bodies.q() # Test constructor without joint limits ik.InverseKinematics(plant=self.plant, with_joint_limits=False) ik.InverseKinematics( plant=self.plant, plant_context=plant_context, with_joint_limits=False) def squaredNorm(x): return np.array([x[0] ** 2 + x[1] ** 2 + x[2] ** 2 + x[3] ** 2]) self.prog.AddConstraint( squaredNorm, [1], [1], self._body1_quat(self.q)) self.prog.AddConstraint( squaredNorm, [1], [1], self._body2_quat(self.q)) self.prog.SetInitialGuess(self._body1_quat(self.q), [1, 0, 0, 0]) self.prog.SetInitialGuess(self._body2_quat(self.q), [1, 0, 0, 0])
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 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.AddForceElement(UniformGravityFieldElement()) cart_pole.Finalize() assert cart_pole.geometry_source_is_registered() 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 test_multibody_state_access(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant() Parser(plant).AddModelFromFile(file_name) plant.Finalize() context = plant.CreateDefaultContext() nq = 2 nv = 2 self.assertEqual(plant.num_positions(), nq) self.assertEqual(plant.num_velocities(), nv) q0 = np.array([3.14, 2.]) v0 = np.array([-0.5, 1.]) x0 = np.concatenate([q0, v0]) # The default state is all values set to zero. x = plant.GetPositionsAndVelocities(context) self.assertTrue(np.allclose(x, np.zeros(4))) # Write into a mutable reference to the state vector. x_ref = plant.GetMutablePositionsAndVelocities(context) x_ref[:] = x0 # Verify that positions and velocities were set correctly. self.assertTrue(np.allclose(plant.GetPositions(context), q0)) self.assertTrue(np.allclose(plant.GetVelocities(context), v0)) # Verify we did modify the state stored in context. x = plant.GetPositionsAndVelocities(context) self.assertTrue(np.allclose(x, x0)) # Now set positions and velocities independently and check them. zeros_2 = np.zeros([2, 1]) x_ref.fill(0) plant.SetPositions(context, q0) self.assertTrue(np.allclose(plant.GetPositions(context), q0)) self.assertTrue(np.allclose(plant.GetVelocities(context), zeros_2)) x_ref.fill(0) plant.SetVelocities(context, v0) self.assertTrue(np.allclose(plant.GetPositions(context), zeros_2)) self.assertTrue(np.allclose(plant.GetVelocities(context), v0)) # Now test SetPositionsAndVelocities(). x_ref.fill(0) plant.SetPositionsAndVelocities(context, x0) self.assertTrue( np.allclose(plant.GetPositionsAndVelocities(context), x0)) # Test existence of context resetting methods. plant.SetDefaultState(context, state=context.get_mutable_state()) # Test existence of limits. self.assertEqual(plant.GetPositionLowerLimits().shape, (nq, )) self.assertEqual(plant.GetPositionUpperLimits().shape, (nq, )) self.assertEqual(plant.GetVelocityLowerLimits().shape, (nv, )) self.assertEqual(plant.GetVelocityUpperLimits().shape, (nv, )) self.assertEqual(plant.GetAccelerationLowerLimits().shape, (nv, )) self.assertEqual(plant.GetAccelerationUpperLimits().shape, (nv, ))
def test_parser(self): # Calls every combination of argments for the Parser methods and # inspects their return type. sdf_file = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") urdf_file = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.urdf") for dut, file_name, model_name, result_dim in ( (Parser.AddModelFromFile, sdf_file, None, int), (Parser.AddModelFromFile, sdf_file, "", int), (Parser.AddModelFromFile, sdf_file, "a", int), (Parser.AddModelFromFile, urdf_file, None, int), (Parser.AddModelFromFile, urdf_file, "", int), (Parser.AddModelFromFile, urdf_file, "a", int), (Parser.AddAllModelsFromFile, sdf_file, None, list), (Parser.AddAllModelsFromFile, urdf_file, None, list), ): plant = MultibodyPlant(time_step=0.01) parser = Parser(plant=plant) if model_name is None: result = dut(parser, file_name=file_name) else: result = dut(parser, file_name=file_name, model_name=model_name) if result_dim is int: self.assertIsInstance(result, ModelInstanceIndex) else: assert result_dim is list self.assertIsInstance(result, list) self.assertIsInstance(result[0], ModelInstanceIndex)
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. 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() 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(scene_graph) # 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) 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.StepTo(0.1) else: simulator.set_target_realtime_rate(1.0) simulator.StepTo(np.inf)