feat: apply ST stop constraints only at real boundaries

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs

@@ -3,7 +3,7 @@ using System.Collections.Generic;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
-/// <summary>Builds one normalized ST QP with exact constant-jerk integration and hard terminal conditions.</summary>
+/// <summary>Builds one normalized ST QP with exact constant-jerk integration and mode-specific stop conditions.</summary>
 public sealed class LongitudinalConstraintBuilder
 {
     private readonly LongitudinalObjectiveBuilder _objectiveBuilder;
@@ -22,8 +22,8 @@ public sealed class LongitudinalConstraintBuilder
         {
             if (input == null || speedLimit == null || iterate == null)
                 throw new ArgumentException("ST input, speed envelope, and iterate are required.");
-            if (Math.Abs(speedLimit.TerminalPathS - input.TerminalPathS) > 1e-12d)
+            if (Math.Abs(speedLimit.PathUpperBoundS - input.PathUpperBoundS) > 1e-12d)
-                throw new ArgumentException("The speed envelope terminal must match actual lateral PathS.");
+                throw new ArgumentException("The speed envelope upper bound must match actual lateral PathS.");
 
             IReadOnlyList<double> expectedTimes = LongitudinalCandidate.CreateKnotTimes(
                 input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds);
@@ -51,16 +51,24 @@ public sealed class LongitudinalConstraintBuilder
             var hessian = new SparseTripletBuilder(layout.VariableCount, layout.VariableCount, true);
             var linearCost = new double[layout.VariableCount];
             _objectiveBuilder.AddTerms(input, speedLimit, layout, iterate, hessian, linearCost);
-            var constraints = new SparseTripletBuilder(8 * layout.KnotCount, layout.VariableCount);
+            int stabilizationStart = input.Mode == EmLongitudinalMode.ExactStopAtBoundary
-            var lower = new List<double>(8 * layout.KnotCount);
+                ? LongitudinalTerminalSchedule.GetStabilizationStartIndex(expectedTimes,
-            var upper = new List<double>(8 * layout.KnotCount);
+                    input.Configuration.Scheduling.OutputTimeStepSeconds)
+                : layout.KnotCount;
+            int stationaryKnotCount = layout.KnotCount - stabilizationStart;
+            int expectedRows = 8 * layout.KnotCount - 2 + 3 * stationaryKnotCount;
+            var constraints = new SparseTripletBuilder(expectedRows, layout.VariableCount);
+            var lower = new List<double>(expectedRows);
+            var upper = new List<double>(expectedRows);
             int row = 0;
             AddVariableBounds(input, speedLimit, iterate, layout, maximumAcceleration, maximumDeceleration, maximumJerk,
                 constraints, lower, upper, ref row);
             AddMonotonicProgress(layout, constraints, lower, upper, ref row);
             AddExactDynamics(expectedTimes, layout, constraints, lower, upper, ref row);
-            AddExactStartAndTerminal(input, layout, constraints, lower, upper, ref row);
+            AddExactStart(input, layout, constraints, lower, upper, ref row);
-            if (row != 8 * layout.KnotCount)
+            if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
+                AddExactStopTail(input, layout, stabilizationStart, constraints, lower, upper, ref row);
+            if (row != expectedRows)
                 throw new InvalidOperationException("ST constraint row accounting is inconsistent.");
             problem = new QuadraticProgram(hessian.Build(), linearCost, constraints.Build(), lower, upper);
             return true;
@@ -79,9 +87,9 @@ public sealed class LongitudinalConstraintBuilder
     {
         for (int index = 0; index < layout.KnotCount; index++)
         {
-            if (iterate.S[index] < 0d || iterate.S[index] > input.TerminalPathS)
+            if (iterate.S[index] < 0d || iterate.S[index] > input.PathUpperBoundS)
                 throw new ArgumentException("The ST iterate progress lies outside actual PathS bounds.");
-            AddSingleVariableRow(constraints, lower, upper, layout.S(index), 0d, input.TerminalPathS, ref row);
+            AddSingleVariableRow(constraints, lower, upper, layout.S(index), 0d, input.PathUpperBoundS, ref row);
             AddSingleVariableRow(constraints, lower, upper, layout.U(index), 0d,
                 Math.Min(input.DirectionMaximumSpeedMetersPerSecond, speedLimit.MaximumSpeedAt(iterate.S[index])), ref row);
             AddSingleVariableRow(constraints, lower, upper, layout.A(index), -maximumDeceleration, maximumAcceleration,
@@ -132,7 +140,7 @@ public sealed class LongitudinalConstraintBuilder
         }
     }
 
-    private static void AddExactStartAndTerminal(LongitudinalPlanningInput input, LongitudinalVariableLayout layout,
+    private static void AddExactStart(LongitudinalPlanningInput input, LongitudinalVariableLayout layout,
         SparseTripletBuilder constraints, IList<double> lower, IList<double> upper, ref int row)
     {
         AddSingleVariableRow(constraints, lower, upper, layout.S(0), 0d, 0d, ref row);
@@ -140,9 +148,18 @@ public sealed class LongitudinalConstraintBuilder
             input.InitialProgressSpeedMetersPerSecond, ref row);
         AddSingleVariableRow(constraints, lower, upper, layout.A(0), input.InitialAccelerationMetersPerSecondSquared,
             input.InitialAccelerationMetersPerSecondSquared, ref row);
-        AddSingleVariableRow(constraints, lower, upper, layout.S(layout.KnotCount - 1), input.TerminalPathS,
+    }
-            input.TerminalPathS, ref row);
+
-        AddSingleVariableRow(constraints, lower, upper, layout.U(layout.KnotCount - 1), 0d, 0d, ref row);
+    private static void AddExactStopTail(LongitudinalPlanningInput input, LongitudinalVariableLayout layout,
+        int stabilizationStart, SparseTripletBuilder constraints, IList<double> lower, IList<double> upper, ref int row)
+    {
+        for (int index = stabilizationStart; index < layout.KnotCount; index++)
+        {
+            AddSingleVariableRow(constraints, lower, upper, layout.S(index), input.StopBoundaryPathS,
+                input.StopBoundaryPathS, ref row);
+            AddSingleVariableRow(constraints, lower, upper, layout.U(index), 0d, 0d, ref row);
+            AddSingleVariableRow(constraints, lower, upper, layout.A(index), 0d, 0d, ref row);
+        }
     }
 
     private static void AddSingleVariableRow(SparseTripletBuilder constraints, IList<double> lower, IList<double> upper,

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs

@@ -56,13 +56,14 @@ public sealed class LongitudinalSolutionValidator
                 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 ||
+                    progress > input.PathUpperBoundS + 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)));
+                double speedLimitAtProgress = speedLimit.MaximumSpeedAt(
+                    Math.Max(0d, Math.Min(input.PathUpperBoundS, progress)));
                 if (speed > speedLimitAtProgress + tolerance)
                 {
                     failureReason = "ST candidate violates the actual-PathS speed envelope at knot " + index +
@@ -83,12 +84,35 @@ public sealed class LongitudinalSolutionValidator
                     return false;
                 }
             }
