drake

How to lock specific states of the base joint 'panda_link0'?

The base joint of the Frank panda arm consists of 7 positions and 6 velocities. However in order to have it mobile I want to lock the translation along the ‘z’ axis and rotation around the ‘x’ and ‘y’ axes. This will enable the InverseKinematics to exclude these components from its calculations.

Using

for joint_index in plant.GetJointIndices(panda_index):
        joint = plant.get_joint(joint_index)
        print(f"Joint:{joint.name()}")
        if "panda_link0" in joint.name():
            joint.Lock(plant_context)

locks all its states. Is there a way to do this without adding it as a constraint to the MP or without modifying the urdf with virtual joints?

Solution

  • You can add equality constraints to InverseKinematics which can achieve the effect that you want, but if you really want

    InverseKinematics to exclude these components from its calculations.

    then you will need to change the kinematic tree in the multibodyplant. I agree that the best way to do that is to add a specific joints between the world and the base link of the panda.

    I did precisely that in this example in the draft mobile manipulation chapter of my course notes. (There is also a problem set question about doing IK on the mobile base that can be found below.)