Template Function pinocchio::Jlog3(const Scalar&, const Eigen::MatrixBase<Vector3Like>&, const Eigen::MatrixBase<Matrix3Like>&)

Function Documentation

template<typename Scalar, typename Vector3Like, typename Matrix3Like>
void pinocchio::Jlog3(const Scalar &theta, const Eigen::MatrixBase<Vector3Like> &log, const Eigen::MatrixBase<Matrix3Like> &Jlog)

Derivative of log3.

This function is the right derivative of log3, that is, for \(R \in SO(3)\) and \(\omega t in \mathfrak{so}(3)\), it provides the linear approximation:

\[ \log_3(R \oplus \omega t) = \log_3(R \exp_3(\omega t)) \approx \log_3(R) + \text{Jlog3}(R) \omega t \]

Equivalently, \(\text{Jlog3}\) is the right Jacobian of \(\log_3\):

\[ \text{Jlog3}(R) = \frac{\partial \log_3(R)}{\partial R} \]

Note that this is the right Jacobian: \(\text{Jlog3}(R) : T_{R} SO(3) \to T_{\log_6(R)} \mathfrak{so}(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.)

If we denote by \(\theta = \log_3(R)\) and \(\log = \log_3(R, \theta)\), then \(\text{Jlog} = \text{Jlog}_3(R)\) can be calculated as:

\[\begin{split} \begin{align*} \text{Jlog} & = \frac{\theta \sin(\theta)}{2 (1 - \cos(\theta))} I_3 + \frac{1}{2} \widehat{\log} + \left(\frac{1}{\theta^2} - \frac{\sin(\theta)}{2\theta(1-\cos(\theta))}\right) \log \log^T \\ & = I_3 + \frac{1}{2} \widehat{\log} + \left(\frac{1}{\theta^2} - \frac{1 + \cos \theta}{2 \theta \sin \theta}\right) \widehat{\log}^2 \end{align*} \end{split}\]

where \(\widehat{v}\) denotes the skew-symmetric matrix obtained from the 3D vector \(v\).

Note

The inputs must be such that \( \theta = \Vert \log \Vert \).

Parameters:

  • theta – [in] the angle value.

  • log – [in] the output of log3.

  • Jlog – [out] the jacobian