-            int terminalIndex = candidate.S.Count - 1;
+            int stabilizationStart = candidate.S.Count;
-            if (!AreClose(candidate.S[terminalIndex], input.TerminalPathS, tolerance) ||
+            if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
-                !AreClose(candidate.U[terminalIndex], 0d, tolerance))
             {
-                failureReason = "ST candidate does not satisfy the exact zero-speed terminal.";
+                stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
-                return false;
+                    candidate.KnotTimes, input.Configuration.Scheduling.OutputTimeStepSeconds);
+                for (int index = stabilizationStart; index < candidate.S.Count; index++)
+                {
+                    if (!AreClose(candidate.S[index], input.StopBoundaryPathS, tolerance) ||
+                        !AreClose(candidate.U[index], 0d, tolerance) ||
+                        !AreClose(candidate.A[index], 0d, tolerance))
+                    {
+                        failureReason = "ST candidate does not satisfy the exact stabilized S/U/A stop tail at knot " +
+                            index + ".";
+                        return false;
+                    }
+                }
+            }
+            if (input.Mode != EmLongitudinalMode.RollingContinuation)
+            {
+                for (int index = 0; index < candidate.S.Count; index++)
+                {
+                    if (!JerkLimitedStoppingMath.TryCalculate(candidate.U[index], candidate.A[index],
+                            maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stop, out _) ||
+                        candidate.S[index] + stop.DistanceMeters > input.StopBoundaryPathS + tolerance)
+                    {
+                        failureReason = "ST candidate leaves the jerk-limited stoppable set at knot " + index + ".";
+                        return false;
+                    }
+                }
             }
 
             var canonicalS = new double[candidate.S.Count];
@@ -103,8 +127,15 @@ public sealed class LongitudinalSolutionValidator
             canonicalS[0] = 0d;
             canonicalU[0] = input.InitialProgressSpeedMetersPerSecond;
             canonicalA[0] = input.InitialAccelerationMetersPerSecondSquared;
-            canonicalS[terminalIndex] = input.TerminalPathS;
+            if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
-            canonicalU[terminalIndex] = 0d;
+            {
+                for (int index = stabilizationStart; index < candidate.S.Count; index++)
+                {
+                    canonicalS[index] = input.StopBoundaryPathS;
+                    canonicalU[index] = 0d;
+                    canonicalA[index] = 0d;
+                }
+            }
             var canonicalCandidate = new LongitudinalCandidate(candidate.KnotTimes, canonicalS, canonicalU, canonicalA,
                 candidate.J);
             if (!canonicalCandidate.SatisfiesExactDiscreteDynamics(tolerance))

ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs

