From f19df53f7329f1922292373e3a975116306a86da Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Tue, 4 Aug 2026 08:46:24 +0800 Subject: [PATCH] feat: optimize lateral paths with SQP --- .../EMPlanner/Lateral/LateralPlanner.cs | 20 ++ .../Lateral/SequentialConvexOptimizer.cs | 294 ++++++++++++++++++ .../EMPlannerVerificationHost/FakeQpSolver.cs | 29 +- .../LateralIntegrationChecks.cs | 236 ++++++++++++++ .../EMPlannerVerificationHost/Program.cs | 9 +- 5 files changed, 583 insertions(+), 5 deletions(-) create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanner.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/SequentialConvexOptimizer.cs create mode 100644 ClumsyPilot/tests/EMPlannerVerificationHost/LateralIntegrationChecks.cs 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)