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)
Esempio n. 2
0
 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)
Esempio n. 4
0
    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)