Add frame Jacobian derivative

[xela-95]

For the calculation of the acceleration of a frame, it would be useful if Jaxsim could calculate the Jacobian derivative for frames, as it already does for links.

[xela-95]

Analytical derivation

Here are summarised the equations that lead to the computation of the Jacobian derivative. We call $W$ the inertial frame, $F$ the Frame for which we want to compute this quantity and with $L$, the link to which the frame $F$ is attached to. Finally, with $O$, we indicate the output representation into which the Jacobian derivative has to be expressed.

Since, ${} ^C \dot H _L = 0 _4$ by definition, it is possible to demonstrate that

$${} ^W \mathbf{v} _{W, F} = {} ^W \mathbf{v} _{W, L}$$

Hence:

$${} ^O J _{W, F/I} = {} ^O X _W \ {} ^W J _{W, L/W} {} ^W T _I \\$$

, where $O$ is the output representation, $I$ is the input one and ${} ^W T _I = diag({} ^W X _I, I _n)$.

$$\begin{align} {} ^O \dot J _{W, F/I} &= \frac{d}{dt} ({} ^O X _W \ {} ^W J _{W, L/W} {} ^W T _I) \\\ &= {} ^O \dot X _W \ {} ^W J _{W, L} {} ^W T _I + {} ^O X _W \ {} ^W \dot J _{W, L/W} {} ^W T _I + {} ^O X _W \ {} ^W J _{W, L/W} {} ^W \dot T _I \end{align}$$

Note that:

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

Input representation

• Inertial ($I=W$):

$$\begin{align} {} ^W T _I &= {} ^W T _W \leftarrow {} ^W X _W \leftarrow {} ^W H _W = I _4 \\\ {} ^W \dot T _I &= {} ^W \dot T _W \leftarrow {} ^W \dot X _W = 0 _6 \end{align}$$

• Body (Base) ($I=B$):

$$\begin{align} {} ^W T _I &= {} ^W T _B \leftarrow {} ^W X _B \leftarrow {} ^W H _B \\\ {} ^W \dot T _I &= {} ^W \dot T _B \leftarrow {} ^W \dot X _B = {} ^W X _B {} ^B \mathbf{v} _{W, B \times} \end{align}$$

• Mixed ($I=B[W]$):

$$\begin{align} {} ^W T _I &= {} ^W T _B[W] \leftarrow {} ^W X _B[W] \leftarrow {} ^W H _B[W] \\\ {} ^W \dot T _I &= {} ^W \dot T _{B[W]} \leftarrow {} ^W \dot X _{B[W]} = {} ^W X _{B[W]} {} ^{B[W]} \mathbf{v} _{W, B[W] \times} \end{align}$$

Output representation

• Inertial ($O=W$):

$$\begin{align} {} ^O X _W &= I _6\\\ {} ^W \mathbf{v} _{W, W \times} &= 0 _{6 \times 6} \end{align}$$

• Body ($O=F$):

$$\begin{align} {} ^O X _W &= {} ^F X _W \leftarrow {} ^F H _W = \begin{bmatrix} {} ^F R _W & {} ^F o _W \\\ 0 _{1 \times 3} & 1 \end{bmatrix}\\\ {} ^W \mathbf{v} _{W, O \times} &= {} ^W \mathbf{v} _{W, F \times} \leftarrow {} ^W \mathbf{v} _{W, F} = {} ^W J _{W,F} \nu \end{align}$$

• Mixed ($O=F[W]$):

$$\begin{align} {} ^O X _{F[W]} &= {} ^{F[W]} X _W \leftarrow {} ^{F[W]} H _W = \begin{bmatrix} I _3 & {} ^F o _W \\\ 0 _{1 \times 3} & 1 \end{bmatrix}\\\ {} ^W \mathbf{v} _{W, O \times} &= {} ^W \mathbf{v} _{W, F[W] \times} \leftarrow {} ^W \mathbf{v} _{W, F[W]} = \begin{bmatrix} {} ^W \dot o _F \\\ 0 _{3 \times 1} \end{bmatrix} \end{align}$$

