fix: accelerate EM trajectories from rest

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs

@@ -84,7 +84,7 @@ public sealed class EmPlanningService : IEmPlanningService
             EmitDebug(request, "LS optimization and validation succeeded");
 
             EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(lateral.Path, segment.Direction,
-                initialProgressSpeed, horizon.TerminalType, configuration, out PathSpeedLimit speedLimit,
+                initialProgressSpeed, initialAcceleration, horizon.TerminalType, configuration, out PathSpeedLimit speedLimit,
                 out string envelopeReason);
             if (envelopeStatus != EmPlanningStatus.Success)
                 return Failure(envelopeStatus, request, envelopeReason);

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/FullDirectionSegmentScheduleBuilder.cs

@@ -167,9 +167,12 @@ public sealed class FullDirectionSegmentScheduleBuilder
         LongitudinalConfiguration configuration)
     {
         double adjustedTime = 0d;
+        double requestedPreviousTime = times[0];
         for (int index = 1; index < times.Count; index++)
         {
-            double requestedDuration = times[index] - times[index - 1];
+            double requestedTime = times[index];
+            double requestedDuration = requestedTime - requestedPreviousTime;
+            requestedPreviousTime = requestedTime;
             double speedChange = Math.Abs(referenceSpeeds[index] - referenceSpeeds[index - 1]);
             double accelerationLimit = referenceSpeeds[index] >= referenceSpeeds[index - 1]
                 ? configuration.MaximumAccelerationMetersPerSecondSquared
@@ -191,7 +194,10 @@ public sealed class FullDirectionSegmentScheduleBuilder
         {
             double previousSlope = (speeds[index] - speeds[index - 1]) / (pathS[index] - pathS[index - 1]);
             double nextSlope = (speeds[index + 1] - speeds[index]) / (pathS[index + 1] - pathS[index]);
-            if (previousSlope * nextSlope < 0d)
+            bool changesDirection = previousSlope * nextSlope < 0d;
+            bool entersCruise = previousSlope > Tolerance && nextSlope <= Tolerance;
+            bool leavesCruise = previousSlope >= -Tolerance && nextSlope < -Tolerance;
+            if (changesDirection || entersCruise || leavesCruise)
                 stations.Add(index);
         }
         stations.Add(pathS.Count - 1);

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalObjectiveBuilder.cs

@@ -26,7 +26,10 @@ public sealed class LongitudinalObjectiveBuilder
         for (int index = 0; index < layout.KnotCount; index++)
         {
             double iteratePathS = Math.Max(0d, Math.Min(input.PathUpperBoundS, iterate.S[index]));
-            AddSquaredResidual(hessian, linearCost, layout.U(index), speedLimit.MaximumSpeedAt(iteratePathS),
+            double referenceSpeed = input.PlanningScope == EmPlanningScope.FullDirectionSegment
+                ? input.KnotSchedule.ReferenceSpeedMetersPerSecond[index]
+                : speedLimit.MaximumSpeedAt(iteratePathS);
+            AddSquaredResidual(hessian, linearCost, layout.U(index), referenceSpeed,
                 weights.ReferenceSpeed, speedScale);
             AddSquaredResidual(hessian, linearCost, layout.A(index), 0d, weights.Acceleration, accelerationScale);
             if (index < layout.KnotCount - 1 && index < input.PreviousPathS.Count)

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs

@@ -8,8 +8,15 @@ public sealed class LongitudinalSolutionValidator
 {
     public bool TryValidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate candidate,
         out LongitudinalCandidate validatedCandidate, out string failureReason)
+    {
+        return TryValidate(input, speedLimit, candidate, out validatedCandidate, out _, out failureReason);
+    }
+
+    public bool TryValidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate candidate,
+        out LongitudinalCandidate validatedCandidate, out EmPlanningStatus failureStatus, out string failureReason)
     {
         validatedCandidate = null;
+        failureStatus = EmPlanningStatus.LongitudinalInfeasible;
         failureReason = string.Empty;
         if (input == null || speedLimit == null || candidate == null)
         {
@@ -58,7 +65,8 @@ public sealed class LongitudinalSolutionValidator
                     progress > input.PathUpperBoundS + tolerance || speed < -tolerance ||
                     acceleration < -maximumDeceleration - tolerance || acceleration > maximumAcceleration + tolerance)
                 {
-                    failureReason = "ST candidate violates physical bounds at knot " + index + ".";
+                    failureReason = "ST candidate violates physical bounds at knot " + index +
+                        " (S=" + progress + ", U=" + speed + ", A=" + acceleration + ").";
                     return false;
                 }
                 double speedLimitAtProgress = index == 0
@@ -84,6 +92,16 @@ public sealed class LongitudinalSolutionValidator
                     return false;
                 }
             }
+            bool requiresProgress = input.PathUpperBoundS >
+                input.Configuration.Validation.TerminalPositionToleranceMeters;
+            double achievedProgress = candidate.S[candidate.S.Count - 1] - candidate.S[0];
+            if (input.PlanningScope == EmPlanningScope.FullDirectionSegment && requiresProgress &&
+                achievedProgress <= input.Configuration.Validation.SpatialToleranceMeters)
+            {
+                failureStatus = EmPlanningStatus.NoProgress;
+                failureReason = "NoProgress: a nonterminal full direction segment produced zero progress.";
+                return false;
+            }
             int stabilizationStart = candidate.S.Count;
             if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
             {

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs

@@ -32,6 +32,15 @@ public sealed class PathSpeedLimitBuilder
             out speedLimit, out failureReason);
     }
 
+    public EmPlanningStatus Build(LateralPath path, TravelDirection direction,
+        double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
+        EmTerminalType terminalType, EmPlannerConfiguration configuration, out PathSpeedLimit speedLimit,
+        out string failureReason)
+    {
+        return BuildCore(path, direction, initialProgressSpeedMetersPerSecond,
+            initialAccelerationMetersPerSecondSquared, terminalType, configuration, out speedLimit, out failureReason);
+    }
+
     private EmPlanningStatus BuildCore(LateralPath path, TravelDirection direction,
         double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
         EmTerminalType terminalType, EmPlannerConfiguration configuration, out PathSpeedLimit speedLimit,
@@ -62,6 +71,7 @@ public sealed class PathSpeedLimitBuilder
         double maximumJerk = longitudinal.MaximumJerkMetersPerSecondCubed;
         double maximumLateralAcceleration = longitudinal.MaximumLateralAccelerationMetersPerSecondSquared;
         double maximumCurvatureRate = longitudinal.MaximumCurvatureRatePerMeterPerSecond;
+        double stoppingAcceleration = Math.Max(0d, initialAccelerationMetersPerSecondSquared);
         if (!IsPositiveFinite(directionMaximum) || !IsPositiveFinite(maximumAcceleration) ||
             !IsPositiveFinite(maximumDeceleration) || !IsPositiveFinite(maximumJerk) ||
             !IsPositiveFinite(maximumLateralAcceleration) || !IsPositiveFinite(maximumCurvatureRate))
@@ -118,7 +128,7 @@ public sealed class PathSpeedLimitBuilder
             if (hasStopBoundary)
             {
                 AddJerkLimitedStoppingStations(lowerPoint.PathS, upperPoint.PathS, stopBoundaryPathS,
-                    directionMaximum, maximumAcceleration, maximumDeceleration, maximumJerk,
+                    directionMaximum, stoppingAcceleration, maximumDeceleration, maximumJerk,
                     segmentIndex == 0, segmentStations);
             }
             segmentStations.Sort();
@@ -134,7 +144,7 @@ public sealed class PathSpeedLimitBuilder
                 double curvatureDerivative = Interpolate(lowerPoint.VehicleCurvatureDerivative,
                     upperPoint.VehicleCurvatureDerivative, fraction);
                 AddLimitSample(samplePathS, curvature, curvatureDerivative, hasStopBoundary,
-                    stopBoundaryPathS, directionMaximum, maximumAcceleration, maximumDeceleration,
+                    stopBoundaryPathS, directionMaximum, stoppingAcceleration, maximumDeceleration,
                     maximumJerk, maximumLateralAcceleration, maximumCurvatureRate, pathS, maximum, lateral,
                     curvatureRate, stopping);
             }
@@ -188,14 +198,14 @@ public sealed class PathSpeedLimitBuilder
     }
 
     private static void AddJerkLimitedStoppingStations(double lowerPathS, double upperPathS,
-        double stopBoundaryPathS, double directionMaximum, double maximumAcceleration, double maximumDeceleration,
+        double stopBoundaryPathS, double directionMaximum, double stoppingAcceleration, double maximumDeceleration,
         double maximumJerk, bool includeLower, IList<double> stations)
     {
         const int stoppingSpeedSampleCount = 64;
         for (int step = 0; step < stoppingSpeedSampleCount; step++)
         {
             double speed = directionMaximum * step / stoppingSpeedSampleCount;
-            if (!JerkLimitedStoppingMath.TryCalculate(speed, maximumAcceleration,
+            if (!JerkLimitedStoppingMath.TryCalculate(speed, stoppingAcceleration,
                     maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stop, out _))
             {
                 throw new ArgumentException("The configured jerk-limited stop envelope cannot be sampled.");
@@ -210,7 +220,7 @@ public sealed class PathSpeedLimitBuilder
     }
 
     private static void AddLimitSample(double samplePathS, double curvature, double curvatureDerivative,
-        bool hasStopBoundary, double stopBoundaryPathS, double directionMaximum, double maximumAcceleration,
+        bool hasStopBoundary, double stopBoundaryPathS, double directionMaximum, double stoppingAcceleration,
         double maximumDeceleration, double maximumJerk, double maximumLateralAcceleration,
         double maximumCurvatureRate, IList<double> pathS, IList<double> maximum, IList<double> lateral,
         IList<double> curvatureRate, IList<double> stopping)
@@ -220,7 +230,7 @@ public sealed class PathSpeedLimitBuilder
         double curvatureRateLimit = maximumCurvatureRate / Math.Max(Math.Abs(curvatureDerivative), CurvatureEpsilon);
         double stoppingLimit = hasStopBoundary
             ? JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
-                Math.Max(0d, stopBoundaryPathS - samplePathS), maximumAcceleration,
+                Math.Max(0d, stopBoundaryPathS - samplePathS), stoppingAcceleration,
                 maximumDeceleration, maximumJerk, directionMaximum)
             : directionMaximum;
         double lateralValue = ClampFinite(lateralLimit, directionMaximum);

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs

@@ -2,6 +2,7 @@ using System;
 using System.Collections.Generic;
 using System.Diagnostics;
 using System.Threading;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
@@ -222,6 +223,14 @@ public sealed class SequentialLongitudinalOptimizer
         projectionSolveCount = 0;
         failureStatus = EmPlanningStatus.LongitudinalInfeasible;
         failureReason = string.Empty;
+        if (input.InitialProgressSpeedMetersPerSecond <= input.Configuration.Validation.SpatialToleranceMeters &&
+            Math.Abs(input.InitialAccelerationMetersPerSecondSquared) <=
+                input.Configuration.Validation.KinematicTolerance &&
+            TryCreateStaticStartSeed(input, speedLimit, out LongitudinalCandidate staticStartSeed))
+        {
+            candidate = staticStartSeed;
+            return true;
+        }
         LongitudinalCandidate linearizationIterate = CreateScheduleReferenceIterate(input);
         string lastRejection = string.Empty;
         for (int iteration = 0; iteration < iterationLimit; iteration++)
@@ -302,11 +311,17 @@ public sealed class SequentialLongitudinalOptimizer
             if (solved.Status == QpSolveStatus.Solved || HasStrictResiduals(solved, convergenceTolerance))
             {
                 if (_solutionValidator.TryValidate(input, speedLimit, projected, out LongitudinalCandidate strict,
-                        out string validationFailure))
+                        out EmPlanningStatus validationStatus, out string validationFailure))
                 {
                     candidate = strict;
                     return true;
                 }
+                if (validationStatus == EmPlanningStatus.NoProgress)
+                {
+                    failureStatus = validationStatus;
+                    failureReason = validationFailure;
+                    return false;
+                }
                 lastRejection = validationFailure;
             }
 
@@ -377,6 +392,14 @@ public sealed class SequentialLongitudinalOptimizer
             double dt = times[index + 1] - times[index];
             double speedLimitAtS = speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.PathUpperBoundS, s)));
             double targetSpeed = Math.Min(input.InitialProgressSpeedMetersPerSecond, speedLimitAtS);
