def setup_dot_diagram(builder, args): ''' Using an existing DiagramBuilder, adds a sim of the Dot robot. Args comes from argparse. The returned controller will need its first port connected to a setpoint source.''' print( "TODO: load in servo calibration dict to a servo calibration object that gets shared" ) with open(args.yaml_path, "r") as f: config_dict = yaml.load(f, Loader=Loader) sdf_path = os.path.join(os.path.dirname(args.yaml_path), config_dict["sdf_path"]) plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0005) parser = Parser(plant) model = parser.AddModelFromFile(sdf_path) # Set initial pose floating above the ground. plant.SetDefaultFreeBodyPose(plant.GetBodyByName("body"), RigidTransform(p=[0., 0., 0.25])) if args.welded: plant.WeldFrames(plant.world_frame(), plant.GetBodyByName("body").body_frame()) else: add_ground(plant) plant.Finalize() controller = builder.AddSystem(ServoController(plant, config_dict)) # Fixed control-rate controller with a low pass filter on its torque output. zoh = builder.AddSystem( ZeroOrderHold(period_sec=0.001, vector_size=controller.n_servos)) filter = builder.AddSystem( FirstOrderLowPassFilter(time_constant=0.02, size=controller.n_servos)) builder.Connect(plant.get_state_output_port(), controller.get_input_port(1)) builder.Connect(controller.get_output_port(0), zoh.get_input_port(0)) builder.Connect(zoh.get_output_port(0), filter.get_input_port(0)) builder.Connect(filter.get_output_port(0), plant.get_actuation_input_port()) return plant, scene_graph, controller
def grasp_poses_example(): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0) parser = Parser(plant, scene_graph) grasp = parser.AddModelFromFile( FindResourceOrThrow( "drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_no_tip.sdf" ), "grasp") brick = parser.AddModelFromFile( FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf")) plant.Finalize() meshcat = ConnectMeshcatVisualizer(builder, scene_graph) diagram = builder.Build() context = diagram.CreateDefaultContext() plant_context = plant.GetMyContextFromRoot(context) B_O = plant.GetBodyByName("base_link", brick) X_WO = plant.EvalBodyPoseInWorld(plant_context, B_O) B_Ggrasp = plant.GetBodyByName("body", grasp) p_GgraspO = [0, 0.12, 0] R_GgraspO = RotationMatrix.MakeXRotation(np.pi / 2.0).multiply( RotationMatrix.MakeZRotation(np.pi / 2.0)) X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO) X_OGgrasp = X_GgraspO.inverse() X_WGgrasp = X_WO.multiply(X_OGgrasp) plant.SetFreBodyPose(plant_context, B_Ggrasp, X_WGgrasp) # Open the fingers, too plant.GetJointByName("left_finger_sliding_joint", grasp).set_translation(plant_context, -0.054) plant.GetJointByName("right_finger_sliding_joint", grasp).set_translation(plant_context, 0.054) meshcat.load() diagram.Publish(context)
def compile_scene_tree_to_mbp_and_sg(scene_tree, timestep=0.001): builder = DiagramBuilder() mbp, scene_graph = AddMultibodyPlantSceneGraph( builder, MultibodyPlant(time_step=timestep)) parser = Parser(mbp) parser.package_map().PopulateFromEnvironment("ROS_PACKAGE_PATH") world_body = mbp.world_body() node_to_free_body_ids_map = {} body_id_to_node_map = {} free_body_poses = [] for node in scene_tree.nodes: node_to_free_body_ids_map[node] = [] if node.tf is not None and node.physics_geometry_info is not None: # Don't have to do anything if this does not introduce geometry. sanity_check_node_tf_and_physics_geom_info(node) phys_geom_info = node.physics_geometry_info # Don't have to do anything if this does not introduce geometry. has_models = len(phys_geom_info.model_paths) > 0 has_prim_geometry = (len(phys_geom_info.visual_geometry) + len(phys_geom_info.collision_geometry)) > 0 if not has_models and not has_prim_geometry: continue node_model_ids = [] # Handle adding primitive geometry by adding it all to one # mbp. if has_prim_geometry: # Contain this primitive geometry in a model instance. model_id = mbp.AddModelInstance(node.name + "::model_%d" % len(node_model_ids)) node_model_ids.append(model_id) # Add a body for this node, and register any of the # visual and collision geometry available. # TODO(gizatt) This tree body index is built in to disambiguate names. # But I forsee a name-to-stuff resolution crisis when inference time comes... # this might get resolved by the solution to that. body = mbp.AddRigidBody(name=node.name, model_instance=model_id, M_BBo_B=phys_geom_info.spatial_inertia) body_id_to_node_map[body.index()] = node tf = torch_tf_to_drake_tf(node.tf) if phys_geom_info.fixed: weld = mbp.WeldFrames(world_body.body_frame(), body.body_frame(), tf) else: node_to_free_body_ids_map[node].append(body.index()) mbp.SetDefaultFreeBodyPose(body, tf) for k, (tf, geometry, color) in enumerate(phys_geom_info.visual_geometry): mbp.RegisterVisualGeometry(body=body, X_BG=torch_tf_to_drake_tf(tf), shape=geometry, name=node.name + "_vis_%03d" % k, diffuse_color=color) for k, (tf, geometry, friction) in enumerate( phys_geom_info.collision_geometry): mbp.RegisterCollisionGeometry( body=body, X_BG=torch_tf_to_drake_tf(tf), shape=geometry, name=node.name + "_col_%03d" % k, coulomb_friction=friction) # Handle adding each model from sdf/urdf. if has_models: for local_tf, model_path, root_body_name, q0_dict in phys_geom_info.model_paths: model_id = parser.AddModelFromFile( resolve_catkin_package_path(parser.package_map(), model_path), node.name + "::" "model_%d" % len(node_model_ids)) node_model_ids.append(model_id) if root_body_name is None: root_body_ind_possibilities = mbp.GetBodyIndices( model_id) assert len(root_body_ind_possibilities) == 1, \ "Please supply root_body_name for model with path %s" % model_path root_body = mbp.get_body( root_body_ind_possibilities[0]) else: root_body = mbp.GetBodyByName(name=root_body_name, model_instance=model_id) body_id_to_node_map[root_body.index()] = node node_tf = torch_tf_to_drake_tf(node.tf) full_model_tf = node_tf.multiply( torch_tf_to_drake_tf(local_tf)) if phys_geom_info.fixed: mbp.WeldFrames(world_body.body_frame(), root_body.body_frame(), full_model_tf) else: node_to_free_body_ids_map[node].append( root_body.index()) mbp.SetDefaultFreeBodyPose(root_body, full_model_tf) # Handle initial joint state if q0_dict is not None: for joint_name in list(q0_dict.keys()): q0_this = q0_dict[joint_name] joint = mbp.GetMutableJointByName( joint_name, model_instance=model_id) # Reshape to make Drake happy. q0_this = q0_this.reshape(joint.num_positions(), 1) joint.set_default_positions(q0_this) return builder, mbp, scene_graph, node_to_free_body_ids_map, body_id_to_node_map