def test_connect_meshcat_visualizer(self): """Cart-Pole with simple geometry. This should produce two cart-poles, with identical geometry, one under the prefix `vis` and another under the prefix `vis2`. You should confirm in the visualizer that both exist (by unchecking one at a time in the MeshCat controls menu)""" 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() vis = ConnectMeshcatVisualizer(builder=builder, prefix="vis", scene_graph=scene_graph, zmq_url=ZMQ_URL, open_browser=False) self.assertIsInstance(vis, MeshcatVisualizer) vis2 = ConnectMeshcatVisualizer( builder=builder, prefix="vis2", output_port=scene_graph.get_query_output_port(), zmq_url=ZMQ_URL, open_browser=False) vis2.set_name("vis2") self.assertIsInstance(vis2, MeshcatVisualizer) diagram = builder.Build() context = diagram.CreateDefaultContext() vis.load(vis.GetMyContextFromRoot(context)) vis2.load(vis2.GetMyContextFromRoot(context)) diagram.Publish(context)
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 test_kuka(self): """Kuka IIWA with mesh geometry.""" file_name = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/sdf/" "iiwa14_no_collision.sdf") builder = DiagramBuilder() kuka, scene_graph = AddMultibodyPlantSceneGraph(builder) Parser(plant=kuka).AddModelFromFile(file_name) kuka.Finalize() visualizer = builder.AddSystem(MeshcatVisualizer(scene_graph, zmq_url=ZMQ_URL, open_browser=False)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() kuka_context = diagram.GetMutableSubsystemContext( kuka, diagram_context) kuka_context.FixInputPort( kuka.get_actuation_input_port().get_index(), np.zeros( kuka.get_actuation_input_port().size())) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.AdvanceTo(.1)
def test_kuka(self): """Kuka IIWA with mesh geometry.""" file_name = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/sdf/" "iiwa14_no_collision.sdf") builder = DiagramBuilder() kuka, scene_graph = AddMultibodyPlantSceneGraph(builder) Parser(plant=kuka).AddModelFromFile(file_name) kuka.Finalize() visualizer = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=ZMQ_URL, open_browser=False)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() kuka_context = diagram.GetMutableSubsystemContext( kuka, diagram_context) kuka_context.FixInputPort( kuka.get_actuation_input_port().get_index(), np.zeros(kuka.get_actuation_input_port().size())) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.AdvanceTo(.1)
def test_cart_pole(self): """Cart-Pole with simple geometry.""" file_name = FindResourceOrThrow( "drake/examples/multibody/cart_pole/cart_pole.sdf") builder = DiagramBuilder() cart_pole, scene_graph = AddMultibodyPlantSceneGraph(builder) Parser(plant=cart_pole).AddModelFromFile(file_name) cart_pole.Finalize() assert cart_pole.geometry_source_is_registered() visualizer = builder.AddSystem(MeshcatVisualizer(scene_graph, zmq_url=ZMQ_URL, open_browser=False)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() cart_pole_context = diagram.GetMutableSubsystemContext( cart_pole, diagram_context) cart_pole_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.AdvanceTo(.1)
def MustardExampleSystem(): builder = DiagramBuilder() # Create the physics engine + scene graph. plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0) parser = Parser(plant) AddPackagePaths(parser) 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_query_output_port(), meshcat.get_geometry_query_input_port()) AddRgbdSensors(builder, plant, scene_graph) diagram = builder.Build() diagram.set_name("depth_camera_demo_system") return diagram
def show_cloud(pc, use_native=False, **kwargs): # kwargs go to ctor. builder = DiagramBuilder() # Add point cloud visualization. if use_native: viz = meshcat.Visualizer(zmq_url=ZMQ_URL) else: plant, scene_graph = AddMultibodyPlantSceneGraph(builder) plant.Finalize() viz = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=ZMQ_URL, open_browser=False)) builder.Connect(scene_graph.get_pose_bundle_output_port(), viz.get_input_port(0)) pc_viz = builder.AddSystem( MeshcatPointCloudVisualizer(viz, **kwargs)) # Make sure the system runs. diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() context = diagram.GetMutableSubsystemContext( pc_viz, diagram_context) context.FixInputPort( pc_viz.GetInputPort("point_cloud_P").get_index(), AbstractValue.Make(pc)) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.StepTo(sim_time)
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 test_deprecated_finalize(self): builder = DiagramBuilder_[float]() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) Parser(plant).AddModelFromFile(FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf")) with catch_drake_warnings(expected_count=1): plant.Finalize(scene_graph)
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_contact_visualizer(self, T): meshcat = Meshcat() params = ContactVisualizerParams() params.publish_period = 0.123 params.default_color = Rgba(0.5, 0.5, 0.5) params.prefix = "py_visualizer" params.delete_on_initialization_event = False params.force_threshold = 0.2 params.newtons_per_meter = 5 params.radius = 0.1 self.assertNotIn("object at 0x", repr(params)) params2 = ContactVisualizerParams(publish_period=0.4) self.assertEqual(params2.publish_period, 0.4) vis = ContactVisualizer_[T](meshcat=meshcat, params=params) vis.Delete() vis.contact_results_input_port() builder = DiagramBuilder_[T]() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.01) plant.Finalize() ContactVisualizer_[T].AddToBuilder(builder=builder, plant=plant, meshcat=meshcat, params=params) ContactVisualizer_[T].AddToBuilder( builder=builder, contact_results_port=plant.get_contact_results_output_port(), meshcat=meshcat, params=params)
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_add_default_visualization(self): """Exercises AddDefaultVisualization. """ builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) plant.Finalize() config = mut.AddDefaultVisualization(builder=builder)
def visualize(q, dt=None): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, MultibodyPlant(0.001)) load_atlas(plant, add_ground=True) plant_context = plant.CreateDefaultContext() ConnectContactResultsToDrakeVisualizer(builder=builder, plant=plant) ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph) diagram = builder.Build() if len(q.shape) == 1: q = np.reshape(q, (1, -1)) for i in range(q.shape[0]): print(f"knot point: {i}") diagram_context = diagram.CreateDefaultContext() plant_context = diagram.GetMutableSubsystemContext(plant, diagram_context) set_null_input(plant, plant_context) plant.SetPositions(plant_context, q[i]) simulator = Simulator(diagram, diagram_context) simulator.set_target_realtime_rate(0.0) simulator.AdvanceTo(0.0001) if not dt is None: time.sleep(5/(np.sum(dt))*dt[i]) else: time.sleep(0.5)
def test_allegro_proximity_geometry(self): """Allegro hand with visual and collision geometry, drawn in proximity-geom mode.""" file_name = FindResourceOrThrow( "drake/manipulation/models/allegro_hand_description/sdf/" "allegro_hand_description_left.sdf") builder = DiagramBuilder() hand, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) Parser(plant=hand).AddModelFromFile(file_name) hand.Finalize() visualizer = builder.AddSystem( MeshcatVisualizer(zmq_url=ZMQ_URL, open_browser=False, role=Role.kProximity)) builder.Connect(scene_graph.get_query_output_port(), visualizer.get_geometry_query_input_port()) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() hand_context = diagram.GetMutableSubsystemContext( hand, diagram_context) hand_actuation_port = hand.get_actuation_input_port() hand_actuation_port.FixValue(hand_context, np.zeros(hand_actuation_port.size())) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.AdvanceTo(.1)
def main(): args_parser = argparse.ArgumentParser( description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) add_filename_and_parser_argparse_arguments(args_parser) add_visualizers_argparse_arguments(args_parser) args = args_parser.parse_args() filename, make_parser = parse_filename_and_parser(args_parser, args) update_visualization, connect_visualizers = parse_visualizers( args_parser, args) # Construct Plant + SceneGraph. builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0) # Load the model file. make_parser(plant).AddModelFromFile(filename) update_visualization(plant, scene_graph) plant.Finalize() connect_visualizers(builder, plant, scene_graph) diagram = builder.Build() context = diagram.CreateDefaultContext() # Use Simulator to dispatch initialization events. # TODO(eric.cousineau): Simplify as part of #10015. Simulator(diagram).Initialize() # Publish draw messages with current state. diagram.Publish(context)
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 testConnectPlanarSceneGraphVisualizer(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 = ConnectPlanarSceneGraphVisualizer( builder=builder, scene_graph=scene_graph, xlim=[0.3, 1.2]) self.assertIsInstance(visualizer, PlanarSceneGraphVisualizer) # Confirm that arguments are passed through. self.assertEqual(visualizer.ax.get_xlim(), (0.3, 1.2)) vis2 = ConnectPlanarSceneGraphVisualizer( builder=builder, scene_graph=scene_graph, output_port=scene_graph.get_pose_bundle_output_port()) vis2.set_name("vis2") self.assertIsInstance(vis2, PlanarSceneGraphVisualizer) diagram = builder.Build() context = diagram.CreateDefaultContext() diagram.Publish(context)
def test_subgraph_add_to_copying(self): """Ensures that index ordering is generally the same when copying a plant using a MultibodyPlantSubgraph.add_to.""" # TODO(eric.cousineau): Increas number of bodies for viz, once # `create_manual_map` can acommodate it. plant_a, scene_graph_a, _ = self.make_arbitrary_multibody_stuff( num_bodies=1) # Check for general ordering with full subgraph "cloning". builder_b = DiagramBuilder() plant_b, scene_graph_b = AddMultibodyPlantSceneGraph( builder_b, plant_a.time_step()) subgraph_a = mut.MultibodyPlantSubgraph( mut.get_elements_from_plant(plant_a, scene_graph_a)) a_to_b = subgraph_a.add_to(plant_b, scene_graph_b) plant_b.Finalize() util.assert_plant_equals(plant_a, scene_graph_a, plant_b, scene_graph_b) a_to_b_expected = self.create_manual_map(plant_a, scene_graph_a, plant_b, scene_graph_b) self.assertEqual(a_to_b, a_to_b_expected) if VISUALIZE: for model in me.get_model_instances(plant_b): util.build_with_no_control(builder_b, plant_b, model) print("test_subgraph_add_to_copying") DrakeVisualizer.AddToBuilder(builder_b, scene_graph_b) diagram = builder_b.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.) simulator.Initialize() diagram.Publish(simulator.get_context()) simulator.AdvanceTo(1.)
def DepthCameraDemoSystem(): builder = DiagramBuilder() # Create the physics engine + scene graph. plant, scene_graph = AddMultibodyPlantSceneGraph(builder) # Add a single object into it. Parser(plant, scene_graph).AddModelFromFile( FindResourceOrThrow( "drake/manipulation/models/ycb/sdf/006_mustard_bottle.sdf")) # Add a rendering engine renderer = "my_renderer" scene_graph.AddRenderer(renderer, MakeRenderEngineVtk(RenderEngineVtkParams())) 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)) # Add a camera to the environment. pose = RigidTransform(RollPitchYaw(-0.2, 0.2, 0), [-.1, -0.1, -.5]) properties = DepthCameraProperties(width=640, height=480, fov_y=np.pi / 4.0, renderer_name=renderer, z_near=0.1, z_far=10.0) camera = builder.AddSystem( RgbdSensor(parent_id=scene_graph.world_frame_id(), X_PB=pose, properties=properties, show_window=False)) camera.set_name("rgbd_sensor") builder.Connect(scene_graph.get_query_output_port(), camera.query_object_input_port()) # Export the camera outputs builder.ExportOutput(camera.color_image_output_port(), "color_image") builder.ExportOutput(camera.depth_image_32F_output_port(), "depth_image") # Add a system to convert the camera output into a point cloud to_point_cloud = builder.AddSystem( DepthImageToPointCloud(camera_info=camera.depth_camera_info(), fields=BaseField.kXYZs | BaseField.kRGBs)) builder.Connect(camera.depth_image_32F_output_port(), to_point_cloud.depth_image_input_port()) builder.Connect(camera.color_image_output_port(), to_point_cloud.color_image_input_port()) # Export the point cloud output. builder.ExportOutput(to_point_cloud.point_cloud_output_port(), "point_cloud") diagram = builder.Build() diagram.set_name("depth_camera_demo_system") return diagram
def simple_mbp(): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) Parser(plant).AddModelFromFile("common/test/models/double_pendulum.sdf") plant.Finalize() ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph) diagram = builder.Build() return plant, scene_graph, diagram
def make_plant(model_directive_path): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) InsertCorrectLoadingMethodHere(model_directive_path, plant) plant.Finalize() ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph) diagram = builder.Build() return plant, scene_graph, diagram
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 parse_model_and_create_context(file): """Tests a model by loading parsing it with a SceneGraph connected, building the relevant diagram, and allocating its default context.""" builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) Parser(plant).AddModelFromFile(file) plant.Finalize() diagram = builder.Build() diagram.CreateDefaultContext()
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_arbitrary_multibody_stuff(self, *, finalize=True, **kwargs): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.01) util.add_arbitrary_multibody_stuff(plant, **kwargs) if finalize: plant.Finalize() diagram = builder.Build() return plant, scene_graph, diagram else: return plant, scene_graph, builder
def test_usage_all(self): from pydrake.all import (AddMultibodyPlantSceneGraph, DiagramBuilder, FindResourceOrThrow, Parser, Simulator) builder = DiagramBuilder() plant, _ = AddMultibodyPlantSceneGraph(builder) Parser(plant).AddModelFromFile( FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf")) plant.Finalize() diagram = builder.Build() simulator = Simulator(diagram)
def __init__(self, config): Diagram.__init__(self) dt = config["mbp_dt"] self._builder = DiagramBuilder() self._mbp, self._sg = AddMultibodyPlantSceneGraph(self._builder, dt) self._finalize_functions = [] self._finalized = False self._rgbd_sensors = dict() self._renderer_name = None
def make_plant_and_context(floating_base=True, springs=True): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) if (springs): addCassieMultibody(plant, scene_graph, floating_base, cassie_urdf, True, True) else: addCassieMultibody(plant, scene_graph, floating_base, cassie_urdf_no_springs, False, True) plant.Finalize() return plant, plant.CreateDefaultContext()
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 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_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 setUp(self): file_name = FindResourceOrThrow( "drake/bindings/pydrake/multibody/test/two_bodies.sdf") builder = DiagramBuilder() self.plant, _ = AddMultibodyPlantSceneGraph( builder, MultibodyPlant(time_step=0.01)) model_instance = Parser(self.plant).AddModelFromFile(file_name) self.plant.Finalize() self.diagram = builder.Build() self.diagram_context = self.diagram.CreateDefaultContext() self.plant_context = self.diagram.GetMutableSubsystemContext( self.plant, self.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=self.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() 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])
class TestInverseKinematics(unittest.TestCase): """ This test reflects inverse_kinematics_test.cc """ def setUp(self): file_name = FindResourceOrThrow( "drake/bindings/pydrake/multibody/test/two_bodies.sdf") builder = DiagramBuilder() self.plant, _ = AddMultibodyPlantSceneGraph( builder, MultibodyPlant(time_step=0.01)) model_instance = Parser(self.plant).AddModelFromFile(file_name) self.plant.Finalize() self.diagram = builder.Build() self.diagram_context = self.diagram.CreateDefaultContext() self.plant_context = self.diagram.GetMutableSubsystemContext( self.plant, self.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=self.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() 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 _body1_quat(self, q): # TODO(eric.cousineau): Replace with state indexing. return q[0:4] def _body1_xyz(self, q): # TODO(eric.cousineau): Replace with state indexing. return q[4:7] def _body2_quat(self, q): # TODO(eric.cousineau): Replace with state indexing. return q[7:11] def _body2_xyz(self, q): # TODO(eric.cousineau): Replace with state indexing. return q[11:14] def test_AddPositionConstraint(self): p_BQ = np.array([0.2, 0.3, 0.5]) p_AQ_lower = np.array([-0.1, -0.2, -0.3]) p_AQ_upper = np.array([-0.05, -0.12, -0.28]) self.ik_two_bodies.AddPositionConstraint( frameB=self.body1_frame, p_BQ=p_BQ, frameA=self.body2_frame, p_AQ_lower=p_AQ_lower, p_AQ_upper=p_AQ_upper) result = mp.Solve(self.prog) self.assertTrue(result.is_success()) q_val = result.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body1_pos = self._body1_xyz(q_val) body2_quat = self._body2_quat(q_val) body2_pos = self._body2_xyz(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() p_AQ = body2_rotmat.transpose().dot( body1_rotmat.dot(p_BQ) + body1_pos - body2_pos) self.assertTrue(np.greater(p_AQ, p_AQ_lower - 1E-6 * np.ones((3, 1))).all()) self.assertTrue(np.less(p_AQ, p_AQ_upper + 1E-6 * np.ones((3, 1))).all()) with warnings.catch_warnings(record=True) as w: warnings.simplefilter('always', DrakeDeprecationWarning) self.assertEqual( self.prog.Solve(), mp.SolutionResult.kSolutionFound) self.assertTrue(np.allclose( self.prog.GetSolution(self.q), q_val)) self.assertEqual(len(w), 2) def test_AddOrientationConstraint(self): theta_bound = 0.2 * math.pi R_AbarA = RotationMatrix(quaternion=Quaternion(0.5, -0.5, 0.5, 0.5)) R_BbarB = RotationMatrix( quaternion=Quaternion(1.0 / 3, 2.0 / 3, 0, 2.0 / 3)) self.ik_two_bodies.AddOrientationConstraint( frameAbar=self.body1_frame, R_AbarA=R_AbarA, frameBbar=self.body2_frame, R_BbarB=R_BbarB, theta_bound=theta_bound) result = mp.Solve(self.prog) self.assertTrue(result.is_success()) q_val = result.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body2_quat = self._body2_quat(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat) R_AB = R_AbarA.matrix().transpose().dot( R_AbarBbar.dot(R_BbarB.matrix())) self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6) with warnings.catch_warnings(record=True) as w: warnings.simplefilter('always', DrakeDeprecationWarning) self.assertEqual( self.prog.Solve(), mp.SolutionResult.kSolutionFound) q_val = self.prog.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body2_quat = self._body2_quat(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() R_AbarBbar = body1_rotmat.transpose().dot(body2_rotmat) R_AB = R_AbarA.matrix().transpose().dot( R_AbarBbar.dot(R_BbarB.matrix())) self.assertGreater(R_AB.trace(), 1 + 2 * math.cos(theta_bound) - 1E-6) self.assertEqual(len(w), 2) def test_AddGazeTargetConstraint(self): p_AS = np.array([0.1, 0.2, 0.3]) n_A = np.array([0.3, 0.5, 1.2]) p_BT = np.array([1.1, 0.2, 1.5]) cone_half_angle = 0.2 * math.pi self.ik_two_bodies.AddGazeTargetConstraint( frameA=self.body1_frame, p_AS=p_AS, n_A=n_A, frameB=self.body2_frame, p_BT=p_BT, cone_half_angle=cone_half_angle) result = mp.Solve(self.prog) self.assertTrue(result.is_success()) q_val = result.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body1_pos = self._body1_xyz(q_val) body2_quat = self._body2_quat(q_val) body2_pos = self._body2_xyz(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() p_WS = body1_pos + body1_rotmat.dot(p_AS) p_WT = body2_pos + body2_rotmat.dot(p_BT) p_ST_W = p_WT - p_WS n_W = body1_rotmat.dot(n_A) self.assertGreater(p_ST_W.dot(n_W), np.linalg.norm( p_ST_W) * np.linalg.norm(n_W) * math.cos(cone_half_angle) - 1E-6) with warnings.catch_warnings(record=True) as w: warnings.simplefilter('always', DrakeDeprecationWarning) self.assertEqual( self.prog.Solve(), mp.SolutionResult.kSolutionFound) q_val = self.prog.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body1_pos = self._body1_xyz(q_val) body2_quat = self._body2_quat(q_val) body2_pos = self._body2_xyz(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() p_WS = body1_pos + body1_rotmat.dot(p_AS) p_WT = body2_pos + body2_rotmat.dot(p_BT) p_ST_W = p_WT - p_WS n_W = body1_rotmat.dot(n_A) self.assertGreater(p_ST_W.dot(n_W), np.linalg.norm( p_ST_W) * np.linalg.norm(n_W) * math.cos(cone_half_angle) - 1E-6) self.assertEqual(len(w), 2) def test_AddAngleBetweenVectorsConstraint(self): na_A = np.array([0.2, -0.4, 0.9]) nb_B = np.array([1.4, -0.1, 1.8]) angle_lower = 0.2 * math.pi angle_upper = 0.2 * math.pi self.ik_two_bodies.AddAngleBetweenVectorsConstraint( frameA=self.body1_frame, na_A=na_A, frameB=self.body2_frame, nb_B=nb_B, angle_lower=angle_lower, angle_upper=angle_upper) result = mp.Solve(self.prog) self.assertTrue(result.is_success()) q_val = result.GetSolution(self.q) body1_quat = self._body1_quat(q_val) body2_quat = self._body2_quat(q_val) body1_rotmat = Quaternion(body1_quat).rotation() body2_rotmat = Quaternion(body2_quat).rotation() na_W = body1_rotmat.dot(na_A) nb_W = body2_rotmat.dot(nb_B) angle = math.acos(na_W.transpose().dot(nb_W) / (np.linalg.norm(na_W) * np.linalg.norm(nb_W))) self.assertLess(math.fabs(angle - angle_lower), 1E-6) with warnings.catch_warnings(record=True) as w: warnings.simplefilter('always', DrakeDeprecationWarning) self.assertEqual( self.prog.Solve(), mp.SolutionResult.kSolutionFound) self.assertTrue(np.allclose( self.prog.GetSolution(self.q), q_val)) self.assertEqual(len(w), 2) def test_AddMinimumDistanceConstraint(self): ik = self.ik_two_bodies W = self.plant.world_frame() B1 = self.body1_frame B2 = self.body2_frame min_distance = 0.1 tol = 1e-2 radius1 = 0.1 radius2 = 0.2 ik.AddMinimumDistanceConstraint(minimal_distance=min_distance) context = self.plant.CreateDefaultContext() R_I = np.eye(3) self.plant.SetFreeBodyPose( context, B1.body(), Isometry3(R_I, [0, 0, 0.01])) self.plant.SetFreeBodyPose( context, B2.body(), Isometry3(R_I, [0, 0, -0.01])) def get_min_distance_actual(): X = partial(self.plant.CalcRelativeTransform, context) distance = norm(X(W, B1).translation() - X(W, B2).translation()) return distance - radius1 - radius2 self.assertLess(get_min_distance_actual(), min_distance - tol) self.prog.SetInitialGuess(ik.q(), self.plant.GetPositions(context)) result = mp.Solve(self.prog) self.assertTrue(result.is_success()) q_val = result.GetSolution(ik.q()) self.plant.SetPositions(context, q_val) self.assertGreater(get_min_distance_actual(), min_distance - tol) with warnings.catch_warnings(record=True) as w: warnings.simplefilter('always', DrakeDeprecationWarning) self.assertEqual( self.prog.Solve(), mp.SolutionResult.kSolutionFound) self.assertTrue(np.allclose( self.prog.GetSolution(ik.q()), q_val)) self.assertEqual(len(w), 2)