Add semi-implicit Euler scheme with quaternion integrated on manifold

[diegoferigo]

This integrator ensures that the quaternion ${}^W \texttt{Q}_B$ part of the system position $\mathbf{q} = ({}^W \mathbf{p}_B, {}^W \texttt{Q}_B, \mathbf{s}) \in \mathbb{R}^3 \times \text{SO}(3) \times \mathbb{R}^n$ remains unary once integrated.

The scheme is the following fixed-step semi-implicit Euler (where $\mathbf{v}$ and $\mathbf{x}$ are the velocity and position of the floating-base robot w/o contact state):

$$\begin{align} \mathbf{v}[k+1] &= \mathbf{v}[k] + \Delta t \, \text{f}(\mathbf{x}[k], \mathbf{u}[k], t[k]) \\\ \mathbf{x}[k+1] &= \mathbf{x}[k] + \Delta t \, \mathbf{v}[k+1] \\\ \end{align}$$

that, after the classic integration on $\mathbb{R}^{7+n}$, replaces the new four quaternion coefficients with those computed as follows:

$${}^W \texttt{Q}_B[k+1] = {}^W \texttt{Q}_B[k] \otimes \exp(\Delta t \, {}^B \boldsymbol{\omega}_{WB}[k+1])$$

Note that, contrarily to the other fixed-step integrators that can operate on generic pytrees and similarly to the existing vanilla semi-implicit Euler, this new integrator works only when the state is an ODEState object. In fact, we need to know how to separate the position and velocity part of the state for this integrator to work.

cc @YuchuanLu if you use this integrator inside a function $\mathbf{x}[k+1] = \text{F}(\mathbf{x}[k], \mathbf{u}[k], t_k)$ that advances your state and you compute the partial derivatives wrt its inputs, the quaternion part should be handled correctly. And I mean, the quaternion norm is not altered by the integrator.

---

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

[traversaro]

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

This does not work, is that expected?

[diegoferigo]

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

This does not work, is that expected?

Read the docs needs few minutes to finalize building the documentation preview. Refer to the docs/readthedocs.org:jaxsim check below. You need a green dot.

Edit: apparenty the correct link does not have the /en. Maybe the GH action needs to be configured better. Though, if you press Details in the check below, you are redirected towards the right link.

[flferretti]

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

This does not work, is that expected?

Read the docs needs few minutes to finalize building the documentation preview. Refer to the docs/readthedocs.org:jaxsim check below. You need a green dot.

Edit: apparenty the correct link does not have the /en. Maybe the GH action needs to be configured better. Though, if you press Details in the check below, you are redirected towards the right link.

As discussed f2f with @diegoferigo, the attribute project-language has to be most likely set to an empty string in the CI configuration file

[YuchuanLu]

Thank you a lot, @diegoferigo !