function

LieGroups.SE23.Quat.inverse

SE_2(3) inverse

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`	`[10]`	{p, v, q}
output	`X_inv`	`Real`	`[10]`	{p_inv, v_inv, q_inv}

Modelica declarations and implementation

Show parameters, interfaces and source

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

function inverse "SE_2(3) inverse"
  input Real X[10] "{p, v, q}";
  output Real X_inv[10] "{p_inv, v_inv, q_inv}";
protected
  Real q_inv[4];
  Real p_rot[3], v_rot[3];
algorithm
  q_inv := LieGroups.SO3.Quat.inverse(X[7:10]);

  // p_inv = -R^{-1} * p
  p_rot := LieGroups.SO3.Quat.rotate(q_inv, X[1:3]);
  X_inv[1] := -p_rot[1];
  X_inv[2] := -p_rot[2];
  X_inv[3] := -p_rot[3];

  // v_inv = -R^{-1} * v
  v_rot := LieGroups.SO3.Quat.rotate(q_inv, X[4:6]);
  X_inv[4] := -v_rot[1];
  X_inv[5] := -v_rot[2];
  X_inv[6] := -v_rot[3];

  X_inv[7] := q_inv[1];
  X_inv[8] := q_inv[2];
  X_inv[9] := q_inv[3];
  X_inv[10] := q_inv[4];
end inverse;