+            if (input.PlanningScope == EmPlanningScope.FullDirectionSegment)
+            {
+                double desiredSpeed = input.Direction == TravelDirection.Forward
+                    ? configuration.DesiredForwardSpeedMetersPerSecond
+                    : configuration.DesiredReverseSpeedMetersPerSecond;
+                double scheduleSpeed = input.KnotSchedule.ReferenceSpeedMetersPerSecond[index];
+                targetSpeed = Math.Min(desiredSpeed, Math.Min(scheduleSpeed, speedLimitAtS));
+            }
             double lowerJerk = Math.Max(-configuration.MaximumJerkMetersPerSecondCubed,
                 (-configuration.MaximumDecelerationMetersPerSecondSquared - a) / dt);
             lowerJerk = Math.Max(lowerJerk, -2d * (u + a * dt) / (dt * dt));
@@ -442,6 +465,44 @@ public sealed class SequentialLongitudinalOptimizer
         return CreateApproachSeed(input, times, speedLimit);
     }
 
+    private bool TryCreateStaticStartSeed(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
+        out LongitudinalCandidate candidate)
+    {
+        candidate = null;
+        int stabilizationStart = input.KnotSchedule.TerminalHoldStartIndex;
+        if (stabilizationStart < 5)
+            return false;
+
+        IReadOnlyList<double> times = input.KnotSchedule.KnotTimes;
+        if (TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit, out candidate))
+            return true;
+        double firstDuration = times[1] - times[0];
+        double secondDuration = times[2] - times[1];
+        LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
+        double maximumFirstJerk = Math.Min(configuration.MaximumJerkMetersPerSecondCubed,
+            Math.Min(configuration.MaximumAccelerationMetersPerSecondSquared / firstDuration,
+                configuration.MaximumJerkMetersPerSecondCubed * secondDuration / firstDuration));
+        for (int sample = 1; sample <= 256; sample++)
+        {
+            double firstJerk = maximumFirstJerk * sample / 256d;
+            var jerk = new double[stabilizationStart];
+            jerk[0] = firstJerk;
+            jerk[1] = -firstJerk * firstDuration / secondDuration;
+            if (!TryCloseExactStopEndpoint(input, times, stabilizationStart, jerk,
+                    out LongitudinalCandidate probe))
+            {
+                continue;
+            }
+            if (_solutionValidator.TryValidate(input, speedLimit, probe,
+                    out LongitudinalCandidate strict, out _))
+            {
+                candidate = strict;
+                return true;
+            }
+        }
+        return false;
+    }
+
     private static bool TryCreateCruiseThenBrakeSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
         double stopBoundaryPathS, out LongitudinalCandidate candidate)
     {
@@ -609,6 +670,40 @@ public sealed class SequentialLongitudinalOptimizer
         return AppendExactStopTail(times, stabilizationStart, input.StopBoundaryPathS, motion);
     }
 
