LieGroups/SO3/Quat/right_jacobian.mo

Download original .moGitHub source

Documentation: LieGroups.SO3.Quat.right_jacobian

within LieGroups.SO3.Quat;
function right_jacobian "Right Jacobian J_r(v) of SO(3)"
  input Real v[3] "Rotation vector";
  output Real J[3,3] "3x3 right Jacobian matrix";
protected
  Real theta_sq;
  Real A "(1 - cos(theta)) / theta^2";
  Real B "(theta - sin(theta)) / theta^3";
  Real theta;
  Real S[3,3] "Skew-symmetric [v]x";
  Real S2[3,3] "[v]x * [v]x";
  // Below 0.1 rad the series error is negligible, while the closed form
  // loses its numerator to cancellation in single-precision generated code.
  constant Real eps = 1e-2;
algorithm
  theta_sq := v[1]^2 + v[2]^2 + v[3]^2;

  S := LieGroups.SO3.Quat.wedge(v);
  S2 := S * S;

  if theta_sq < eps then
    A := 0.5 - theta_sq / 24.0;
    B := 1.0/6.0 - theta_sq / 120.0;
  else
    // Keep the closed form finite for compilers/evaluators that inspect both
    // branches during symbolic processing.
    theta := sqrt(max(theta_sq, eps));
    A := (1.0 - cos(theta)) / (theta * theta);
    B := (theta - sin(theta)) / (theta * theta * theta);
  end if;

  // J_r = I - A * [v]x + B * [v]x^2  (note: minus sign on A term vs left Jacobian)
  J := identity(3) - A * S + B * S2;

  annotation(Documentation(info="<html>
    <p>Right Jacobian of SO(3): J_r(v) = I - ((1-cos θ)/θ²)[v]× + ((θ-sin θ)/θ³)[v]ײ</p>
    <p>Related to left Jacobian by: J_r(v) = J_l(-v)</p>
  </html>"));
end right_jacobian;