Template Function pinocchio::Jlog6(const SE3Tpl<Scalar, Options>&)

Function Documentation

template<typename Scalar, int Options>
Eigen::Matrix<Scalar, 6, 6, Options> pinocchio::Jlog6(const SE3Tpl<Scalar, Options> &M)

  Derivative of log6.

This function is the right derivative of log6, that is, for MSE(3) and ξinse(3), it provides the linear approximation:

log6(Mξ)=log6(Mexp6(ξ))log6(M)+Jlog6(M)ξ

Equivalently, Jlog6 is the right Jacobian of log6:

Jlog6(M)=log6(M)M

Note that this is the right Jacobian: Jlog6(M):TMSE(3)Tlog6(M)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:

Jlog6(M)=(Jlog3(R)JJlog3(R)0Jlog3(R))
where
M=(exp(r)p01)
J=12[p]×+β(||r||)rTp||r||rrT(||r||β(||r||)+2β(||r||))prT+rTpβ(||r||)I3+β(||r||)rpT
and
β(x)=(1x2sinx2x(1cosx))

\cheatsheet For (A,B)SE(3)2, let M1(A,B)=AB and m1=log6(M1). Then, we have the following partial (right) Jacobians:

  • m1A=Jlog6(M1)AdB1,

  • m1B=Jlog6(M1).

\cheatsheet Let ASE(3), M2(A)=A1 and m2=log6(M2). Then, we have the following partial (right) Jacobian:

  • m2A=Jlog6(M2)AdA.

Parameters:

M[in] The rigid transformation.