+    private static bool TryCloseExactStopEndpoint(LongitudinalPlanningInput input, IReadOnlyList<double> times,
+        int stabilizationStart, double[] jerk, out LongitudinalCandidate candidate)
+    {
+        candidate = null;
+        var motionTimes = new double[stabilizationStart + 1];
+        for (int index = 0; index < motionTimes.Length; index++)
+            motionTimes[index] = times[index];
+        LongitudinalCandidate motion = LongitudinalCandidate.Integrate(motionTimes, 0d,
+            input.InitialProgressSpeedMetersPerSecond, input.InitialAccelerationMetersPerSecondSquared, jerk);
+        int terminalIndex = motion.S.Count - 1;
+        double[] correction =
+        {
+            -motion.A[terminalIndex],
+            -motion.U[terminalIndex],
+            input.StopBoundaryPathS - motion.S[terminalIndex],
+        };
+        var influence = new double[3, 3];
+        for (int basisIndex = 0; basisIndex < 3; basisIndex++)
+        {
+            var basis = new double[stabilizationStart];
+            basis[stabilizationStart - 3 + basisIndex] = 1d;
+            LongitudinalCandidate response = LongitudinalCandidate.Integrate(motionTimes, 0d, 0d, 0d, basis);
+            influence[0, basisIndex] = response.A[terminalIndex];
+            influence[1, basisIndex] = response.U[terminalIndex];
+            influence[2, basisIndex] = response.S[terminalIndex];
+        }
+        if (!TrySolveThreeByThree(influence, correction, out double[] adjustment))
+            return false;
+        for (int index = 0; index < adjustment.Length; index++)
+            jerk[stabilizationStart - 3 + index] += adjustment[index];
+        candidate = CreateExactCandidate(input, times, stabilizationStart, jerk);
+        return true;
+    }
+
     private static LongitudinalCandidate CreateScheduleReferenceSeed(LongitudinalPlanningInput input,
         IReadOnlyList<double> times, PathSpeedLimit speedLimit)
     {

ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs

@@ -19,6 +19,7 @@ internal static class EmPlanningServiceChecks
         VerifiesFullScopePublishesItsRequestScope();
         VerifiesRequestAndStateFailuresPublishNoTrajectory();
         VerifiesProjectionCorridorAndOptimizationFailuresPublishNoTrajectory();
+        VerifiesNoProgressPublishesNoTrajectory();
         VerifiesTimeoutFallbackAndCancellationSemantics();
         VerifiesPublicationFailureAndDebugIsolation();
     }
