def test_copying(self): """Ensures that index ordering is generally the same when copying a plant using a subgraph.""" time_step = 0.01 builder_a = DiagramBuilder() plant_a, scene_graph_a = AddMultibodyPlantSceneGraph( builder_a, time_step) util.add_arbitrary_multibody_stuff(plant_a) # Ensure that this is "physically" valid. plant_a.Finalize() if VISUALIZE: for model in me.get_model_instances(plant_a): util.no_control(builder_a, plant_a, model) print("test_composition_array_with_scene_graph") ConnectDrakeVisualizer(builder_a, scene_graph_a) diagram = builder_a.Build() simulator = Simulator(diagram) simulator.Initialize() diagram.Publish(simulator.get_context()) simulator.AdvanceTo(1.) # Checking for determinism, making a slight change to trigger an error. for slight_difference in [False, True]: builder_b = DiagramBuilder() plant_b, scene_graph_b = AddMultibodyPlantSceneGraph( builder_b, time_step) util.add_arbitrary_multibody_stuff( plant_b, slight_difference=slight_difference) plant_b.Finalize() if not slight_difference: util.assert_plant_equals(plant_a, scene_graph_a, plant_b, scene_graph_b) else: with self.assertRaises(AssertionError): util.assert_plant_equals(plant_a, scene_graph_a, plant_b, scene_graph_b) # Check for general ordering with full subgraph "cloning". builder_b = DiagramBuilder() plant_b, scene_graph_b = AddMultibodyPlantSceneGraph( builder_b, time_step) subgraph_a = mut.MultibodyPlantSubgraph( mut.get_elements_from_plant(plant_a, scene_graph_a)) subgraph_a.add_to(plant_b, scene_graph_b) plant_b.Finalize() util.assert_plant_equals(plant_a, scene_graph_a, plant_b, scene_graph_b)
def 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 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 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_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.AddForceElement(UniformGravityFieldElement()) kuka.Finalize() 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 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_subgraph_add_to_copying(self): """Ensures that index ordering is generally the same when copying a plant using a MultibodyPlantSubgraph.add_to.""" # TODO(eric.cousineau): Increas number of bodies for viz, once # `create_manual_map` can acommodate it. plant_a, scene_graph_a, _ = self.make_arbitrary_multibody_stuff( num_bodies=1) # Check for general ordering with full subgraph "cloning". builder_b = DiagramBuilder() plant_b, scene_graph_b = AddMultibodyPlantSceneGraph( builder_b, plant_a.time_step()) subgraph_a = mut.MultibodyPlantSubgraph( mut.get_elements_from_plant(plant_a, scene_graph_a)) a_to_b = subgraph_a.add_to(plant_b, scene_graph_b) plant_b.Finalize() util.assert_plant_equals(plant_a, scene_graph_a, plant_b, scene_graph_b) a_to_b_expected = self.create_manual_map(plant_a, scene_graph_a, plant_b, scene_graph_b) self.assertEqual(a_to_b, a_to_b_expected) if VISUALIZE: for model in me.get_model_instances(plant_b): util.build_with_no_control(builder_b, plant_b, model) print("test_subgraph_add_to_copying") DrakeVisualizer.AddToBuilder(builder_b, scene_graph_b) diagram = builder_b.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.) simulator.Initialize() diagram.Publish(simulator.get_context()) simulator.AdvanceTo(1.)
def test_procedural_geometry(): """ This test ensures we can draw procedurally added primitive geometry that is added to the world model instance (which has a slightly different naming scheme than geometry with a non-default / non-world model instance). """ builder = DiagramBuilder() mbp, scene_graph = AddMultibodyPlantSceneGraph(builder) world_body = mbp.world_body() box_shape = Box(1., 2., 3.) # This rigid body will be added to the world model instance since # the model instance is not specified. box_body = mbp.AddRigidBody( "box", SpatialInertia(mass=1.0, p_PScm_E=np.array([0., 0., 0.]), G_SP_E=UnitInertia(1.0, 1.0, 1.0))) mbp.WeldFrames(world_body.body_frame(), box_body.body_frame(), RigidTransform()) mbp.RegisterVisualGeometry(box_body, RigidTransform.Identity(), box_shape, "ground_vis", np.array([0.5, 0.5, 0.5, 1.])) mbp.RegisterCollisionGeometry(box_body, RigidTransform.Identity(), box_shape, "ground_col", CoulombFriction(0.9, 0.8)) mbp.Finalize() # frames_to_draw = {"world": {"box"}} # visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph)) # builder.Connect(scene_graph.get_pose_bundle_output_port(), # visualizer.get_input_port(0)) diagram = builder.Build()
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 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 __init__(self, config): self._config = config builder = DiagramBuilder() dt = config['env']['mbp_dt'] mbp, sg = AddMultibodyPlantSceneGraph(builder, MultibodyPlant(dt), SceneGraph()) parser = Parser(mbp, sg) # renderer_params = RenderEngineVtkParams() # renderer_name = "vtk_renderer" # sg.AddRenderer(renderer_name, MakeRenderEngineVtk(renderer_params)) self._mbp = mbp self._sg = sg self._parser = parser self._builder = builder self._rgbd_sensors = dict() self._finalized = False self._built = False self._diagram = None # add rendered # Add renderer to scene renderer_params = RenderEngineVtkParams() self._renderer_name = "vtk_renderer" sg.AddRenderer(self._renderer_name, MakeRenderEngineVtk(renderer_params))
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_multibody_plant_construction_api(self, T): # SceneGraph does not support `Expression` type. DiagramBuilder = DiagramBuilder_[T] SpatialInertia = SpatialInertia_[float] RigidTransform = RigidTransform_[T] CoulombFriction = CoulombFriction_[T] builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) spatial_inertia = SpatialInertia() body = plant.AddRigidBody(name="new_body", M_BBo_B=spatial_inertia) new_model_instance = plant.AddModelInstance("new_model_instance") body = plant.AddRigidBody(name="new_body_2", M_BBo_B=spatial_inertia, model_instance=new_model_instance) box = Box(width=0.5, depth=1.0, height=2.0) body_X_BG = RigidTransform() body_friction = CoulombFriction(static_friction=0.6, dynamic_friction=0.5) if T == float: plant.RegisterVisualGeometry( body=body, X_BG=body_X_BG, shape=box, name="new_body_visual", diffuse_color=[1., 0.64, 0.0, 0.5]) plant.RegisterCollisionGeometry( body=body, X_BG=body_X_BG, shape=box, name="new_body_collision", coulomb_friction=body_friction)
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_add_default_visualization(self): """Exercises AddDefaultVisualization. """ builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) plant.Finalize() config = mut.AddDefaultVisualization(builder=builder)
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 construct_environment(masses: typing.List, box_sizes: typing.List): """ Construct an environment with many free boxes. @param masses masses[i] is the mass of box i. @param box_sizes box_sizes[i] is the size of box i. """ builder = DiagramBuilder_[AutoDiffXd]() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) # Add the ground as a big box. ground_box = plant.AddRigidBody( "ground", SpatialInertia(1, np.array([0, 0, 0]), UnitInertia(1, 1, 1))) X_WG = RigidTransform([0, 0, -0.05]) ground_geometry_id = plant.RegisterCollisionGeometry( ground_box, RigidTransform(), Box(10, 10, 0.1), "ground", CoulombFriction(0.9, 0.8)) plant.RegisterVisualGeometry( ground_box, RigidTransform(), Box(10, 10, 0.1), "ground", [0.5, 0.5, 0.5, 1.]) plant.WeldFrames(plant.world_frame(), ground_box.body_frame(), X_WG) # Add boxes assert isinstance(masses, list) assert isinstance(box_sizes, list) assert len(masses) == len(box_sizes) num_boxes = len(masses) boxes = [] boxes_geometry_id = [] for i in range(num_boxes): box_name = f"box{i}" boxes.append(plant.AddRigidBody( box_name, SpatialInertia( masses[i], np.array([0, 0, 0]), UnitInertia(1, 1, 1)))) box_shape = Box(box_sizes[i][0], box_sizes[i][1], box_sizes[i][2]) boxes_geometry_id.append(plant.RegisterCollisionGeometry( boxes[i], RigidTransform(), box_shape, f"{box_name}_box", CoulombFriction(0.9, 0.8))) plant.RegisterVisualGeometry( ground_box, RigidTransform(), box_shape, f"{box_name}_box", [0.5, 0.5, 0.5, 1.]) # Add small spheres at the corners of the box. vertices = np.array([ [1, 1, 1], [1, 1, -1], [1, -1, 1], [1, -1, -1], [-1, 1, 1], [-1, -1, 1], [-1, 1, -1], [-1, -1, -1]]) *\ np.tile(box_sizes[i]/2, (8, 1)) sphere_shape = Sphere(0.001) for j in range(8): plant.RegisterCollisionGeometry( boxes[i], RigidTransform(vertices[j]), sphere_shape, f"{box_name}_sphere{j}", CoulombFriction(0.9, 0.8)) plant.RegisterVisualGeometry( boxes[i], RigidTransform(vertices[j]), sphere_shape, f"{box_name}_sphere{j}", [0.5, 0.5, 0.5, 1]) plant.Finalize() diagram = builder.Build() return Environment( plant=plant, scene_graph=scene_graph, diagram=diagram, boxes=boxes, ground_geometry_id=ground_geometry_id, boxes_geometry_id=boxes_geometry_id)
def test_allegro_proximity_geometry(self): """Allegro hand with visual and collision geometry, drawn in proximity-geom mode.""" file_name = FindResourceOrThrow( "drake/manipulation/models/allegro_hand_description/sdf/" "allegro_hand_description_left.sdf") builder = DiagramBuilder() hand, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) Parser(plant=hand).AddModelFromFile(file_name) hand.Finalize() visualizer = builder.AddSystem( MeshcatVisualizer(zmq_url=ZMQ_URL, open_browser=False, role=Role.kProximity)) builder.Connect(scene_graph.get_query_output_port(), visualizer.get_geometry_query_input_port()) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() hand_context = diagram.GetMutableSubsystemContext( hand, diagram_context) hand_actuation_port = hand.get_actuation_input_port() hand_actuation_port.FixValue(hand_context, np.zeros(hand_actuation_port.size())) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.AdvanceTo(.1)
def test_iris_cspace(self): limits_urdf = """ <robot name="limits"> <link name="movable"> <collision> <geometry><box size="1 1 1"/></geometry> </collision> </link> <joint name="movable" type="prismatic"> <axis xyz="1 0 0"/> <limit lower="-2" upper="2"/> <parent link="world"/> <child link="movable"/> </joint> </robot>""" builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) Parser(plant).AddModelFromString(limits_urdf, "urdf") plant.Finalize() diagram = builder.Build() context = diagram.CreateDefaultContext() options = mut.IrisOptions() with catch_drake_warnings(expected_count=1): region = mut.IrisInConfigurationSpace( plant=plant, context=plant.GetMyContextFromRoot(context), sample=[0], options=options) plant.SetPositions(plant.GetMyMutableContextFromRoot(context), [0]) region = mut.IrisInConfigurationSpace( plant=plant, context=plant.GetMyContextFromRoot(context), options=options) self.assertIsInstance(region, mut.ConvexSet) self.assertEqual(region.ambient_dimension(), 1) self.assertTrue(region.PointInSet([1.0])) self.assertFalse(region.PointInSet([3.0]))
def test_textured_meshes(self): """Draws a solid green box (the texture map is just a green pixel) to test the texture override pathway. You should confirm that you see a green box in the visualizer.""" object_file_path = FindResourceOrThrow( "drake/systems/sensors/test/models/box_with_mesh.sdf") # Find the texture path just to ensure it exists and # we're testing the code path we want to. FindResourceOrThrow("drake/systems/sensors/test/models/meshes/box.png") builder = DiagramBuilder() plant = MultibodyPlant(0.002) _, scene_graph = AddMultibodyPlantSceneGraph(builder, plant) object_model = Parser(plant=plant).AddModelFromFile(object_file_path) plant.Finalize() # Add meshcat visualizer. visualizer = builder.AddSystem( MeshcatVisualizer(zmq_url=ZMQ_URL, open_browser=False)) builder.Connect(scene_graph.get_query_output_port(), visualizer.get_geometry_query_input_port()) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.AdvanceTo(1.0)
def test_meta_arbitrary_multibody_stuff(self): """Ensures that the test model is logical and can be compared / falsified.""" plant_a, scene_graph_a, _ = self.make_arbitrary_multibody_stuff( finalize=False) # Ensure that we span all relevant joint classes. joint_cls_set = {type(x) for x in me.get_joints(plant_a)} self.assertEqual(joint_cls_set, set(util.JOINT_CLS_LIST)) # Ensure that we can copy via a subgraph pre-Finalize. 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)) subgraph_a.add_to(plant_b, scene_graph_b) # Check equivalence. util.assert_plant_equals(plant_a, scene_graph_a, plant_b, scene_graph_b) # Ensure that this is "physically" valid. plant_a.Finalize() # Checking for determinism, making a slight change to trigger an error. for slight_difference in [False, True]: plant_b, scene_graph_b, _ = self.make_arbitrary_multibody_stuff( slight_difference=slight_difference) if not slight_difference: util.assert_plant_equals(plant_a, scene_graph_a, plant_b, scene_graph_b) else: with self.assertRaises(AssertionError): util.assert_plant_equals(plant_a, scene_graph_a, plant_b, scene_graph_b)
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 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 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_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, 0.0) Parser(plant=cart_pole).AddModelFromFile(file_name) cart_pole.Finalize() self.assertTrue(cart_pole.geometry_source_is_registered()) visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() cart_pole_context = diagram.GetMutableSubsystemContext( cart_pole, diagram_context) vis_context = diagram.GetMutableSubsystemContext( visualizer, diagram_context) cart_pole.get_actuation_input_port().FixValue(cart_pole_context, 0) cart_slider = cart_pole.GetJointByName("CartSlider") pole_pin = cart_pole.GetJointByName("PolePin") cart_slider.set_translation(context=cart_pole_context, translation=0.) pole_pin.set_angle(context=cart_pole_context, angle=2.) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.AdvanceTo(.1) visualizer.draw(vis_context) self.assertEqual(visualizer.ax.get_title(), "t = 0.1",)
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 run_pendulum_example(args): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) parser = Parser(plant) parser.AddModelFromFile( FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf")) plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base")) plant.Finalize() pose_bundle_output_port = scene_graph.get_pose_bundle_output_port() Tview = np.array([[1., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]], dtype=np.float64) visualizer = builder.AddSystem( PlanarSceneGraphVisualizer(scene_graph, T_VW=Tview, xlim=[-1.2, 1.2], ylim=[-1.2, 1.2])) builder.Connect(pose_bundle_output_port, visualizer.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.Initialize() simulator.set_target_realtime_rate(1.0) # Fix the input port to zero. plant_context = diagram.GetMutableSubsystemContext( plant, simulator.get_mutable_context()) plant_context.FixInputPort(plant.get_actuation_input_port().get_index(), np.zeros(plant.num_actuators())) plant_context.SetContinuousState([0.5, 0.1]) simulator.AdvanceTo(args.duration)
def setUp(self): builder_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 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