@@ -19,6 +19,7 @@ internal static class LongitudinalModelChecks
         VerifiesStoppingPrecheckOnlyAppliesToRealStopBoundaries();
         VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime();
         VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
+        VerifiesModeSpecificSolutionValidation();
     }
 
     private static void VerifiesJerkLimitedStoppingProfileEndsAtRest()
@@ -313,6 +314,17 @@ internal static class LongitudinalModelChecks
 
         Verification.True(new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()).TryBuild(input, envelope,
             integrated, out QuadraticProgram problem, out string failureReason), "ST QP builds: " + failureReason);
+        var rollingInput = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0.02d,
+            EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
+            new[] { 0d, 0.10d, 0.20d, 0.30d, 0.40d },
+            new[] { 0.20d, 0.20d, 0.20d, 0.20d, 0.20d });
+        EmPlanningStatus rollingSpeedStatus = new PathSpeedLimitBuilder().Build(rollingInput,
+            out PathSpeedLimit rollingEnvelope, out string rollingSpeedFailure);
+        Verification.Equal(EmPlanningStatus.Success, rollingSpeedStatus,
+            "unit-scale rolling speed envelope: " + rollingSpeedFailure);
+        Verification.True(new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()).TryBuild(
+            rollingInput, rollingEnvelope, integrated, out QuadraticProgram rollingProblem, out string rollingFailure),
+            "rolling ST QP builds: " + rollingFailure);
         Verification.NearlyEqual(30d, MatrixValue(problem.UpperTriangularP, layout.U(0), layout.U(0)),
             "normalized speed and previous-U P coefficient");
         Verification.NearlyEqual(2d, MatrixValue(problem.UpperTriangularP, layout.A(0), layout.A(0)),
@@ -344,10 +356,29 @@ internal static class LongitudinalModelChecks
             "exact initial U");
         Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.A(0), 1d } }, 0.02d),
             "exact initial A");
-        Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.S(4), 1d } }, 2d),
+        Verification.Equal(0, CountExactEqualityRows(rollingProblem,
-            "exact terminal S");
+            new Dictionary<int, double> { { layout.S(4), 1d } }, rollingInput.PathUpperBoundS),
-        Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.U(4), 1d } }, 0d),
+            "rolling has no exact terminal S");
-            "exact terminal U");
+        Verification.Equal(0, CountExactEqualityRows(rollingProblem,
+            new Dictionary<int, double> { { layout.U(4), 1d } }, 0d),
+            "rolling has no exact terminal U");
+        Verification.Equal(0, CountExactEqualityRows(rollingProblem,
+            new Dictionary<int, double> { { layout.A(4), 1d } }, 0d),
+            "rolling has no exact terminal A");
+        int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
+            integrated.KnotTimes, configuration.Scheduling.OutputTimeStepSeconds);
+        for (int index = stabilizationStart; index < layout.KnotCount; index++)
+        {
+            Verification.Equal(1, CountExactEqualityRows(problem,
+                new Dictionary<int, double> { { layout.S(index), 1d } }, input.StopBoundaryPathS),
+                "stop tail exact S " + index);
+            Verification.Equal(1, CountExactEqualityRows(problem,
+                new Dictionary<int, double> { { layout.U(index), 1d } }, 0d),
+                "stop tail exact U " + index);
+            Verification.Equal(1, CountExactEqualityRows(problem,
+                new Dictionary<int, double> { { layout.A(index), 1d } }, 0d),
+                "stop tail exact A " + index);
+        }
         Verification.Equal(1, CountBoundedRow(problem, new Dictionary<int, double>
         {
             { layout.S(1), 1d }, { layout.S(0), -1d },
@@ -367,6 +398,62 @@ internal static class LongitudinalModelChecks
         }, 0d), "exact ST progress equation");
     }
 
