function

Tests.ManualGuidanceTests.headingTwistOffAxisMagnitude

Roll and pitch content of an extended pose advanced by a heading twist

Public declarations

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

Role	Name	Type	Dimensions / attributes / default	Description
output	`magnitude`	`Real`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function headingTwistOffAxisMagnitude
    "Roll and pitch content of an extended pose advanced by a heading twist"
    output Real magnitude;
  protected
    Real element[10];
    Real advanced[10];
  algorithm
    element := LieGroups.SE23.Quat.exp_map(
      {1.0, -2.0, 0.5, 0.4, 0.1, -0.2, 0.0, 0.0, 0.9});
    advanced := LieGroups.SE23.Quat.product(
      element,
      LieGroups.SE23.Quat.exp_map(
        {0.3, 0.2, -0.1, 0.05, -0.05, 0.2, 0.0, 0.0, -1.7}));
    // Indices 7 to 10 are {qw, qx, qy, qz}; a pure heading rotation has qx and
    // qy identically zero.
    magnitude := abs(element[8]) + abs(element[9])
      + abs(advanced[8]) + abs(advanced[9]);
  end headingTwistOffAxisMagnitude;