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
