function

LieGroups.SE2.log_map

Logarithmic map: SE(2) -> se(2)

Public declarations

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

Role	Name	Type	Dimensions / attributes / default	Description
input	`X`	`Real`	`[3]`	Group element {px, py, theta}
output	`xi`	`Real`	`[3]`	Lie algebra {vx, vy, omega}

Modelica declarations and implementation

Show parameters, interfaces and source

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

function log_map "Logarithmic map: SE(2) -> se(2)"
  input Real X[3] "Group element {px, py, theta}";
  output Real xi[3] "Lie algebra {vx, vy, omega}";
protected
  Real theta;
  Real a "sin(theta)/theta";
  Real b "(1-cos(theta))/theta";
  Real det_V;
  constant Real eps = 1e-8;
algorithm
  theta := X[3];

  if abs(theta) < eps then
    a := 1.0 - theta^2 / 6.0;
    b := theta / 2.0 - theta^3 / 24.0;
  else
    a := sin(theta) / theta;
    b := (1.0 - cos(theta)) / theta;
  end if;

  // V_inv = {{a, b}, {-b, a}} / (a^2 + b^2)
  det_V := a^2 + b^2;
  xi[1] := (a*X[1] + b*X[2]) / det_V;
  xi[2] := (-b*X[1] + a*X[2]) / det_V;
  xi[3] := theta;
end log_map;