Support for URDF Mimic Joints

[RKJenamani]

The URDF format does not support cycles in the Joint Tree (for example, 4- or 5-bar linkages). Instead, the kinematics/dynamics of these relationships are specified using the <mimic> joint declaration, where the position of a joint is set to be some linear function of another joint (i.e. joint2 = m * joint1 + offset). See https://wiki.ros.org/urdf/XML/joint

The URDF loader in DART (https://github.com/dartsim/dart/blob/main/dart/utils/urdf/DartLoader.cpp) handles this by registering the joint as normal, and then just setting the ActuatorType to "MIMIC", which is used during dynamic simulation to set the set-point of a given joint based on the position of its corresponding mimic joint.

However, Aikido does not use DART's dynamic simulation. We set the positions/velocities of joints ourselves to do Kinematic simulation and collision detection. Therefore, there is no obvious way to handle mimic joints. And the additional Degrees of Freedom that are registered lead to a StateSpace that doesn't reflect the actual configuration space of the robot.

This makes it impossible to plan and execute a trajectory on, for example, ADA's grippers, which have 4-bar (Gen2) or 5-bar (Gen3) linkages respectively.

Existing workaround: in the URDF, set the joint type of the mimic joint to "fixed". This allows the StateSpace to be constructed only considering the base joint. However, this forces the gripper to go into collision with itself when closed. So you need to disable all self-collision in the hand. This works, and is what we use for ADA Gen2, but it leads to a non-realistic collision model for the gripper, which affects the ability of planning algorithms to accurately plan around objects in the environment.

How We Can Fix This: Update the MetaSkeletonStateSpace Object
https://github.com/personalrobotics/aikido/blob/master/src/statespace/dart/MetaSkeletonStateSpace.cpp

1. When creating the state space, make sure that the mimic joints are not registered as joint sub-spaces, and update the Properties object accordingly (i.e. the number of DoFs in the properties object = MetaSkeleton Dofs - Mimic Dofs)
2. For convertStateToPositions and setState, do the mimic multiplier+offset calculates to set the positions of the MetaSkeleton (or, respectively, return the positions Eigen Vector) accordingly.
3. for convertPositionsToState, accept an Eigen Vector of either the length of the number of true DoFs, or the length of the number of MetaSkeleton Dofs (as a utility for passing in MetaSkeleton.getPositions()). If the latter, check to make sure that the mimic constraints are upheld (to appropriate float precision) before creating the state object.
4. Make sure that all relevant calls for setting the state of the MetaSkeleton (esp. in collision, control, and robot) do it through the statespace rather than calling setPositions directly