function

LieGroups.SO3.Quat.right_jacobian

Right Jacobian J_r(v) of SO(3)

Right Jacobian of SO(3): J_r(v) = I - ((1-cos θ)/θ²)[v]× + ((θ-sin θ)/θ³)[v]×²

Related to left Jacobian by: J_r(v) = J_l(-v)

Public declarations

Explicit source declarations; dimensions, modifications and defaults are shown as written, without evaluation.

Role	Name	Type	Dimensions / attributes / default	Description
input	`v`	`Real`	`[3]`	Rotation vector
output	`J`	`Real`	`[3,3]`	3x3 right Jacobian matrix

Modelica declarations and implementation

Show parameters, interfaces and source

Parameters, inputs, outputs, defaults and equations from the source file.

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;