function

LieGroups.SE2.product

SE(2) group product: (t1,R1) * (t2,R2) = (R1*t2+t1, R1*R2)

Public declarations

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

Role	Name	Type	Dimensions / attributes / default	Description
input	`X1`	`Real`	`[3]`	{px1, py1, theta1}
input	`X2`	`Real`	`[3]`	{px2, py2, theta2}
output	`X`	`Real`	`[3]`	{px, py, theta}

Modelica declarations and implementation

Show parameters, interfaces and source

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

function product "SE(2) group product: (t1,R1) * (t2,R2) = (R1*t2+t1, R1*R2)"
  input Real X1[3] "{px1, py1, theta1}";
  input Real X2[3] "{px2, py2, theta2}";
  output Real X[3] "{px, py, theta}";
protected
  Real c1, s1;
algorithm
  c1 := cos(X1[3]);
  s1 := sin(X1[3]);
  // p = R(theta1) * p2 + p1
  X[1] := c1*X2[1] - s1*X2[2] + X1[1];
  X[2] := s1*X2[1] + c1*X2[2] + X1[2];
  // theta = theta1 + theta2
  X[3] := X1[3] + X2[3];
end product;