Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Extend Support for Mimic Joint #2441

Open
wants to merge 166 commits into
base: devel
Choose a base branch
from
Open

Conversation

MegMll
Copy link
Collaborator

@MegMll MegMll commented Sep 30, 2024

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.

Copy link

@github-actions github-actions bot left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

👋 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.

Copy link
Contributor

@jcarpent jcarpent left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I will provide a detailed review later.

@abussy-aldebaran
Copy link
Contributor

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

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe you could add typedef for mimic joints ?

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

: mimiced_name(_mimiced_name)
, multiplier(_multiplier)
, offset(_offset)
, axis(_axis)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

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());
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Megane Millan added 28 commits December 16, 2024 11:39
… to DEFINITION for transformJointIntoMimic
@MegMll
Copy link
Collaborator Author

MegMll commented Dec 16, 2024

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 !

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants