function

Tests.ManualGuidanceTests.leashProjectionResidual

Failure of the leashed pose to sit at the leash along the error geodesic

Public declarations

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

Role	Name	Type	Dimensions / attributes / default	Description
input	`leash_m`	`Real`
output	`residual`	`Real`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function leashProjectionResidual
    "Failure of the leashed pose to sit at the leash along the error geodesic"
    input Real leash_m;
    output Real residual;
  protected
    Real vehiclePose[10];
    Real farPose[10];
    Real error[9];
    Real clamped[9];
    Real leashedPose[10];
    Real recovered[9];
  algorithm
    vehiclePose := LieGroups.SE23.Quat.exp_map(
      {2.0, -1.0, 3.0, 0.5, 0.2, -0.1, 0.0, 0.0, 0.7});
    farPose := LieGroups.SE23.Quat.product(
      vehiclePose,
      LieGroups.SE23.Quat.exp_map(
        {30.0, 40.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}));
    error := LieGroups.SE23.Quat.log_map(
      LieGroups.SE23.Quat.product(
        LieGroups.SE23.Quat.inverse(vehiclePose), farPose));
    clamped := cat(1,
      MathUtilities.limitNorm(error[1:2], leash_m),
      {error[3]},
      error[4:6],
      {0.0, 0.0, error[9]});
    leashedPose := LieGroups.SE23.Quat.product(
      vehiclePose, LieGroups.SE23.Quat.exp_map(clamped));
    recovered := LieGroups.SE23.Quat.log_map(
      LieGroups.SE23.Quat.product(
        LieGroups.SE23.Quat.inverse(vehiclePose), leashedPose));
    // The leashed pose must lie at exactly the leash distance and on the same
    // ray of the error logarithm as the unleashed one.
    residual := abs(MathUtilities.norm2(recovered[1:2]) - leash_m)
      + abs(recovered[1] * error[2] - recovered[2] * error[1])
        / MathUtilities.norm2(error[1:2]);
  end leashProjectionResidual;