Closed diegoferigo closed 1 month ago
cc @Giulero
FYI @DanielePucci
I am mostly travelling today and this PR is math heavy, so feel free to go ahead!
TL;DR Nice @diegoferigo! Just a curiosity: I imagine that we have a test checking the time derivative w.r.t. $J(t)$?
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 :tada: 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.
Super, thanks @diegoferigo !
This PR adds a new RBDA to compute the derivative of the free-floating Jacobian $\dot{J}_{W,L}$. If:
we first compute the following full Jacobian derivative (with all columns filled, similarly to what was done in #121):
and then mask the joint-related columns using the support parent array $\kappa(i)$ of the i-th link associated to the frame $L$:
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[^1]:
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:Finally:
where $I \in \{W, \, B,\, B[W]\}$ and $O \in \{W, \, L,\, L[W] \}$.
[^1]: Diego Ferigo, Eq (2.28) pag. 67, Simulation Architectures for Reinforcement Learning applied to Robotics, Ph.D. thesis, URL.
📚 Documentation preview 📚: https://jaxsim--169.org.readthedocs.build//169/