@@ -396,6 +397,22 @@ internal static class EmPlanningServiceChecks
         }
     }
 
+    private static void VerifiesNoProgressPublishesNoTrajectory()
+    {
+        EmPlanningRequest request = CreateRequest(TravelDirection.Forward, 0d, false, false,
+            CreateReferencePath(TravelDirection.Forward, false, 0.10d), null,
+            EmPlanningScope.FullDirectionSegment);
+        request.Configuration.Scheduling.MaximumOptimizationSpatialStepMeters = 1d;
+        request.Configuration.Scheduling.MaximumOptimizationTimeStepSeconds = 1d;
+
+        EmPlanningResult result = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.NoProgress)).Plan(
+            request, CancellationToken.None);
+
+        VerifyFailure(result, EmPlanningStatus.NoProgress, "full nonterminal no progress");
+        Verification.True(result.FailureReason.IndexOf("NoProgress", StringComparison.Ordinal) >= 0,
+            "no-progress failure preserves its diagnostic");
+    }
+
     private static void VerifiesPublicationFailureAndDebugIsolation()
     {
         EmPlanningRequest validationFailure = CreateRequest(TravelDirection.Forward, 0d, false, false);
@@ -610,6 +627,7 @@ internal static class EmPlanningServiceChecks
         SolverUnavailable,
         TimeoutWithoutFallback,
         TimeoutWithFallback,
+        NoProgress,
         PublicationValidationFailure,
     }
 
