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 (ccrba)
- Reachable Workspace

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.

Current TODO list

Following our IRL meeting, here is the todo list:

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 · 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;

Why ? There is no joint typedef in model.hpp

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 ?

…mic in a model

include/pinocchio/algorithm/rnea.hxx

jorisv · 2025-02-07T16:51:33Z

include/pinocchio/algorithm/jacobian.hxx

@@ -149,6 +149,7 @@ namespace pinocchio
      PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6);

      Matrix6xLikeOut & Jout_ = Jout.const_cast_derived();
+      Jout_.setZero();


Why setZero here ? It should be done in the part that will do a +=.

jorisv · 2025-02-10T13:59:49Z

include/pinocchio/multibody/data.hpp

@@ -324,6 +324,10 @@ namespace pinocchio
    /// \brief End index of the Joint motion subspace
    std::vector<int> end_idx_v_fromRow;

+    /// \brief Extended model mapping of the joint rows (idx_v_extended_fromRow[idx_v_extended] =
+    /// idx_v)
+    std::vector<int> idx_v_extended_fromRow;


This should be named idx_v_fromRow.

jorisv · 2025-02-10T14:28:46Z

include/pinocchio/algorithm/jacobian.hxx


-          v_out = v_in;
+          v_out += v_in;


Like for CRBA, the += can hurt performances. So it should be done in a dedicated for loop.

jorisv · 2025-02-10T14:33:29Z

include/pinocchio/algorithm/jacobian.hxx

@@ -319,7 +325,7 @@ namespace pinocchio
        data.iMf[parent] = data.liMi[i] * data.iMf[i];

        Matrix6xLike & J_ = J.const_cast_derived();
-        jmodel.jointCols(J_) = data.iMf[i].actInv(jdata.S());
+        jmodel.jointCols(J_) += data.iMf[i].actInv(jdata.S());


Same, we should add a second step in computeJointJacobian to avoid the +=.

jorisv · 2025-02-10T16:07:39Z

bindings/python/algorithm/expose-model.cpp

@@ -83,6 +83,22 @@ namespace pinocchio
      return bp::make_tuple(ancestor_id, index_ancestor_in_support1, index_ancestor_in_support2);
    }

+    template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
+    ModelTpl<Scalar, Options, JointCollectionTpl> transformJointIntoMimic(


You can use the same pattern as findCommonAncestor_proxy, this avoid to specify the full signature in the definition.

jorisv · 2025-02-10T16:09:36Z

bindings/python/algorithm/expose-model.cpp

+      bp::def(
+        "transformJointIntoMimic",
+        (Model(*)(
+          const Model &, const JointIndex &, const JointIndex &, const double &, const double &))(


Use transformJointIntoMimic_proxy to note specify the full signature

bindings/python/parsers/urdf/model.cpp

jorisv · 2025-02-10T16:23:25Z

include/pinocchio/multibody/joint/joint-basic-visitors.hxx

@@ -193,6 +193,32 @@ namespace pinocchio
        PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type, I), update_I));
  }

+  template<typename InputType, typename ReturnType>
+  struct jointConfigExtendedModelSelectorVisitor


jointConfigExtendedModelSelectorVisitor => JointConfigExtendedModelSelectorVisitor

include/pinocchio/parsers/urdf.hpp

…ize and bindigns for mimic_pairs)

jorisv · 2025-02-11T09:41:29Z

include/pinocchio/parsers/urdf/model.hxx

    {
-      return buildModel(filename, rootJoint, "root_joint", model, verbose);
+      return buildModel(filename, rootJoint, "root_joint", model, mimic, verbose);


Suggested change

return buildModel(filename, rootJoint, "root_joint", model, mimic, verbose);

return buildModel(filename, rootJoint, "root_joint", model, verbose, mimic);

Be careful, this error is in a lot of place

jorisv · 2025-02-11T09:50:14Z

include/pinocchio/serialization/joints-model.hpp

+      //      ar & make_nvp("base_class",base_object< pinocchio::JointModelBase<JointType>
+      //      >(joint));


Remove this dead code.

jorisv · 2025-02-11T09:52:22Z

bindings/python/algorithm/expose-model.cpp

@@ -83,6 +83,22 @@ namespace pinocchio
      return bp::make_tuple(ancestor_id, index_ancestor_in_support1, index_ancestor_in_support2);
    }