+    private static void VerifiesModeSpecificSolutionValidation()
+    {
+        EmPlannerConfiguration configuration = CreateTaskFourConfiguration();
+        IReadOnlyList<double> exactTimes = LongitudinalCandidate.CreateKnotTimes(0.30d, 0.10d);
+        LongitudinalCandidate nonstationaryExact = LongitudinalCandidate.Integrate(
+            exactTimes, 0d, 0.10d, 0d, new[] { -4d, 0d, 0d });
+        LateralPath exactPath = CreateStraightPath(nonstationaryExact.S[nonstationaryExact.S.Count - 1]);
+        var exactInput = new LongitudinalPlanningInput(exactPath, TravelDirection.Forward, 0.10d, 0d,
+            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
+            Array.Empty<double>(), Array.Empty<double>());
+        var rollingForExact = new LongitudinalPlanningInput(exactPath, TravelDirection.Forward, 0.10d, 0d,
+            EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
+            Array.Empty<double>(), Array.Empty<double>());
+        EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(rollingForExact,
+            out PathSpeedLimit rollingEnvelope, out string speedFailure);
+        Verification.Equal(EmPlanningStatus.Success, speedStatus, "exact-validation rolling envelope: " + speedFailure);
+
+        var validator = new LongitudinalSolutionValidator();
+        Verification.True(!validator.TryValidate(exactInput, rollingEnvelope, nonstationaryExact,
+            out _, out string exactFailure), "exact stops reject a nonstationary internal tail");
+        Verification.True(exactFailure.IndexOf("exact stabilized", StringComparison.Ordinal) >= 0,
+            "exact-stop failure identifies the stabilized tail: " + exactFailure);
+
+        LateralPath rollingPath = CreateStraightPath(0.10d);
+        var rollingInput = new LongitudinalPlanningInput(rollingPath, TravelDirection.Forward, 0.10d, 0d,
+            EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
+            Array.Empty<double>(), Array.Empty<double>());
+        speedStatus = new PathSpeedLimitBuilder().Build(rollingInput, out PathSpeedLimit openEnvelope, out speedFailure);
+        Verification.Equal(EmPlanningStatus.Success, speedStatus, "rolling-validation envelope: " + speedFailure);
+        LongitudinalCandidate rollingCandidate = LongitudinalCandidate.Integrate(
+            exactTimes, 0d, 0.10d, 0d, new[] { 0d, 0d, 0d });
+        Verification.True(validator.TryValidate(rollingInput, openEnvelope, rollingCandidate,
+            out _, out string rollingFailure), "rolling nonzero terminal speed validates: " + rollingFailure);
+
+        EmPlannerConfiguration approachConfiguration = CreateTaskFourConfiguration();
+        approachConfiguration.Scheduling.TimeHorizonSeconds = 0.15d;
+        approachConfiguration.Scheduling.OutputTimeStepSeconds = 0.05d;
+        IReadOnlyList<double> approachTimes = LongitudinalCandidate.CreateKnotTimes(0.15d, 0.05d);
+        LongitudinalCandidate unstoppablyFastApproach = LongitudinalCandidate.Integrate(
+            approachTimes, 0d, 0.20d, 0d, new[] { 0d, -10d, 10d });
+        LateralPath approachPath = CreateStraightPath(0.035d);
+        var approachInput = new LongitudinalPlanningInput(approachPath, TravelDirection.Forward, 0.20d, 0d,
+            EmTerminalType.Goal, EmLongitudinalMode.ApproachStopBoundary, approachConfiguration,
+            Array.Empty<double>(), Array.Empty<double>());
+        var rollingForApproach = new LongitudinalPlanningInput(approachPath, TravelDirection.Forward, 0.20d, 0d,
+            EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, approachConfiguration,
+            Array.Empty<double>(), Array.Empty<double>());
+        speedStatus = new PathSpeedLimitBuilder().Build(rollingForApproach,
+            out PathSpeedLimit approachEnvelope, out speedFailure);
+        Verification.Equal(EmPlanningStatus.Success, speedStatus, "approach-validation rolling envelope: " + speedFailure);
+        Verification.True(!validator.TryValidate(approachInput, approachEnvelope, unstoppablyFastApproach,
+            out _, out string approachFailure), "approach candidates outside the stoppable set are rejected");
+        Verification.True(approachFailure.IndexOf("stoppable set", StringComparison.Ordinal) >= 0,
+            "approach failure identifies the jerk-limited stoppable set");
+    }
+
     private static LateralPath CreatePath(IReadOnlyList<PathFixture> fixtures)
     {
         var points = new List<LateralPathPoint>(fixtures.Count);
@@ -412,6 +499,30 @@ internal static class LongitudinalModelChecks
         return configuration;
     }
 
+    private static EmPlannerConfiguration CreateTaskFourConfiguration()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Scheduling.TimeHorizonSeconds = 0.30d;
+        configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
+        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
+        configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
+        configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d;
+        configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
+        configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 10d;
+        configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
+        configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
+        return configuration;
+    }
+
+    private static LateralPath CreateStraightPath(double pathUpperBoundS)
+    {
+        return CreatePath(new[]
+        {
+            new PathFixture(0d, 0d, 0d, 0d),
+            new PathFixture(pathUpperBoundS, pathUpperBoundS, 0d, 0d),
+        });
+    }
+
     private static double MaximumJerkLimitedStopSpeed(LongitudinalPlanningInput input, double pathS)
     {
         LongitudinalConfiguration limits = input.Configuration.Longitudinal;