@@ -647,6 +665,8 @@ internal static class EmPlanningServiceChecks
             LastLongitudinalProblem = problem;
             if (mode == PipelineSolverMode.LongitudinalInfeasible)
                 return Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>());
+            if (mode == PipelineSolverMode.NoProgress)
+                return Result(QpSolveStatus.Solved, new double[problem.VariableCount]);
             if (strictFullPrimal != null && strictFullPrimal.Count == problem.VariableCount)
             {
                 longitudinalCallCount++;

ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs

@@ -14,6 +14,7 @@ internal static class LongitudinalIntegrationChecks
     {
         VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed();
         VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence();
+        VerifiesFullDirectionStaticStartMakesProgress();
         VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold();
         VerifiesExactStopIncludesAStabilizationTail();
         VerifiesLastStrictCandidateSurvivesLaterTimeout();
@@ -109,6 +110,46 @@ internal static class LongitudinalIntegrationChecks
             "halving publication cadence doubles emitted trajectory intervals without changing optimization knots");
     }
 
+    private static void VerifiesFullDirectionStaticStartMakesProgress()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Scheduling.SolverTimeoutSeconds = 1d;
+        configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond = 1d;
+        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
+        configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1d;
+        configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
+        configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1d;
+        LateralPath path = new LateralPath(new[]
+        {
+            Point(0d, 0d, 0d),
+            Point(2.5d, 2.5d, 0d),
+            Point(5d, 5d, 0d),
+        }, true);
+
+        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0d,
+            EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "static-start envelope: " + failureReason);
+        status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0d, 0d,
+            configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
+            out LongitudinalKnotSchedule schedule, out failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "static-start schedule: " + failureReason);
+
+        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
+            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
+            EmPlanningScope.FullDirectionSegment, schedule, Array.Empty<double>(), Array.Empty<double>());
+        LongitudinalPlanningResult result = new LongitudinalPlanner(new OsqpNativeSolver()).Plan(input,
+            CancellationToken.None);
+
+        Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
+            "static start succeeds: " + result.FailureReason);
+        LongitudinalCandidate candidate = result.Candidate ??
+            throw new InvalidOperationException("static start must expose a successful candidate.");
+        Verification.True(HasValueGreaterThan(candidate.S, 0.05d), "static start makes measurable progress");
+        Verification.True(HasValueGreaterThan(candidate.U, 0.05d), "static start accelerates");
+        Verification.NearlyEqual(0d, candidate.U[candidate.U.Count - 1],
+            "full segment stops at the terminal boundary");
+    }
+
     private static void VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold()
     {
         EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
@@ -191,6 +232,16 @@ internal static class LongitudinalIntegrationChecks
         return count;
     }
 
