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 tangent space. (for all classical joint nv==nvExtened but for mimic nv==0 and nvExtended==primary.nvExtended). The suffix vExtended stand for extended tangent space and is mainly used for computing jacobians rows and columns.

Features of the joint mimic:

Extended Model

The model 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 tangent space.
The suffix vExtended stand for extended tangent space.
The choice of using nv=0 (resp. nq=0) for a mimic joint, allows for a transparent merge of the mimic joint contribution to the dynamics of the model.
The extended tangent space (nvExtended) is useful to compute an extended Jacobian that includes the effects of both primary and mimic joints.

Changes were also introduced in vector and matrices accessors:

• Configuration Vector accessor is now split in 2:

  • jointConfig is the same as before for all joints, except mimic, where it will return a 0-sized vector
  • jointMappedConfig is the same as jointConfig for most joint, except mimic where it will return the configuration vector of the joint it is mimicking

• Tangent Vector accessor is split in 2 in a similar fashion:

  • jointVelocity is the same as before for all joints, except mimic, where it will return a 0-sized vector
  • jointMappedVelocity is the same as jointVelocity for most joint, except mimic where it will return the velocity vector of the joint it is mimicking

• All matrices accessors are also split in 2:

  • jointRows, jointCols, jointBlock are the same as before for all joints except mimic. For mimic, it will return the row/column/block corresponding to its mimicked index.
  • jointExtendedRows, jointExtendedCols, jointExtendedBlock will do the same jointRows, jointCols, jointBlock for all joints except mimic. For mimic, it will return its own row/column/block using its nvExtended and idx_vExtended

New Model and Data informations

To fully support this new joint, and keep code efficiency, we introduce new vectors in model and data:

• model.mimicking_joints - in this vector, we store all the indexes of mimicking joint (JointMimicModel)
• model.mimicked_joints - Here are all the indexes of the mimicked joints (the one directing the joint couple)
• data.mimic_subtree_joint - It stores the index of the first joint in the mimicking joint subtree, that is not a mimicking joint.

Joint which allows a mimic

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.

Attention Revolute, prismatic or helicoidal joints that are inside a composite joint, should not be mimicked, it will lead to undefined behaviour and should be avoided. Same for mimic joint inside a composite joint.

Algorithms

The following algorithms support the new joint mimic :
- RNEA
- CRBA
- Forward Kinematics
- Jacobians and Frames
- Centroidal Algorithm (ccrba)
- Reachable Workspace

Parsing

The mimic tag in a urdf file can now be taken into account if the user decides it. Otherwise it will be a fully autonomous joint in the model.
Only mimic in urdf are taken into account. We are aware a similar mechanism exists for mjcf model, but it is not supported for now.

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_vExtended/nvExtended : representing the dimension and index in the "extended model"
• jointCols, jointRows, etc method has been split into jointCols and jointExtendedCols (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

Next

Non linear mimic support

We can add a functor to JointModelMimicTpl constructor argument. This functor will hold the functon to apply to the config and velocity vectors.

This can be added latter as:

• a new JointModelMimicTpl template argument set with a default value
• a std::function stored in JointModelMimicTpl

The latter options is less effective.
In both case, this will avoid a call to a visitor, since the configVectorAffineTransform can be solved at the JointModelMimicTpl construction.

Special Thanks:

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

Review TODO list

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) (@MegMll)
• 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
• Old MimicJoint API: see if we can keep the same major @jcarpent
• getRelativePlacement: ask to @jcarpent if we keep it
• Use extended instead of full in the documentation (@MegMll)
• Add ConfigVectorAffineTransform in helical joints (@jorisv)
  • Check in all joint if we need to add or not a ConfigVectorAffineTransform specialization
• NoAffine transform must assert
• Don't use mutable in JointDataMimicTpl accessors (@MegMll)
• Nice JointModelMimicTpl and JointDataMimicTpl cout
• Improve JointModelMimicTpl doc and comment
• Store JointModelMimicTpl::m_jmodel_ref nq and nv to avoid visitor call
• Rename JointModelMimicTpl::m_jmodel_ref into m_jmodel_mimicking
• Check dIntegrate with @jcarpent
• Restore data nvSubtree as it was before (and check data)
  • idx_v and nvsubtree doesn't make sens for Mimic joint.
• Fix serialization issue (@MegMll)
• Fix cast issue (@MegMll)
• Fix reduce model issue (@MegMll)
• Impove changelog
• Fix setIndexes (@MegMll)
• Update urdf parsing
• Check the jointConfig/jointVelocitySelector (maybe some are badly named or missing)
  • jointApplyVelocitySelector, jointApplyConfigSelector
• Add Workspace as supported algorithm (and use mimic model in workspace unit test)
• Add TU for double mimic in a model (primary different and same primary)
• Modify CRBA
  • Local (no malloc)
  • World
• Modify Jacobian algorithms
• Modify Serialization of Mimic Data (probably missig some infos)
• Add vector of joint index to keep track of mimic joints to model (serialize and bind it)
  • One for the mimicking (model)
  • One for the mimicked (model)
• Vector of first non mimicking joint in the mimic subtree (data)
• Check buildReducedModel et AppendModel with new model attributes
• Throw an exception in Python binding if the algorithm doesn't support mimic
• Check rnea (test failing for now with two mimic joints with differents primary)
• Add test for kinematics and mimic joints
• Squash all
• Example python
• Add assert for subtreejacobians (in center-of-mass.hxx)
• Add test for new data (mimic_subtree_index)
  • check
  • unittest
• Add lots of comments for crba patches and jacobian patches
• Use new data struct for local mimic patch in crba
• Remove unused structure in model
• Test getFrameJacobian
• Codacy: don't care
• Rebase
• Run benchmark
  ...

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

[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

[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 ?

[jorisv]

I think we can patch this the same way we patch translateJacobian: First pass only on the non mimic, second pass only on the mimic. Like we are doing in CRBA.

[jorisv]

I think is better to show we can compute aba with M_mimic and C_mimic instead of using G.

[jorisv]

We should test idx_v_extended_fromRow, mimic_parents_fromRow and non_mimic_parents_fromRow.

[jorisv]

We should patch standard test to check idx_vExtendeds, nvExtendeds, mimicking_joints and mimicked_joints.