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 RSO(3) and ωtinso(3), it provides the linear approximation:

log3(Rωt)=log3(Rexp3(ωt))log3(R)+Jlog3(R)ωt

Equivalently, Jlog3 is the right Jacobian of log3:

Jlog3(R)=log3(R)R

Note that this is the right Jacobian: Jlog3(R):TRSO(3)Tlog6(R)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 θ=log3(R) and log=log3(R,θ), then Jlog=Jlog3(R) can be calculated as:

Jlog=θsin(θ)2(1cos(θ))I3+12log^+(1θ2sin(θ)2θ(1cos(θ)))loglogT=I3+12log^+(1θ21+cosθ2θsinθ)log^2

where v^ denotes the skew-symmetric matrix obtained from the 3D vector v.

Note

The inputs must be such that θ=log.

Parameters:
  • theta[in] the angle value.

  • log[in] the output of log3.

  • Jlog[out] the jacobian