Add algorithm to compute the Jacobian derivative of a link

[diegoferigo]

This PR adds a new RBDA to compute the derivative of the free-floating Jacobian $\dot{J}_{W,L}$. If:

• $L$ is the frame of the link,
• $I$ is the frame of the input velocity representation used by the generalized velocity ${}^I \boldsymbol{\nu}$,
• $O$ is frame of the output velocity representation corresponding to the result of the product ${}^O \dot{J}_{W,L/I} \; {}^I \boldsymbol{\nu}$,

we first compute the following full Jacobian derivative (with all columns filled, similarly to what was done in #121):

$${}^B \dot{J}_{W,\_/B} \in \mathbb{R}^{6\times(6+n)}$$

and then mask the joint-related columns using the support parent array $\kappa(i)$ of the i-th link associated to the frame $L$:

$${}^B \dot{J}_{W,L/B} = M(\kappa(i)) \; {}^B \dot{J}_{W,\_/B}$$

The mask in practice sets to zero all the joint-related columns corresponding to the chains not supporting the link $L$, i.e. not in the path $\pi_B(L)$.

All of this is computed with the new jaxsim.rbda.jacobian.jacobian_derivative_full_doubly_left function.

This is not enough, since we are interested to compute quantities like $\dot{J} \boldsymbol{\nu}$. Note that in this specific case, we already have this product computed by #127, it's only an use-case example. Though, we also need to change the output and input velocity representations to something else in order to support body-fixed, mixed, and inertial-fixed variants. Therefore, we need to properly convert ${}^B \dot{J}_{W,L/B}$ to a generic ${}^O \dot{J}_{W,L/I}$.

The most generic formulation of the change of representations of the free-floating Jacobian is the following¹:

$${}^O J_{W,L/I} = {}^O \mathbf{X}_B \; {}^B J_{W,L/B} \; \text{diag}({}^B \mathbf{X}_I,\: \mathbf{I}_n)$$

Therefore, if we want the derivative ${}^O \dot{J}_{W,L/I}$, we need to also consider the derivatives of the different $\mathbf{X}$ matrices. The jaxsim.api.link.jacobian_derivative accounts for them. For this, we can use the following relation:

$${}^A \dot{\mathbf{X}}_B = {}^A \mathbf{X}_B \; {}^B \mathbf{v}_{A,B} \times$$

Finally:

$$\begin{align} {}^O \dot{J}_{W,L/I} =& {}^O \dot{\mathbf{X}}_B \; {}^B J_{W,L/B} \; \text{diag}({}^B \mathbf{X}_I,\: \mathbf{I}_n) \\\ +& {}^O \mathbf{X}_B \; {}^B \dot{J}_{W,L/B} \; \text{diag}({}^B \mathbf{X}_I,\: \mathbf{I}_n) \\\ +& {}^O \mathbf{X}_B \; {}^B J_{W,L/B} \; \text{diag}({}^B \dot{\mathbf{X}}_I,\: \mathbf{0}_n) \end{align}$$

where $I \in \{W, \, B,\, B[W]\}$ and $O \in \{W, \, L,\, L[W] \}$.

---

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

Footnotes

1. Diego Ferigo, Eq (2.28) pag. 67, Simulation Architectures for Reinforcement Learning applied to Robotics, Ph.D. thesis, URL. ↩

[diegoferigo]

cc @Giulero

[diegoferigo]

FYI @DanielePucci

[flferretti]

Great, thanks Diego! LGTM

[traversaro]

I am mostly travelling today and this PR is math heavy, so feel free to go ahead!

[DanielePucci]

TL;DR Nice @diegoferigo! Just a curiosity: I imagine that we have a test checking the time derivative w.r.t. $J(t)$?

[diegoferigo]

Just a curiosity: I imagine that we have a test checking the time derivative w.r.t. J(t)?

Since I implemented in #127 the computation of the product $(\dot{J} \boldsymbol{\nu}) \in \mathbb{R}^6$ in all the supported velocity representations, this PR includes a test that checks that such value is equal to the multiplication of the new $\dot{J} \in \mathbb{R}^{6\times(6+n)}$ with $\boldsymbol{\nu} \in \mathbb{R}^{6+n}$.

Out of curiosity, I took advantage of your question to check if we can compute $\dot{J}$ using AD instead of its analytical form. And it seems working fine 🎉 I will extend the test in a new PR to add this check. It can be an interesting entrypoint for all those that want to exploit AD to compute gradients of quantities whose analytical form is too complicated to obtain.

[DanielePucci]

Super, thanks @diegoferigo !