Tests.PlanningTests

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

model PlanningTests "Dubins family, endpoint, optimality, invariance, and forward-motion tests"
  function run
    output Boolean passed;
  protected
    constant Real tolerance = 2.0e-9;
    constant Real pi = 2.0 * asin(1.0);
    Planning.Dubins.PathType types[6];
    Planning.Dubins.Candidate candidateResult;
    Planning.Dubins.Candidate candidates[6];
    Planning.Dubins.Path candidatePath;
    Planning.Dubins.Path straightPath;
    Planning.Dubins.Path plannedPath;
    Planning.Dubins.Path transformedPath;
    Planning.Dubins.Path scaledPath;
    Planning.Dubins.Pose pose;
    Planning.Dubins.Pose previousPose;
    Planning.Dubins.Pose currentPose;
    Planning.DubinsPolynomial.State smoothState;
    Planning.DubinsPolynomial.State smoothStart;
    Planning.DubinsPolynomial.State smoothGoal;
    Real offsetCoefficient[3, 8];
    Real optimizedCoefficient[3, 8];
    Real hermiteCoefficient[6];
    Boolean offsetAccepted;
    Boolean optimizedAccepted;
    Boolean hermiteAccepted;
    Real continuityResidual;
    Real smoothingCost;
    Real seedCost;
    Real optimizedCost;
    Real rollAngle;
    Real rollRate;
    Real minimumLength;
    Real displacement[2];
    Real gamma;
    Real rotation[2, 2];
    Real translation[2];
    Real startPosition[2];
    Real goalPosition[2];
    Real startHeading;
    Real goalHeading;
    Real radius;
  algorithm
    types := {
      Planning.Dubins.PathType.LSL,
      Planning.Dubins.PathType.RSR,
      Planning.Dubins.PathType.LSR,
      Planning.Dubins.PathType.RSL,
      Planning.Dubins.PathType.RLR,
      Planning.Dubins.PathType.LRL};

    straightPath := Planning.Dubins.plan(
      {0.0, 0.0}, 0.0, {10.0, 0.0}, 0.0, 1.0);
    pose := Planning.Dubins.evaluate(straightPath, 1.0);
    assert(straightPath.feasible and abs(straightPath.length - 10.0) < tolerance,
      "Straight Dubins path had incorrect length");
    assert(Tests.Assertions.maxAbsVector(pose.position - {10.0, 0.0}) < tolerance and
        abs(Planning.Dubins.wrapAngle(pose.heading)) < tolerance,
      "Straight Dubins path did not reach its terminal pose");
    (offsetCoefficient, offsetAccepted) :=
      Planning.DubinsPolynomial.smoothOffsets(
        straightPath, 0.0, 0.0, 0.0, 0.0);
    assert(offsetAccepted and
        Planning.DubinsPolynomial.junctionContinuityResidual(
          straightPath, offsetCoefficient) < tolerance,
      "Dubins-polynomial smoothing rejected a path with zero-length turns");
    for sampleIndex in 0:10 loop
      smoothState := Planning.DubinsPolynomial.evaluate(
        straightPath, offsetCoefficient, sampleIndex / 10.0);
      assert(Tests.Assertions.maxAbsVector(
          smoothState.position - {sampleIndex, 0.0}) < tolerance and
          abs(smoothState.curvature) < tolerance and
          abs(smoothState.curvatureDerivative) < tolerance,
        "Smoothed straight Dubins path was not geometrically exact");
    end for;

    startPosition := {0.0, 0.0};
    startHeading := 0.0;
    goalPosition := {2.0, 1.0};
    goalHeading := 1.2;
    radius := 1.0;
    for familyIndex in 1:6 loop
      candidateResult := Planning.Dubins.candidate(
        startPosition, startHeading, goalPosition, goalHeading,
        radius, types[familyIndex]);
      assert(candidateResult.feasible,
        "Expected all six Dubins families to be feasible for the coverage case");
      candidatePath.startPosition := startPosition;
      candidatePath.startHeading := startHeading;
      candidatePath.goalPosition := goalPosition;
      candidatePath.goalHeading := goalHeading;
      candidatePath.turnRadius := radius;
      candidatePath.pathType := candidateResult.pathType;
      candidatePath.normalizedSegmentLength :=
        candidateResult.normalizedSegmentLength;
      candidatePath.length := candidateResult.length;
      candidatePath.feasible := candidateResult.feasible;
      pose := Planning.Dubins.evaluate(candidatePath, 1.0);
      assert(Tests.Assertions.maxAbsVector(pose.position - goalPosition) < 2.0e-8 and
          abs(Planning.Dubins.wrapAngle(pose.heading - goalHeading)) < 2.0e-8,
        "A Dubins family did not satisfy the terminal boundary conditions");
    end for;

    plannedPath := Planning.Dubins.plan(
      {0.0, 0.0}, -0.4, {10.0, 7.0}, 1.3, 1.7);
    for familyIndex in 1:6 loop
      candidates[familyIndex] := Planning.Dubins.candidate(
        {0.0, 0.0}, -0.4, {10.0, 7.0}, 1.3, 1.7,
        types[familyIndex]);
    end for;
    minimumLength := candidates[1].length;
    for familyIndex in 2:6 loop
      if candidates[familyIndex].feasible then
        minimumLength := min(minimumLength, candidates[familyIndex].length);
      end if;
    end for;
    assert(abs(plannedPath.length - minimumLength) < tolerance,
      "Dubins planner did not select the shortest feasible family");
    candidatePath := Planning.Dubins.plan(
      {0.0, 0.0}, -0.4, {10.0, 7.0}, 1.3, 1.7,
      allowThreeTurnPaths=false);
    assert(candidatePath.pathType <> Planning.Dubins.PathType.RLR and
        candidatePath.pathType <> Planning.Dubins.PathType.LRL,
      "Dubins planner selected a disabled three-turn family");
    pose := Planning.Dubins.evaluate(plannedPath, 1.0);
    assert(Tests.Assertions.maxAbsVector(
        pose.position - plannedPath.goalPosition) < 2.0e-8 and
        abs(Planning.Dubins.wrapAngle(
          pose.heading - plannedPath.goalHeading)) < 2.0e-8,
      "Selected Dubins path did not reach its goal");

    previousPose := Planning.Dubins.evaluate(plannedPath, 0.0);
    for sampleIndex in 1:100 loop
      currentPose := Planning.Dubins.evaluate(plannedPath, sampleIndex / 100.0);
      displacement := currentPose.position - previousPose.position;
      assert(displacement * {cos(previousPose.heading), sin(previousPose.heading)}
          >= -1.0e-10,
        "Dubins evaluator produced backward motion");
      assert(Tests.Assertions.maxAbsVector(displacement)
          <= 0.02 * plannedPath.length,
        "Dubins evaluator was not position-continuous");
      previousPose := currentPose;
    end for;

    gamma := 0.7;
    rotation := [cos(gamma), -sin(gamma); sin(gamma), cos(gamma)];
    translation := {3.0, -2.0};
    transformedPath := Planning.Dubins.plan(
      translation + rotation * plannedPath.startPosition,
      plannedPath.startHeading + gamma,
      translation + rotation * plannedPath.goalPosition,
      plannedPath.goalHeading + gamma,
      plannedPath.turnRadius);
    assert(abs(transformedPath.length - plannedPath.length) < 2.0e-8,
      "Dubins path length was not rigid-transform invariant");

    scaledPath := Planning.Dubins.plan(
      2.5 * plannedPath.startPosition,
      plannedPath.startHeading,
      2.5 * plannedPath.goalPosition,
      plannedPath.goalHeading,
      2.5 * plannedPath.turnRadius);
    assert(abs(scaledPath.length - 2.5 * plannedPath.length) < 2.0e-8,
      "Dubins path did not scale with position and turn radius");

    (hermiteCoefficient, hermiteAccepted) :=
      Polynomials.hermiteCoefficients(
        {1.0, -0.5, 0.25}, {2.0, 0.75, -0.1}, 1.7);
    assert(hermiteAccepted and
        abs(Polynomials.evaluateDerivative(hermiteCoefficient, 0.0, 0) - 1.0)
          < tolerance and
        abs(Polynomials.evaluateDerivative(hermiteCoefficient, 0.0, 1) + 0.5)
          < tolerance and
        abs(Polynomials.evaluateDerivative(hermiteCoefficient, 0.0, 2) - 0.25)
          < tolerance and
        abs(Polynomials.evaluateDerivative(hermiteCoefficient, 1.7, 0) - 2.0)
          < tolerance and
        abs(Polynomials.evaluateDerivative(hermiteCoefficient, 1.7, 1) - 0.75)
          < tolerance and
        abs(Polynomials.evaluateDerivative(hermiteCoefficient, 1.7, 2) + 0.1)
          < tolerance,
      "Dimension-generic Hermite polynomial construction failed");

    (offsetCoefficient, offsetAccepted) :=
      Planning.DubinsPolynomial.smoothOffsets(
        path=plannedPath,
        startCurvature=0.0,
        startCurvatureDerivative=0.0,
        goalCurvature=0.0,
        goalCurvatureDerivative=0.0);
    assert(offsetAccepted,
      "Coverage path contained a zero-length segment and could not be smoothed");
    continuityResidual :=
      Planning.DubinsPolynomial.junctionContinuityResidual(
        plannedPath, offsetCoefficient);
    assert(continuityResidual < 2.0e-7,
      "Dubins-polynomial seed was not C3 at a nominal segment junction");
    seedCost := 0.3 * Planning.DubinsPolynomial.derivativeCost(
      plannedPath, offsetCoefficient, 0)
      + Planning.DubinsPolynomial.derivativeCost(
        plannedPath, offsetCoefficient, 1)
      + 50.0 * Planning.DubinsPolynomial.derivativeCost(
        plannedPath, offsetCoefficient, 3);
    (optimizedCoefficient, optimizedAccepted) :=
      Planning.DubinsPolynomial.jointOptimizeOffsets(
        plannedPath, 0.0, 0.0, 0.0, 0.0);
    optimizedCost := 0.3 * Planning.DubinsPolynomial.derivativeCost(
      plannedPath, optimizedCoefficient, 0)
      + Planning.DubinsPolynomial.derivativeCost(
        plannedPath, optimizedCoefficient, 1)
      + 50.0 * Planning.DubinsPolynomial.derivativeCost(
        plannedPath, optimizedCoefficient, 3);
    assert(optimizedAccepted and optimizedCost <= seedCost * (1.0 + 1.0e-7),
      "Joint analytical Dubins-polynomial optimization increased its objective");
    assert(Planning.DubinsPolynomial.junctionContinuityResidual(
        plannedPath, optimizedCoefficient) < 2.0e-7,
      "True-derivative repair did not restore C3 junction continuity");
    offsetCoefficient := optimizedCoefficient;
    smoothStart := Planning.DubinsPolynomial.evaluate(
      plannedPath, offsetCoefficient, 0.0);
    smoothGoal := Planning.DubinsPolynomial.evaluate(
      plannedPath, offsetCoefficient, 1.0);
    assert(Tests.Assertions.maxAbsVector(
        smoothStart.position - plannedPath.startPosition) < tolerance and
        Tests.Assertions.maxAbsVector(
          smoothGoal.position - plannedPath.goalPosition) < 2.0e-8 and
        abs(Planning.Dubins.wrapAngle(
          smoothStart.heading - plannedPath.startHeading)) < tolerance and
        abs(Planning.Dubins.wrapAngle(
          smoothGoal.heading - plannedPath.goalHeading)) < 2.0e-8 and
        abs(smoothStart.curvature) < 2.0e-8 and
        abs(smoothGoal.curvature) < 2.0e-8,
      "Dubins-polynomial endpoint pose or curvature constraints failed");
    for sampleIndex in 0:100 loop
      smoothState := Planning.DubinsPolynomial.evaluate(
        plannedPath, offsetCoefficient, sampleIndex / 100.0);
      rollAngle := Planning.DubinsPolynomial.coordinatedRollAngle(
        smoothState.curvature, 7.0);
      rollRate := Planning.DubinsPolynomial.coordinatedRollRate(
        smoothState.curvature, smoothState.curvatureDerivative, 7.0);
      assert(smoothState.metricScale > 1.0e-6 and
          abs(smoothState.firstDerivative * smoothState.firstDerivative - 1.0)
            < 2.0e-9 and
          Tests.Assertions.isFiniteVector(smoothState.position) and
          Tests.Assertions.isFiniteVector(smoothState.thirdDerivative) and
          abs(rollAngle) < 0.5 * pi and abs(rollRate) < 1.0e100,
        "Dubins-polynomial evaluation was singular or non-finite");
    end for;
    smoothingCost := Planning.DubinsPolynomial.derivativeCost(
      plannedPath, offsetCoefficient, 3);
    assert(smoothingCost >= 0.0,
      "Integrated polynomial derivative cost was negative");
    passed := true;
  end run;

  parameter Boolean passed = run();
equation
  assert(passed, "Dubins planning assertions did not complete");
end PlanningTests;