function

LieGroups.SE2.to_Matrix

Convert SE(2) element to 3x3 homogeneous matrix

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	`M`	`Real`	`[3,3]`	3x3 matrix {{R, p}, {0, 1}}

Modelica declarations and implementation

Show parameters, interfaces and source

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

function to_Matrix "Convert SE(2) element to 3x3 homogeneous matrix"
  input Real X[3] "{px, py, theta}";
  output Real M[3,3] "3x3 matrix {{R, p}, {0, 1}}";
protected
  Real c, s;
algorithm
  c := cos(X[3]);
  s := sin(X[3]);
  M[1,1] := c;   M[1,2] := -s;  M[1,3] := X[1];
  M[2,1] := s;   M[2,2] := c;   M[2,3] := X[2];
  M[3,1] := 0;   M[3,2] := 0;   M[3,3] := 1;
end to_Matrix;