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 nj (resp idx_j) value, holding the dimension of the extended robot/joint tangeant space. (for all classical joint nv==nj but for mimic nv==0 and nj==primary.nj). The suffix j stand for Jacobian as those values are mainly used for computing jacobians rows and columns.

Algorithms:

• The following algorithm 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 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]

I got a -Wreorder warning. axis should be first in the initialization list.

[abussy-aldebaran]

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