Add algorithm to compute the free-floating Coriolis matrix

[diegoferigo]

This PR introduces a new function to compute the free-floating Coriolis matrix $C(\mathbf{q}, \boldsymbol{\nu}) \in \mathbb{R}^{(6+n)\times(6+n)}$:

• The Coriolis matrix is not computed as a rigid body dynamics algorithm, as other quantities of the equations of motions. Instead, it computes the matrix for body-fixed velocity representation by using the following equation¹:

• Then, it converts the body-fixed representation to either inertial-fixed or mixed representations using²:

Note that this PR does not compute $C$ using iterative algorithms, therefore its computation can be pretty slow, especially for models with many degrees of freedom. In particular, converting the body-fixed Coriolis matrix to either inertial-fixed or mixed requires the computation of the mass matrix $M$, that means also a call of CRBA.

Nonetheless, it can be useful having at least one implementation, even if not fast. It can be useful to prototype controllers that need the standalone $C$ and, if anyone in the future is willing to propose an iterative algorithm, it can be used as ground thruth.

cc @ami-iit/vertical_control-oriented-learning

---

📚 Documentation preview 📚: https://jaxsim--172.org.readthedocs.build//172/

Footnotes

1. Silvio Traversaro, Eq. (3.58b) pag. 54, Modelling, Estimation, and Identification of Humanoid Robots Dynamics, Ph.D. thesis, URL. ↩

2. Silvio Traversaro, Eq. (3.60b) pag. 56, Modelling, Estimation, and Identification of Humanoid Robots Dynamics, Ph.D. thesis, URL. ↩

[diegoferigo]

Here below a quick benchmark performed in body-fixed on my laptop with JAX running on CPU:

DoFs	JIT compilation	Runtime
5	3.45 s	111 µs ± 9.68 µs
10	3.65 s	419 µs ± 130 µs
20	3.71 s	577 µs ± 280 µs
40	4.20 s	1.35 ms ± 225 µs

Note that in other velocity representations, there's the extra overhead of computing $M$.

[flferretti]

Thanks a lot Diego! I just left a couple of comments

[flferretti]

Given the maximum magnitude of the force applied and the mass of the box, isn't this a bit too permissive?

[diegoferigo]

Here below the numbers of the failed test that triggered the increase of tolerance.

2024-06-10T12:49:55.8041107Z E       assert equals failed
2024-06-10T12:49:55.8041602Z E         �-Array([0.11897, 0.42528, 1.3108�  �+approx([0.11894265258451964 ± 1� 
2024-06-10T12:49:55.8042215Z E         �-7], dtype=float64)�               �+.2e-05, 0.42517935733540135 ± 4� 
2024-06-10T12:49:55.8042731Z E                                          �+.3e-05, 1.3109064638266603 ± 1.� 
2024-06-10T12:49:55.8043176Z E                                          �+3e-04])�

I had to increase the minimum error to 0.001 that is large but still ok. Not sure if you had a look at taking smaller steps in this test, I believe that the problem is not the tolerance but how the ground truth is computed.

[flferretti]

Alright, we can handle this in a different PR. It's strange that the tests were passing before this