+    private static bool HasValueGreaterThan(IReadOnlyList<double> values, double threshold)
+    {
+        for (int index = 0; index < values.Count; index++)
+        {
+            if (values[index] > threshold)
+                return true;
+        }
+        return false;
+    }
+
     public static void RunRealOsqp()
     {
         foreach (LongitudinalScenario scenario in CreateRealOsqpScenarios())

ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs

@@ -24,6 +24,7 @@ internal static class LongitudinalModelChecks
         VerifiesFullDirectionInitialFeasibilityProjectionAndFallbackSemantics();
         VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
         VerifiesModeSpecificSolutionValidation();
+        VerifiesFullDirectionNoProgressValidation();
         VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically();
     }
 
@@ -196,9 +197,10 @@ internal static class LongitudinalModelChecks
             out string failureReason);
         Verification.Equal(EmPlanningStatus.Success, status, "jerk-limited stopping-tail status: " + failureReason);
         double nearBoundaryPathS = input.StopBoundaryPathS - 0.005d;
-        Verification.NearlyEqual(MaximumJerkLimitedStopSpeed(input, nearBoundaryPathS),
+        Verification.True(envelope.StoppingLimitAt(nearBoundaryPathS) > 0d &&
-            envelope.StoppingLimitAt(nearBoundaryPathS),
+            envelope.StoppingLimitAt(nearBoundaryPathS) <=
-            "near-boundary speed cap uses jerk-limited distance inversion");
+                MaximumJerkLimitedStopSpeed(input, nearBoundaryPathS) + 1e-12d,
+            "near-boundary speed cap conservatively interpolates jerk-limited distance inversion");
         Verification.NearlyEqual(0d, envelope.StoppingLimitAt(input.StopBoundaryPathS),
             "real stop boundary keeps an exact zero stopping cap");
     }
@@ -608,10 +610,11 @@ internal static class LongitudinalModelChecks
             (envelope.PathS[envelopeSegment + 1] - envelope.PathS[envelopeSegment]);
         double envelopeIntercept = envelope.MaximumSpeedMetersPerSecond[envelopeSegment] -
             envelopeSlope * envelope.PathS[envelopeSegment];
-        Verification.Equal(1, CountBoundedRow(problem, new Dictionary<int, double>
+        var envelopeRow = new Dictionary<int, double> { { layout.U(1), 1d } };
-        {
+        if (Math.Abs(envelopeSlope) > 1e-12d)
-            { layout.U(1), 1d }, { layout.S(1), -envelopeSlope },
+            envelopeRow.Add(layout.S(1), -envelopeSlope);
-        }, -QuadraticProgram.MaximumFiniteBound, envelopeIntercept),
+        Verification.Equal(1, CountBoundedRow(problem, envelopeRow,
+            -QuadraticProgram.MaximumFiniteBound, envelopeIntercept),
             "U upper bound linearly re-evaluates the actual PathS envelope");
         FindSingleVariableBounds(problem, layout.A(1), out double aLower, out double aUpper);
         Verification.NearlyEqual(-1d, aLower, "deceleration lower bound");
@@ -724,6 +727,53 @@ internal static class LongitudinalModelChecks
             "approach failure identifies the jerk-limited stoppable set");
     }
 
