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 add_rope_and_ground(self, include_ground=True): if include_ground: ground_model = add_ground(self.mbp, self.sg) self._model_ids["ground"] = ground_model for rope_name, rope_config in iteritems(self._config['env']['ropes']): X_W = transform_from_dict(rope_config) rope_model = add_rope(self.mbp, self.sg, self._config['rope'], X_W, rope_name) self._model_names_to_mask.append(rope_name) self._model_ids[rope_name] = rope_model link_length = np.linalg.norm(np.subtract(self._config["env"]["ropes"]["rope_2"]["pos"], self._config["env"]["ropes"]["rope_1"]["pos"])) lx, ly, lz = self._config["env"]["ropes"]["rope_1"]["pos"] rx, ry, rz = self._config["env"]["ropes"]["rope_2"]["pos"] # TODO: not completely right roll = math.pi / 2 + math.atan2(rz - lz, ry - ly) pitch = 0 yaw = -math.atan2(rx - lx, ry - ly) link_I = SpatialInertia(mass=1, p_PScm_E=np.array([0, 0, 0]), G_SP_E=UnitInertia(1, 1, 1)) link_name = "middle_link" link_body = self._mbp.AddRigidBody(link_name, rope_model, link_I) cylinder_geom = Cylinder(self._config["rope"]["rope_radius"], link_length) X = RigidTransform() self._mbp.RegisterVisualGeometry(link_body, X, cylinder_geom, "middle_vis1", [0.5, 0.5, 0.5, 0.5]) self._mbp.RegisterCollisionGeometry(link_body, X, cylinder_geom, "middle_collision", CoulombFriction(0.0, 0.0)) self._mbp.WeldFrames(self._mbp.world_frame(), self._mbp.GetFrameByName(f"middle_link", rope_model), RigidTransform(RollPitchYaw(roll, pitch, yaw), [(lx + rx) / 2.0, (ly + ry) / 2.0, (lz + rz) / 2.0]))
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 random_geometry(body): # Creates a random geometry. i = i_next() box = Box( width=random.uniform(0.1, 0.3), depth=random.uniform(0.1, 0.3), height=random.uniform(0.1, 0.3), ) plant.RegisterVisualGeometry( body=body, X_BG=random_X(), shape=box, name=f"visual_{i}", diffuse_color=[random.random(), 0, 0, 0.75], ) static_friction = random.uniform(0.1, 1.) plant.RegisterCollisionGeometry(body=body, X_BG=random_X(), shape=box, name=f"collision_{i}", coulomb_friction=CoulombFriction( static_friction=static_friction, dynamic_friction=static_friction / 2, ))
def test_procedural_geometry(self): """ This test ensures we can draw procedurally added primitive geometry that is added to the world model instance (which has a slightly different naming scheme than geometry with a non-default / non-world model instance). """ builder = DiagramBuilder() mbp, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) world_body = mbp.world_body() box_shape = Box(1., 2., 3.) # This rigid body will be added to the world model instance since # the model instance is not specified. box_body = mbp.AddRigidBody( "box", SpatialInertia(mass=1.0, p_PScm_E=np.array([0., 0., 0.]), G_SP_E=UnitInertia(1.0, 1.0, 1.0))) mbp.WeldFrames(world_body.body_frame(), box_body.body_frame(), RigidTransform()) mbp.RegisterVisualGeometry(box_body, RigidTransform.Identity(), box_shape, "ground_vis", np.array([0.5, 0.5, 0.5, 1.])) mbp.RegisterCollisionGeometry(box_body, RigidTransform.Identity(), box_shape, "ground_col", CoulombFriction(0.9, 0.8)) mbp.Finalize() frames_to_draw = {"world": {"box"}} visualizer = builder.AddSystem(PlanarSceneGraphVisualizer(scene_graph)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() vis_context = diagram.GetMutableSubsystemContext( visualizer, diagram_context) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.AdvanceTo(.1) visualizer.draw(vis_context) self.assertEqual( visualizer.ax.get_title(), "t = 0.1", )
def test_multibody_plant_construction_api(self): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) spatial_inertia = SpatialInertia() body = plant.AddRigidBody(name="new_body", M_BBo_B=spatial_inertia) 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) 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 add_box_at_location(mbp, name, color, pose, mass=0.25, inertia=UnitInertia(3E-3, 3E-3, 3E-3)): ''' Adds a 5cm cube at the specified pose. Uses a planar floating base in the x-z plane. ''' no_mass_no_inertia = SpatialInertia(mass=0., p_PScm_E=np.array([0., 0., 0.]), G_SP_E=UnitInertia(0., 0., 0.)) body_mass_and_inertia = SpatialInertia(mass=mass, p_PScm_E=np.array([0., 0., 0.]), G_SP_E=inertia) shape = Box(0.05, 0.05, 0.05) model_instance = mbp.AddModelInstance(name) body = mbp.AddRigidBody(name, model_instance, body_mass_and_inertia) RegisterVisualAndCollisionGeometry(mbp, body, RigidTransform(), shape, name, color, CoulombFriction(0.9, 0.8)) body_pre_z = mbp.AddRigidBody("{}_pre_z".format(name), model_instance, no_mass_no_inertia) body_pre_theta = mbp.AddRigidBody("{}_pre_theta".format(name), model_instance, no_mass_no_inertia) world_carrot_origin = mbp.AddFrame(frame=FixedOffsetFrame( name="world_{}_origin".format(name), P=mbp.world_frame(), X_PF=pose)) body_joint_x = PrismaticJoint(name="{}_x".format(name), frame_on_parent=world_carrot_origin, frame_on_child=body_pre_z.body_frame(), axis=[1, 0, 0], damping=0.) mbp.AddJoint(body_joint_x) body_joint_z = PrismaticJoint(name="{}_z".format(name), frame_on_parent=body_pre_z.body_frame(), frame_on_child=body_pre_theta.body_frame(), axis=[0, 0, 1], damping=0.) mbp.AddJoint(body_joint_z) body_joint_theta = RevoluteJoint( name="{}_theta".format(name), frame_on_parent=body_pre_theta.body_frame(), frame_on_child=body.body_frame(), axis=[0, 1, 0], damping=0.) mbp.AddJoint(body_joint_theta)
def make_box(mbp, name): inertia = SpatialInertia.MakeFromCentralInertia( 1, [0, 0, 0], RotationalInertia(1 / 600, 1 / 120, 1 / 120)) body = mbp.AddRigidBody(name, inertia) shape = Box(1, 0.1, 0.1) mbp.RegisterVisualGeometry(body=body, X_BG=RigidTransform(), shape=shape, name=f"{name}_visual", diffuse_color=[1., 0.64, 0.0, 0.5]) body_friction = CoulombFriction(static_friction=0.6, dynamic_friction=0.5) mbp.RegisterCollisionGeometry(body=body, X_BG=RigidTransform(), shape=shape, name="{name}_collision", coulomb_friction=body_friction) return body
def load_atlas(plant, add_ground=False): atlas_file = FindResourceOrThrow("drake/examples/atlas/urdf/atlas_minimal_contact.urdf") # atlas_file = FindResourceOrThrow("drake/examples/atlas/urdf/atlas_convex_hull.urdf") atlas = Parser(plant).AddModelFromFile(atlas_file) if add_ground: static_friction = 1.0 green = np.array([0.5, 1.0, 0.5, 1.0]) # plant.RegisterVisualGeometry(plant.world_body(), RigidTransform(), HalfSpace(), # "GroundVisuaGeometry", green) ground_friction = CoulombFriction(1.0, 1.0) plant.RegisterCollisionGeometry(plant.world_body(), RigidTransform(), HalfSpace(), "GroundCollisionGeometry", ground_friction) plant.Finalize() plant.set_penetration_allowance(1.0e-3) plant.set_stiction_tolerance(1.0e-3)
def test_procedural_geometry_deprecated_api(self): """ This test ensures we can draw procedurally added primitive geometry that is added to the world model instance (which has a slightly different naming scheme than geometry with a non-default / non-world model instance). """ builder = DiagramBuilder() mbp, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) world_body = mbp.world_body() box_shape = Box(1., 2., 3.) # This rigid body will be added to the world model instance since # the model instance is not specified. box_body = mbp.AddRigidBody( "box", SpatialInertia(mass=1.0, p_PScm_E=np.array([0., 0., 0.]), G_SP_E=UnitInertia(1.0, 1.0, 1.0))) mbp.WeldFrames(world_body.body_frame(), box_body.body_frame(), RigidTransform()) mbp.RegisterVisualGeometry(box_body, RigidTransform.Identity(), box_shape, "ground_vis", np.array([0.5, 0.5, 0.5, 1.])) mbp.RegisterCollisionGeometry(box_body, RigidTransform.Identity(), box_shape, "ground_col", CoulombFriction(0.9, 0.8)) mbp.Finalize() frames_to_draw = {"world": {"box"}} visualizer = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=ZMQ_URL, open_browser=False, frames_to_draw=frames_to_draw)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() simulator = Simulator(diagram) simulator.set_publish_every_time_step(False) with catch_drake_warnings(expected_count=1): simulator.AdvanceTo(.1)
def add_procedurally_generated_table(self): mbp = self._mbp dims = self._config['env']['table']['size'] box_shape = Box(*dims) T_W_B = RigidTransform(p=np.array([0., 0., -dims[2]/2.])) # This rigid body will be added to the world model instance since # the model instance is not specified. box_body = mbp.AddRigidBody("table", SpatialInertia( mass=1.0, p_PScm_E=np.array([0., 0., 0.]), G_SP_E=UnitInertia(1.0, 1.0, 1.0))) mbp.WeldFrames(mbp.world_frame(), box_body.body_frame(), T_W_B) color = np.array(self._config['env']['table']['color']) mbp.RegisterVisualGeometry( box_body, RigidTransform(), box_shape, "table_vis", color) friction_params = self._config['env']['table']['coulomb_friction'] mbp.RegisterCollisionGeometry( box_body, RigidTransform(), box_shape, "table_collision", CoulombFriction(*friction_params))
def add_procedurally_generated_table( mbp, # multi-body plant table_config, # dict ): """ Adds a procedurally generated table to the scene param table_config: dict with keys ['size', 'color'] """ world_body = mbp.world_body() dims = table_config['size'] # box_shape = Box(1., 2., 3.) box_shape = Box(*dims) translation = np.zeros(3) translation[2] = -dims[2] / 2.0 T_W_B = RigidTransform(p=translation) # This rigid body will be added to the world model instance since # the model instance is not specified. box_body = mbp.AddRigidBody( "table", SpatialInertia(mass=1.0, p_PScm_E=np.array([0., 0., 0.]), G_SP_E=UnitInertia(1.0, 1.0, 1.0))) mbp.WeldFrames(world_body.body_frame(), box_body.body_frame(), T_W_B) # this is a grey color color = np.array(table_config['color']) mbp.RegisterVisualGeometry(box_body, RigidTransform.Identity(), box_shape, "table_vis", color) # friction friction_params = table_config['coulomb_friction'] mbp.RegisterCollisionGeometry(box_body, RigidTransform.Identity(), box_shape, "table_collision", CoulombFriction(*friction_params))
def test_exploding_iiwa_sim(self): """ Shows a simulation of a falling + exploding IIWA which "changes topology" by being rebuilt without joints. """ builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-3) iiwa_file = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/urdf/" "iiwa14_spheres_dense_elbow_collision.urdf") Parser(plant).AddModelFromFile(iiwa_file, "iiwa") # Add ground plane. X_FH = HalfSpace.MakePose([0, 0, 1], [0, 0, 0]) plant.RegisterCollisionGeometry(plant.world_body(), X_FH, HalfSpace(), "ground_plane_collision", CoulombFriction(0.8, 0.3)) plant.Finalize() # Loosey-goosey - no control. for model in me.get_model_instances(plant): util.build_with_no_control(builder, plant, model) if VISUALIZE: print("test_exploding_iiwa_sim") role = Role.kIllustration DrakeVisualizer.AddToBuilder( builder, scene_graph, params=DrakeVisualizerParams(role=role)) ConnectContactResultsToDrakeVisualizer(builder, plant, scene_graph) diagram = builder.Build() # Set up context. d_context = diagram.CreateDefaultContext() context = plant.GetMyContextFromRoot(d_context) # - Hoist IIWA up in the air. plant.SetFreeBodyPose(context, plant.GetBodyByName("base"), RigidTransform([0, 0, 1.])) # - Set joint velocities to "spin" it in the air. for joint in me.get_joints(plant): if isinstance(joint, RevoluteJoint): me.set_joint_positions(plant, context, joint, 0.7) me.set_joint_velocities(plant, context, joint, -5.) def monitor(d_context): # Stop the simulation once there's any contact. context = plant.GetMyContextFromRoot(d_context) query_object = plant.get_geometry_query_input_port().Eval(context) if query_object.HasCollisions(): return EventStatus.ReachedTermination(plant, "Contact") else: return EventStatus.DidNothing() # Forward simulate. simulator = Simulator(diagram, d_context) simulator.Initialize() simulator.set_monitor(monitor) if VISUALIZE: simulator.set_target_realtime_rate(1.) simulator.AdvanceTo(2.) # Try to push a bit further. simulator.clear_monitor() simulator.AdvanceTo(d_context.get_time() + 0.05) diagram.Publish(d_context) # Recreate simulator. builder_new = DiagramBuilder() plant_new, scene_graph_new = AddMultibodyPlantSceneGraph( builder_new, time_step=plant.time_step()) subgraph = mut.MultibodyPlantSubgraph( mut.get_elements_from_plant(plant, scene_graph)) # Remove all joints; make them floating bodies. for joint in me.get_joints(plant): subgraph.remove_joint(joint) # Remove massless bodies. # For more info, see: https://stackoverflow.com/a/62035705/7829525 for body in me.get_bodies(plant): if body is plant.world_body(): continue if body.default_mass() == 0.: subgraph.remove_body(body) # Finalize. to_new = subgraph.add_to(plant_new, scene_graph_new) plant_new.Finalize() if VISUALIZE: DrakeVisualizer.AddToBuilder( builder_new, scene_graph_new, params=DrakeVisualizerParams(role=role)) ConnectContactResultsToDrakeVisualizer(builder_new, plant_new, scene_graph_new) diagram_new = builder_new.Build() # Remap state. d_context_new = diagram_new.CreateDefaultContext() d_context_new.SetTime(d_context.get_time()) context_new = plant_new.GetMyContextFromRoot(d_context_new) to_new.copy_state(context, context_new) # Simulate. simulator_new = Simulator(diagram_new, d_context_new) simulator_new.Initialize() diagram_new.Publish(d_context_new) if VISUALIZE: simulator_new.set_target_realtime_rate(1.) simulator_new.AdvanceTo(context_new.get_time() + 2) if VISUALIZE: input(" Press enter...")
def test_friction_api(self): CoulombFriction(static_friction=0.7, dynamic_friction=0.6)
def add_pusher(self): """ Adds a cylindrical pusher object :return: :rtype: """ mbp = self.mbp parser = self.parser pusher_model_idx = parser.AddModelFromFile(paths.xy_slide, "pusher") base_link = mbp.GetBodyByName("base", pusher_model_idx) # weld it to the world mbp.WeldFrames(mbp.world_frame(), base_link.body_frame()) self.models['pusher'] = pusher_model_idx radius = 0.01 length = 0.1 pusher_shape = Cylinder(radius, length) # This rigid body will be added to the pusher model instance world_body = mbp.world_body() pusher_body = mbp.AddRigidBody( "pusher_body", pusher_model_idx, SpatialInertia(mass=10.0, p_PScm_E=np.array([0., 0., 0.]), G_SP_E=UnitInertia(1.0, 1.0, 1.0))) self._rigid_bodies['pusher'] = pusher_body # weld it to EE frame at particular height translation = np.zeros(3) translation[ 2] = length / 2.0 + 0.001 # give it a little room for collision stuff T_EE_P = RigidTransform(p=translation) ee_link = mbp.GetBodyByName("end_effector", pusher_model_idx) mbp.WeldFrames(ee_link.body_frame(), pusher_body.body_frame(), T_EE_P) # color it green color = np.array([0., 1., 0., 1.]) mbp.RegisterVisualGeometry(pusher_body, RigidTransform.Identity(), pusher_shape, "pusher_vis", color) mbp.RegisterCollisionGeometry(pusher_body, RigidTransform.Identity(), pusher_shape, "pusher_collision", CoulombFriction(0.9, 0.8)) def export_port_func(): # export relevant ports actuation_input_port = mbp.get_actuation_input_port( pusher_model_idx) state_output_port = mbp.get_state_output_port(pusher_model_idx) self._port_names["pusher_state_output"] = "pusher_state_output" self._port_names[ "pusher_actuation_input"] = "pusher_actuation_input" self.builder.ExportOutput(state_output_port, self._port_names["pusher_state_output"]) self.builder.ExportInput( actuation_input_port, self._port_names["pusher_actuation_input"]) self._export_port_functions.append(export_port_func)
# Add "tabletop" (ground) world_body = mbp.world_body() ground_shape = Box(1., 1., 1.) ground_body = mbp.AddRigidBody( "ground", SpatialInertia(mass=10.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(), ground_body.body_frame(), RigidTransform()) RegisterVisualAndCollisionGeometry(mbp, ground_body, RigidTransform(p=[0, 0, -0.5]), ground_shape, "ground", np.array([0.5, 0.5, 0.5, 1.]), CoulombFriction(0.9, 0.8)) ycb_object_dir = os.path.join(getDrakePath(), "manipulation/models/ycb/sdf/") ycb_object_sdfs = os.listdir(ycb_object_dir) ycb_object_sdfs = [ os.path.join(ycb_object_dir, path) for path in ycb_object_sdfs ] # Add random objects parser = Parser(mbp, scene_graph) n_objects = np.random.randint(1, 12) obj_ids = [] for k in range(n_objects): obj_i = np.random.randint(len(ycb_object_sdfs)) parser.AddModelFromFile(file_name=ycb_object_sdfs[obj_i],
def visualize(theta1, theta2, theta3, theta4=0.0, phi=0.0): file_name = "res/stair_climb.sdf" builder = DiagramBuilder() stair_climb, scene_graph = AddMultibodyPlantSceneGraph(builder) # stair_climb.RegisterAsSourceForSceneGraph(scene_graph) Parser(plant=stair_climb).AddModelFromFile(file_name) # stair_climb.AddForceElement(UniformGravityFieldElement()) front_wheel_x, front_wheel_y = eom.findFrontWheelPosition( theta1, theta2, theta3) front_wheel_x = front_wheel_x.Evaluate() front_wheel_y = front_wheel_y.Evaluate() step = Box(STEP_DEPTH, STEP_WIDTH, STEP_HEIGHT) step_pos = RigidTransform( [STEP_POSITION + STEP_DEPTH / 2.0, 0.0, STEP_HEIGHT / 2.0]) stair_climb.RegisterCollisionGeometry( stair_climb.world_body(), RigidTransform([0.0, 0.0, 0.0]), HalfSpace(), "GroundCollision", CoulombFriction(COEFF_FRICTION, COEFF_FRICTION)) stair_climb.RegisterVisualGeometry(stair_climb.world_body(), RigidTransform([0.0, 0.0, 0.0]), HalfSpace(), "GroundVisual", np.array([0.5, 0.5, 0.5, 0.5])) # Color stair_climb.RegisterCollisionGeometry( stair_climb.world_body(), step_pos, step, "StepCollision", CoulombFriction(COEFF_FRICTION, COEFF_FRICTION)) stair_climb.RegisterVisualGeometry(stair_climb.world_body(), step_pos, step, "StepVisual", np.array([1.0, 1.0, 0.0, 1.0])) # Color stair_climb.Finalize() ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() stair_climb_context = diagram.GetMutableSubsystemContext( stair_climb, diagram_context) stair_climb_context.FixInputPort( stair_climb.get_actuation_input_port().get_index(), [0, 0, 0, 0, 0]) theta1_joint = stair_climb.GetJointByName("theta1") theta2_joint = stair_climb.GetJointByName("theta2") theta3_joint = stair_climb.GetJointByName("theta3") theta4_joint = stair_climb.GetJointByName("theta4") phi_joint = stair_climb.GetJointByName("phi") theta1_joint.set_angle(context=stair_climb_context, angle=theta1) theta2_joint.set_angle(context=stair_climb_context, angle=theta2) theta3_joint.set_angle(context=stair_climb_context, angle=theta3) theta4_joint.set_angle(context=stair_climb_context, angle=theta4) phi_joint.set_angle(context=stair_climb_context, angle=phi) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(0.001) simulator.Initialize() simulator.AdvanceTo(0.0)
def test_proximity_properties(self): """ Tests the utility functions (not related to hydroelastic contact) for setting values in ProximityProperties (as defined in proximity_properties.h). """ props = mut.ProximityProperties() reference_friction = CoulombFriction(0.25, 0.125) mut.AddContactMaterial(dissipation=2.7, point_stiffness=3.9, friction=reference_friction, properties=props) self.assertTrue( props.HasProperty("material", "hunt_crossley_dissipation")) self.assertEqual( props.GetProperty("material", "hunt_crossley_dissipation"), 2.7) self.assertTrue( props.HasProperty("material", "point_contact_stiffness")) self.assertEqual( props.GetProperty("material", "point_contact_stiffness"), 3.9) self.assertTrue(props.HasProperty("material", "coulomb_friction")) stored_friction = props.GetProperty("material", "coulomb_friction") self.assertEqual(stored_friction.static_friction(), reference_friction.static_friction()) self.assertEqual(stored_friction.dynamic_friction(), reference_friction.dynamic_friction()) props = mut.ProximityProperties() res_hint = 0.175 E = 1e8 mut.AddRigidHydroelasticProperties(resolution_hint=res_hint, properties=props) self.assertTrue(props.HasProperty("hydroelastic", "compliance_type")) self.assertFalse(mut_testing.PropertiesIndicateCompliantHydro(props)) self.assertTrue(props.HasProperty("hydroelastic", "resolution_hint")) self.assertEqual(props.GetProperty("hydroelastic", "resolution_hint"), res_hint) props = mut.ProximityProperties() mut.AddRigidHydroelasticProperties(properties=props) self.assertTrue(props.HasProperty("hydroelastic", "compliance_type")) self.assertFalse(mut_testing.PropertiesIndicateCompliantHydro(props)) self.assertFalse(props.HasProperty("hydroelastic", "resolution_hint")) props = mut.ProximityProperties() res_hint = 0.275 mut.AddCompliantHydroelasticProperties(resolution_hint=res_hint, hydroelastic_modulus=E, properties=props) self.assertTrue(props.HasProperty("hydroelastic", "compliance_type")) self.assertTrue(mut_testing.PropertiesIndicateCompliantHydro(props)) self.assertTrue(props.HasProperty("hydroelastic", "resolution_hint")) self.assertEqual(props.GetProperty("hydroelastic", "resolution_hint"), res_hint) self.assertTrue( props.HasProperty("hydroelastic", "hydroelastic_modulus")) self.assertEqual( props.GetProperty("hydroelastic", "hydroelastic_modulus"), E) props = mut.ProximityProperties() slab_thickness = 0.275 mut.AddCompliantHydroelasticPropertiesForHalfSpace( slab_thickness=slab_thickness, hydroelastic_modulus=E, properties=props) self.assertTrue(props.HasProperty("hydroelastic", "compliance_type")) self.assertTrue(mut_testing.PropertiesIndicateCompliantHydro(props)) self.assertTrue(props.HasProperty("hydroelastic", "slab_thickness")) self.assertEqual(props.GetProperty("hydroelastic", "slab_thickness"), slab_thickness) self.assertTrue( props.HasProperty("hydroelastic", "hydroelastic_modulus")) self.assertEqual( props.GetProperty("hydroelastic", "hydroelastic_modulus"), E)
def add_arbitrary_multibody_stuff(plant, num_bodies=30, slight_difference=False): """ Deterministic, physically valid, jumble of arbitrary stuff. The goal of this factory is to: - Produce a set of elements and cases that exercise each code path in `MultibodyPlantSubgraph.add_to` and `MultibodyPlantElementsMap`. - Ensure each element has somewhat "random" but unique properties for each element produced. - Allow for a slight difference to show that strict plant comparison can be falsified. """ count = defaultdict(lambda: 0) def i_next(key=None): # Increments for a given key. count[key] += 1 return count[key] def maybe(): # Returns True or False randomly. return random.choice([True, False]) def random_model_instance(): # Returns a "random" model instance (by incrementing). i = i_next() return plant.AddModelInstance(f"model_{i}") def random_position(): # Returns a random position. return [0.2 * random.random(), 0, 0] def random_X(): # Returns a random pose. return RigidTransform(random_position()) def random_body(): # Returns a random body, with an incrementing name. inertia = SpatialInertia( mass=random.uniform(0.2, 1.), p_PScm_E=random_position(), G_SP_E=UnitInertia( Ixx=random.uniform(0.2, 0.3), Iyy=random.uniform(0.2, 0.3), Izz=random.uniform(0.2, 0.3), ), ) return plant.AddRigidBody( name=f"body_{i_next()}", M_BBo_B=inertia, model_instance=random_model_instance(), ) def random_frame(parent_frame): # Returns a random frame, with an incrementing name. i = i_next() return plant.AddFrame( FixedOffsetFrame( name=f"frame_{i}", P=parent_frame, X_PF=random_X(), model_instance=parent_frame.model_instance(), )) def random_joint(parent, child): # Returns a random joint, but with an incrementing name. Note that we # use a separate index so that we ensure we can loop through all # joints. i = i_next("joint") name = f"joint_{i}" joint_cls = JOINT_CLS_LIST[i % len(JOINT_CLS_LIST)] frame_on_parent = random_frame(parent.body_frame()) frame_on_child = random_frame(child.body_frame()) axis = np.zeros(3) axis[i_next() % 3] = 1 damping = random.random() if joint_cls == BallRpyJoint: joint = BallRpyJoint( name, frame_on_parent=frame_on_parent, frame_on_child=frame_on_child, damping=damping, ) elif joint_cls == PrismaticJoint: joint = PrismaticJoint( name, frame_on_parent=frame_on_parent, frame_on_child=frame_on_child, axis=axis, damping=damping, ) elif joint_cls == RevoluteJoint: joint = RevoluteJoint( name, frame_on_parent=frame_on_parent, frame_on_child=frame_on_child, axis=axis, damping=damping, ) elif joint_cls == UniversalJoint: joint = UniversalJoint( name, frame_on_parent=frame_on_parent, frame_on_child=frame_on_child, damping=damping, ) elif joint_cls == WeldJoint: joint = WeldJoint( name, frame_on_parent_P=frame_on_parent, frame_on_child_C=frame_on_child, X_PC=random_X(), ) else: assert False return plant.AddJoint(joint) def random_joint_actuator(joint): # Creates a random joint actuator. assert joint is not None i = i_next() return plant.AddJointActuator(f"actuator_{i}", joint, effort_limit=random.uniform(1, 2)) def random_geometry(body): # Creates a random geometry. i = i_next() box = Box( width=random.uniform(0.1, 0.3), depth=random.uniform(0.1, 0.3), height=random.uniform(0.1, 0.3), ) plant.RegisterVisualGeometry( body=body, X_BG=random_X(), shape=box, name=f"visual_{i}", diffuse_color=[random.random(), 0, 0, 0.75], ) static_friction = random.uniform(0.1, 1.) plant.RegisterCollisionGeometry(body=body, X_BG=random_X(), shape=box, name=f"collision_{i}", coulomb_friction=CoulombFriction( static_friction=static_friction, dynamic_friction=static_friction / 2, )) # Add ground plane. X_FH = HalfSpace.MakePose([0, 0, 1], [0, 0, 0]) plant.RegisterCollisionGeometry(plant.world_body(), X_FH, HalfSpace(), "ground_plane_collision", CoulombFriction(0.8, 0.3)) grid_rows = 5 prev_body = None for i in range(num_bodies): random.seed(i) body = random_body() grid_col = i % grid_rows grid_row = int(i / grid_rows) if slight_difference: grid_row += 1 plant.SetDefaultFreeBodyPose(body, RigidTransform([grid_col, grid_row, 2])) random_frame(body.body_frame()) # Consider attaching a joint and/or frame to the world. if maybe() or num_bodies < 3: prev_body = plant.world_body() random_frame(plant.world_frame()) if prev_body is not None and (maybe() or num_bodies < 3): joint = random_joint(prev_body, body) if joint.num_velocities() == 1 and (maybe() or num_bodies < 3): random_joint_actuator(joint) if plant.geometry_source_is_registered(): random_geometry(body) prev_body = body
def do_main(): rospy.init_node('run_dishrack_interaction', anonymous=False) #np.random.seed(42) for outer_iter in range(200): try: builder = DiagramBuilder() mbp, scene_graph = AddMultibodyPlantSceneGraph( builder, MultibodyPlant(time_step=0.0025)) # Add ground world_body = mbp.world_body() ground_shape = Box(2., 2., 2.) ground_body = mbp.AddRigidBody( "ground", SpatialInertia(mass=10.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(), ground_body.body_frame(), RigidTransform(p=[0, 0, -1])) mbp.RegisterVisualGeometry(ground_body, RigidTransform.Identity(), ground_shape, "ground_vis", np.array([0.5, 0.5, 0.5, 1.])) mbp.RegisterCollisionGeometry(ground_body, RigidTransform.Identity(), ground_shape, "ground_col", CoulombFriction(0.9, 0.8)) parser = Parser(mbp, scene_graph) dish_bin_model = "/home/gizatt/projects/scene_generation/models/dish_models/bus_tub_01_decomp/bus_tub_01_decomp.urdf" cupboard_model = "/home/gizatt/projects/scene_generation/models/dish_models/shelf_two_levels.sdf" candidate_model_files = { #"mug": "/home/gizatt/drake/manipulation/models/mug/mug.urdf", "mug_1": "/home/gizatt/projects/scene_generation/models/dish_models/mug_1_decomp/mug_1_decomp.urdf", #"plate_11in": "/home/gizatt/drake/manipulation/models/dish_models/plate_11in_decomp/plate_11in_decomp.urdf", #"/home/gizatt/drake/manipulation/models/mug_big/mug_big.urdf", #"/home/gizatt/drake/manipulation/models/dish_models/bowl_6p25in_decomp/bowl_6p25in_decomp.urdf", #"/home/gizatt/drake/manipulation/models/dish_models/plate_8p5in_decomp/plate_8p5in_decomp.urdf", } # Decide how many of each object to add max_num_objs = 6 num_objs = [ np.random.randint(0, max_num_objs) for k in range(len(candidate_model_files.keys())) ] # Actually produce their initial poses + add them to the sim poses = [] # [quat, pos] all_object_instances = [] all_manipulable_body_ids = [] total_num_objs = sum(num_objs) object_ordering = list(range(total_num_objs)) k = 0 random.shuffle(object_ordering) print("ordering: ", object_ordering) for class_k, class_entry in enumerate( candidate_model_files.items()): for model_instance_k in range(num_objs[class_k]): class_name, class_path = class_entry model_name = "%s_%d" % (class_name, model_instance_k) all_object_instances.append([class_name, model_name]) model_id = parser.AddModelFromFile(class_path, model_name=model_name) all_manipulable_body_ids += mbp.GetBodyIndices(model_id) # Put them in a randomly ordered line, for placing y_offset = (object_ordering[k] / float(total_num_objs) - 0.5) # RAnge -0.5 to 0.5 poses.append([ RollPitchYaw(np.random.uniform( 0., 2. * np.pi, size=3)).ToQuaternion().wxyz(), [-0.25, y_offset, 0.1] ]) k += 1 #$poses.append([ # RollPitchYaw(np.random.uniform(0., 2.*np.pi, size=3)).ToQuaternion().wxyz(), # [np.random.uniform(-0.2, 0.2), np.random.uniform(-0.1, 0.1), np.random.uniform(0.1, 0.3)]]) # Build a desk parser.AddModelFromFile(cupboard_model) mbp.WeldFrames(world_body.body_frame(), mbp.GetBodyByName("shelf_origin_body").body_frame(), RigidTransform(p=[0.0, 0, 0.0])) #parser.AddModelFromFile(dish_bin_model) #mbp.WeldFrames(world_body.body_frame(), mbp.GetBodyByName("bus_tub_01_decomp_body_link").body_frame(), # RigidTransform(p=[0.0, 0., 0.], rpy=RollPitchYaw(np.pi/2., 0., 0.))) mbp.AddForceElement(UniformGravityFieldElement()) mbp.Finalize() hydra_sg_spy = builder.AddSystem( HydraInteractionLeafSystem( mbp, scene_graph, all_manipulable_body_ids=all_manipulable_body_ids)) #builder.Connect(scene_graph.get_query_output_port(), # hydra_sg_spy.get_input_port(0)) builder.Connect(scene_graph.get_pose_bundle_output_port(), hydra_sg_spy.get_input_port(0)) builder.Connect(mbp.get_state_output_port(), hydra_sg_spy.get_input_port(1)) builder.Connect(hydra_sg_spy.get_output_port(0), mbp.get_applied_spatial_force_input_port()) visualizer = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url="tcp://127.0.0.1:6000", draw_period=0.01)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() mbp_context = diagram.GetMutableSubsystemContext( mbp, diagram_context) sg_context = diagram.GetMutableSubsystemContext( scene_graph, diagram_context) q0 = mbp.GetPositions(mbp_context).copy() for k in range(len(poses)): offset = k * 7 q0[(offset):(offset + 4)] = poses[k][0] q0[(offset + 4):(offset + 7)] = poses[k][1] mbp.SetPositions(mbp_context, q0) simulator = Simulator(diagram, diagram_context) simulator.set_target_realtime_rate(1.0) simulator.set_publish_every_time_step(False) simulator.Initialize() ik = InverseKinematics(mbp, mbp_context) q_dec = ik.q() prog = ik.prog() def squaredNorm(x): return np.array([x[0]**2 + x[1]**2 + x[2]**2 + x[3]**2]) for k in range(len(poses)): # Quaternion norm prog.AddConstraint(squaredNorm, [1], [1], q_dec[(k * 7):(k * 7 + 4)]) # Trivial quaternion bounds prog.AddBoundingBoxConstraint(-np.ones(4), np.ones(4), q_dec[(k * 7):(k * 7 + 4)]) # Conservative bounds on on XYZ prog.AddBoundingBoxConstraint(np.array([-2., -2., -2.]), np.array([2., 2., 2.]), q_dec[(k * 7 + 4):(k * 7 + 7)]) def vis_callback(x): vis_diagram_context = diagram.CreateDefaultContext() mbp.SetPositions( diagram.GetMutableSubsystemContext(mbp, vis_diagram_context), x) pose_bundle = scene_graph.get_pose_bundle_output_port().Eval( diagram.GetMutableSubsystemContext(scene_graph, vis_diagram_context)) context = visualizer.CreateDefaultContext() context.FixInputPort(0, AbstractValue.Make(pose_bundle)) visualizer.Publish(context) prog.AddVisualizationCallback(vis_callback, q_dec) prog.AddQuadraticErrorCost(np.eye(q0.shape[0]) * 1.0, q0, q_dec) ik.AddMinimumDistanceConstraint(0.001, threshold_distance=1.0) prog.SetInitialGuess(q_dec, q0) print("Solving") # print "Initial guess: ", q0 start_time = time.time() solver = SnoptSolver() #solver = NloptSolver() sid = solver.solver_type() # SNOPT prog.SetSolverOption(sid, "Print file", "test.snopt") prog.SetSolverOption(sid, "Major feasibility tolerance", 1e-3) prog.SetSolverOption(sid, "Major optimality tolerance", 1e-2) prog.SetSolverOption(sid, "Minor feasibility tolerance", 1e-3) prog.SetSolverOption(sid, "Scale option", 0) #prog.SetSolverOption(sid, "Elastic weight", 1e1) #prog.SetSolverOption(sid, "Elastic mode", "Yes") # NLOPT #prog.SetSolverOption(sid, "initial_step", 0.1) #prog.SetSolverOption(sid, "xtol_rel", 1E-2) #prog.SetSolverOption(sid, "xtol_abs", 1E-2) #prog.SetSolverOption(sid, "Major step limit", 2) print("Solver opts: ", prog.GetSolverOptions(solver.solver_type())) result = mp.Solve(prog) print("Solve info: ", result) print("Solved in %f seconds" % (time.time() - start_time)) #print(IpoptSolver().Solve(prog)) print(result.get_solver_id().name()) q0_proj = result.GetSolution(q_dec) # print "Final: ", q0_proj mbp.SetPositions(mbp_context, q0_proj) simulator.StepTo(1000) raise StopIteration() except StopIteration: print(colored("Stopped, saving and restarting", 'yellow')) qf = mbp.GetPositions(mbp_context) # Decide whether to accept: all objs within bounds save = True for k in range(len(all_object_instances)): offset = k * 7 q = qf[offset:(offset + 7)] if q[4] <= -0.25 or q[4] >= 0.25 or q[5] <= -0.2 or q[ 5] >= 0.2 or q[6] <= -0.1: save = False break if save: print(colored("Saving", "green")) save_config(all_object_instances, qf, "mug_rack_environments_human.yaml") else: print( colored("Not saving due to bounds violation: " + str(q), "yellow")) except Exception as e: print(colored("Suffered other exception " + str(e), "red")) sys.exit(-1) except: print( colored("Suffered totally unknown exception! Probably sim.", "red"))
def __init__(self, config=None): if config is None: config_path = os.path.join(os.path.dirname(__file__), "config.yaml") config = yaml.safe_load(open(config_path, 'r')) self._config = config self._step_dt = config["step_dt"] self._model_name = config["model_name"] self._sim_diagram = DrakeSimDiagram(config) mbp = self._sim_diagram.mbp builder = self._sim_diagram.builder # === Add table ===== dims = config["table"]["size"] color = np.array(config["table"]["color"]) friction_params = config["table"]["coulomb_friction"] box_shape = Box(*dims) X_W_T = RigidTransform(p=np.array([0., 0., -dims[2] / 2.])) # This rigid body will be added to the world model instance since # the model instance is not specified. box_body = mbp.AddRigidBody( "table", SpatialInertia(mass=1.0, p_PScm_E=np.array([0., 0., 0.]), G_SP_E=UnitInertia(1.0, 1.0, 1.0))) mbp.WeldFrames(mbp.world_frame(), box_body.body_frame(), X_W_T) mbp.RegisterVisualGeometry(box_body, RigidTransform(), box_shape, "table_vis", color) mbp.RegisterCollisionGeometry(box_body, RigidTransform(), box_shape, "table_collision", CoulombFriction(*friction_params)) # === Add pusher ==== parser = Parser(mbp, self._sim_diagram.sg) self._pusher_model_id = parser.AddModelFromFile( path_util.pusher_peg, "pusher") base_link = mbp.GetBodyByName("base", self._pusher_model_id) mbp.WeldFrames(mbp.world_frame(), base_link.body_frame()) def pusher_port_func(): actuation_input_port = mbp.get_actuation_input_port( self._pusher_model_id) state_output_port = mbp.get_state_output_port( self._pusher_model_id) builder.ExportInput(actuation_input_port, "pusher_actuation_input") builder.ExportOutput(state_output_port, "pusher_state_output") self._sim_diagram.finalize_functions.append(pusher_port_func) # === Add slider ==== parser = Parser(mbp, self._sim_diagram.sg) self._slider_model_id = parser.AddModelFromFile( path_util.model_paths[self._model_name], self._model_name) def slider_port_func(): state_output_port = mbp.get_state_output_port( self._slider_model_id) builder.ExportOutput(state_output_port, "slider_state_output") self._sim_diagram.finalize_functions.append(slider_port_func) if "rgbd_sensors" in config and config["rgbd_sensors"]["enabled"]: self._sim_diagram.add_rgbd_sensors_from_config(config) if "visualization" in config: if not config["visualization"]: pass elif config["visualization"] == "meshcat": self._sim_diagram.connect_to_meshcat() elif config["visualization"] == "drake_viz": self._sim_diagram.connect_to_drake_visualizer() else: raise ValueError("Unknown visualization:", config["visualization"]) self._sim_diagram.finalize() builder = DiagramBuilder() builder.AddSystem(self._sim_diagram) pid = builder.AddSystem( PidController(kp=[0, 0], kd=[1000, 1000], ki=[0, 0])) builder.Connect(self._sim_diagram.GetOutputPort("pusher_state_output"), pid.get_input_port_estimated_state()) builder.Connect( pid.get_output_port_control(), self._sim_diagram.GetInputPort("pusher_actuation_input")) builder.ExportInput(pid.get_input_port_desired_state(), "pid_input_port_desired_state") self._diagram = builder.Build() self._pid_desired_state_port = self._diagram.get_input_port(0) self._simulator = Simulator(self._diagram) self.reset() self.action_space = spaces.Box(low=-1., high=1., shape=(2, ), dtype=np.float32) # TODO: Observation space for images is currently too loose self.observation_space = convert_observation_to_space( self.get_observation())
def do_exploding_iiwa_sim(self, plant_src, scene_graph_src, context_src): # Show a simulation which "changes state" by being rebuilt. builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-3) subgraph_src = mut.MultibodyPlantSubgraph( mut.get_elements_from_plant(plant_src, scene_graph_src)) to_plant = subgraph_src.add_to(plant, scene_graph) # Add ground plane. X_FH = HalfSpace.MakePose([0, 0, 1], [0, 0, 0]) plant.RegisterCollisionGeometry(plant.world_body(), X_FH, HalfSpace(), "ground_plane_collision", CoulombFriction(0.8, 0.3)) plant.Finalize() # Loosey-goosey - no control. for model in me.get_model_instances(plant): util.no_control(builder, plant, model) if VISUALIZE: print("do_exploding_iiwa_sim") role = Role.kIllustration ConnectDrakeVisualizer(builder, scene_graph, role=role) ConnectContactResultsToDrakeVisualizer(builder, plant) diagram = builder.Build() # Set up context. d_context = diagram.CreateDefaultContext() context = plant.GetMyContextFromRoot(d_context) to_plant.copy_state(context_src, context) # - Hoist IIWA up in the air. plant.SetFreeBodyPose(context, plant.GetBodyByName("base"), RigidTransform([0, 0, 1.])) # - Set joint velocities to "spin" it in the air. for joint in me.get_joints(plant): if isinstance(joint, RevoluteJoint): me.set_joint_positions(plant, context, joint, 0.7) me.set_joint_velocities(plant, context, joint, -5.) def monitor(d_context): # Stop the simulation once there's any contact. context = plant.GetMyContextFromRoot(d_context) query_object = plant.get_geometry_query_input_port().Eval(context) if query_object.HasCollisions(): return EventStatus.ReachedTermination(plant, "Contact") else: return EventStatus.DidNothing() # Forward simulate. simulator = Simulator(diagram, d_context) simulator.Initialize() simulator.set_monitor(monitor) if VISUALIZE: simulator.set_target_realtime_rate(1.) simulator.AdvanceTo(2.) # Try to push a bit further. simulator.clear_monitor() simulator.AdvanceTo(d_context.get_time() + 0.05) diagram.Publish(d_context) # Recreate simulator. builder_new = DiagramBuilder() plant_new, scene_graph_new = AddMultibodyPlantSceneGraph( builder_new, time_step=plant.time_step()) subgraph = mut.MultibodyPlantSubgraph( mut.get_elements_from_plant(plant, scene_graph)) # Remove all joints; make them floating bodies. for joint in me.get_joints(plant): subgraph.remove_joint(joint) # Remove massless bodies. for body in me.get_bodies(plant): if body is plant.world_body(): continue if body.default_mass() == 0.: subgraph.remove_body(body) # Finalize. to_new = subgraph.add_to(plant_new, scene_graph_new) plant_new.Finalize() if VISUALIZE: ConnectDrakeVisualizer(builder_new, scene_graph_new, role=role) ConnectContactResultsToDrakeVisualizer(builder_new, plant_new) diagram_new = builder_new.Build() # Remap state. d_context_new = diagram_new.CreateDefaultContext() d_context_new.SetTime(d_context.get_time()) context_new = plant_new.GetMyContextFromRoot(d_context_new) to_new.copy_state(context, context_new) # Simulate. simulator_new = Simulator(diagram_new, d_context_new) simulator_new.Initialize() diagram_new.Publish(d_context_new) if VISUALIZE: simulator_new.set_target_realtime_rate(1.) simulator_new.AdvanceTo(context_new.get_time() + 2) if VISUALIZE: input(" Press enter...")