Add semi-implicit Euler scheme with quaternion integrated on manifold #73
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
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):
that, after the classic integration on$\mathbb{R}^{7+n}$ , replaces the new four quaternion coefficients with those computed as follows:
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/