def weld_gripper(mbp, robot_index, gripper_index):
X_EeGripper = create_transform([0, 0, 0.081], [np.pi / 2, 0, np.pi / 2])
robot_body = get_model_bodies(mbp, robot_index)[-1]
gripper_body = get_model_bodies(mbp, gripper_index)[0]
mbp.AddJoint(
WeldJoint(name="weld_gripper_to_robot_ee",
parent_frame_P=robot_body.body_frame(),
child_frame_C=gripper_body.body_frame(),
X_PC=X_EeGripper))
def test_multibody_add_joint(self):
"""
Tests joint constructors and `AddJoint`.
"""
instance_file = FindResourceOrThrow(
"drake/examples/double_pendulum/models/double_pendulum.sdf")
# Add different joints between multiple model instances.
# TODO(eric.cousineau): Remove the multiple instances and use
# programmatically constructed bodies once this API is exposed in
# Python.
num_joints = 2
plant = MultibodyPlant()
parser = Parser(plant)
instances = []
for i in range(num_joints + 1):
instance = parser.AddModelFromFile(instance_file,
"instance_{}".format(i))
instances.append(instance)
proximal_frame = "base"
distal_frame = "lower_link"
joints = [
RevoluteJoint(
name="revolve_things",
frame_on_parent=plant.GetBodyByName(distal_frame,
instances[1]).body_frame(),
frame_on_child=plant.GetBodyByName(proximal_frame,
instances[2]).body_frame(),
axis=[0, 0, 1],
damping=0.),
WeldJoint(
name="weld_things",
parent_frame_P=plant.GetBodyByName(distal_frame,
instances[0]).body_frame(),
child_frame_C=plant.GetBodyByName(proximal_frame,
instances[1]).body_frame(),
X_PC=Isometry3.Identity()),
]
for joint in joints:
joint_out = plant.AddJoint(joint)
self.assertIs(joint, joint_out)
# The model is now complete.
plant.Finalize()
for joint in joints:
self._test_joint_api(joint)
def weld_to_world(mbp, model_index, world_pose):
mbp.AddJoint(
WeldJoint(name="weld_to_world",
parent_frame_P=mbp.world_body().body_frame(),
child_frame_C=get_base_body(mbp, model_index).body_frame(),
X_PC=world_pose))