Passivity-based RNEA Algorithms

[Cfather]

This pull request corresponds to Discussion #2406.

Motivation

which introduces a modified RNEA algorithm for passivity-based controllers. To be more specific, the passivity-based RNEA algorithm computes the following term:
$$\tau = M(q) \ddot{q}_r + C(q, \dot{q}) \dot{q}_r + g(q).$$
Here $\dot{q}_r = \dot{q}_d + K_r (q_d - q)$ is defined as auxiliary velocity, where $q_d$ is the desired position and $\dot{q}_d$ is the desired velocity. $K_r$ is a positive definite diagonal matrix, that serves as feedback gains.
A more detailed introduction to passivity-based controllers can be found in Section 6.5.3 in Springer Handbook of Robotics.

Changes

The main difference between this and the original RNEA is that it needs to plug in two different velocities in the Coriolis vector. Here are the changes:

1. A new function called passivityRNEA is introduced in include/pinocchio/algorithm/rnea.hpp. The implementation is in include/pinocchio/algorithm/rnea.hxx.
2. A vector of MotionTpl called v_r is introduced in include/pinocchio/multibody/data.hpp, which corresponds to auxiliary joint velocities expressed in the local frame of the joint.
3. Two new unit tests are introduced in unittest/rnea.cpp to verify the correctness of passivityRNEA. One test compares between passivityRNEA and rnea when the auxiliary joint velocities are equal to joint velocities. The other test compares between passivityRNEA and computeCoriolisMatrix to ensure Christoffel-consistent factorization inside passivityRNEA.

Notes

1. There are several options to implement the body-level Coriolis factorization. I chose the one mentioned in Christoffel-consistent factorization and covariant derivatives #1663 but I also left the other option (the most straightforward one) in the comments.
2. I'm not familiar with the convention in Pinocchio. Please let me know if you need to change the names of the variables or the functions. Or you can edit the changes yourself on this branch.

Future Work

It might be useful to also implement a passivity-based regressor for inverse dynamics. To be more specific, we can introduce a passivity-based regressor $Y(q, \dot{q}, \dot{q}_r, \ddot{q}_r)$ so that $Y(q, \dot{q}, \dot{q}_r, \ddot{q}_r)\pi$ is equal to the passivityRNEA output, where $\pi$ is the dynamic parameters of the robot.
A Matlab version of the code can be found at spatial_v2_extended, provided by @pwensing.

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

Where is option 2? We might also add all the conventions and use a switch case to select the most appropriate one if needed.

[Cfather]

Yeah I can add option 2 with more comments.

[Cfather]

I have added option 2 but commented both option 1 and 2 out. not sure what's the best way to incorporate the switch case. Maybe add the factorization option inside the template of this algorithm?
option 1 and option 2 will pass test_passivityrnea_vs_rnea test but not test_passivityrnea_compute_coriolis test since they are not consistent with the implementation in the Coriolis matrix.
option 3 will pass both tests.

[jcarpent]

We should rather try to not duplicate code if possible.

[Cfather]

Yep, I just figured out how to deal with that.

[Cfather]

I reused addForceCrossMatrix inside passivityRNEA. However, I have to change the order so that passivityRNEA appears after computeCoriolisMatrix.

[jcarpent]

Thanks @Cfather for adding these features to Pinocchio. It definitely makes sense to have them in Pinocchio.

[Cfather]

Hi, @jcarpent, apologize for pestering you, is there anything else I can do for this pull request?