function

LieGroups.SE2.adjoint

Adjoint Ad_X for SE(2), translation-first ordering

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]`	{px, py, theta}
output	`Ad`	`Real`	`[3,3]`	3x3 adjoint matrix

Modelica declarations and implementation

Show parameters, interfaces and source

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

function adjoint "Adjoint Ad_X for SE(2), translation-first ordering"
  input Real X[3] "{px, py, theta}";
  output Real Ad[3,3] "3x3 adjoint matrix";
protected
  Real c, s;
algorithm
  c := cos(X[3]);
  s := sin(X[3]);
  // Ad_X = {{R(theta), [py, -px]^T}, {0, 0, 1}}
  Ad[1,1] := c;   Ad[1,2] := -s;  Ad[1,3] := X[2];
  Ad[2,1] := s;   Ad[2,2] := c;   Ad[2,3] := -X[1];
  Ad[3,1] := 0;   Ad[3,2] := 0;   Ad[3,3] := 1;
end adjoint;