feat: optimize lateral paths with SQP
This commit is contained in:
梁薄云
@@ -0,0 +1,20 @@
|
||||
using System;
|
||||
using System.Threading;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Public lateral-planning entry point backed by the solver-neutral SQP optimizer.</summary>
|
||||
public sealed class LateralPlanner
|
||||
{
|
||||
private readonly SequentialConvexOptimizer _optimizer;
|
||||
|
||||
public LateralPlanner(IQpSolver qpSolver)
|
||||
{
|
||||
_optimizer = new SequentialConvexOptimizer(qpSolver ?? throw new ArgumentNullException(nameof(qpSolver)));
|
||||
}
|
||||
|
||||
public LateralPlanningResult Plan(LateralPlanningInput input, CancellationToken cancellationToken)
|
||||
{
|
||||
return _optimizer.Optimize(input, cancellationToken);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,294 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Threading;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Runs bounded lateral SQP iterations and retains only independently validated candidates.</summary>
|
||||
public sealed class SequentialConvexOptimizer
|
||||
{
|
||||
private const double StationTolerance = 1e-12d;
|
||||
private readonly IQpSolver _qpSolver;
|
||||
private readonly LateralConstraintBuilder _constraintBuilder;
|
||||
private readonly LateralGeometryEvaluator _geometryEvaluator;
|
||||
private readonly LateralSolutionValidator _solutionValidator;
|
||||
|
||||
public SequentialConvexOptimizer(IQpSolver qpSolver)
|
||||
: this(qpSolver, new LateralConstraintBuilder(new LateralObjectiveBuilder()), new LateralGeometryEvaluator(),
|
||||
new LateralSolutionValidator())
|
||||
{
|
||||
}
|
||||
|
||||
internal SequentialConvexOptimizer(IQpSolver qpSolver, LateralConstraintBuilder constraintBuilder,
|
||||
LateralGeometryEvaluator geometryEvaluator, LateralSolutionValidator solutionValidator)
|
||||
{
|
||||
_qpSolver = qpSolver ?? throw new ArgumentNullException(nameof(qpSolver));
|
||||
_constraintBuilder = constraintBuilder ?? throw new ArgumentNullException(nameof(constraintBuilder));
|
||||
_geometryEvaluator = geometryEvaluator ?? throw new ArgumentNullException(nameof(geometryEvaluator));
|
||||
_solutionValidator = solutionValidator ?? throw new ArgumentNullException(nameof(solutionValidator));
|
||||
}
|
||||
|
||||
public LateralPlanningResult Optimize(LateralPlanningInput input, CancellationToken cancellationToken)
|
||||
{
|
||||
if (input == null)
|
||||
return Failed(EmPlanningStatus.InvalidInput, "Lateral planning input is required.");
|
||||
|
||||
if (!TryCreateSettings(input, out QpSolverSettings settings, out TimeSpan totalBudget, out double convergenceTolerance,
|
||||
out string configurationFailure))
|
||||
{
|
||||
return Failed(EmPlanningStatus.InvalidInput, configurationFailure);
|
||||
}
|
||||
|
||||
LateralCandidate iterate = CreateInitialIterate(input);
|
||||
var warmStart = Array.Empty<double>();
|
||||
LateralPath lastValidatedPath = null;
|
||||
double previousObjective = 0d;
|
||||
bool hasPreviousObjective = false;
|
||||
var stopwatch = Stopwatch.StartNew();
|
||||
|
||||
for (int iteration = 0; iteration < input.Configuration.Solver.MaximumOuterIterations; iteration++)
|
||||
{
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.Cancelled, "Lateral SQP was cancelled.");
|
||||
|
||||
TimeSpan remainingBudget = totalBudget - stopwatch.Elapsed;
|
||||
if (remainingBudget <= TimeSpan.Zero)
|
||||
return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.SolverTimedOut,
|
||||
"Lateral SQP exhausted its solve budget.");
|
||||
|
||||
if (!_constraintBuilder.TryBuild(input, iterate, out QuadraticProgram problem, out string failureReason))
|
||||
{
|
||||
return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.LateralInfeasible,
|
||||
"Lateral SQP constraints are infeasible: " + failureReason);
|
||||
}
|
||||
|
||||
QpSolveResult solved = _qpSolver.Solve(problem,
|
||||
new QpSolverSettings(settings.MaximumIterations, settings.AbsoluteTolerance, settings.RelativeTolerance,
|
||||
remainingBudget, settings.EnableWarmStart, settings.EnablePolishing, settings.EnableNativeVerboseOutput),
|
||||
warmStart, cancellationToken);
|
||||
|
||||
if (solved == null)
|
||||
return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.Failed, "The lateral QP solver returned no result.");
|
||||
|
||||
if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
|
||||
{
|
||||
return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.SolverTimedOut,
|
||||
"The lateral QP solver timed out: " + solved.Diagnostic);
|
||||
}
|
||||
if (solved.Status == QpSolveStatus.Cancelled)
|
||||
return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.Cancelled,
|
||||
"The lateral QP solver was cancelled: " + solved.Diagnostic);
|
||||
if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
|
||||
{
|
||||
return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.LateralInfeasible,
|
||||
"The lateral QP solver reported infeasibility: " + solved.Diagnostic);
|
||||
}
|
||||
if (solved.Status == QpSolveStatus.SolverUnavailable)
|
||||
return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.SolverUnavailable,
|
||||
"The lateral QP solver is unavailable: " + solved.Diagnostic);
|
||||
if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
|
||||
{
|
||||
return FallbackOrFailure(lastValidatedPath, EmPlanningStatus.Failed,
|
||||
"The lateral QP solver failed: " + solved.Diagnostic);
|
||||
}
|
||||
if (solved.Status == QpSolveStatus.SolvedInaccurate && !HasStrictResiduals(solved, input.Configuration.Solver.StrictResidualTolerance))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
if (!TryCreateCandidate(input.ReferenceStations, solved.Primal, out LateralCandidate candidate))
|
||||
continue;
|
||||
if (!_geometryEvaluator.TryEvaluate(input, candidate, out LateralPath evaluatedPath, out _))
|
||||
continue;
|
||||
if (!_solutionValidator.TryValidate(input, candidate, evaluatedPath, out LateralPath validatedPath, out _))
|
||||
continue;
|
||||
|
||||
double maximumLateralChange = MaximumLateralChange(iterate, candidate);
|
||||
double relativeObjectiveImprovement = hasPreviousObjective
|
||||
? RelativeObjectiveImprovement(previousObjective, solved.Objective)
|
||||
: double.PositiveInfinity;
|
||||
lastValidatedPath = CopyPath(validatedPath);
|
||||
iterate = candidate;
|
||||
warmStart = CopyValues(solved.Primal);
|
||||
previousObjective = solved.Objective;
|
||||
hasPreviousObjective = true;
|
||||
|
||||
if (maximumLateralChange <= convergenceTolerance && relativeObjectiveImprovement <= convergenceTolerance)
|
||||
return new LateralPlanningResult(EmPlanningStatus.Success, lastValidatedPath, string.Empty);
|
||||
}
|
||||
|
||||
return lastValidatedPath == null
|
||||
? Failed(EmPlanningStatus.LateralInfeasible, "No independently validated lateral candidate was found.")
|
||||
: new LateralPlanningResult(EmPlanningStatus.Success, lastValidatedPath, string.Empty);
|
||||
}
|
||||
|
||||
private static bool TryCreateSettings(LateralPlanningInput input, out QpSolverSettings settings, out TimeSpan totalBudget,
|
||||
out double convergenceTolerance, out string failureReason)
|
||||
{
|
||||
settings = null;
|
||||
totalBudget = TimeSpan.Zero;
|
||||
convergenceTolerance = 0d;
|
||||
failureReason = string.Empty;
|
||||
SolverConfiguration solver = input.Configuration.Solver;
|
||||
SchedulingConfiguration scheduling = input.Configuration.Scheduling;
|
||||
if (solver == null || scheduling == null || solver.MaximumOuterIterations <= 0 ||
|
||||
!IsPositiveFinite(solver.AbsoluteTolerance) || !IsPositiveFinite(solver.RelativeTolerance) ||
|
||||
!IsPositiveFinite(solver.StrictResidualTolerance) || !IsPositiveFinite(scheduling.SolverTimeoutSeconds))
|
||||
{
|
||||
failureReason = "The lateral SQP solver configuration is invalid.";
|
||||
return false;
|
||||
}
|
||||
try
|
||||
{
|
||||
totalBudget = TimeSpan.FromSeconds(scheduling.SolverTimeoutSeconds);
|
||||
settings = new QpSolverSettings(solver.MaximumOsqpIterations, solver.AbsoluteTolerance, solver.RelativeTolerance,
|
||||
totalBudget, solver.WarmStart, solver.Polish, solver.NativeVerbose);
|
||||
convergenceTolerance = solver.StrictResidualTolerance;
|
||||
return true;
|
||||
}
|
||||
catch (ArgumentException exception)
|
||||
{
|
||||
failureReason = exception.Message;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
private static LateralCandidate CreateInitialIterate(LateralPlanningInput input)
|
||||
{
|
||||
int stationCount = input.ReferenceStations.Count;
|
||||
var l = new double[stationCount];
|
||||
var dl = new double[stationCount];
|
||||
var ddl = new double[stationCount];
|
||||
var dddl = new double[stationCount - 1];
|
||||
bool coversAllStations = input.PreviousTrajectorySeed.Count >= 2 &&
|
||||
input.PreviousTrajectorySeed[0].ReferenceS <= input.ReferenceStations[0] + StationTolerance &&
|
||||
input.PreviousTrajectorySeed[input.PreviousTrajectorySeed.Count - 1].ReferenceS >=
|
||||
input.ReferenceStations[stationCount - 1] - StationTolerance;
|
||||
|
||||
for (int index = 0; index < stationCount; index++)
|
||||
{
|
||||
LateralInterval corridor = input.Corridor.Stations[index];
|
||||
l[index] = coversAllStations
|
||||
? InterpolateSeedL(input.PreviousTrajectorySeed, input.ReferenceStations[index])
|
||||
: Clamp(0d, corridor.MinimumL, corridor.MaximumL);
|
||||
}
|
||||
|
||||
double startDenominator = 1d - input.StartProjection.ReferencePoint.GeometricCurvature *
|
||||
input.StartProjection.LateralOffset;
|
||||
l[0] = input.StartProjection.LateralOffset;
|
||||
dl[0] = startDenominator * Math.Tan(input.StartProjection.HeadingError);
|
||||
return new LateralCandidate(input.ReferenceStations, l, dl, ddl, dddl);
|
||||
}
|
||||
|
||||
private static bool TryCreateCandidate(IReadOnlyList<double> stations, IReadOnlyList<double> primal,
|
||||
out LateralCandidate candidate)
|
||||
{
|
||||
candidate = null;
|
||||
if (primal == null)
|
||||
return false;
|
||||
try
|
||||
{
|
||||
var layout = new LateralVariableLayout(stations.Count);
|
||||
if (primal.Count != layout.VariableCount)
|
||||
return false;
|
||||
var l = new double[layout.StationCount];
|
||||
var dl = new double[layout.StationCount];
|
||||
var ddl = new double[layout.StationCount];
|
||||
var dddl = new double[layout.StationCount - 1];
|
||||
for (int index = 0; index < layout.StationCount; index++)
|
||||
{
|
||||
l[index] = primal[layout.L(index)];
|
||||
dl[index] = primal[layout.DL(index)];
|
||||
ddl[index] = primal[layout.DDL(index)];
|
||||
}
|
||||
for (int index = 0; index < dddl.Length; index++)
|
||||
dddl[index] = primal[layout.DDDL(index)];
|
||||
candidate = new LateralCandidate(stations, l, dl, ddl, dddl);
|
||||
return true;
|
||||
}
|
||||
catch (ArgumentException)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
private static bool HasStrictResiduals(QpSolveResult result, double tolerance)
|
||||
{
|
||||
return IsPositiveFinite(tolerance) && result.PrimalResidual >= 0d && result.DualResidual >= 0d &&
|
||||
result.PrimalResidual <= tolerance && result.DualResidual <= tolerance;
|
||||
}
|
||||
|
||||
private static double MaximumLateralChange(LateralCandidate previous, LateralCandidate current)
|
||||
{
|
||||
double maximum = 0d;
|
||||
for (int index = 0; index < previous.L.Count; index++)
|
||||
maximum = Math.Max(maximum, Math.Abs(current.L[index] - previous.L[index]));
|
||||
return maximum;
|
||||
}
|
||||
|
||||
private static double RelativeObjectiveImprovement(double previous, double current)
|
||||
{
|
||||
return Math.Abs(previous - current) / Math.Max(1d, Math.Abs(previous));
|
||||
}
|
||||
|
||||
private static LateralPlanningResult FallbackOrFailure(LateralPath path, EmPlanningStatus failureStatus, string failureReason)
|
||||
{
|
||||
return path == null
|
||||
? Failed(failureStatus, failureReason)
|
||||
: new LateralPlanningResult(EmPlanningStatus.SuccessWithFallback, path, failureReason);
|
||||
}
|
||||
|
||||
private static LateralPlanningResult Failed(EmPlanningStatus status, string reason)
|
||||
{
|
||||
return new LateralPlanningResult(status, null, reason);
|
||||
}
|
||||
|
||||
private static LateralPath CopyPath(LateralPath source)
|
||||
{
|
||||
var points = new List<LateralPathPoint>(source.Points.Count);
|
||||
for (int index = 0; index < source.Points.Count; index++)
|
||||
{
|
||||
LateralPathPoint point = source.Points[index];
|
||||
points.Add(new LateralPathPoint(point.ReferenceS, point.PathS, point.L, point.DL, point.DDL, point.DDDL,
|
||||
point.X, point.Y, point.VehicleYaw, point.GeometricCurvature, point.VehicleCurvature,
|
||||
point.VehicleCurvatureDerivative));
|
||||
}
|
||||
return new LateralPath(points, true);
|
||||
}
|
||||
|
||||
private static double[] CopyValues(IReadOnlyList<double> source)
|
||||
{
|
||||
var copy = new double[source.Count];
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
copy[index] = source[index];
|
||||
return copy;
|
||||
}
|
||||
|
||||
private static double InterpolateSeedL(IReadOnlyList<FrenetProjection> seed, double referenceS)
|
||||
{
|
||||
if (referenceS <= seed[0].ReferenceS)
|
||||
return seed[0].LateralOffset;
|
||||
for (int index = 1; index < seed.Count; index++)
|
||||
{
|
||||
if (referenceS <= seed[index].ReferenceS)
|
||||
{
|
||||
FrenetProjection lower = seed[index - 1];
|
||||
FrenetProjection upper = seed[index];
|
||||
double span = upper.ReferenceS - lower.ReferenceS;
|
||||
return span <= StationTolerance ? upper.LateralOffset : lower.LateralOffset +
|
||||
(upper.LateralOffset - lower.LateralOffset) * (referenceS - lower.ReferenceS) / span;
|
||||
}
|
||||
}
|
||||
return seed[seed.Count - 1].LateralOffset;
|
||||
}
|
||||
|
||||
private static double Clamp(double value, double minimum, double maximum)
|
||||
{
|
||||
return Math.Max(minimum, Math.Min(maximum, value));
|
||||
}
|
||||
|
||||
private static bool IsPositiveFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value) && value > 0d;
|
||||
}
|
||||
}
|
||||
@@ -8,11 +8,24 @@ namespace EMPlannerVerificationHost;
|
||||
|
||||
internal sealed class FakeQpSolver : IQpSolver
|
||||
{
|
||||
private readonly QpSolveResult _result;
|
||||
private readonly Queue<QpSolveResult> _results;
|
||||
private readonly List<QuadraticProgram> _problems = new List<QuadraticProgram>();
|
||||
private readonly List<IReadOnlyList<double>> _warmStarts = new List<IReadOnlyList<double>>();
|
||||
|
||||
public FakeQpSolver(QpSolveResult result)
|
||||
: this(new[] { result })
|
||||
{
|
||||
_result = result ?? throw new ArgumentNullException(nameof(result));
|
||||
}
|
||||
|
||||
public FakeQpSolver(IEnumerable<QpSolveResult> results)
|
||||
{
|
||||
if (results == null)
|
||||
throw new ArgumentNullException(nameof(results));
|
||||
_results = new Queue<QpSolveResult>();
|
||||
foreach (QpSolveResult result in results)
|
||||
_results.Enqueue(result ?? throw new ArgumentException("Fake solver results cannot contain null values.", nameof(results)));
|
||||
if (_results.Count == 0)
|
||||
throw new ArgumentException("At least one fake solver result is required.", nameof(results));
|
||||
LastWarmStart = Array.Empty<double>();
|
||||
}
|
||||
|
||||
@@ -22,6 +35,12 @@ internal sealed class FakeQpSolver : IQpSolver
|
||||
|
||||
public IReadOnlyList<double> LastWarmStart { get; private set; }
|
||||
|
||||
public IReadOnlyList<QuadraticProgram> Problems => new ReadOnlyCollection<QuadraticProgram>(_problems);
|
||||
|
||||
public IReadOnlyList<IReadOnlyList<double>> WarmStarts => new ReadOnlyCollection<IReadOnlyList<double>>(_warmStarts);
|
||||
|
||||
public int SolveCallCount => _problems.Count;
|
||||
|
||||
public QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings, IReadOnlyList<double> warmStart,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
@@ -34,6 +53,10 @@ internal sealed class FakeQpSolver : IQpSolver
|
||||
copy.Add(warmStart[index]);
|
||||
}
|
||||
LastWarmStart = new ReadOnlyCollection<double>(copy);
|
||||
return _result;
|
||||
_problems.Add(LastProblem);
|
||||
_warmStarts.Add(LastWarmStart);
|
||||
if (_results.Count == 0)
|
||||
throw new InvalidOperationException("Fake solver was called more often than its scripted result sequence.");
|
||||
return _results.Dequeue();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,236 @@
|
||||
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.");
|
||||
}
|
||||
}
|
||||
@@ -8,9 +8,9 @@ internal static class Program
|
||||
{
|
||||
if (args.Length != 1 || (args[0] != "foundation" && args[0] != "segmentation" && args[0] != "frenet" &&
|
||||
args[0] != "corridor" && args[0] != "optimization" && args[0] != "osqp" && args[0] != "osqp-loader" && args[0] != "osqp-probe" &&
|
||||
args[0] != "all-foundation" && args[0] != "lateral-model"))
|
||||
args[0] != "all-foundation" && args[0] != "lateral-model" && args[0] != "lateral-integration"))
|
||||
{
|
||||
Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model");
|
||||
Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration");
|
||||
return 2;
|
||||
}
|
||||
|
||||
@@ -55,6 +55,11 @@ internal static class Program
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.LateralModelChecks.Run();
|
||||
Console.WriteLine("PASS lateral-model");
|
||||
}
|
||||
if (args[0] == "lateral-integration")
|
||||
{
|
||||
MultiWheelC.TrajectoryPlanning.EMPlanner.LateralIntegrationChecks.Run();
|
||||
Console.WriteLine("PASS lateral-integration");
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
catch (Exception exception)
|
||||
|
||||
Reference in New Issue
Block a user