function

LieGroups.SE3.Quat.product

SE(3) group product

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`	`[7]`	{p1, q1} = {px,py,pz, qw,qx,qy,qz}
input	`X2`	`Real`	`[7]`	{p2, q2}
output	`X`	`Real`	`[7]`	{p, q}

Modelica declarations and implementation

Show parameters, interfaces and source

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

function product "SE(3) group product"
  input Real X1[7] "{p1, q1} = {px,py,pz, qw,qx,qy,qz}";
  input Real X2[7] "{p2, q2}";
  output Real X[7] "{p, q}";
protected
  Real p_rot[3];
  Real q_prod[4];
algorithm
  // p = R(q1)*p2 + p1
  p_rot := LieGroups.SO3.Quat.rotate(X1[4:7], X2[1:3]);
  X[1] := p_rot[1] + X1[1];
  X[2] := p_rot[2] + X1[2];
  X[3] := p_rot[3] + X1[3];

  // q = q1 * q2
  q_prod := LieGroups.SO3.Quat.product(X1[4:7], X2[4:7]);
  X[4] := q_prod[1];
  X[5] := q_prod[2];
  X[6] := q_prod[3];
  X[7] := q_prod[4];
end product;