Extend Support for Mimic Joint

[MegMll]

Overview

This PR enhances support for the mimic joint.
Mimic joint refer to a joint which configuration depends on the configuration of another joint (that we named the primary joint). The relation is $q_{mimic} = \alpha * q_{primary} + \beta$ (where $\alpha, \beta$ are defined by the user)

Key Updates:

URDF Parsing:

The mimic tag can now be parsed from urdf, and add the corresponding mimic joint to the model.
A mimic flag has been added to the parser to enable this feature or not (default to false) in order to preserve backward compatibility.

Robot and Joint Model Modifications:

• The already existing mimic joint type has been update to allow support in more algorithms.
  • The main change being that instead of being templated on the primary joint type, the mimic joint now holds a variant of joint for the primary.
• The robot and joint nq and nv (resp. idx_q, idx_v) values have now been extended with a nvExtended (resp idx_vExtended) value, holding the dimension of the extended robot/joint tangeant space. (for all classical joint nv==nvExtened but for mimic nv==0 and nvExtended==primary.nvExtended). The suffix vExtended stand for Jacobian as those values are mainly used for computing jacobians rows and columns.

Features of the joint mimic:

Joint

Not all the joints are able to be mimicked. Please see the following list for the joints that can be mimicked:
- Prismatics
- Revolutes
- Helicoidals
Please also note that the mimic joint must be after its primary (i.e. id_mimicking > id_mimicked) in the kinematic tree for it to be created in pinocchio. If that's not the case, the model will not be created.

Algorithms

The following algorithms support the new joint mimic :
- RNEA
- CRBA
- Forward Kinematics
- Jacobians and Frames
- Centroidal Algorithm

Testing & Validation:

• Added unit tests to validate mimic joint support for all the above algorithms.
• Verified URDF parsing for different robot models that include mimic joints.

Limitations:

• Algorithms such as ABA, or any derivatives are not supported and should trigger asserts.
• Currently, the mimic joint should always be after the primary joint (i.e. id_mimicking > id_mimicked) in the kinematic tree. (assert is triggered if not)

Feedback Needed:

The naming of the new member variable and functions is pretty arbitrary, we are eager to change them to better suit pinocchio naming standards.
Non exhaustively, here are a list of the lesser quality one (that should most probably be renamed) :

• idx_j/nj : representing the dimension and index in the "extended model"
• jointCols, jointRows, etc method has been split into jointVelCols and jointJacCols (respectively if they refer to idx_v/nv or idx_j/nj)
• Similarly jointConfigSelector was splitted into jointConfigFromDofSelector and jointConfigFromNqSelector to meet the need for mimic joint to select config either from the parent joint or don't select anything
• Similarly jointVelocitySelector was splitted into jointVelocityFromDofSelector and jointConfigFromNqSelector to meet the need for mimic joint to select config either from the parent joint or don't select anything

Special Thanks:

A special thank you to @EtienneAr for his guidance and advice throughout this development process.

[github-actions]

👋 Hi,
This is a reminder message to assign an extra build label to this Pull Request if needed.
By default, this PR will be build with minimal build options (URDF support and Python bindings)
The possible extra labels are:

• build_collision (build Pinocchio with coal support)
• build_casadi (build Pinocchio with CasADi support)
• build_autodiff (build Pinocchio with CppAD support)
• build_codegen (build Pinocchio with CppADCodeGen support)
• build_extra (build Pinocchio with extra algorithms)
• build_mpfr (build Pinocchio with Boost.Multiprecision support)
• build_sdf (build Pinocchio with SDF parser)
• build_accelerate (build Pinocchio with APPLE Accelerate framework support)
• build_all (build Pinocchio with ALL the options stated above)

Thanks.
The Pinocchio development team.

[jcarpent]

I will provide a detailed review later.

[abussy-aldebaran]

Hi,

I started using mimic joints, so I'm following this PR with interest !
I tested this branch with forward and inverse kinematics (with pseudo-inverses of the Jacobians), and it seems to work fine 👍

How/Where can I report any bug I find ?
I'll try to add MREs as unittests. For instance, I found : abussy-aldebaran@5a8f205

[abussy-aldebaran]

Maybe you could add typedef for mimic joints ?

typedef JointModelMimicTpl<Scalar, Options, JointCollectionTpl> JointModelMimic;
typedef JointDataMimicTpl<Scalar, Options, JointCollectionTpl> JointDataMimic;

[abussy-aldebaran]

Isn't a setMimicIndices missing ?
See abussy-aldebaran@5a8f205
and abussy-aldebaran@afef317

[MegMll]

Hi,

I started using mimic joints, so I'm following this PR with interest ! I tested this branch with forward and inverse kinematics (with pseudo-inverses of the Jacobians), and it seems to work fine 👍

How/Where can I report any bug I find ? I'll try to add MREs as unittests. For instance, I found : abussy-aldebaran@5a8f205

Hi,

Thanks a lot for your interest in this PR, ! In order to simplify the PR and the review process, if you want to add new unittests, please do it directly on this branch.

If you don't feel confortable with this solution, we'll find another way.

As for bugs, please add comments here, and we'll take a look at it.

Thanks again !

[abussy-aldebaran]

In order to simplify the PR and the review process, if you want to add new unittests, please do it directly on this branch.

What do you mean ? Opening a PR in your forked repository ?

[EtienneAr]

Following our IRL meeting, here is the todo list:

• Refactor the translateJacobian to prevent performance drop (due to nested for loops and visitor call increase)
• Make sure the API (C++ & Python) is not broken by this PR (i.e. check that the non-mimic test did not change)
• Add a check "model has mimic" in robot model to factorize code, and use that check in algorithms
• Double check the necessity of the SE3 static_cast
• Add specific check for mimic in check.hxx (instead of just skipping with an if).
• Revert all changes in unsupported algo
• Rename nj, idx_j -> nv_extended, idx_v_extended
• Make sure that jcalc is easily upgradable for non linear mimic (withoput API break)
• Edit this PR to list more limitations of this feature: Which joints type are mimickable, what are the contraints on the joint ids, ...

The following point are Nice To Have and should be addressed in separate issues / PRs:

• Function to go from "Reduced" mimic model to full "extended" model
• Store extra data in robot model/data to speed up so algo
• Make proper safe/unsafe function for every algo to check (or not) input parameters, for performance issues and to be able to check for python bindings
• Function that give the constraint matrix from a "Reduced" mimic model

[jorisv]

I don't think this should be added to the default checker list.
Default checker validate if the model is correct, Mimic doesn't imply the model is not valid, it only doesn't work on some algorithms…

[jorisv]

First boost::get result can be stored.

[abussy-aldebaran]

No directly push on this branch, so that the new unittests can be reviewed, here as well.

Could you grant me the access rights to push, if that's still ok with you ?