Add $\dot{J}$ function

[Giulero]

We might need the implementation of the derivative of the Jacobian function $\dot{J}$, or at least the term $\dot{J}\nu$, i.e. the acceleration of a frame when the acceleration of the configuration of the robot $\dot{\nu}$ is zero.

[traversaro]

$\dot{J}\nu$ is tipically trivial, you just need to use the forward pass of kinematics that computes acceleration but setting $\dot{\nu}$ to 0. See https://github.com/robotology/idyntree/blob/2cc450a8b07922d5db5c43fc4b723994e9691420/src/high-level/src/KinDynComputations.cpp#L1836 for trhe related iDynTree implementation.

[Giulero]

I was doing some checks in the thesis and in ADAM, and I went back to this issue. Maybe I'm doing something strange.
I want to compute $\dot{J} \nu$, let's say in left trivialized representation.

The velocity of a generic link $i$ with respect to the inertial frame $I$ is

$$ {}^i \mathrm{v} _{I, i} = {}^i \mathrm{v} _{I, B} + {}^i \mathrm{v} _{B, i} \\ = {}^i X_B {}^B \mathrm{v} _{I, B} + {}^i \mathrm{v} _{B, i}. $$

The left trivialized velocity ${}^i \mathrm{v}_{B, i}$ can be computed as the sum of the parent-to-child velocity of all the links between the base and the link $i$ and is given by

$$ \begin{align} {}^i \mathrm{v} _{B, i} & = \sum _{k \in k(i)} {}^i \mathrm{v} _{\lambda(k),k} \\ & = \sum _{k \in k(i)} {}^{i} X _{k} {}^{k} \mathrm{v} _{\lambda(k),k} \\ & = \sum _{k \in k(i)} {}^{i} X _{k} S \dot{s}_k \tag{1} \end{align} $$

where $k(i)$ is the list of links between the base and the link $i$, $S$ the joint motion subspace, $\dot{s} _k$ the k-th joint velocity, and $X$

$$ X = \begin{bmatrix} R & [p] _\times R \\ 0 _{3 \times 3} & R \end{bmatrix}. $$

In matrix form, the left trivialized link velocity can be written as

$$ {}^i \mathrm{v} _{{I}, i} = \begin{bmatrix} {}^i X _B & {}^i J _{B,i}^{\dot{s}} \end{bmatrix} \begin{bmatrix} {}^B \mathrm{v} _{I, B} \\ \dot{s} \end{bmatrix} = {}^i J _{I,i}^B {}^B\nu $$

This computation is actually the Jacobian one and the computations that I do return the same results iDynTree does.
Everything's right.

If I want to move to the link acceleration, when $\nu = 0$, I could differentiate the previous equations. ${}^i \mathrm{\dot{v}} _{B, i}$ is

$$ {}^i \mathrm{\dot{v}} _{B, i} = \sum _{k \in k(i)} {}^{i} \dot{X} _{k} S \dot{s}_k $$

And the acceleration whole ${}^i \mathrm{\dot{v}} _{{I}, i}$ equal to $\dot{J} \nu$

$$ {}^i \mathrm{\dot{v}} _{{I}, i} = {}^i \dot{X} _B {}^B \mathrm{v} _{I, B} + \sum _{k \in k(i)} {}^{i} \dot{X} _{k} S \dot{s}_k $$

where

$$ \dot{X} = \begin{bmatrix} \dot{R} & [\dot{p}] _\times R + [p] _\times \dot{R} \\ 0 _{3 \times 3} & \dot{R} \end{bmatrix}, $$

where I'm using the velocity quantities computed at each step in (1).

Now, when I compute $\dot{J}\nu$ vith zero base velocity and compare it to iDynTree, I obtain the same result only for the angular component, i.e., the last 3 elements of the vector. Since the motion subspace $S = [0 0 0 1 1 1]^\top$, only the quantities on the right part of $\dot{X}$ are used. Since the angular part is right, the term $\dot{R} = [\omega]_\times R$ should be right.
It seems that I'm messing up something in the term $[\dot{p}] _\times R + [p] _\times \dot{R}$.

Also, when ONLY the joint velocities are zero the link velocity is

$$ {}^i \mathrm{\dot{v}} _{{I}, i} = {}^i \dot{X} _B {}^B \mathrm{v} _{I, B} $$

However, iDynTree returns a zero vector. I believe that I did some wrong manipulation in the equations.

[Giulero]

Found it!

$\dot{p}$ in $([\dot{p}] _\times R + [p] _\times \dot{R})$ is equal to

$$ \dot{p} = v - p \times \omega $$

and gives me the same result as iDynTree.
But I still have to set the acceleration term relative to the base equal to zero.

[traversaro]

It is not clear to me if you are trying to implement $\dot{J}\nu$ or $\dot{J}$.

[Giulero]

I'm trying to implement $\dot{J}\nu$! But using this procedure I could also implement $\dot{J}$ I guess.

[traversaro]

Ok! So I am a bit confused by:

But I still have to set the acceleration term relative to the base equal to zero.

To compute $\dot{J} \nu$ you need to set the acceleration of both base and joint to zero, no?

I do not know if the confusion is related to that, but in the past when working on this something that confused me a lot is that zero acceleration in one convention (like left-trivialized) is not zero acceleration in another convention, see the code in https://github.com/robotology/idyntree/blob/2cc450a8b07922d5db5c43fc4b723994e9691420/src/high-level/src/KinDynComputations.cpp#L430 .

[Giulero]

Uh yeah, wrong choice of words 🥲 !

I mean that this term

$$ {}^i \mathrm{\dot{v}} _{{I}, i} = {}^i \dot{X} _B {}^B \mathrm{v} _{I, B} $$

should be set to zero to have the same result from iDynTree.

zero acceleration in one convention (like left-trivialized) is not zero acceleration in another convention

I see, I'm gonna check this also!