fix: accelerate EM trajectories from rest

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),
-            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);