Extend Support for Mimic Joint #2441

MegMll · 2024-09-30T14:41:25Z

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 · 2024-12-12T14:01:50Z

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 · 2024-12-12T15:13:31Z

include/pinocchio/multibody/model.hpp

Maybe you could add typedef for mimic joints ?

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

include/pinocchio/parsers/urdf/model.hxx

abussy-aldebaran · 2024-12-13T09:03:54Z

include/pinocchio/multibody/joint/joint-mimic.hpp

-      res.setIndexes(id(), idx_q(), idx_v());
+        m_jmodel_ref.template cast<NewScalar>(), ScalarCast<NewScalar, Scalar>::cast(m_scaling),
+        ScalarCast<NewScalar, Scalar>::cast(m_offset));
+      res.setIndexes(id(), idx_q(), idx_v(), idx_j());


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

MegMll · 2024-12-16T10:43:01Z

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 · 2024-12-26T08:16:47Z

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 · 2025-01-15T11:55:37Z

include/pinocchio/algorithm/geometry.hxx

+        geom_data.oMg[i] =
+          (static_cast<GeometryModel::SE3>(data.oMi[joint_id])
+           * geom_model.geometryObjects[i].placement);


Is the static_cast mandatory ? This code build without it.

Did you try with CppAd ? I removed all the other cast, and ci with cppAd, does not work

jorisv · 2025-01-15T13:59:26Z

include/pinocchio/algorithm/check.hxx

    }

    for (JointIndex j = 1; int(j) < model.njoints; ++j)
    {
+      if (boost::get<JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>(&model.joints[j]))


Why you don't check this joint ?
Add a comment to explains or use the if to jump/change invalid check for mimic.

jorisv · 2025-01-15T14:57:47Z

include/pinocchio/algorithm/centroidal.hxx

-        ColsBlock Ag_cols = jmodel.jointCols(data.Ag);
-        motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols);
+        ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag);
+        motionSet::inertiaAction<ADDTO>(data.oYcrb[i], J_cols, Ag_cols);


Did you fix a bug in this pass ?

No, the default behavior is not changed

Before this backward pass, Ag is set to zero (line 177 of this file).
The columns of the DoF that does not involve any mimicking, are selected only once in this pass and thus "added to" only once. So for those Dof, the ADDTO act the same as a SETTO.

(But for the mimic DoF this allows for summing the results)

jorisv · 2025-01-15T15:09:29Z

include/pinocchio/algorithm/compute-all-terms.hxx

+        assert(
+          (std::is_same<JointModel, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>::value
+           == false)
+          && "Algorithm not supported for mimic joints");


This assert is nice, but how to warn the user in Python binding ?

All the asserts were changed to PINOCCHIO_THROW, so that it works in release mode (and in python)

jorisv · 2025-01-15T15:21:13Z

