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

robotology / idyntree

Multibody Dynamics Library designed for Free Floating Robots

BSD 3-Clause "New" or "Revised" License

172 stars 67 forks source link

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

Open traversaro opened 3 years ago

traversaro commented 3 years ago

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:

https://github.com/robotology/idyntree/blob/d5bbceab9e8ae52943b31199f6e024d12ff52159/src/model/src/Dynamics.cpp#L149 The resulting matrix is then processed in https://github.com/robotology/idyntree/blob/d5bbceab9e8ae52943b31199f6e024d12ff52159/src/high-level/src/KinDynComputations.cpp#L1816 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 commented 3 years ago

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

traversaro commented 3 years ago

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 https://github.com/stack-of-tasks/pinocchio/commit/69ccceac97ebd34722723d3c753d359783e33c93, but I am not sure if that one was implemented from some kind of written reference, perhaps @jcarpent has some reference for that.

jcarpent commented 3 years ago

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 commented 3 years ago

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

traversaro commented 3 years ago

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 commented 3 years ago

Cool!

GiulioRomualdi commented 2 years ago

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

traversaro commented 2 years ago

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

diegoferigo commented 3 months ago

FYI I finalized a pretty slow implementation of the Coriolis matrix (i.e. not implemented on the kinematic tree) in https://github.com/ami-iit/jaxsim/pull/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.