+    template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
+    ModelTpl<Scalar, Options, JointCollectionTpl> transformJointIntoMimic_proxy(
+      const ModelTpl<Scalar, Options, JointCollectionTpl> & input_model,


You can simplify (like findCommonAncestor) by using context::Model.

transform mimic cannot take context, because it's not cppad compatible

Because of some equality tests ?

I did not have time to investigate, it just won't compile, and since it's in a file with the PINOCCHIO_SKIP_ALGORITHM define, i just thought it made sense to use double.

If we want it with casadi and autodiff, i can add it to the todo list

jorisv · 2025-02-11T13:32:54Z

unittest/casadi/joints.cpp

@@ -290,7 +291,7 @@ struct TestADOnJoints
  void operator()(const pinocchio::JointModelBase<JointModel_> &) const
  {
    JointModel_ jmodel = init<JointModel_>::run();
-    jmodel.setIndexes(0, 0, 0);
+    // jmodel.setIndexes(0, 0, 0, 0);


Remove dead code

jorisv · 2025-02-11T13:33:09Z

unittest/casadi/joints.cpp

-  static void test(const pinocchio::JointModelMimic<JointModel_> & /*jmodel*/)
+  template<typename Scalar, int Options, template<typename, int> class JointCollection>
+  static void
+  test(const pinocchio::JointModelMimicTpl<Scalar, Options, JointCollection> & /*jmodel*/)


Mimic joint are not working with casadi ?

We did the same test as for joint composite

jorisv · 2025-02-11T14:29:25Z

unittest/joint-motion-subspace.cpp

-template<class JointModel>
-struct buildModel<JointModelMimic<JointModel>>
-{
-  typedef JointModelMimic<JointModel> JointModel_;
+// template<class JointModel>
+// struct buildModel<JointModelMimic<JointModel>>
+// {
+//   typedef JointModelMimic<JointModel> JointModel_;

-  static Model run(const JointModel_ & jmodel)
-  {
-    Model model;
-    model.addJoint(0, jmodel.jmodel(), SE3::Identity(), "joint");
-    model.addJoint(0, jmodel, SE3::Identity(), "joint_mimic");
+//   static Model run(const JointModel_ & jmodel)
+//   {
+//     Model model;
+//     model.addJoint(0, jmodel.jmodel(), SE3::Identity(), "joint");
+//     model.addJoint(0, jmodel, SE3::Identity(), "joint_mimic");

-    return model;
-  }
-};
+//     return model;
+//   }
+// };


If this code is not used anymore, delete it.

jorisv · 2025-02-11T14:32:12Z

unittest/joint-motion-subspace.cpp

@@ -429,7 +440,7 @@ struct TestJointConstraint
  void operator()(const JointModelBase<JointModel> &) const
  {
    JointModel jmodel = init<JointModel>::run();
-    jmodel.setIndexes(0, 0, 0);
+    // jmodel.setIndexes(0, 0, 0, 0);


No dead code.

jorisv · 2025-02-11T14:34:26Z

unittest/kinematics.cpp

We can add a test with a mimic joint (like for crba, create the extended model and compare with the mimic one)

jorisv · 2025-02-11T14:43:57Z

unittest/sample-models.cpp

 }

 BOOST_AUTO_TEST_CASE(build_model_sample_manipulator)
 {
  pinocchio::Model model;
-  pinocchio::buildModels::manipulator(model);
+  pinocchio::buildModels::manipulator(model, true);


Like for humanoid random, keep the test without mimic.

jorisv · 2025-02-11T14:46:27Z

unittest/sample-models.cpp

@@ -67,4 +100,71 @@ BOOST_AUTO_TEST_CASE(build_model_sample_humanoid)
   * direct geometry with respect to reference values. */
 }

+BOOST_AUTO_TEST_CASE(compare_mimic)


I don't get the purpose of this test.
It test RNEA, CRBA, nonLinearEffect, forwardKinematics. But these test should already be done in the rnea, crba and kinematics test file.

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 added 17 commits February 4, 2025 13:50

[bindings/sampleModel] Add missing arguments

cce159e

[bindings/model] Expose transformJointIntoMimic

9e374dd

[bindings/joint] Expose new aatributes idx_vExtended and nvExtended

9e21813

[bindings/sampleModel] Coherent with cpp implementation

1a013e0

[joint/Mimic] Add velocitySelector and fix typo

1b4ca4c

[bindings/model] Fix scalar of transformJointIntoMimic

bc59825

[algo/model] Fix reduced model for mimic

2f2e5ee

[algo/model] Remove useless typedef that causes compilation warning

65adbb1

[data] Use nvExtended instead of nv in nvSubtree

5046018

[unittest] Reactivate mimic in reachable workspace

e7d89a2

[unittest] Build mimic model generic

735b807

[unittest] Use generic buildMimic function and add test for double mi…

628a599

…mic in a model

[Common-operation] Assert that it's not possible

ccd0241

[joint/helical] Add affine transform to mimic

76c6ce8

[unittest] Fix acceleration

11998b4

[EtienneAr Feedback] joint-mimic: Remove mutable from joint data

bca6914

[serialization] Fix mimic data serialization

8695187

jorisv requested changes Feb 7, 2025

View reviewed changes

Megane Millan added 5 commits February 10, 2025 09:26

[algo/centroidal] Add comments to explain mimic

0d14165

[algo/rnea] Add comment to explain mimic

6c711ed

[unittest/mimic] Remove test for no transform (since it throws now)

8b6e9f4

Apply pre-commit

1a30833

[Serialization/Data] Fix mimic data serialization

95688f7

jorisv requested changes Feb 10, 2025

View reviewed changes

Megane Millan added 4 commits February 10, 2025 18:45

[parser/urdf] Don't break api

4df852a

[bindings/model] Use proxy

9eebd7e

[model] Iintroduce mimic_joints and mimic_pairs value (missing serial…

a004312

…ize and bindigns for mimic_pairs)

[algo/model] Add new model attributes to transformMimicJoint

017368d

jorisv requested changes Feb 11, 2025

View reviewed changes

[data] Fix nvSubtree

22f87ea

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

jorisv Jan 31, 2025

abussy-aldebaran Dec 13, 2024

MegMll commented Dec 16, 2024

abussy-aldebaran commented Dec 26, 2024

jorisv Feb 7, 2025

jorisv Feb 10, 2025

jorisv Feb 10, 2025

jorisv Feb 10, 2025

jorisv Feb 10, 2025

jorisv Feb 10, 2025

jorisv Feb 10, 2025

jorisv Feb 11, 2025

jorisv Feb 11, 2025

jorisv Feb 11, 2025

MegMll Feb 11, 2025

jorisv Feb 11, 2025

MegMll Feb 11, 2025

jorisv Feb 11, 2025

jorisv Feb 11, 2025

MegMll Feb 11, 2025

jorisv Feb 11, 2025

jorisv Feb 11, 2025

jorisv Feb 11, 2025

jorisv Feb 11, 2025

jorisv Feb 11, 2025

	return buildModel(filename, rootJoint, "root_joint", model, mimic, verbose);
	return buildModel(filename, rootJoint, "root_joint", model, verbose, mimic);

		// ar & make_nvp("base_class",base_object< pinocchio::JointModelBase<JointType>
		// >(joint));

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:

Next

Non linear mimic support

Special Thanks:

Current TODO list

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

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

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

Choose a reason for hiding this comment

MegMll commented Sep 30, 2024 •

edited

Loading