include/pinocchio/algorithm/constrained-dynamics.hxx

        J_cols.transpose() * data.dFda.middleCols(jmodel.idx_v(), data.nvSubtree[i]);
      data.oYcrb[parent] += data.oYcrb[i];

      if (ContactMode)
      {
-        jmodel.jointVelocitySelector(data.nle).noalias() =
+        jmodel.jointVelocityFromNvSelector(data.nle).noalias() =


Did we support constrained dynamics ?

include/pinocchio/algorithm/crba.hxx

jorisv · 2025-01-15T16:13:34Z

include/pinocchio/algorithm/crba.hxx

+      assert(secondary_id > primary_id && "Mimicking joint id is before the primary.");
+
+      size_t ancestor_prim, ancestor_sec;
+      findCommonAncestor(model, primary_id, secondary_id, ancestor_prim, ancestor_sec);


Instead of computing this data at each call, it's maybe possible to store it in JointDataMimicTpl.

I agree with you that computing every time seems like a waste, but I am not sure that it is worth storing it.

When JointDataMimicTpl is instantiated, it doesn't have any available information regarding the kinematic structure of the whole model. (The JointModelMimicTpl does not contain this information either)

include/pinocchio/algorithm/crba.hxx

… and mimiced -> mimicked for naming consistency

…ard passes

EtienneAr · 2025-01-23T16:39:31Z

… to the new convention

github-actions bot reviewed Sep 30, 2024

View reviewed changes

MegMll added the build_all label Sep 30, 2024

MegMll force-pushed the topic/mimic branch from 9ffcc95 to c7c198f Compare October 3, 2024 16:14

MegMll mentioned this pull request Oct 4, 2024

Context::Scalar in Geometry and GeometryObject #2445

Open

MegMll force-pushed the topic/mimic branch 4 times, most recently from c43fad1 to 52358ed Compare October 11, 2024 15:05

MegMll force-pushed the topic/mimic branch from fd73db2 to 55b2aae Compare October 11, 2024 16:13

jcarpent requested changes Oct 15, 2024

View reviewed changes

MegMll force-pushed the topic/mimic branch from cdbe70d to 53d7cd2 Compare October 28, 2024 15:10

MegMll force-pushed the topic/mimic branch 6 times, most recently from 3f25772 to 5ea8402 Compare November 19, 2024 08:38

MegMll force-pushed the topic/mimic branch from 5ea8402 to e867702 Compare November 28, 2024 14:37

jorisv force-pushed the topic/mimic branch from e867702 to 8f9c66e Compare December 4, 2024 13:58

MegMll force-pushed the topic/mimic branch from 8f9c66e to bd466b3 Compare December 11, 2024 09:24

abussy-aldebaran reviewed Dec 12, 2024

View reviewed changes

abussy-aldebaran reviewed Dec 13, 2024

View reviewed changes

include/pinocchio/parsers/urdf/model.hxx Show resolved Hide resolved

abussy-aldebaran reviewed Dec 13, 2024

View reviewed changes

MegMll force-pushed the topic/mimic branch 2 times, most recently from bd466b3 to b90f6e0 Compare December 16, 2024 10:39

MegMll force-pushed the topic/mimic branch 2 times, most recently from ea956c8 to 083d240 Compare January 10, 2025 10:04

Megane Millan and others added 5 commits January 15, 2025 15:13

[EtienneAr Feedback] Add explicit to single argument constructors

d9ad2a4

Applied pre-commit

aa23cc9

Fix init problem

5b4d282

[algo/jacobian] Fix rebasing

c8ae316

python: Fix linting

d04a675

jorisv requested changes Jan 15, 2025

View reviewed changes

Megane Millan added 9 commits January 16, 2025 10:51

[kinematics] Add documentation to getRelativePlacement

ae3098d

[algo/model] Change error message to be more precise

d7a0da5

[algo/model] Access directly to variables

cfbcb35

[algo/model] Remove casting

a7f18ce

[algo/model] Simplify function

c29d601

[algo/jacobian] Remove useless line

995dfee

[algo/jacobian] Remove useless cast

103499a

[algo/kinematics] Access directly variables

e117132

[algo/kinematics] Remove inline

5aa1149

MegMll force-pushed the topic/mimic branch from ecb2f5e to 5aa1149 Compare January 16, 2025 09:55

Megane Millan added 9 commits January 16, 2025 11:00

[algo/crba] access variables directly

d2e887a

[algo/crba] Remove inline

1864a26

Change naming convention

064c2a5

Added assert for non supported algorithms and apply precommit

3fa5eaa

[EtienneAr feedback] Fix -Wreorder for urdf parsing of mimic joints /…

0abc4f6

… and mimiced -> mimicked for naming consistency

[EtienneAr feedback] Simplify getRelativePlacement by doing two backw…

8fdf846

…ard passes

[EtienneAr feedback] Remove unused scaling in JointDataMimic

96bd68f

[algorithms] Use throw instead of assert

f6a28ee

[parser/urdf] Correct urdf parser

6258e8b

MegMll force-pushed the topic/mimic branch from 56f412f to 6258e8b Compare January 23, 2025 09:29

Apply pre-commit

9f8fa73

Megane Millan added 2 commits January 24, 2025 11:25

Renaming nj and idx_j into nvExtended and idx_vExtended to be conform…

09735a7

… to the new convention

[EtienneAr feedback] Revert all changes in unsupported algorithms

05925c1

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Extend Support for Mimic Joint #2441

Extend Support for Mimic Joint #2441

MegMll commented Sep 30, 2024 •

edited

Loading

github-actions bot left a comment

jcarpent left a comment

abussy-aldebaran commented Dec 12, 2024

abussy-aldebaran Dec 12, 2024

abussy-aldebaran Dec 13, 2024

MegMll commented Dec 16, 2024

abussy-aldebaran commented Dec 26, 2024

jorisv Jan 15, 2025

MegMll Jan 16, 2025

jorisv Jan 15, 2025

jorisv Jan 15, 2025

EtienneAr Jan 19, 2025

jorisv Jan 15, 2025

MegMll Jan 20, 2025

jorisv Jan 15, 2025

jorisv Jan 15, 2025

EtienneAr Jan 20, 2025

EtienneAr commented Jan 23, 2025 •

edited by MegMll

Loading

Extend Support for Mimic Joint #2441

Are you sure you want to change the base?

Extend Support for Mimic Joint #2441

Conversation

MegMll commented Sep 30, 2024 • edited Loading

Overview

Key Updates:

URDF Parsing:

Robot and Joint Model Modifications:

Features of the joint mimic:

Joint

Algorithms

Testing & Validation:

Limitations:

Feedback Needed:

Special Thanks:

github-actions bot left a comment

Choose a reason for hiding this comment

jcarpent left a comment

Choose a reason for hiding this comment

abussy-aldebaran commented Dec 12, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

MegMll commented Dec 16, 2024

abussy-aldebaran commented Dec 26, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

EtienneAr commented Jan 23, 2025 • edited by MegMll Loading

MegMll commented Sep 30, 2024 •

edited

Loading

EtienneAr commented Jan 23, 2025 •

edited by MegMll

Loading