Template Function pinocchio::Jlog6

Function Documentation

template<typename Scalar, int Options, typename Matrix6Like>
void pinocchio::Jlog6(const SE3Tpl<Scalar, Options> &M, const Eigen::MatrixBase<Matrix6Like> &Jlog)

Derivative of log6.

This function is the right derivative of log6, that is, for \(M \in SE(3)\) and \(\xi in \mathfrak{se}(3)\), it provides the linear approximation:

\[ \log_6(M \oplus \xi) = \log_6(M \exp_6(\xi)) \approx \log_6(M) + \text{Jlog6}(M) \xi \]

Equivalently, \(\text{Jlog6}\) is the right Jacobian of \(\log_6\):

\[ \text{Jlog6}(M) = \frac{\partial \log_6(M)}{\partial M} \]

Note that this is the right Jacobian: \(\text{Jlog6}(M) : T_{M} SE(3) \to T_{\log_6(M)} \mathfrak{se}(3)\). (By convention, calculations in Pinocchio always perform right differentiation, i.e., Jacobians are in local coordinates (also known as body coordinates), unless otherwise specified.)

Internally, it is calculated using the following formulas:

\[\begin{split} \text{Jlog6}(M) = \left(\begin{array}{cc} \text{Jlog3}(R) & J * \text{Jlog3}(R) \\ 0 & \text{Jlog3}(R) \\ \end{array}\right) \end{split}\]
where
\[\begin{split} M = \left(\begin{array}{cc} \exp(\mathbf{r}) & \mathbf{p} \\ 0 & 1 \\ \end{array}\right) \end{split}\]
\[\begin{split} \begin{eqnarray} J &=& \left.\frac{1}{2}[\mathbf{p}]_{\times} + \beta'(||r||) \frac{\mathbf{r}^T\mathbf{p}}{||r||}\mathbf{r}\mathbf{r}^T - (||r||\beta'(||r||) + 2 \beta(||r||)) \mathbf{p}\mathbf{r}^T\right.\\ &&\left. + \mathbf{r}^T\mathbf{p}\beta(||r||)I_3 + \beta(||r||)\mathbf{r}\mathbf{p}^T\right. \end{eqnarray} \end{split}\]
and
\[ \beta(x)=\left(\frac{1}{x^2} - \frac{\sin x}{2x(1-\cos x)}\right) \]

\cheatsheet For \((A,B) \in SE(3)^2\), let \(M_1(A, B) = A B\) and \(m_1 = \log_6(M_1) \). Then, we have the following partial (right) Jacobians:

  • \( \frac{\partial m_1}{\partial A} = Jlog_6(M_1) Ad_B^{-1} \),

  • \( \frac{\partial m_1}{\partial B} = Jlog_6(M_1) \).

\cheatsheet Let \(A \in SE(3)\), \(M_2(A) = A^{-1}\) and \(m_2 = \log_6(M_2)\). Then, we have the following partial (right) Jacobian:

  • \( \frac{\partial m_2}{\partial A} = - Jlog_6(M_2) Ad_A \).