237 lines
11 KiB
C#
237 lines
11 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Threading;
|
|
using EMPlannerVerificationHost;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
internal static class LateralIntegrationChecks
|
|
{
|
|
public static void Run()
|
|
{
|
|
VerifiesValidatedCandidateSurvivesLaterTimeout();
|
|
VerifiesInvalidVectorsAndInaccurateResidualsNeverBecomeFallbacks();
|
|
VerifiesTrustRegionWarmStartAndOuterIterationLimit();
|
|
VerifiesCancellationAndTimeoutWithoutCandidate();
|
|
VerifiesLateralPlannerDelegatesToTheSequentialOptimizer();
|
|
}
|
|
|
|
private static void VerifiesValidatedCandidateSurvivesLaterTimeout()
|
|
{
|
|
LateralPlanningInput input = CreateInput();
|
|
double[] valid = CreatePrimal(input, 0.02d);
|
|
var solver = new FakeQpSolver(new[]
|
|
{
|
|
Result(QpSolveStatus.Solved, valid, 10d),
|
|
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 10d),
|
|
});
|
|
|
|
LateralPlanningResult result = new SequentialConvexOptimizer(solver).Optimize(input, CancellationToken.None);
|
|
|
|
Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
|
|
"timeout after an independently validated candidate returns fallback success");
|
|
LateralPath fallbackPath = result.Path ?? throw new InvalidOperationException("Fallback path was not returned.");
|
|
Verification.True(fallbackPath.IsIndependentlyValidated,
|
|
"fallback path remains independently validated");
|
|
Verification.NearlyEqual(0.02d, fallbackPath.Points[1].L,
|
|
"first valid candidate remains the fallback path");
|
|
}
|
|
|
|
private static void VerifiesInvalidVectorsAndInaccurateResidualsNeverBecomeFallbacks()
|
|
{
|
|
LateralPlanningInput input = CreateInput();
|
|
double[] valid = CreatePrimal(input, 0.02d);
|
|
double[] invalid = CreatePrimal(input, 0.40d);
|
|
|
|
var solver = new FakeQpSolver(new[]
|
|
{
|
|
Result(QpSolveStatus.Solved, valid, 10d),
|
|
Result(QpSolveStatus.Solved, invalid, 9d),
|
|
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 9d),
|
|
});
|
|
LateralPlanningResult preserved = new SequentialConvexOptimizer(solver).Optimize(input, CancellationToken.None);
|
|
Verification.Equal(EmPlanningStatus.SuccessWithFallback, preserved.Status,
|
|
"invalid solved vector does not discard an earlier fallback");
|
|
Verification.NearlyEqual(0.02d, preserved.Path.Points[1].L,
|
|
"invalid solved vector does not replace the fallback candidate");
|
|
|
|
var inaccurateResidual = new FakeQpSolver(new[]
|
|
{
|
|
Result(QpSolveStatus.SolvedInaccurate, valid, 10d, 2e-5d, 0d),
|
|
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 10d),
|
|
});
|
|
LateralPlanningResult rejectedResidual = new SequentialConvexOptimizer(inaccurateResidual).Optimize(input,
|
|
CancellationToken.None);
|
|
Verification.Equal(EmPlanningStatus.SolverTimedOut, rejectedResidual.Status,
|
|
"SolvedInaccurate above strict residual threshold is rejected");
|
|
Verification.True(ReferenceEquals(null, rejectedResidual.Path),
|
|
"rejected inaccurate result does not publish a path");
|
|
|
|
var inaccurateGeometry = new FakeQpSolver(new[]
|
|
{
|
|
Result(QpSolveStatus.SolvedInaccurate, invalid, 10d, 0d, 0d),
|
|
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 10d),
|
|
});
|
|
LateralPlanningResult rejectedGeometry = new SequentialConvexOptimizer(inaccurateGeometry).Optimize(input,
|
|
CancellationToken.None);
|
|
Verification.Equal(EmPlanningStatus.SolverTimedOut, rejectedGeometry.Status,
|
|
"SolvedInaccurate still requires full independent lateral validation");
|
|
}
|
|
|
|
private static void VerifiesTrustRegionWarmStartAndOuterIterationLimit()
|
|
{
|
|
LateralPlanningInput input = CreateInput();
|
|
var trustSolver = new FakeQpSolver(new[]
|
|
{
|
|
Result(QpSolveStatus.Solved, CreatePrimal(input, 0.02d), 10d),
|
|
Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 10d),
|
|
});
|
|
new SequentialConvexOptimizer(trustSolver).Optimize(input, CancellationToken.None);
|
|
|
|
var layout = new LateralVariableLayout(input.ReferenceStations.Count);
|
|
FindSingleVariableBounds(trustSolver.Problems[0], layout.L(1), out double initialLower, out double initialUpper);
|
|
FindSingleVariableBounds(trustSolver.Problems[1], layout.L(1), out double nextLower, out double nextUpper);
|
|
Verification.NearlyEqual(-0.05d, initialLower, "initial trust-region lower bound");
|
|
Verification.NearlyEqual(0.05d, initialUpper, "initial trust-region upper bound");
|
|
Verification.NearlyEqual(-0.03d, nextLower, "trust region is centered on previous iterate");
|
|
Verification.NearlyEqual(0.07d, nextUpper, "trust region never exceeds 0.05m around previous iterate");
|
|
Verification.Equal(layout.VariableCount, trustSolver.WarmStarts[1].Count,
|
|
"next QP receives the complete previous primal warm start");
|
|
Verification.NearlyEqual(0.02d, trustSolver.WarmStarts[1][layout.L(1)],
|
|
"warm start retains the prior lateral iterate");
|
|
|
|
var limitResults = new List<QpSolveResult>();
|
|
for (int index = 1; index <= 5; index++)
|
|
limitResults.Add(Result(QpSolveStatus.Solved, CreatePrimal(input, 0.02d * index), 100d - index));
|
|
var limitSolver = new FakeQpSolver(limitResults);
|
|
LateralPlanningResult limited = new SequentialConvexOptimizer(limitSolver).Optimize(input, CancellationToken.None);
|
|
Verification.Equal(5, limitSolver.SolveCallCount, "outer loop stops after at most five QP calls");
|
|
Verification.Equal(EmPlanningStatus.Success, limited.Status, "last feasible candidate succeeds at outer iteration limit");
|
|
}
|
|
|
|
private static void VerifiesCancellationAndTimeoutWithoutCandidate()
|
|
{
|
|
LateralPlanningInput input = CreateInput();
|
|
var cancellationSolver = new FakeQpSolver(Result(QpSolveStatus.Solved, CreatePrimal(input, 0d), 1d));
|
|
using var cancellation = new CancellationTokenSource();
|
|
cancellation.Cancel();
|
|
LateralPlanningResult cancelled = new SequentialConvexOptimizer(cancellationSolver).Optimize(input,
|
|
cancellation.Token);
|
|
Verification.Equal(EmPlanningStatus.Cancelled, cancelled.Status, "cancellation before a solver call is cancelled");
|
|
Verification.Equal(0, cancellationSolver.SolveCallCount, "cancelled solve does not invoke the solver");
|
|
|
|
var timeoutSolver = new FakeQpSolver(Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d));
|
|
LateralPlanningResult timeout = new SequentialConvexOptimizer(timeoutSolver).Optimize(input, CancellationToken.None);
|
|
Verification.Equal(EmPlanningStatus.SolverTimedOut, timeout.Status,
|
|
"timeout without a feasible candidate is solver timed out");
|
|
Verification.True(ReferenceEquals(null, timeout.Path), "timeout without candidate does not publish a path");
|
|
}
|
|
|
|
private static void VerifiesLateralPlannerDelegatesToTheSequentialOptimizer()
|
|
{
|
|
LateralPlanningInput input = CreateInput();
|
|
double[] zero = CreatePrimal(input, 0d);
|
|
var solver = new FakeQpSolver(new[]
|
|
{
|
|
Result(QpSolveStatus.Solved, zero, 1d),
|
|
Result(QpSolveStatus.Solved, zero, 1d),
|
|
});
|
|
LateralPlanningResult result = new LateralPlanner(solver).Plan(input, CancellationToken.None);
|
|
Verification.Equal(EmPlanningStatus.Success, result.Status, "lateral planner returns SQP success");
|
|
}
|
|
|
|
private static LateralPlanningInput CreateInput()
|
|
{
|
|
var points = new List<SmoothedPathPoint>
|
|
{
|
|
Point(0d, 0d),
|
|
Point(1d, 1d),
|
|
Point(2d, 2d),
|
|
};
|
|
var segment = new DirectionSegmentView(0, TravelDirection.Forward, points,
|
|
new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
|
|
new ReferenceBoundary(0, 2d, EmBoundaryType.Goal, 2d), 0d);
|
|
var corridor = new StaticCorridor(new[]
|
|
{
|
|
new LateralInterval(0d, -0.3d, 0.3d, 0d),
|
|
new LateralInterval(1d, -0.3d, 0.3d, 0d),
|
|
new LateralInterval(2d, -0.3d, 0.3d, 0d),
|
|
});
|
|
var vehicle = new VehicleParameters
|
|
{
|
|
LengthMeters = 0.1d,
|
|
WidthMeters = 0.1d,
|
|
SafetyMarginMeters = 0d,
|
|
MaximumCurvaturePerMeter = 1d,
|
|
};
|
|
return new LateralPlanningInput(segment, corridor,
|
|
new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), 0d, 0d, 0d),
|
|
EmTerminalType.Goal, vehicle, EmPlannerConfiguration.CreateDefault(), Array.Empty<FrenetProjection>());
|
|
}
|
|
|
|
private static SmoothedPathPoint Point(double x, double pathS)
|
|
{
|
|
return new SmoothedPathPoint(x, 0d, 0d, 0d, pathS, TravelDirection.Forward, 0d, 0d, 0d, 1d,
|
|
false, SmoothedPathPointSource.Anchor);
|
|
}
|
|
|
|
private static QpSolveResult Result(QpSolveStatus status, IReadOnlyList<double> primal, double objective,
|
|
double primalResidual = 0d, double dualResidual = 0d)
|
|
{
|
|
return new QpSolveResult(status, primal, objective, primalResidual, dualResidual, 1, TimeSpan.Zero,
|
|
status.ToString(), string.Empty);
|
|
}
|
|
|
|
private static double[] CreatePrimal(LateralPlanningInput input, double middleL)
|
|
{
|
|
var layout = new LateralVariableLayout(input.ReferenceStations.Count);
|
|
double c = 6d * middleL;
|
|
var primal = new double[layout.VariableCount];
|
|
primal[layout.L(0)] = 0d;
|
|
primal[layout.L(1)] = middleL;
|
|
primal[layout.L(2)] = 0d;
|
|
primal[layout.DL(0)] = 0d;
|
|
primal[layout.DL(1)] = 0d;
|
|
primal[layout.DL(2)] = 0d;
|
|
primal[layout.DDL(0)] = c;
|
|
primal[layout.DDL(1)] = -c;
|
|
primal[layout.DDL(2)] = c;
|
|
primal[layout.DDDL(0)] = -2d * c;
|
|
primal[layout.DDDL(1)] = 2d * c;
|
|
return primal;
|
|
}
|
|
|
|
private static void FindSingleVariableBounds(QuadraticProgram problem, int variable, out double lower, out double upper)
|
|
{
|
|
for (int row = 0; row < problem.ConstraintCount; row++)
|
|
{
|
|
int matchingEntries = 0;
|
|
double coefficient = 0d;
|
|
for (int column = 0; column < problem.ConstraintMatrix.ColumnCount; column++)
|
|
{
|
|
for (int index = problem.ConstraintMatrix.ColumnPointers[column];
|
|
index < problem.ConstraintMatrix.ColumnPointers[column + 1]; index++)
|
|
{
|
|
if (problem.ConstraintMatrix.RowIndices[index] == row)
|
|
{
|
|
matchingEntries++;
|
|
if (column == variable)
|
|
coefficient = problem.ConstraintMatrix.Values[index];
|
|
}
|
|
}
|
|
}
|
|
if (matchingEntries == 1 && Math.Abs(coefficient - 1d) <= 1e-12d &&
|
|
Math.Abs(problem.LowerBounds[row] - problem.UpperBounds[row]) > 1e-12d)
|
|
{
|
|
lower = problem.LowerBounds[row];
|
|
upper = problem.UpperBounds[row];
|
|
return;
|
|
}
|
|
}
|
|
throw new InvalidOperationException("Expected single-variable lateral trust-region row was not found.");
|
|
}
|
|
}
|