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 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 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)
