fix: accelerate EM trajectories from rest

2026-08-07 07:51:43 +08:00
parent 162f1a24e6
commit 0bba8d7e61
9 changed files with 273 additions and 20 deletions
@@ -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);
@@ -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);
@@ -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)
@@ -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)
            {
@@ -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);
@@ -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)
    {
@@ -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++;
@@ -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())
@@ -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),
-            envelope.StoppingLimitAt(nearBoundaryPathS),
-            "near-boundary speed cap uses jerk-limited distance inversion");
+        Verification.True(envelope.StoppingLimitAt(nearBoundaryPathS) > 0d &&
+            envelope.StoppingLimitAt(nearBoundaryPathS) <=
+                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>
-        {
-            { layout.U(1), 1d }, { layout.S(1), -envelopeSlope },
-        }, -QuadraticProgram.MaximumFiniteBound, envelopeIntercept),
+        var envelopeRow = new Dictionary<int, double> { { layout.U(1), 1d } };
+        if (Math.Abs(envelopeSlope) > 1e-12d)
+            envelopeRow.Add(layout.S(1), -envelopeSlope);
+        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);