where for coinciseness I indicated with $\leftarrow$ the fact that the left quantity is computed applying classic operations from the right quantity.

[xela-95]

@diegoferigo @flferretti please double check the math above when you have time :)

[flferretti]

Thanks a lot for your work @xela-95! It looks good to me overall, I just have a couple doubts.

${} ^O X _W = I _4$

I guess it should be ${} ^O X _W = I _6$

${} ^W \mathbf{v} _{W, W \times} = 0 _6$

It should probably be a ${} ^W \mathbf{v} _{W, W \times} = 0 _{6 \times 6}$, but I guess you wanted to indicate exactly that.

$${} ^F H _W \begin{bmatrix} I _3 & {} ^F o _W \\ 0 _{3 \times 1} & 1 \end{bmatrix}$$

I'm not sure whether an equal sign is missing and I guess that the zero should be a $0 _{1 \times 3}$ (same goes for the mixed):

$${} ^F H _W \color{red}{=} \begin{bmatrix} I _3 & {} ^F o _W \\ 0 _{1 \times 3} & 1 \end{bmatrix}$$

[xela-95]

Thanks a lot @flferretti! I updated the math accordingly.

[diegoferigo]

@xela-95 I double checked your calculations and they look correct also to me. As I suggested last time, I recommend to compute for body-fixed and mixed representations the transforms ${}^W \mathbf{H}_F$ and ${}^W \mathbf{H}_{F[W]}$ first, and then invert them with jaxsim.math.Transform.inverse. It's quite common in the implementation to forget the rotation term necessary to compute ${}^F \mathbf{p}_W$, it's easier to just get ${}^W \mathbf{p}_F$ from FK and invert the transform with the helper (that is better wrt jnp.linalg.inv because it does not invert anything, it just relies on transposition and sign inversion).

[xela-95]

Since the test against AD was passing for inertial and body representation and not for mixed, I double checked the formulation to compute ${} ^W \mathbf{v} _{W, F[W]}$.

Since the definition of right-trivialized velocity is:

$${} ^A \mathbf{v} _{A, B} = \begin{bmatrix} {} ^A \dot o _B - {} ^A \dot R _B {} ^A R _B ^T {} ^A o _B \\\ {} ^A \dot R _B {} ^A R _B ^T \end{bmatrix}$$

if instead of $B$ we use $B[A]$ we get:

$${} ^A \mathbf{v} _{A, B[A]} = \begin{bmatrix} {} ^A \dot o _B - {} ^A \dot R _A {} ^A R _A ^T {} ^A o _B \\\ {} ^A \dot R _A {} ^A R _A ^T \end{bmatrix} = \begin{bmatrix} {} ^A \dot o _B \\\ 0 _{3 \times 1} \end{bmatrix}$$

hence this kind of velocity has the linear part of the mixed velocity ${} ^{B[A]} \mathbf{v} _{A,B}$ and zero as angular component.

[diegoferigo]

In view of #210 (comment), would it be helpful to summarize publicly the theory we developed for the computation of the frame Jacobian? And I mean, the formulation that includes the three components of the jacobian derivative.

[xela-95]

In view of #210 (comment), would it be helpful to summarize publicly the theory we developed for the computation of the frame Jacobian? And I mean, the formulation that includes the three components of the jacobian derivative.

Yep absolutely, I'm doing it right now

[diegoferigo]

I just remembered that I wrote the approach in #169 since it was necessary also for the computation of $\dot{J}$ of the link frames. Maybe you can take inspiration from that and just write waht are the differences in the case of frames that are rigidly attached to links.

In other words, we compute the derivative of the jacobian of a link frame ${}^O \dot{J}_{W,L/B}$ using a RBDA operating on the kinematic tree that computes the full jacobian derivative ${}^B \dot{J}_{W, _/B}$. The three-derivative formulation is used there for obtaining the real link jacobian in the three supported velocity representations. A similar processing is needed for frames rigidly attached to links, as this issue and the corresponding PR implement.

[xela-95]

Just updated the mathematical description in #208 (comment)