Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Support for URDF Mimic Joints #612

Open
RKJenamani opened this issue Sep 24, 2021 · 0 comments
Open

Support for URDF Mimic Joints #612

RKJenamani opened this issue Sep 24, 2021 · 0 comments

Comments

@RKJenamani
Copy link
Member

RKJenamani commented Sep 24, 2021

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
@egordon egordon removed their assignment Sep 28, 2021
@egordon egordon added this to the Aikido 0.5.0 milestone Sep 28, 2021
@egordon egordon changed the title Support for Passive Joints Support for URDF Mimic Joints Sep 28, 2021
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

No branches or pull requests

2 participants