[Core] iDynTree::Transform::log() and iDynTree::SpatialMotionVector::exp() computations seems to be wrong?

[prashanthr05]

Here, the log of SE(3) says the linear part is unchanged

idyntree/src/core/src/Transform.cpp

Line 534 in 7733e56

// the linear part is not changed by the log

which does not seem to be coherent with the definition of SE(3) logarithm described in Appendix section B of the paper Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems.

Looks like we are missing a multiplication of the linear part with a left/right Jacobian inverse of $SO(3)$.

where, the left Jacobian of $SO(3)$ is defined as,

$$ J_{l_{SO(3)}} = \sum_{n = 0}^{\infty} \frac{1}{(n+1)!} [\omega_\times]^n = (I_3 + \frac{1 - \text{cos}(||\omega||)}{||\omega||^{2}} [\omega _{\times}] + \frac{||\omega|| - \text{sin}(||\omega||)}{||\omega||^{3}} [\omega _{\times}]^{2})$$ where $\omega$ is the AngularMotionVector3.

When simplified further, $$ J_{l_{SO(3)}} = \frac{\text{sin}(||\omega||)}{||\omega||}I_3 + \frac{1 - \text{cos}(||\omega||)}{||\omega||} [\phi _{\times}] + \bigg(1 - \frac{\text{sin}(||\omega||)}{||\omega||}\bigg) \phi\phi^T$$ where $\phi = \frac{\omega}{||\omega||}$

The inverse left Jacobian is given by,
$$ J^{-1} _{l _{SO(3)}} = \frac{||\omega||}{2} \text{cot} \bigg(\frac{||\omega||}{2}\bigg) I _3 + \bigg( 1 - \frac{||\omega||}{2} \text{cot} \bigg(\frac{||\omega||}{2}\bigg) \bigg) \phi \phi^T - \frac{||\omega||}{2} [\phi _{\times}]$$

cc @traversaro

[prashanthr05]

Similarly, the linear part of the exp() method is missing a multiplication with the Jacobian of SO(3).

idyntree/src/core/src/SpatialMotionVector.cpp

Line 100 in 7733e56

// the linear part is not changed by the exp

This should be,
$$ T = \begin{bmatrix}
I_3 + \frac{(1 - \text{cos}(||\omega||))}{||\omega||^{2}} \omega \omega^{T} + \frac{\text{sin}(||\omega||)}{||\omega ||} \omega _{\times} & (I_3 + \frac{1 - \text{cos}(||\omega||)}{||\omega||^{2}} \omega _{\times} + \frac{||\omega|| - \text{sin}(||\omega||)}{||\omega||^{3}} \omega^{2} _{\times}) v\\
0 _{3\times 1} & 1
\end{bmatrix} $$

where the rotation part is obtained using Rodrigues formula (which is computed correctly using the AxisAngle computations of Eigen.)

[prashanthr05]

The PR #562 was merged.