Implement algorithm to compute the C Coriolis Matrix such that dotM = C + C^T

[traversaro]

It is useful to compute the Coriolis matrix in a numerical efficient manner. Two initial references are :

• On the closed form computation of thedynamic matrices and their differentiations
• Numerical Methods to Compute the CoriolisMatrix and Christoffel Symbols for Rigid-BodySystems.

For reference, the code to compute the mass matrix (for floating base systems, and using the left-trivialized base velocity) is:

• idyntree/src/model/src/Dynamics.cpp

  Line 149 in d5bbcea

  bool CompositeRigidBodyAlgorithm(const Model& model,

  
  The resulting matrix is then processed in

  idyntree/src/high-level/src/KinDynComputations.cpp

  Line 1816 in d5bbcea

  bool KinDynComputations::getFreeFloatingMassMatrix(MatrixDynSize& freeFloatingMassMatrix)

  to support also the possibility to express the base velocity as right-trivialized or mixed convention (that is the default that we typically use).
  This code basically implements the CRBA algorithm you can find in Featherstone RBDA book, but adapted for floating base systems.

[traversaro]

cc @singhbal-baljinder @robotology/iit-dynamic-interaction-control

[traversaro]

Apparently an algorithm similar to the one published in Numerical Methods to Compute the CoriolisMatrix and Christoffel Symbols for Rigid-BodySystems. was added in Pinocchio 3 years ago in stack-of-tasks/pinocchio@69cccea, but I am not sure if that one was implemented from some kind of written reference, perhaps @jcarpent has some reference for that.

[jcarpent]

If you want, this feature is already implemented in Pinocchio: https://github.com/stack-of-tasks/pinocchio/blob/820d0f85fbabddce20924a6e0f781fb2be5029e9/src/algorithm/rnea.hpp#L152.
I did that for the analytical derivatives at the beginning ;)

[jcarpent]

Oh, amazing, we had the same thoughts. I don't have any reference, but I may write a technical note if you want.

[traversaro]

Oh, amazing, we had the same thoughts. I don't have any reference, but I may write a technical note if you want.

Thanks @jcarpent for the quick reply! Don't worry, I think the code is enough as a reference. Writing it down with proper notation may be a perfect exercise for some of our students. : )

[jcarpent]

Cool!

[GiulioRomualdi]

I can try to tackle the problem, however I think I need a short meeting with @traversaro to better discuss the possible approaches

[traversaro]

Sure, let's schedule it when you prefer using the Teams' scheduling helper.

[diegoferigo]

FYI I finalized a pretty slow implementation of the Coriolis matrix (i.e. not implemented on the kinematic tree) in ami-iit/jaxsim#172. It's not useful for real-time computation (unless 500 µs for 20 DoFs is acceptable), but at least it can provide a ground truth in all three velocity representations (inertial-fixed, body-fixed, and mixed) if anyone is willing to explore/implement more efficient solutions.