diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanner.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanner.cs
new file mode 100644
index 0000000..e257492
--- /dev/null
+++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanner.cs
@@ -0,0 +1,20 @@
+using System;
+using System.Threading;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// Pure longitudinal planning facade that depends only on the solver-neutral IQpSolver boundary.
+public sealed class LongitudinalPlanner
+{
+ private readonly SequentialLongitudinalOptimizer _optimizer;
+
+ public LongitudinalPlanner(IQpSolver qpSolver)
+ {
+ _optimizer = new SequentialLongitudinalOptimizer(qpSolver ?? throw new ArgumentNullException(nameof(qpSolver)));
+ }
+
+ public LongitudinalPlanningResult Plan(LongitudinalPlanningInput input, CancellationToken cancellationToken)
+ {
+ return _optimizer.Optimize(input, cancellationToken);
+ }
+}
diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs
new file mode 100644
index 0000000..eec8022
--- /dev/null
+++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs
@@ -0,0 +1,153 @@
+using System;
+using System.Collections.Generic;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// Independently validates ST candidates directly in physical units before they may become fallbacks.
+public sealed class LongitudinalSolutionValidator
+{
+ public bool TryValidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate candidate,
+ out LongitudinalCandidate validatedCandidate, out string failureReason)
+ {
+ validatedCandidate = null;
+ failureReason = string.Empty;
+ if (input == null || speedLimit == null || candidate == null)
+ {
+ failureReason = "ST input, speed envelope, and candidate are required.";
+ return false;
+ }
+ try
+ {
+ if (!PathSpeedLimitBuilder.TryGetLimits(input, out _, out double maximumAcceleration,
+ out double maximumDeceleration, out double maximumJerk, out _, out _, out failureReason))
+ {
+ return false;
+ }
+ IReadOnlyList expectedTimes = LongitudinalCandidate.CreateKnotTimes(
+ input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds);
+ double tolerance = RequireNonnegative(input.Configuration.Validation.KinematicTolerance, nameof(tolerance));
+ if (!HasMatchingTimes(candidate.KnotTimes, expectedTimes, tolerance))
+ {
+ failureReason = "ST candidate knot times do not match the configured horizon.";
+ return false;
+ }
+ if (candidate.S.Count != expectedTimes.Count || candidate.U.Count != expectedTimes.Count ||
+ candidate.A.Count != expectedTimes.Count || candidate.J.Count != expectedTimes.Count - 1)
+ {
+ failureReason = "ST candidate value counts do not match its time knots.";
+ return false;
+ }
+ if (!candidate.SatisfiesExactDiscreteDynamics(tolerance))
+ {
+ failureReason = "ST candidate violates exact constant-jerk dynamics.";
+ return false;
+ }
+ if (!AreClose(candidate.S[0], 0d, tolerance) ||
+ !AreClose(candidate.U[0], input.InitialProgressSpeedMetersPerSecond, tolerance) ||
+ !AreClose(candidate.A[0], input.InitialAccelerationMetersPerSecondSquared, tolerance))
+ {
+ failureReason = "ST candidate does not satisfy the exact initial state.";
+ return false;
+ }
+
+ for (int index = 0; index < candidate.S.Count; index++)
+ {
+ double progress = candidate.S[index];
+ double speed = candidate.U[index];
+ double acceleration = candidate.A[index];
+ if (!IsFinite(progress) || !IsFinite(speed) || !IsFinite(acceleration) || progress < -tolerance ||
+ progress > input.TerminalPathS + tolerance || speed < -tolerance ||
+ acceleration < -maximumDeceleration - tolerance || acceleration > maximumAcceleration + tolerance)
+ {
+ failureReason = "ST candidate violates physical bounds at knot " + index + ".";
+ return false;
+ }
+ double speedLimitAtProgress = speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.TerminalPathS, progress)));
+ if (speed > speedLimitAtProgress + tolerance)
+ {
+ failureReason = "ST candidate violates the actual-PathS speed envelope at knot " + index +
+ " (S=" + progress + ", U=" + speed + ", limit=" + speedLimitAtProgress + ").";
+ return false;
+ }
+ if (index > 0 && progress < candidate.S[index - 1] - tolerance)
+ {
+ failureReason = "ST candidate PathS decreases at knot " + index + ".";
+ return false;
+ }
+ }
+ for (int index = 0; index < candidate.J.Count; index++)
+ {
+ if (!IsFinite(candidate.J[index]) || Math.Abs(candidate.J[index]) > maximumJerk + tolerance)
+ {
+ failureReason = "ST candidate violates the jerk bound at interval " + index + ".";
+ return false;
+ }
+ }
+ int terminalIndex = candidate.S.Count - 1;
+ if (!AreClose(candidate.S[terminalIndex], input.TerminalPathS, tolerance) ||
+ !AreClose(candidate.U[terminalIndex], 0d, tolerance))
+ {
+ failureReason = "ST candidate does not satisfy the exact zero-speed terminal.";
+ return false;
+ }
+
+ var canonicalS = new double[candidate.S.Count];
+ var canonicalU = new double[candidate.U.Count];
+ var canonicalA = new double[candidate.A.Count];
+ for (int index = 0; index < candidate.S.Count; index++)
+ {
+ canonicalS[index] = candidate.S[index];
+ canonicalU[index] = candidate.U[index];
+ canonicalA[index] = candidate.A[index];
+ }
+ canonicalS[0] = 0d;
+ canonicalU[0] = input.InitialProgressSpeedMetersPerSecond;
+ canonicalA[0] = input.InitialAccelerationMetersPerSecondSquared;
+ canonicalS[terminalIndex] = input.TerminalPathS;
+ canonicalU[terminalIndex] = 0d;
+ var canonicalCandidate = new LongitudinalCandidate(candidate.KnotTimes, canonicalS, canonicalU, canonicalA,
+ candidate.J);
+ if (!canonicalCandidate.SatisfiesExactDiscreteDynamics(tolerance))
+ {
+ failureReason = "Canonical ST hard-boundary values exceed the dynamics tolerance.";
+ return false;
+ }
+ validatedCandidate = canonicalCandidate;
+ return true;
+ }
+ catch (ArgumentException exception)
+ {
+ failureReason = exception.Message;
+ return false;
+ }
+ }
+
+ private static bool HasMatchingTimes(IReadOnlyList actual, IReadOnlyList expected, double tolerance)
+ {
+ if (actual.Count != expected.Count)
+ return false;
+ for (int index = 0; index < expected.Count; index++)
+ {
+ if (!AreClose(actual[index], expected[index], tolerance))
+ return false;
+ }
+ return true;
+ }
+
+ private static bool AreClose(double actual, double expected, double tolerance)
+ {
+ return Math.Abs(actual - expected) <= tolerance;
+ }
+
+ private static double RequireNonnegative(double value, string parameterName)
+ {
+ if (!IsFinite(value) || value < 0d)
+ throw new ArgumentOutOfRangeException(parameterName);
+ return value;
+ }
+
+ private static bool IsFinite(double value)
+ {
+ return !double.IsNaN(value) && !double.IsInfinity(value);
+ }
+}
diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs
new file mode 100644
index 0000000..70b03eb
--- /dev/null
+++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs
@@ -0,0 +1,461 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Threading;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// Bounded ST envelope iteration retaining only independently validated physical candidates.
+public sealed class SequentialLongitudinalOptimizer
+{
+ private const int MaximumEnvelopeIterations = 5;
+ private const double OrdinaryEnvelopeProbeLookaheadSteps = 1d;
+ private const double OrdinaryTerminalProbeFraction = 0.5d;
+ private readonly IQpSolver _qpSolver;
+ private readonly PathSpeedLimitBuilder _speedLimitBuilder;
+ private readonly LongitudinalConstraintBuilder _constraintBuilder;
+ private readonly LongitudinalSolutionValidator _solutionValidator;
+
+ public SequentialLongitudinalOptimizer(IQpSolver qpSolver)
+ : this(qpSolver, new PathSpeedLimitBuilder(), new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()),
+ new LongitudinalSolutionValidator())
+ {
+ }
+
+ internal SequentialLongitudinalOptimizer(IQpSolver qpSolver, PathSpeedLimitBuilder speedLimitBuilder,
+ LongitudinalConstraintBuilder constraintBuilder, LongitudinalSolutionValidator solutionValidator)
+ {
+ _qpSolver = qpSolver ?? throw new ArgumentNullException(nameof(qpSolver));
+ _speedLimitBuilder = speedLimitBuilder ?? throw new ArgumentNullException(nameof(speedLimitBuilder));
+ _constraintBuilder = constraintBuilder ?? throw new ArgumentNullException(nameof(constraintBuilder));
+ _solutionValidator = solutionValidator ?? throw new ArgumentNullException(nameof(solutionValidator));
+ }
+
+ public LongitudinalPlanningResult Optimize(LongitudinalPlanningInput input, CancellationToken cancellationToken)
+ {
+ if (input == null)
+ return Failed(EmPlanningStatus.InvalidInput, "Longitudinal planning input is required.");
+ if (cancellationToken.IsCancellationRequested)
+ return Failed(EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled.");
+ if (!TryCreateSettings(input, out QpSolverSettings settings, out TimeSpan totalBudget, out double convergenceTolerance,
+ out int iterationLimit, out string configurationFailure))
+ {
+ return Failed(EmPlanningStatus.InvalidInput, configurationFailure);
+ }
+
+ EmPlanningStatus speedStatus = _speedLimitBuilder.Build(input, out PathSpeedLimit speedLimit, out string speedFailure);
+ if (speedStatus != EmPlanningStatus.Success)
+ return Failed(speedStatus, speedFailure);
+
+ LongitudinalCandidate iterate = CreateInitialIterate(input);
+ double[] warmStart = ToPrimal(iterate);
+ bool hasDynamicsConsistentInitialWarmStart = iterate.SatisfiesExactDiscreteDynamics(1e-12d);
+ LongitudinalCandidate lastStrictCandidate;
+ if (!_solutionValidator.TryValidate(input, speedLimit, iterate, out lastStrictCandidate, out _))
+ lastStrictCandidate = null;
+ string lastCandidateRejection = string.Empty;
+ bool hasPreviousObjective = false;
+ double previousObjective = 0d;
+ var stopwatch = Stopwatch.StartNew();
+
+ for (int iteration = 0; iteration < iterationLimit; iteration++)
+ {
+ if (cancellationToken.IsCancellationRequested)
+ return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled.");
+ TimeSpan remainingBudget = totalBudget - stopwatch.Elapsed;
+ if (remainingBudget <= TimeSpan.Zero)
+ {
+ return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverTimedOut,
+ "Longitudinal optimization exhausted its solve budget.");
+ }
+ if (!_constraintBuilder.TryBuild(input, speedLimit, iterate, out QuadraticProgram problem, out string buildFailure))
+ {
+ return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.LongitudinalInfeasible,
+ "Longitudinal constraints are infeasible: " + buildFailure);
+ }
+
+ QpSolveResult solved = _qpSolver.Solve(problem,
+ new QpSolverSettings(settings.MaximumIterations, settings.AbsoluteTolerance, settings.RelativeTolerance,
+ remainingBudget, settings.EnableWarmStart && (iteration > 0 || hasDynamicsConsistentInitialWarmStart),
+ settings.EnablePolishing,
+ settings.EnableNativeVerboseOutput),
+ warmStart, cancellationToken);
+ if (solved == null)
+ return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Failed, "The longitudinal QP solver returned no result.");
+ if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
+ {
+ return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverTimedOut,
+ "The longitudinal QP solver timed out (status=" + solved.NativeStatus +
+ ", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual +
+ ", dual=" + solved.DualResidual + "): " + solved.Diagnostic);
+ }
+ if (solved.Status == QpSolveStatus.Cancelled)
+ return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Cancelled,
+ "The longitudinal QP solver was cancelled: " + solved.Diagnostic);
+ if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
+ {
+ return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.LongitudinalInfeasible,
+ "The longitudinal QP solver reported infeasibility: " + solved.Diagnostic);
+ }
+ if (solved.Status == QpSolveStatus.SolverUnavailable)
+ return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverUnavailable,
+ "The longitudinal QP solver is unavailable: " + solved.Diagnostic);
+ if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
+ {
+ return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Failed,
+ "The longitudinal QP solver failed: " + solved.Diagnostic);
+ }
+ if (solved.Status == QpSolveStatus.SolvedInaccurate && !HasStrictResiduals(solved, convergenceTolerance))
+ {
+ lastCandidateRejection = "SolvedInaccurate residuals exceed the strict acceptance tolerance" +
+ " (primal=" + solved.PrimalResidual + ", dual=" + solved.DualResidual + ").";
+ if (TryCreateCandidate(iterate.KnotTimes, solved.Primal, out LongitudinalCandidate inaccurateCandidate))
+ warmStart = ToPrimal(inaccurateCandidate);
+ continue;
+ }
+ if (!TryCreateCandidate(iterate.KnotTimes, solved.Primal, out LongitudinalCandidate candidate))
+ {
+ lastCandidateRejection = "The solver primal does not match the ST variable layout.";
+ continue;
+ }
+ if (!_solutionValidator.TryValidate(input, speedLimit, candidate, out LongitudinalCandidate validated,
+ out string validationFailure))
+ {
+ string rejection = validationFailure + CreateEnvelopeDiagnostic(speedLimit, iterate, candidate,
+ iteration + 1);
+ lastCandidateRejection = string.IsNullOrEmpty(lastCandidateRejection)
+ ? rejection
+ : lastCandidateRejection + " | " + rejection;
+ if (TryCreateEnvelopeIterate(input, iterate, candidate, out LongitudinalCandidate nextIterate))
+ {
+ iterate = nextIterate;
+ warmStart = ToPrimal(candidate);
+ }
+ continue;
+ }
+
+ double maximumChange = MaximumProgressOrSpeedChange(iterate, validated);
+ double relativeObjectiveImprovement = hasPreviousObjective
+ ? RelativeObjectiveImprovement(previousObjective, solved.Objective)
+ : double.PositiveInfinity;
+ lastStrictCandidate = CopyCandidate(validated);
+ iterate = validated;
+ warmStart = ToPrimal(validated);
+ previousObjective = solved.Objective;
+ hasPreviousObjective = true;
+ if (maximumChange <= convergenceTolerance && relativeObjectiveImprovement <= convergenceTolerance)
+ return new LongitudinalPlanningResult(EmPlanningStatus.Success, lastStrictCandidate, string.Empty);
+ }
+
+ return lastStrictCandidate == null
+ ? Failed(EmPlanningStatus.LongitudinalInfeasible, "No strictly validated longitudinal candidate was found. " +
+ lastCandidateRejection)
+ : new LongitudinalPlanningResult(EmPlanningStatus.Success, lastStrictCandidate, string.Empty);
+ }
+
+ private static bool TryCreateSettings(LongitudinalPlanningInput input, out QpSolverSettings settings,
+ out TimeSpan totalBudget, out double convergenceTolerance, out int iterationLimit, out string failureReason)
+ {
+ settings = null;
+ totalBudget = TimeSpan.Zero;
+ convergenceTolerance = 0d;
+ iterationLimit = 0;
+ failureReason = string.Empty;
+ if (input.Configuration == null || input.Configuration.Solver == null || input.Configuration.Scheduling == null)
+ {
+ failureReason = "Longitudinal solver configuration is required.";
+ return false;
+ }
+ SolverConfiguration solver = input.Configuration.Solver;
+ SchedulingConfiguration scheduling = input.Configuration.Scheduling;
+ if (solver.MaximumOuterIterations <= 0 || solver.MaximumOsqpIterations <= 0 ||
+ !IsPositiveFinite(solver.AbsoluteTolerance) || !IsPositiveFinite(solver.RelativeTolerance) ||
+ !IsPositiveFinite(solver.StrictResidualTolerance) || !IsPositiveFinite(scheduling.SolverTimeoutSeconds))
+ {
+ failureReason = "Longitudinal 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;
+ iterationLimit = Math.Min(MaximumEnvelopeIterations, solver.MaximumOuterIterations);
+ return true;
+ }
+ catch (ArgumentException exception)
+ {
+ failureReason = exception.Message;
+ return false;
+ }
+ }
+
+ private static LongitudinalCandidate CreateInitialIterate(LongitudinalPlanningInput input)
+ {
+ IReadOnlyList times = LongitudinalCandidate.CreateKnotTimes(input.Configuration.Scheduling.TimeHorizonSeconds,
+ input.Configuration.Scheduling.OutputTimeStepSeconds);
+ if (TryCreateCruiseThenBrakeSeed(input, times, out LongitudinalCandidate brakingSeed))
+ return brakingSeed;
+ int knotCount = times.Count;
+ var s = new double[knotCount];
+ var u = new double[knotCount];
+ var a = new double[knotCount];
+ var j = new double[knotCount - 1];
+ double horizon = times[knotCount - 1];
+ double requestedSpeed = Math.Min(input.DirectionMaximumSpeedMetersPerSecond,
+ Math.Max(0d, input.TerminalPathS / horizon));
+ LongitudinalStoppingProfile terminalStop = LongitudinalStoppingMath.Calculate(requestedSpeed, 0d,
+ input.Configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared,
+ input.Configuration.Longitudinal.MaximumJerkMetersPerSecondCubed);
+ double cruiseDistance = Math.Max(0d, input.TerminalPathS - terminalStop.DistanceMeters);
+ for (int index = 0; index < knotCount; index++)
+ {
+ double fraction = (double)index / (knotCount - 1);
+ s[index] = Math.Min(input.TerminalPathS, cruiseDistance * fraction + terminalStop.DistanceMeters * fraction * fraction);
+ u[index] = index == 0 ? input.InitialProgressSpeedMetersPerSecond : requestedSpeed;
+ a[index] = index == 0 ? input.InitialAccelerationMetersPerSecondSquared : 0d;
+ }
+ s[0] = 0d;
+ s[knotCount - 1] = input.TerminalPathS;
+ u[knotCount - 1] = 0d;
+ a[knotCount - 1] = 0d;
+ return new LongitudinalCandidate(times, s, u, a, j);
+ }
+
+ private static bool TryCreateCruiseThenBrakeSeed(LongitudinalPlanningInput input, IReadOnlyList times,
+ out LongitudinalCandidate candidate)
+ {
+ candidate = null;
+ double initialSpeed = input.InitialProgressSpeedMetersPerSecond;
+ double initialAcceleration = input.InitialAccelerationMetersPerSecondSquared;
+ if (initialSpeed <= 0d || Math.Abs(initialAcceleration) > 1e-12d)
+ return false;
+ double timeStep = times[1] - times[0];
+ for (int index = 1; index < times.Count - 1; index++)
+ {
+ if (Math.Abs((times[index + 1] - times[index]) - timeStep) > 1e-12d)
+ return false;
+ }
+ LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
+ int intervalCount = times.Count - 1;
+ int maximumRampIntervals = Math.Min(intervalCount / 2, checked((int)Math.Floor(
+ configuration.MaximumDecelerationMetersPerSecondSquared /
+ (configuration.MaximumJerkMetersPerSecondCubed * timeStep))));
+ for (int rampIntervals = maximumRampIntervals; rampIntervals >= 1; rampIntervals--)
+ {
+ for (int plateauIntervals = 0; 2 * rampIntervals + plateauIntervals <= intervalCount; plateauIntervals++)
+ {
+ double jerkMagnitude = initialSpeed / (rampIntervals * (rampIntervals + plateauIntervals) *
+ timeStep * timeStep);
+ double peakDeceleration = jerkMagnitude * rampIntervals * timeStep;
+ if (jerkMagnitude > configuration.MaximumJerkMetersPerSecondCubed + 1e-12d ||
+ peakDeceleration > configuration.MaximumDecelerationMetersPerSecondSquared + 1e-12d)
+ {
+ continue;
+ }
+ int brakingIntervals = 2 * rampIntervals + plateauIntervals;
+ double brakingDistance = 0.5d * initialSpeed * brakingIntervals * timeStep;
+ if (brakingDistance > input.TerminalPathS + 1e-12d)
+ continue;
+ int maximumCruiseIntervals = intervalCount - brakingIntervals;
+ int cruiseIntervals = Math.Min(maximumCruiseIntervals, Math.Max(0, checked((int)Math.Floor(
+ (input.TerminalPathS - brakingDistance) / (initialSpeed * timeStep) + 1e-12d))));
+ var jerk = new double[intervalCount];
+ int cursor = cruiseIntervals;
+ for (int index = 0; index < rampIntervals; index++)
+ jerk[cursor++] = -jerkMagnitude;
+ cursor += plateauIntervals;
+ for (int index = 0; index < rampIntervals; index++)
+ jerk[cursor++] = jerkMagnitude;
+ LongitudinalCandidate integrated = LongitudinalCandidate.Integrate(times, 0d, initialSpeed, 0d, jerk);
+ if (integrated.S[integrated.S.Count - 1] <= input.TerminalPathS + 1e-12d)
+ {
+ candidate = integrated;
+ return true;
+ }
+ }
+ }
+ return false;
+ }
+
+ private static bool TryCreateCandidate(IReadOnlyList times, IReadOnlyList primal,
+ out LongitudinalCandidate candidate)
+ {
+ candidate = null;
+ if (primal == null)
+ return false;
+ try
+ {
+ var layout = new LongitudinalVariableLayout(times.Count);
+ if (primal.Count != layout.VariableCount)
+ return false;
+ var s = new double[layout.KnotCount];
+ var u = new double[layout.KnotCount];
+ var a = new double[layout.KnotCount];
+ var j = new double[layout.KnotCount - 1];
+ for (int index = 0; index < layout.KnotCount; index++)
+ {
+ s[index] = primal[layout.S(index)];
+ u[index] = primal[layout.U(index)];
+ a[index] = primal[layout.A(index)];
+ }
+ for (int index = 0; index < layout.KnotCount - 1; index++)
+ j[index] = primal[layout.J(index)];
+ candidate = new LongitudinalCandidate(times, s, u, a, j);
+ return true;
+ }
+ catch (ArgumentException)
+ {
+ return false;
+ }
+ }
+
+ private static double[] ToPrimal(LongitudinalCandidate candidate)
+ {
+ var layout = new LongitudinalVariableLayout(candidate.KnotTimes.Count);
+ var primal = new double[layout.VariableCount];
+ for (int index = 0; index < layout.KnotCount; index++)
+ {
+ primal[layout.S(index)] = candidate.S[index];
+ primal[layout.U(index)] = candidate.U[index];
+ primal[layout.A(index)] = candidate.A[index];
+ }
+ for (int index = 0; index < layout.KnotCount - 1; index++)
+ primal[layout.J(index)] = candidate.J[index];
+ return primal;
+ }
+
+ private static bool TryCreateEnvelopeIterate(LongitudinalPlanningInput input, LongitudinalCandidate previous,
+ LongitudinalCandidate candidate, out LongitudinalCandidate nextIterate)
+ {
+ nextIterate = null;
+ if (candidate.S.Count != previous.S.Count)
+ return false;
+ var candidateProgressSamples = new double[candidate.S.Count];
+ double priorProgress = double.NegativeInfinity;
+ double priorPreviousProgress = double.NegativeInfinity;
+ for (int index = 0; index < candidate.S.Count; index++)
+ {
+ double candidateProgress = candidate.S[index];
+ double previousProgress = previous.S[index];
+ if (!IsFinite(previousProgress) || previousProgress < 0d || previousProgress > input.TerminalPathS ||
+ previousProgress < priorPreviousProgress)
+ {
+ return false;
+ }
+ if (!IsFinite(candidateProgress))
+ {
+ candidateProgress = previousProgress;
+ }
+ candidateProgress = Math.Max(0d, Math.Min(input.TerminalPathS, candidateProgress));
+ if (index == candidate.S.Count - 1)
+ candidateProgress = input.TerminalPathS;
+ candidateProgress = Math.Max(priorProgress, candidateProgress);
+ candidateProgressSamples[index] = candidateProgress;
+ priorProgress = candidateProgress;
+ priorPreviousProgress = previousProgress;
+ }
+ var progress = new double[candidate.S.Count];
+ double previousNextProgress = 0d;
+ for (int index = 0; index < progress.Length; index++)
+ {
+ double candidateProgress = candidateProgressSamples[index];
+ if (index == 0 || index == progress.Length - 1 || candidateProgress >= input.TerminalPathS)
+ {
+ progress[index] = candidateProgress;
+ }
+ else
+ {
+ double timeStep = candidate.KnotTimes[index + 1] - candidate.KnotTimes[index];
+ double iterationAdvance = Math.Max(0d, candidateProgress - previous.S[index]);
+ double candidateSpeed = IsFinite(candidate.U[index]) ? Math.Max(0d, candidate.U[index]) : 0d;
+ double lookaheadAdvance = IsFinite(candidate.U[index])
+ ? OrdinaryEnvelopeProbeLookaheadSteps * candidateSpeed * timeStep
+ : 0d;
+ double terminalLimitedAdvance = OrdinaryTerminalProbeFraction *
+ (input.TerminalPathS - candidateProgress);
+ double advance = Math.Min(Math.Max(iterationAdvance, lookaheadAdvance), terminalLimitedAdvance);
+ progress[index] = candidateProgress + advance;
+ }
+ progress[index] = Math.Max(previousNextProgress, progress[index]);
+ previousNextProgress = progress[index];
+ }
+ nextIterate = new LongitudinalCandidate(candidate.KnotTimes, progress, previous.U, previous.A, previous.J);
+ return true;
+ }
+
+ 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 string CreateEnvelopeDiagnostic(PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
+ LongitudinalCandidate candidate, int iteration)
+ {
+ int worstIndex = -1;
+ double worstExcess = double.NegativeInfinity;
+ for (int index = 0; index < candidate.S.Count; index++)
+ {
+ if (!IsFinite(candidate.S[index]) || !IsFinite(candidate.U[index]))
+ continue;
+ double candidateProgress = Math.Max(0d, Math.Min(speedLimit.TerminalPathS, candidate.S[index]));
+ double limit = speedLimit.MaximumSpeedAt(candidateProgress);
+ double excess = candidate.U[index] - limit;
+ if (excess > worstExcess)
+ {
+ worstExcess = excess;
+ worstIndex = index;
+ }
+ }
+ if (worstIndex < 0)
+ return "";
+ return " Envelope iteration " + iteration + " used PathS=" + iterate.S[worstIndex] +
+ " and produced PathS=" + candidate.S[worstIndex] + " at its largest speed-envelope excess.";
+ }
+
+ private static double MaximumProgressOrSpeedChange(LongitudinalCandidate previous, LongitudinalCandidate current)
+ {
+ double maximum = 0d;
+ for (int index = 0; index < previous.S.Count; index++)
+ {
+ maximum = Math.Max(maximum, Math.Abs(current.S[index] - previous.S[index]));
+ maximum = Math.Max(maximum, Math.Abs(current.U[index] - previous.U[index]));
+ }
+ return maximum;
+ }
+
+ private static double RelativeObjectiveImprovement(double previous, double current)
+ {
+ return Math.Abs(previous - current) / Math.Max(1d, Math.Abs(previous));
+ }
+
+ private static LongitudinalPlanningResult FallbackOrFailure(LongitudinalCandidate candidate,
+ EmPlanningStatus failureStatus, string failureReason)
+ {
+ return candidate == null
+ ? Failed(failureStatus, failureReason)
+ : new LongitudinalPlanningResult(EmPlanningStatus.SuccessWithFallback, candidate, failureReason);
+ }
+
+ private static LongitudinalPlanningResult Failed(EmPlanningStatus status, string reason)
+ {
+ return new LongitudinalPlanningResult(status, null, reason);
+ }
+
+ private static LongitudinalCandidate CopyCandidate(LongitudinalCandidate source)
+ {
+ return new LongitudinalCandidate(source.KnotTimes, source.S, source.U, source.A, source.J);
+ }
+
+ private static bool IsPositiveFinite(double value)
+ {
+ return IsFinite(value) && value > 0d;
+ }
+
+ private static bool IsFinite(double value)
+ {
+ return !double.IsNaN(value) && !double.IsInfinity(value);
+ }
+}
diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs
new file mode 100644
index 0000000..05a540f
--- /dev/null
+++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs
@@ -0,0 +1,484 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.IO;
+using System.Threading;
+using EMPlannerVerificationHost;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal static class LongitudinalIntegrationChecks
+{
+ public static void Run()
+ {
+ VerifiesLastStrictCandidateSurvivesLaterTimeout();
+ VerifiesInvalidAndInaccurateCandidatesNeverBecomeFallbacks();
+ VerifiesEnvelopeLinearizationAdvancesAfterStrictRejection();
+ VerifiesRejectedTerminalPathSStillUpdatesEnvelope();
+ VerifiesValidatedEndpointsAreCanonical();
+ VerifiesWarmStartAndFiveIterationLimit();
+ VerifiesNonzeroSpeedSeedIsStrictlyFeasible();
+ VerifiesCancellationInfeasibilityAndPlannerDelegation();
+ RunRealOsqpInCleanPluginBundle();
+ }
+
+ public static void RunRealOsqp()
+ {
+ foreach (LongitudinalScenario scenario in CreateRealOsqpScenarios())
+ {
+ LongitudinalPlanningResult first = new LongitudinalPlanner(new OsqpNativeSolver()).Plan(scenario.Input,
+ CancellationToken.None);
+ LongitudinalPlanningResult second = new LongitudinalPlanner(new OsqpNativeSolver()).Plan(scenario.Input,
+ CancellationToken.None);
+ VerifyRealScenario(scenario, first);
+ VerifyRealScenario(scenario, second);
+ VerifyDeterministicResult(scenario.Name, first, second);
+ }
+ }
+
+ private static void RunRealOsqpInCleanPluginBundle()
+ {
+ string pluginDirectory = Path.Combine(Path.GetTempPath(), "em-planner-longitudinal-real-" + Guid.NewGuid().ToString("N"));
+ try
+ {
+ Directory.CreateDirectory(pluginDirectory);
+ foreach (string sourcePath in Directory.GetFiles(AppContext.BaseDirectory))
+ File.Copy(sourcePath, Path.Combine(pluginDirectory, Path.GetFileName(sourcePath)), false);
+ string nativeSource = Path.GetFullPath(Path.Combine(AppContext.BaseDirectory, "..", "..", "..", "..", "..",
+ "ThirdParty", "OSQP", "win-x64", "osqp.dll"));
+ Verification.True(File.Exists(nativeSource), "pinned OSQP DLL is available for the real longitudinal bundle");
+ File.Copy(nativeSource, Path.Combine(pluginDirectory, "osqp.dll"), true);
+
+ var startInfo = new ProcessStartInfo
+ {
+ FileName = Path.Combine(pluginDirectory, "EMPlannerVerificationHost.exe"),
+ Arguments = "longitudinal-real-osqp-probe",
+ WorkingDirectory = pluginDirectory,
+ UseShellExecute = false,
+ CreateNoWindow = true,
+ RedirectStandardOutput = true,
+ RedirectStandardError = true,
+ };
+ using (var process = new Process { StartInfo = startInfo })
+ {
+ process.Start();
+ string standardOutput = process.StandardOutput.ReadToEnd();
+ string standardError = process.StandardError.ReadToEnd();
+ process.WaitForExit();
+ if (process.ExitCode != 0 || standardOutput.IndexOf("PASS longitudinal-real-osqp", StringComparison.Ordinal) < 0)
+ {
+ throw new InvalidOperationException("Real longitudinal OSQP clean-plugin probe exited " + process.ExitCode + ": " +
+ standardError + standardOutput);
+ }
+ }
+ }
+ finally
+ {
+ if (Directory.Exists(pluginDirectory))
+ Directory.Delete(pluginDirectory, true);
+ }
+ }
+
+ private static void VerifiesLastStrictCandidateSurvivesLaterTimeout()
+ {
+ LongitudinalPlanningInput input = CreateFakeInput(out LongitudinalCandidate valid);
+ var solver = new FakeQpSolver(new[]
+ {
+ Result(QpSolveStatus.Solved, ToPrimal(valid), 10d),
+ Result(QpSolveStatus.TimeLimit, Array.Empty(), 10d),
+ });
+
+ LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
+ CancellationToken.None);
+ Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
+ "timeout after strict candidate returns fallback success");
+ LongitudinalCandidate fallback = result.Candidate ?? throw new InvalidOperationException("Fallback candidate was missing.");
+ Verification.NearlyEqual(valid.S[1], fallback.S[1], "last strict candidate remains the fallback");
+ }
+
+ private static void VerifiesInvalidAndInaccurateCandidatesNeverBecomeFallbacks()
+ {
+ LongitudinalPlanningInput input = CreateFakeInput(out LongitudinalCandidate valid);
+ double[] invalid = ToPrimal(valid);
+ var layout = new LongitudinalVariableLayout(valid.KnotTimes.Count);
+ invalid[layout.U(1)] = 10d;
+ var invalidSolver = new FakeQpSolver(new[]
+ {
+ Result(QpSolveStatus.Solved, invalid, 1d),
+ Result(QpSolveStatus.TimeLimit, Array.Empty(), 1d),
+ });
+ LongitudinalPlanningResult invalidResult = new SequentialLongitudinalOptimizer(invalidSolver).Optimize(input,
+ CancellationToken.None);
+ Verification.Equal(EmPlanningStatus.SolverTimedOut, invalidResult.Status,
+ "invalid solver vector cannot become fallback");
+ Verification.True(invalidResult.Candidate == null, "invalid solver vector publishes no candidate");
+
+ var inaccurateSolver = new FakeQpSolver(new[]
+ {
+ Result(QpSolveStatus.SolvedInaccurate, ToPrimal(valid), 1d, 2e-5d, 0d),
+ Result(QpSolveStatus.TimeLimit, Array.Empty(), 1d),
+ });
+ LongitudinalPlanningResult inaccurateResult = new SequentialLongitudinalOptimizer(inaccurateSolver).Optimize(input,
+ CancellationToken.None);
+ Verification.Equal(EmPlanningStatus.SolverTimedOut, inaccurateResult.Status,
+ "inaccurate residual candidate cannot become fallback");
+ Verification.True(inaccurateResult.Candidate == null, "inaccurate residual publishes no candidate");
+ double[] inaccuratePrimal = ToPrimal(valid);
+ for (int index = 0; index < inaccuratePrimal.Length; index++)
+ {
+ Verification.NearlyEqual(inaccuratePrimal[index], inaccurateSolver.WarmStarts[1][index],
+ "inaccurate finite primal only warms the next ST QP " + index);
+ }
+ }
+
+ private static void VerifiesWarmStartAndFiveIterationLimit()
+ {
+ LongitudinalPlanningInput input = CreateFakeInput(out LongitudinalCandidate valid);
+ var firstSolveOnly = new FakeQpSolver(Result(QpSolveStatus.TimeLimit, Array.Empty(), 1d));
+ new SequentialLongitudinalOptimizer(firstSolveOnly).Optimize(input, CancellationToken.None);
+ Verification.Equal(true, firstSolveOnly.LastSettings != null && firstSolveOnly.LastSettings.EnableWarmStart,
+ "first ST solve enables a dynamics-consistent native warm start");
+ Verification.True(FromPrimal(valid.KnotTimes, firstSolveOnly.WarmStarts[0]).SatisfiesExactDiscreteDynamics(1e-12d),
+ "first ST warm start satisfies exact constant-jerk dynamics");
+
+ var results = new List();
+ for (int index = 0; index < 5; index++)
+ results.Add(Result(QpSolveStatus.Solved, ToPrimal(valid), 100d - 10d * index));
+ var solver = new FakeQpSolver(results);
+
+ LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
+ CancellationToken.None);
+ Verification.Equal(EmPlanningStatus.Success, result.Status, "five solved iterations publish success");
+ Verification.Equal(5, solver.SolveCallCount, "ST has a hard five-envelope-iteration maximum");
+ Verification.Equal(new LongitudinalVariableLayout(valid.KnotTimes.Count).VariableCount, solver.WarmStarts[0].Count,
+ "first ST linearization seed remains a complete primal vector");
+ Verification.Equal(true, solver.LastSettings != null && solver.LastSettings.EnableWarmStart,
+ "later ST solves enable native warm start");
+ for (int index = 0; index < solver.WarmStarts[1].Count; index++)
+ Verification.NearlyEqual(ToPrimal(valid)[index], solver.WarmStarts[1][index], "strict candidate warms the next QP " + index);
+ }
+
+ private static void VerifiesEnvelopeLinearizationAdvancesAfterStrictRejection()
+ {
+ LongitudinalPlanningInput baseline = CreateFakeInput(out LongitudinalCandidate candidate);
+ var curvedPath = new LateralPath(new[]
+ {
+ Point(0d, 0d, 0d),
+ Point(1d, candidate.S[1], 10000d),
+ Point(2d, baseline.TerminalPathS, 0d),
+ }, true);
+ var input = new LongitudinalPlanningInput(curvedPath, TravelDirection.Forward,
+ baseline.InitialProgressSpeedMetersPerSecond, baseline.InitialAccelerationMetersPerSecondSquared,
+ baseline.TerminalType, baseline.Configuration, Array.Empty(), Array.Empty());
+ EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
+ out string speedFailure);
+ Verification.Equal(EmPlanningStatus.Success, speedStatus, "curved-envelope setup: " + speedFailure);
+ Verification.True(candidate.U[1] > envelope.MaximumSpeedAt(candidate.S[1]),
+ "scripted candidate violates its own curvature speed envelope");
+ var solver = new FakeQpSolver(new[]
+ {
+ Result(QpSolveStatus.Solved, ToPrimal(candidate), 2d),
+ Result(QpSolveStatus.TimeLimit, Array.Empty(), 2d),
+ });
+
+ new SequentialLongitudinalOptimizer(solver).Optimize(input, CancellationToken.None);
+ Verification.Equal(2, solver.SolveCallCount, "rejected candidate reaches the next envelope iteration");
+ var layout = new LongitudinalVariableLayout(candidate.KnotTimes.Count);
+ FindSingleVariableBounds(solver.Problems[1], layout.U(1), out _, out double secondUpper);
+ double initialProgress = solver.WarmStarts[0][layout.S(1)];
+ double timeStep = candidate.KnotTimes[2] - candidate.KnotTimes[1];
+ double expectedAdvance = Math.Max(Math.Max(0d, candidate.S[1] - initialProgress), candidate.U[1] * timeStep);
+ double expectedProgress = candidate.S[1] + Math.Min(expectedAdvance,
+ 0.5d * (input.TerminalPathS - candidate.S[1]));
+ Verification.True(expectedProgress > candidate.S[1], "scripted rejection advances the ST PathS envelope probe");
+ Verification.NearlyEqual(envelope.MaximumSpeedAt(expectedProgress), secondUpper,
+ "next ST QP samples the bounded forward-extrapolated PathS envelope");
+ }
+
+ private static void VerifiesRejectedTerminalPathSStillUpdatesEnvelope()
+ {
+ LongitudinalPlanningInput baseline = CreateFakeInput(out LongitudinalCandidate valid);
+ double[] perturbedProgress = new double[valid.S.Count];
+ for (int index = 0; index < perturbedProgress.Length; index++)
+ perturbedProgress[index] = valid.S[index];
+ perturbedProgress[perturbedProgress.Length - 1] += 0.05d;
+ var toleranceCandidate = new LongitudinalCandidate(valid.KnotTimes, perturbedProgress, valid.U, valid.A, valid.J);
+ var curvedPath = new LateralPath(new[]
+ {
+ Point(0d, 0d, 0d),
+ Point(1d, valid.S[1], 10000d),
+ Point(2d, baseline.TerminalPathS, 0d),
+ }, true);
+ var input = new LongitudinalPlanningInput(curvedPath, TravelDirection.Forward,
+ baseline.InitialProgressSpeedMetersPerSecond, baseline.InitialAccelerationMetersPerSecondSquared,
+ baseline.TerminalType, baseline.Configuration, Array.Empty(), Array.Empty());
+ var solver = new FakeQpSolver(new[]
+ {
+ Result(QpSolveStatus.Solved, ToPrimal(toleranceCandidate), 2d),
+ Result(QpSolveStatus.TimeLimit, Array.Empty(), 2d),
+ });
+
+ new SequentialLongitudinalOptimizer(solver).Optimize(input, CancellationToken.None);
+ Verification.Equal(2, solver.SolveCallCount, "rejected terminal PathS still reaches a new envelope iteration");
+ var layout = new LongitudinalVariableLayout(valid.KnotTimes.Count);
+ FindSingleVariableBounds(solver.Problems[0], layout.U(1), out _, out double firstUpper);
+ FindSingleVariableBounds(solver.Problems[1], layout.U(1), out _, out double secondUpper);
+ Verification.True(Math.Abs(firstUpper - secondUpper) > 1e-12d,
+ "rejected terminal PathS is projected before the next envelope sample");
+ }
+
+ private static void VerifiesCancellationInfeasibilityAndPlannerDelegation()
+ {
+ LongitudinalPlanningInput input = CreateFakeInput(out LongitudinalCandidate valid);
+ using (var cancellation = new CancellationTokenSource())
+ {
+ cancellation.Cancel();
+ var cancellationSolver = new FakeQpSolver(Result(QpSolveStatus.Solved, ToPrimal(valid), 1d));
+ LongitudinalPlanningResult cancelled = new SequentialLongitudinalOptimizer(cancellationSolver).Optimize(input,
+ cancellation.Token);
+ Verification.Equal(EmPlanningStatus.Cancelled, cancelled.Status, "cancellation before QP solve");
+ Verification.Equal(0, cancellationSolver.SolveCallCount, "cancelled ST does not call the QP solver");
+ }
+
+ var infeasibleSolver = new FakeQpSolver(Result(QpSolveStatus.PrimalInfeasible, Array.Empty(), 1d));
+ LongitudinalPlanningResult infeasible = new SequentialLongitudinalOptimizer(infeasibleSolver).Optimize(input,
+ CancellationToken.None);
+ Verification.Equal(EmPlanningStatus.LongitudinalInfeasible, infeasible.Status, "QP infeasibility is longitudinal");
+
+ var plannerSolver = new FakeQpSolver(new[]
+ {
+ Result(QpSolveStatus.Solved, ToPrimal(valid), 2d),
+ Result(QpSolveStatus.Solved, ToPrimal(valid), 1d),
+ Result(QpSolveStatus.Solved, ToPrimal(valid), 1d),
+ });
+ LongitudinalPlanningResult delegated = new LongitudinalPlanner(plannerSolver).Plan(input, CancellationToken.None);
+ Verification.Equal(EmPlanningStatus.Success, delegated.Status, "LongitudinalPlanner delegates to the ST optimizer");
+ }
+
+ private static void VerifiesNonzeroSpeedSeedIsStrictlyFeasible()
+ {
+ LongitudinalPlanningInput input = CreateRealScenario("seed", TravelDirection.Forward, 0.20d, 0d, 0.20d, 0d).Input;
+ var solver = new FakeQpSolver(Result(QpSolveStatus.TimeLimit, Array.Empty(), 1d));
+ LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
+ CancellationToken.None);
+ IReadOnlyList times = LongitudinalCandidate.CreateKnotTimes(
+ input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds);
+ LongitudinalCandidate seed = FromPrimal(times, solver.WarmStarts[0]);
+ EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
+ out string speedFailure);
+ Verification.Equal(EmPlanningStatus.Success, speedStatus, "nonzero-speed seed envelope: " + speedFailure);
+ Verification.True(new LongitudinalSolutionValidator().TryValidate(input, envelope, seed, out _,
+ out string validationFailure), "nonzero-speed seed is strictly feasible: " + validationFailure);
+ Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
+ "strictly validated initial seed survives an immediate solver timeout");
+ Verification.True(result.Candidate != null,
+ "strictly validated initial seed is retained as the timeout fallback");
+ }
+
+ private static void VerifiesValidatedEndpointsAreCanonical()
+ {
+ LongitudinalPlanningInput input = CreateFakeInput(out LongitudinalCandidate valid);
+ double toleranceOffset = 0.5d * input.Configuration.Validation.KinematicTolerance;
+ double[] progress = new double[valid.S.Count];
+ double[] speed = new double[valid.U.Count];
+ for (int index = 0; index < progress.Length; index++)
+ {
+ progress[index] = valid.S[index];
+ speed[index] = valid.U[index];
+ }
+ progress[progress.Length - 1] += toleranceOffset;
+ speed[speed.Length - 1] += toleranceOffset;
+ var toleranceCandidate = new LongitudinalCandidate(valid.KnotTimes, progress, speed, valid.A, valid.J);
+ var solver = new FakeQpSolver(new[]
+ {
+ Result(QpSolveStatus.Solved, ToPrimal(toleranceCandidate), 2d),
+ Result(QpSolveStatus.TimeLimit, Array.Empty(), 2d),
+ });
+
+ LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
+ CancellationToken.None);
+
+ Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
+ "canonical strict candidate remains the timeout fallback");
+ LongitudinalCandidate canonical = result.Candidate ??
+ throw new InvalidOperationException("Canonical fallback candidate was missing.");
+ Verification.Equal(input.TerminalPathS, canonical.S[canonical.S.Count - 1],
+ "validated terminal PathS is canonicalized exactly");
+ Verification.Equal(0d, canonical.U[canonical.U.Count - 1],
+ "validated terminal speed is canonicalized exactly");
+ }
+
+ private static IReadOnlyList CreateRealOsqpScenarios()
+ {
+ return new[]
+ {
+ CreateRealScenario("forward", TravelDirection.Forward, 0.50d, 0d, 0d, 0d),
+ CreateRealScenario("reverse", TravelDirection.Reverse, 0.50d, 0d, 0d, 0d),
+ CreateRealScenario("curvature-limited", TravelDirection.Forward, 0.35d, 20d, 0d, 0d),
+ CreateRealScenario("jerk-limited-stop", TravelDirection.Forward, 0.20d, 0d, 0.20d, 0d),
+ CreateRealScenario("short-segment", TravelDirection.Forward, 0.05d, 0d, 0d, 0d),
+ CreateRealScenario("zero-start-speed", TravelDirection.Forward, 0.50d, 0d, 0d, 0d),
+ };
+ }
+
+ private static LongitudinalScenario CreateRealScenario(string name, TravelDirection direction, double terminalPathS,
+ double middleCurvature, double initialSpeed, double initialAcceleration)
+ {
+ EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+ configuration.Scheduling.SolverTimeoutSeconds = 1d;
+ configuration.Validation.KinematicTolerance = 1e-5d;
+ var points = new[]
+ {
+ Point(0d, 0d, 0d),
+ Point(1d, terminalPathS * 0.5d, middleCurvature),
+ Point(2d, terminalPathS, 0d),
+ };
+ return new LongitudinalScenario(name, new LongitudinalPlanningInput(new LateralPath(points, true), direction,
+ initialSpeed, initialAcceleration, EmTerminalType.Goal, configuration, Array.Empty(), Array.Empty()));
+ }
+
+ private static void VerifyRealScenario(LongitudinalScenario scenario, LongitudinalPlanningResult result)
+ {
+ Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
+ scenario.Name + " returns a strict profile: " + result.FailureReason);
+ LongitudinalCandidate candidate = result.Candidate ?? throw new InvalidOperationException(scenario.Name + " candidate missing.");
+ Verification.NearlyEqual(scenario.Input.TerminalPathS, candidate.S[candidate.S.Count - 1],
+ scenario.Name + " exact terminal PathS");
+ Verification.NearlyEqual(0d, candidate.U[candidate.U.Count - 1], scenario.Name + " exact terminal speed");
+ PathSpeedLimitBuilder builder = new PathSpeedLimitBuilder();
+ EmPlanningStatus speedStatus = builder.Build(scenario.Input, out PathSpeedLimit envelope, out string speedFailure);
+ Verification.Equal(EmPlanningStatus.Success, speedStatus, scenario.Name + " envelope: " + speedFailure);
+ Verification.True(new LongitudinalSolutionValidator().TryValidate(scenario.Input, envelope, candidate,
+ out _, out string validationFailure), scenario.Name + " strict physical validation: " + validationFailure);
+ }
+
+ private static void VerifyDeterministicResult(string name, LongitudinalPlanningResult first, LongitudinalPlanningResult second)
+ {
+ Verification.Equal(first.Status, second.Status, name + " deterministic status");
+ LongitudinalCandidate left = first.Candidate ?? throw new InvalidOperationException(name + " first candidate missing.");
+ LongitudinalCandidate right = second.Candidate ?? throw new InvalidOperationException(name + " second candidate missing.");
+ for (int index = 0; index < left.S.Count; index++)
+ {
+ Verification.NearlyEqual(left.S[index], right.S[index], name + " deterministic S " + index);
+ Verification.NearlyEqual(left.U[index], right.U[index], name + " deterministic U " + index);
+ Verification.NearlyEqual(left.A[index], right.A[index], name + " deterministic A " + index);
+ }
+ for (int index = 0; index < left.J.Count; index++)
+ Verification.NearlyEqual(left.J[index], right.J[index], name + " deterministic J " + index);
+ }
+
+ private static LongitudinalPlanningInput CreateFakeInput(out LongitudinalCandidate valid)
+ {
+ EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+ configuration.Scheduling.TimeHorizonSeconds = 1d;
+ configuration.Scheduling.OutputTimeStepSeconds = 0.25d;
+ configuration.Scheduling.SolverTimeoutSeconds = 1d;
+ configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
+ configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
+ configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1d;
+ configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
+ configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1d;
+ configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
+ configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
+ IReadOnlyList times = LongitudinalCandidate.CreateKnotTimes(1d, 0.25d);
+ valid = LongitudinalCandidate.Integrate(times, 0d, 0.10d, 0d,
+ new[] { -0.45714285714285714d, 0d, 0d, 0d });
+ double terminalPathS = valid.S[valid.S.Count - 1];
+ var path = new LateralPath(new[]
+ {
+ Point(0d, 0d, 0d),
+ Point(1d, terminalPathS * 0.5d, 0d),
+ Point(2d, terminalPathS, 0d),
+ }, true);
+ return new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0d, EmTerminalType.Goal,
+ configuration, Array.Empty(), Array.Empty());
+ }
+
+ private static LateralPathPoint Point(double referenceS, double pathS, double curvature)
+ {
+ return new LateralPathPoint(referenceS, pathS, 0d, 0d, 0d, 0d, pathS, 0d, 0d, curvature, curvature, 0d);
+ }
+
+ 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[] ToPrimal(LongitudinalCandidate candidate)
+ {
+ var layout = new LongitudinalVariableLayout(candidate.KnotTimes.Count);
+ var primal = new double[layout.VariableCount];
+ for (int index = 0; index < layout.KnotCount; index++)
+ {
+ primal[layout.S(index)] = candidate.S[index];
+ primal[layout.U(index)] = candidate.U[index];
+ primal[layout.A(index)] = candidate.A[index];
+ }
+ for (int index = 0; index < layout.KnotCount - 1; index++)
+ primal[layout.J(index)] = candidate.J[index];
+ return primal;
+ }
+
+ private static LongitudinalCandidate FromPrimal(IReadOnlyList times, IReadOnlyList primal)
+ {
+ var layout = new LongitudinalVariableLayout(times.Count);
+ var s = new double[layout.KnotCount];
+ var u = new double[layout.KnotCount];
+ var a = new double[layout.KnotCount];
+ var j = new double[layout.KnotCount - 1];
+ for (int index = 0; index < layout.KnotCount; index++)
+ {
+ s[index] = primal[layout.S(index)];
+ u[index] = primal[layout.U(index)];
+ a[index] = primal[layout.A(index)];
+ }
+ for (int index = 0; index < j.Length; index++)
+ j[index] = primal[layout.J(index)];
+ return new LongitudinalCandidate(times, s, u, a, j);
+ }
+
+ private static void FindSingleVariableBounds(QuadraticProgram problem, int variable, out double lower, out double upper)
+ {
+ for (int row = 0; row < problem.ConstraintCount; row++)
+ {
+ int found = 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)
+ {
+ found++;
+ if (column == variable)
+ coefficient = problem.ConstraintMatrix.Values[index];
+ }
+ }
+ }
+ if (found == 1 && Math.Abs(coefficient - 1d) <= 1e-12d)
+ {
+ lower = problem.LowerBounds[row];
+ upper = problem.UpperBounds[row];
+ return;
+ }
+ }
+ throw new InvalidOperationException("No single-variable bounds were found for ST variable " + variable + ".");
+ }
+
+ private sealed class LongitudinalScenario
+ {
+ public LongitudinalScenario(string name, LongitudinalPlanningInput input)
+ {
+ Name = name;
+ Input = input;
+ }
+
+ public string Name { get; }
+
+ public LongitudinalPlanningInput Input { get; }
+ }
+}
diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs
index 107992e..6b1980f 100644
--- a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs
+++ b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs
@@ -10,9 +10,10 @@ internal static class Program
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] != "lateral-integration" &&
args[0] != "lateral-real-osqp" && args[0] != "lateral-real-osqp-probe" && args[0] != "lateral-all" &&
- args[0] != "longitudinal-model"))
+ args[0] != "longitudinal-model" && args[0] != "longitudinal-integration" &&
+ args[0] != "longitudinal-real-osqp-probe"))
{
- Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration|lateral-real-osqp|lateral-all");
+ Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration|lateral-real-osqp|lateral-all|longitudinal-model|longitudinal-integration");
return 2;
}
@@ -77,6 +78,16 @@ internal static class Program
MultiWheelC.TrajectoryPlanning.EMPlanner.LongitudinalModelChecks.Run();
Console.WriteLine("PASS longitudinal-model");
}
+ if (args[0] == "longitudinal-integration")
+ {
+ MultiWheelC.TrajectoryPlanning.EMPlanner.LongitudinalIntegrationChecks.Run();
+ Console.WriteLine("PASS longitudinal-integration");
+ }
+ if (args[0] == "longitudinal-real-osqp-probe")
+ {
+ MultiWheelC.TrajectoryPlanning.EMPlanner.LongitudinalIntegrationChecks.RunRealOsqp();
+ Console.WriteLine("PASS longitudinal-real-osqp");
+ }
return 0;
}
catch (Exception exception)