Fix body task Jacobian

[stephane-caron]

@aescande pointed out that the Jacobian calculation in the body task is wrong, following a mistake we can find in several existing inverse kinematics.

Current implementation

Currently the Jacobian of the task is computed as the local Jacobian of the body frame (using getFrameJacobian in Pinocchio):

$$ J(q) := {}b J{0b}(q) = \frac{\partial T{0b}(q)}{\partial q} = \lim{\tau \to 0} \frac{T{0b}(q \oplus \tau) \ominus T{0b}(q)}{\tau} = \left.\frac{\partial \log (T{b0}(q) \cdot T{0b}(q \oplus \exp(\tau)))}{\partial \tau} \right|_{\tau=0} $$

where $0$ denotes the inertial frame and $b$ the body frame. We use the conventions (right derivatives, which is why ${}_b J_{0b}$ is local) and notations from the micro Lie theory: $\oplus$ is necessarily the right-plus, and $\ominus$ is the right-minus operator. The local Jacobian is the right Jacobian in micro-Lie terminology (see equation (41a) of the writeup).

Correct derivation

However, the task error, i.e. the function we want to bring to zero, is:

$$ e(q) := {}b \xi{0b} = T{0t} \ominus T{0b} = \log(T{b0} T{0t}) = \log(T_{bt}) $$

Formally, the error is a function $e : \mathcal{C} \to T_q \mathcal{C}$ from the configuration space $\mathcal{C}$ (a manifold) to its tangent space $T_q \mathcal{C}$ (also a manifold) at $q$.

Note how we are using a local rather than global error. Its derivative is the local task Jacobian. Differentiation on manifolds is a bit verbose so brace yourselves (a again see the micro Lie theory for support):

$$ \begin{align} J(q) & := \frac{\partial e}{\partial q} \ & = \frac{\partial \log(T{bt})}{\partial q} \ & = \frac{\partial \log(T{b0} T{0t})}{\partial q} \ & = \frac{\partial \log(T{b0} T{0t})}{\partial T{b0}} \frac{\partial T{b0}}{\partial T{0b}} \frac{\partial T_{0b}}{\partial q} \end{align} $$

At this point, we can use the Jlog6 function of Pinocchio for the right Jacobian of the logarithm $\log_6 : SE(3) \to se(3)$. The documentation of this function recalls a number of useful identities. Let's use those:

$$ \begin{align} \frac{\partial \log(AB)}{\partial A} & = \text{Jlog}6(AB) Ad{A}^{-1} & \frac{\partial M^{-1}}{\partial M} & = -Ad_{M} \end{align} $$

In our context, it leads us to:

$$ \begin{align} \frac{\partial e}{\partial q} & = \text{Jlog}6(T{bt}) Ad^{-1}{T{0t}} (-Ad{T{0b}}) \frac{\partial T_{0b}}{\partial q} \ & = -\text{Jlog}6(T{bt}) Ad{T{tb}} \frac{\partial T_{0b}}{\partial q} \end{align} $$

Thus $J(q) = -\text{Jlog}_6(T_{bt}) Ad_{T_{tb}} {}_b J_{0b}(q)$.

[stephane-caron]

In relation to this PR, see also the jlog example by @Gregwar.

[stephane-caron]

@aescande The Jacobian above, $J(q) = -\text{Jlog}_6(T_{bt}) Ad_{T_{tb}} {}_b J_{0b}(q)$, is not the one implemented in this PR (but it matches equation (82) from the micro Lie theory writeup), although empirically the code from the current commit 02b7a2c603756b756644bb748c4820362305e8b2 "works" :sweat_smile:

I don't see an identity between $-\text{Jlog}_6(T_{bt}) Ad_{T_{tb}}$ and $\text{Jlog}_6 (T_{tb})$ but perhaps there is a way to relate the two?

[stephane-caron]

@aescande Checked with finite differences https://github.com/tasts-robots/pink/pull/23/files this is correct :ok_hand:

I don't see an identity between $-\text{Jlog}_6(T_{bt}) Ad_{T_{tb}}$ and $\text{Jlog}_6 (T_{tb})$ but perhaps there is a way to relate the two?

Actually the two are related :slightly_smiling_face: I have to run now, I will write down the proof later (signed: Fermat).

[stephane-caron]

So, the identity is actually:

$$ \text{Jlog}(X^{-1}) Ad_X = \text{Jlog}(X) $$

It is a consequence of $\log(X^{-1}) = -\log(X)$ or equivalently $\exp(-\tau) = \exp(\tau)^{-1}$. Differentiating the former yields:

$$ \begin{align} \log(X^{-1}) & = -\log(X) \ \frac{\partial \log(X^{-1})}{\partial X} & = -\frac{\partial \log(X)}{\partial X} \ -\text{Jlog}(X^{-1}) Ad_X & = -\text{Jlog}(X) \end{align} $$

where we used again the differential of $\log(X^{-1})$ as above.

[stephane-caron]

Circling back, we checked analytically and numerically (finite differences) that the body task Jacobian is correct.

Using the direct formula $J(q) = \text{Jlog}(T_{tb}) {}_b J_{0b}(q)$ in fine as the other variant has an extra adjoint matrix product.