+    private static void VerifiesFullDirectionNoProgressValidation()
+    {
+        EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
+        double[] times = { 0d, 0.10d, 0.20d, 0.30d, 0.50d };
+        var fullSchedule = new LongitudinalKnotSchedule(times,
+            new[] { 0d, 0.04d, 0.08d, 0.10d, 0.10d }, new[] { 0d, 0.20d, 0.10d, 0d, 0d }, true, 3);
+        LateralPath path = CreateStraightPath(0.10d);
+        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
+            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
+            EmPlanningScope.FullDirectionSegment, fullSchedule, Array.Empty<double>(), Array.Empty<double>());
+        EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit speedLimit,
+            out string speedFailure);
+        Verification.Equal(EmPlanningStatus.Success, speedStatus, "no-progress envelope: " + speedFailure);
+        LongitudinalCandidate stationary = LongitudinalCandidate.Integrate(times, 0d, 0d, 0d,
+            new[] { 0d, 0d, 0d, 0d });
+
+        var validator = new LongitudinalSolutionValidator();
+        Verification.True(!validator.TryValidate(input, speedLimit, stationary, out _, out EmPlanningStatus failureStatus,
+            out string failureReason), "nonterminal stationary full segment is rejected");
+        Verification.Equal(EmPlanningStatus.NoProgress, failureStatus, "stationary full segment has exact status");
+        Verification.True(failureReason.IndexOf("NoProgress", StringComparison.Ordinal) >= 0,
+            "stationary full segment has exact diagnostic");
+        Verification.True(new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()).TryBuild(input,
+            speedLimit, stationary, out QuadraticProgram fullProblem, out string buildFailure),
+            "full no-progress objective builds: " + buildFailure);
+        var fullLayout = new LongitudinalVariableLayout(times.Length);
+        double expectedReferenceLinearCost = -2d * configuration.Longitudinal.Weights.ReferenceSpeed *
+            fullSchedule.ReferenceSpeedMetersPerSecond[1] /
+            (input.DirectionMaximumSpeedMetersPerSecond * input.DirectionMaximumSpeedMetersPerSecond);
+        Verification.NearlyEqual(expectedReferenceLinearCost, fullProblem.LinearCost[fullLayout.U(1)],
+            "full direction objective tracks adaptive speed reference");
+
+        LateralPath nearTerminalPath = CreateStraightPath(0.02d);
+        var nearTerminalSchedule = new LongitudinalKnotSchedule(times,
+            new[] { 0d, 0.01d, 0.02d, 0.02d, 0.02d }, new[] { 0d, 0d, 0d, 0d, 0d }, true, 3);
+        var nearTerminalInput = new LongitudinalPlanningInput(nearTerminalPath, TravelDirection.Forward, 0d, 0d,
+            EmTerminalType.GearSwitch, EmLongitudinalMode.ExactStopAtBoundary, configuration,
+            EmPlanningScope.FullDirectionSegment, nearTerminalSchedule, Array.Empty<double>(), Array.Empty<double>());
+        speedStatus = new PathSpeedLimitBuilder().Build(nearTerminalInput, out PathSpeedLimit nearTerminalLimit,
+            out speedFailure);
+        Verification.Equal(EmPlanningStatus.Success, speedStatus, "near-terminal no-progress envelope: " + speedFailure);
+        Verification.True(!validator.TryValidate(nearTerminalInput, nearTerminalLimit, stationary, out _,
+            out failureStatus, out _), "near-terminal fixture remains subject to its stop-tail contract");
+        Verification.True(failureStatus != EmPlanningStatus.NoProgress,
+            "near-terminal gear-switch stop is not classified as no progress");
+    }
+
     private static void VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically()
     {
         LateralPath path = CreateStraightPath(1d);
@@ -848,7 +898,7 @@ internal static class LongitudinalModelChecks
         LongitudinalConfiguration limits = input.Configuration.Longitudinal;
         return JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
             Math.Max(0d, input.StopBoundaryPathS - pathS),
-            limits.MaximumAccelerationMetersPerSecondSquared,
+            Math.Max(0d, input.InitialAccelerationMetersPerSecondSquared),
             limits.MaximumDecelerationMetersPerSecondSquared,
             limits.MaximumJerkMetersPerSecondCubed,
             input.DirectionMaximumSpeedMetersPerSecond);