def copy_joint(self, joint_src):
"""Copies a joint to be added to the destination plant."""
assert isinstance(joint_src, Joint)
plant_dest = self.plant_dest
frame_on_parent_dest = self.frames[joint_src.frame_on_parent()]
frame_on_child_dest = self.frames[joint_src.frame_on_child()]
# N.B. We use `type(x) == cls`, not `isinstance(x, cls)`, so that we
# know we recreate the exact types.
if type(joint_src) == BallRpyJoint:
joint_dest = BallRpyJoint(
name=joint_src.name(),
frame_on_parent=frame_on_parent_dest,
frame_on_child=frame_on_child_dest,
# TODO(eric.cousineau): Bind this?
# damping=joint_src.damping(),
)
elif type(joint_src) == PrismaticJoint:
joint_dest = PrismaticJoint(
name=joint_src.name(),
frame_on_parent=frame_on_parent_dest,
frame_on_child=frame_on_child_dest,
axis=joint_src.translation_axis(),
damping=joint_src.damping(),
)
elif type(joint_src) == RevoluteJoint:
joint_dest = RevoluteJoint(
name=joint_src.name(),
frame_on_parent=frame_on_parent_dest,
frame_on_child=frame_on_child_dest,
axis=joint_src.revolute_axis(),
damping=joint_src.damping(),
)
elif type(joint_src) == UniversalJoint:
joint_dest = UniversalJoint(
name=joint_src.name(),
frame_on_parent=frame_on_parent_dest,
frame_on_child=frame_on_child_dest,
damping=joint_src.damping(),
)
elif type(joint_src) == WeldJoint:
joint_dest = WeldJoint(
name=joint_src.name(),
parent_frame_P=frame_on_parent_dest,
child_frame_C=frame_on_child_dest,
X_PC=joint_src.X_PC(),
)
else:
assert False, f"Cannot clone: {type(joint_src)}"
joint_dest.set_position_limits(joint_src.position_lower_limits(),
joint_src.position_upper_limits())
joint_dest.set_velocity_limits(joint_src.velocity_lower_limits(),
joint_src.velocity_upper_limits())
joint_dest.set_acceleration_limits(
joint_src.acceleration_lower_limits(),
joint_src.acceleration_upper_limits())
joint_dest.set_default_positions(joint_src.default_positions())
plant_dest.AddJoint(joint_dest)
_add_item(self.joints, joint_src, joint_dest)
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 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=RigidTransform.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)