function

LieGroups.SE3.Quat.inverse

SE(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`	`[7]`	{p, q}
output	`X_inv`	`Real`	`[7]`	{p_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(3) inverse"
  input Real X[7] "{p, q}";
  output Real X_inv[7] "{p_inv, q_inv}";
protected
  Real q_inv[4];
  Real p_rot[3];
algorithm
  q_inv := LieGroups.SO3.Quat.inverse(X[4:7]);
  // p_inv = -R^T * p = -R(q_inv) * 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];
  X_inv[4] := q_inv[1];
  X_inv[5] := q_inv[2];
  X_inv[6] := q_inv[3];
  X_inv[7] := q_inv[4];
end inverse;