diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanner.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanner.cs
new file mode 100644
index 0000000..cfe75c4
--- /dev/null
+++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanner.cs
@@ -0,0 +1,20 @@
+using System;
+using System.Threading;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// Public lateral-planning entry point backed by the solver-neutral SQP optimizer.
+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);
+ }
+}
diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/SequentialConvexOptimizer.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/SequentialConvexOptimizer.cs
new file mode 100644
index 0000000..3693481
--- /dev/null
+++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/SequentialConvexOptimizer.cs
@@ -0,0 +1,294 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Threading;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// Runs bounded lateral SQP iterations and retains only independently validated candidates.
+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();
+ 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 stations, IReadOnlyList 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(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 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 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;
+ }
+}
diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/FakeQpSolver.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/FakeQpSolver.cs
index 9d64a74..9651709 100644
--- a/ClumsyPilot/tests/EMPlannerVerificationHost/FakeQpSolver.cs
+++ b/ClumsyPilot/tests/EMPlannerVerificationHost/FakeQpSolver.cs
@@ -8,11 +8,24 @@ namespace EMPlannerVerificationHost;
internal sealed class FakeQpSolver : IQpSolver
{
- private readonly QpSolveResult _result;
+ private readonly Queue _results;
+ private readonly List _problems = new List();
+ private readonly List> _warmStarts = new List>();
public FakeQpSolver(QpSolveResult result)
+ : this(new[] { result })
{
- _result = result ?? throw new ArgumentNullException(nameof(result));
+ }
+
+ public FakeQpSolver(IEnumerable results)
+ {
+ if (results == null)
+ throw new ArgumentNullException(nameof(results));
+ _results = new Queue();
+ 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();
}
@@ -22,6 +35,12 @@ internal sealed class FakeQpSolver : IQpSolver
public IReadOnlyList LastWarmStart { get; private set; }
+ public IReadOnlyList Problems => new ReadOnlyCollection(_problems);
+
+ public IReadOnlyList> WarmStarts => new ReadOnlyCollection>(_warmStarts);
+
+ public int SolveCallCount => _problems.Count;
+
public QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings, IReadOnlyList warmStart,
CancellationToken cancellationToken)
{
@@ -34,6 +53,10 @@ internal sealed class FakeQpSolver : IQpSolver
copy.Add(warmStart[index]);
}
LastWarmStart = new ReadOnlyCollection(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();
}
}
diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LateralIntegrationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LateralIntegrationChecks.cs
new file mode 100644
index 0000000..030ae09
--- /dev/null
+++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LateralIntegrationChecks.cs
@@ -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(), 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(), 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(), 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(), 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(), 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();
+ 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(), 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
+ {
+ 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());
+ }
+
+ 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 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.");
+ }
+}
diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs
index 4dbd108..1f1c4e6 100644
--- a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs
+++ b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs
@@ -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)