feat: keep rolling speed envelopes open

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimit.cs

@@ -11,7 +11,7 @@ public sealed class PathSpeedLimit
 
     internal PathSpeedLimit(IReadOnlyList<double> pathS, IReadOnlyList<double> maximumSpeed,
         IReadOnlyList<double> lateralAccelerationLimit, IReadOnlyList<double> curvatureRateLimit,
-        IReadOnlyList<double> stoppingLimit, double directionMaximumSpeedMetersPerSecond)
+        IReadOnlyList<double> stoppingLimit, double directionMaximumSpeedMetersPerSecond, bool hasStopBoundary)
     {
         PathS = CopyStrictStations(pathS, nameof(pathS));
         MaximumSpeedMetersPerSecond = CopyFiniteNonnegative(maximumSpeed, PathS.Count, nameof(maximumSpeed));
@@ -22,13 +22,15 @@ public sealed class PathSpeedLimit
         StoppingSpeedLimitsMetersPerSecond = CopyFiniteNonnegative(stoppingLimit, PathS.Count, nameof(stoppingLimit));
         if (!IsFinite(directionMaximumSpeedMetersPerSecond) || directionMaximumSpeedMetersPerSecond <= 0d)
             throw new ArgumentOutOfRangeException(nameof(directionMaximumSpeedMetersPerSecond));
-        if (MaximumSpeedMetersPerSecond[MaximumSpeedMetersPerSecond.Count - 1] != 0d ||
+        if (hasStopBoundary &&
-            StoppingSpeedLimitsMetersPerSecond[StoppingSpeedLimitsMetersPerSecond.Count - 1] != 0d)
+            (MaximumSpeedMetersPerSecond[MaximumSpeedMetersPerSecond.Count - 1] != 0d ||
+             StoppingSpeedLimitsMetersPerSecond[StoppingSpeedLimitsMetersPerSecond.Count - 1] != 0d))
         {
-            throw new ArgumentException("Terminal PathS speed limits must be exactly zero.");
+            throw new ArgumentException("A real stop boundary must have an exact zero speed limit.");
         }
 
         DirectionMaximumSpeedMetersPerSecond = directionMaximumSpeedMetersPerSecond;
+        HasStopBoundary = hasStopBoundary;
     }
 
     public IReadOnlyList<double> PathS { get; }
@@ -43,7 +45,12 @@ public sealed class PathSpeedLimit
 
     public double DirectionMaximumSpeedMetersPerSecond { get; }
 
-    public double TerminalPathS { get { return PathS[PathS.Count - 1]; } }
+    public bool HasStopBoundary { get; }
+
+    public double PathUpperBoundS { get { return PathS[PathS.Count - 1]; } }
+
+    [Obsolete("Use PathUpperBoundS.")]
+    public double TerminalPathS { get { return PathUpperBoundS; } }
 
     public double MaximumSpeedAt(double pathS)
     {
@@ -67,11 +74,11 @@ public sealed class PathSpeedLimit
 
     private double Interpolate(IReadOnlyList<double> values, double pathS)
     {
-        if (!IsFinite(pathS) || pathS < PathS[0] - StationTolerance || pathS > TerminalPathS + StationTolerance)
+        if (!IsFinite(pathS) || pathS < PathS[0] - StationTolerance || pathS > PathUpperBoundS + StationTolerance)
             throw new ArgumentOutOfRangeException(nameof(pathS));
         if (pathS <= PathS[0])
             return values[0];
-        if (pathS >= TerminalPathS)
+        if (pathS >= PathUpperBoundS)
             return values[values.Count - 1];
 
         for (int index = 1; index < PathS.Count; index++)

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs

@@ -34,16 +34,19 @@ public sealed class PathSpeedLimitBuilder
             return EmPlanningStatus.InvalidInput;
         }
 
-        if (!JerkLimitedStoppingMath.TryCalculate(input.InitialProgressSpeedMetersPerSecond,
+        if (input.HasStopBoundary)
-                input.InitialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk,
-                out JerkLimitedStoppingProfile stopProfile, out failureReason))
         {
-            return EmPlanningStatus.InvalidInput;
+            if (!JerkLimitedStoppingMath.TryCalculate(input.InitialProgressSpeedMetersPerSecond,
-        }
+                    input.InitialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk,
-        if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > input.TerminalPathS)
+                    out JerkLimitedStoppingProfile stopProfile, out failureReason))
-        {
+            {
-            failureReason = "The available actual PathS distance is insufficient for the jerk-limited stop.";
+                return EmPlanningStatus.InvalidInput;
-            return EmPlanningStatus.StoppingDistanceInsufficient;
+            }
+            if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > input.StopBoundaryPathS)
+            {
+                failureReason = "The available actual PathS distance is insufficient for the jerk-limited stop.";
+                return EmPlanningStatus.StoppingDistanceInsufficient;
+            }
         }
         if (input.Configuration.Scheduling == null || !IsPositiveFinite(input.Configuration.Scheduling.OutputTimeStepSeconds))
         {
@@ -66,9 +69,12 @@ public sealed class PathSpeedLimitBuilder
             var segmentStations = new List<double>(subdivisions + 16);
             for (int subdivision = segmentIndex == 0 ? 0 : 1; subdivision <= subdivisions; subdivision++)
                 segmentStations.Add(Interpolate(lowerPoint.PathS, upperPoint.PathS, (double)subdivision / subdivisions));
-            AddDiscreteStoppingTailStations(lowerPoint.PathS, upperPoint.PathS, input.TerminalPathS, directionMaximum,
+            if (input.HasStopBoundary)
-                maximumDeceleration, maximumJerk, input.Configuration.Scheduling.OutputTimeStepSeconds,
+            {
-                segmentIndex == 0, segmentStations);
+                AddJerkLimitedStoppingStations(lowerPoint.PathS, upperPoint.PathS, input.StopBoundaryPathS,
+                    directionMaximum, maximumAcceleration, maximumDeceleration, maximumJerk,
+                    segmentIndex == 0, segmentStations);
+            }
             segmentStations.Sort();
             double previousStation = double.NegativeInfinity;
             for (int stationIndex = 0; stationIndex < segmentStations.Count; stationIndex++)
@@ -81,15 +87,17 @@ public sealed class PathSpeedLimitBuilder
                 double curvature = Interpolate(lowerPoint.VehicleCurvature, upperPoint.VehicleCurvature, fraction);
                 double curvatureDerivative = Interpolate(lowerPoint.VehicleCurvatureDerivative,
                     upperPoint.VehicleCurvatureDerivative, fraction);
-                AddLimitSample(samplePathS, curvature, curvatureDerivative, input.TerminalPathS, directionMaximum,
+                AddLimitSample(samplePathS, curvature, curvatureDerivative, input.HasStopBoundary,
-                    maximumDeceleration, maximumLateralAcceleration, maximumCurvatureRate, pathS, maximum, lateral,
+                    input.StopBoundaryPathS, directionMaximum, maximumAcceleration, maximumDeceleration,
+                    maximumJerk, maximumLateralAcceleration, maximumCurvatureRate, pathS, maximum, lateral,
                     curvatureRate, stopping);
             }
         }
 
         try
         {
-            speedLimit = new PathSpeedLimit(pathS, maximum, lateral, curvatureRate, stopping, directionMaximum);
+            speedLimit = new PathSpeedLimit(pathS, maximum, lateral, curvatureRate, stopping, directionMaximum,
+                input.HasStopBoundary);
             return EmPlanningStatus.Success;
         }
         catch (ArgumentException exception)
@@ -133,34 +141,20 @@ public sealed class PathSpeedLimitBuilder
         return true;
     }
 
-    private static void AddDiscreteStoppingTailStations(double lowerPathS, double upperPathS, double terminalPathS,
+    private static void AddJerkLimitedStoppingStations(double lowerPathS, double upperPathS,
-        double directionMaximum, double maximumDeceleration, double maximumJerk, double timeStep, bool includeLower,
+        double stopBoundaryPathS, double directionMaximum, double maximumAcceleration, double maximumDeceleration,
-        IList<double> stations)
+        double maximumJerk, bool includeLower, IList<double> stations)
     {
-        double speedIncrement = maximumDeceleration * timeStep;
+        const int stoppingSpeedSampleCount = 64;
-        int tailStationCount = Math.Max(1, checked((int)Math.Ceiling(directionMaximum / speedIncrement)));
+        for (int step = 0; step < stoppingSpeedSampleCount; step++)
-        for (int step = 1; step <= tailStationCount; step++)
         {
-            double stopDuration = step * timeStep;
+            double speed = directionMaximum * step / stoppingSpeedSampleCount;
-            double remainingDistance = 0.5d * maximumDeceleration * stopDuration * stopDuration;
+            if (!JerkLimitedStoppingMath.TryCalculate(speed, maximumAcceleration,
-            if (remainingDistance >= terminalPathS)
+                    maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stop, out _))
-                break;
+            {
-            double station = terminalPathS - remainingDistance;
+                throw new ArgumentException("The configured jerk-limited stop envelope cannot be sampled.");
-            bool aboveLower = includeLower
+            }
-                ? station >= lowerPathS - StationMergeToleranceMeters
+            double station = stopBoundaryPathS - stop.DistanceMeters;
-                : station > lowerPathS + StationMergeToleranceMeters;
-            if (aboveLower && station <= upperPathS + StationMergeToleranceMeters)
-                stations.Add(Math.Max(lowerPathS, Math.Min(upperPathS, station)));
-        }
-        int jerkTailStationCount = Math.Max(1, checked((int)Math.Ceiling(
-            maximumDeceleration / (maximumJerk * timeStep))));
-        for (int step = 1; step <= jerkTailStationCount; step++)
-        {
-            double releaseDuration = step * timeStep;
-            double remainingDistance = maximumJerk * releaseDuration * releaseDuration * releaseDuration / 6d;
-            if (remainingDistance >= terminalPathS)
-                break;
-            double station = terminalPathS - remainingDistance;
             bool aboveLower = includeLower
                 ? station >= lowerPathS - StationMergeToleranceMeters
                 : station > lowerPathS + StationMergeToleranceMeters;
@@ -169,14 +163,20 @@ public sealed class PathSpeedLimitBuilder
         }
     }
 
-    private static void AddLimitSample(double samplePathS, double curvature, double curvatureDerivative, double terminalPathS,
+    private static void AddLimitSample(double samplePathS, double curvature, double curvatureDerivative,
-        double directionMaximum, double maximumDeceleration, double maximumLateralAcceleration, double maximumCurvatureRate,
+        bool hasStopBoundary, double stopBoundaryPathS, double directionMaximum, double maximumAcceleration,
-        IList<double> pathS, IList<double> maximum, IList<double> lateral, IList<double> curvatureRate, IList<double> stopping)
+        double maximumDeceleration, double maximumJerk, double maximumLateralAcceleration,
+        double maximumCurvatureRate, IList<double> pathS, IList<double> maximum, IList<double> lateral,
+        IList<double> curvatureRate, IList<double> stopping)
     {
-        bool terminal = samplePathS >= terminalPathS;
+        bool terminal = hasStopBoundary && samplePathS >= stopBoundaryPathS;
         double lateralLimit = Math.Sqrt(maximumLateralAcceleration / Math.Max(Math.Abs(curvature), CurvatureEpsilon));
         double curvatureRateLimit = maximumCurvatureRate / Math.Max(Math.Abs(curvatureDerivative), CurvatureEpsilon);
-        double stoppingLimit = Math.Sqrt(2d * maximumDeceleration * Math.Max(0d, terminalPathS - samplePathS));
+        double stoppingLimit = hasStopBoundary
+            ? JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
+                Math.Max(0d, stopBoundaryPathS - samplePathS), maximumAcceleration,
+                maximumDeceleration, maximumJerk, directionMaximum)
+            : directionMaximum;
         double lateralValue = ClampFinite(lateralLimit, directionMaximum);
         double curvatureRateValue = ClampFinite(curvatureRateLimit, directionMaximum);
         double stoppingValue = terminal ? 0d : ClampFinite(stoppingLimit, directionMaximum);

ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs

@@ -12,10 +12,11 @@ internal static class LongitudinalModelChecks
     {
         VerifiesJerkLimitedStoppingProfileEndsAtRest();
         VerifiesStoppedReachabilityUsesTheSameJerkModel();
+        VerifiesRollingEnvelopeDoesNotStopAtWindowEnd();
         VerifiesFinitePathSIndexedSpeedEnvelope();
         VerifiesStoppingEnvelopeIsRefinedOnActualPathS();
-        VerifiesStoppingEnvelopeUsesDiscreteTimeTailStations();
+        VerifiesStoppingEnvelopeUsesJerkLimitedStoppingMath();
-        VerifiesStoppingPrecheckBeforeQpAssembly();
+        VerifiesStoppingPrecheckOnlyAppliesToRealStopBoundaries();
         VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime();
         VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
     }
@@ -56,6 +57,40 @@ internal static class LongitudinalModelChecks
             "distance inversion stays inside the direction speed range");
     }
 
+    private static void VerifiesRollingEnvelopeDoesNotStopAtWindowEnd()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        LateralPath path = CreatePath(new[]
+        {
+            new PathFixture(0d, 0d, 0d, 0d),
+            new PathFixture(1d, 1d, 0d, 0d),
+        });
+        var rolling = new LongitudinalPlanningInput(path, TravelDirection.Forward,
+            0d, 0d, EmTerminalType.RollingSafetyStop,
+            EmLongitudinalMode.RollingContinuation, configuration,
+            Array.Empty<double>(), Array.Empty<double>());
+
+        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(
+            rolling, out PathSpeedLimit envelope, out string failure);
+
+        Verification.Equal(EmPlanningStatus.Success, status, "rolling envelope: " + failure);
+        Verification.True(envelope.MaximumSpeedAt(rolling.PathUpperBoundS) > 0d,
+            "rolling window end keeps a nonzero speed allowance");
+        Verification.True(!envelope.HasStopBoundary, "rolling envelope has no stop boundary");
+        Verification.NearlyEqual(rolling.PathUpperBoundS, envelope.PathUpperBoundS,
+            "rolling envelope reports its PathS upper bound");
+
+        var approach = new LongitudinalPlanningInput(path, TravelDirection.Forward,
+            0d, 0d, EmTerminalType.Goal,
+            EmLongitudinalMode.ApproachStopBoundary, configuration,
+            Array.Empty<double>(), Array.Empty<double>());
+        status = new PathSpeedLimitBuilder().Build(approach, out PathSpeedLimit approachEnvelope, out failure);
+        Verification.Equal(EmPlanningStatus.Success, status, "approach envelope: " + failure);
+        Verification.True(approachEnvelope.HasStopBoundary, "approach envelope retains its real stop boundary");
+        Verification.NearlyEqual(0d, approachEnvelope.StoppingLimitAt(approach.StopBoundaryPathS),
+            "approach stop boundary has an exact zero stopping limit");
+    }
+
     private static void VerifiesFinitePathSIndexedSpeedEnvelope()
     {
         LateralPath directionPath = CreatePath(new[]
@@ -96,8 +131,8 @@ internal static class LongitudinalModelChecks
         Verification.NearlyEqual(0.50d / 4d, envelope.CurvatureRateLimitAt(2d), "curvature-rate limit");
         Verification.True(double.IsFinite(envelope.LateralAccelerationLimitAt(0d)) &&
             double.IsFinite(envelope.CurvatureRateLimitAt(0d)), "zero curvature limits stay finite");
-        Verification.NearlyEqual(Math.Sqrt(2d * 0.30d * (5d - 4d)), envelope.StoppingLimitAt(4d),
+        Verification.NearlyEqual(MaximumJerkLimitedStopSpeed(input, 4d), envelope.StoppingLimitAt(4d),
-            "stopping speed limit");
+            "stopping speed limit uses the complete jerk-limited model");
         Verification.NearlyEqual(Math.Sqrt(0.20d / 20d), envelope.MaximumSpeedAt(4d),
             "combined limit chooses the finite minimum");
         double interpolationQueryPathS = 1.013d;
@@ -133,11 +168,11 @@ internal static class LongitudinalModelChecks
             out string failureReason);
         Verification.Equal(EmPlanningStatus.Success, status, "refined stopping envelope status: " + failureReason);
         Verification.True(envelope.PathS.Count > path.Points.Count, "stopping envelope inserts actual-PathS refinement stations");
-        Verification.NearlyEqual(Math.Sqrt(2d * 0.30d * (2d - 1.5d)), envelope.MaximumSpeedAt(1.5d),
+        Verification.NearlyEqual(MaximumJerkLimitedStopSpeed(input, 1.5d), envelope.MaximumSpeedAt(1.5d),
-            "refined stopping envelope avoids a sparse terminal chord");
+            "refined stopping envelope uses the complete jerk-limited stopping cap");
     }
 
-    private static void VerifiesStoppingEnvelopeUsesDiscreteTimeTailStations()
+    private static void VerifiesStoppingEnvelopeUsesJerkLimitedStoppingMath()
     {
         EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
         configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
@@ -153,22 +188,16 @@ internal static class LongitudinalModelChecks
 
         EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
             out string failureReason);
-        Verification.Equal(EmPlanningStatus.Success, status, "discrete stopping-tail status: " + failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "jerk-limited stopping-tail status: " + failureReason);
-        double timeStep = configuration.Scheduling.OutputTimeStepSeconds;
+        double nearBoundaryPathS = input.StopBoundaryPathS - 0.005d;
-        double deceleration = configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared;
+        Verification.NearlyEqual(MaximumJerkLimitedStopSpeed(input, nearBoundaryPathS),
-        double firstTailDistance = 0.5d * deceleration * timeStep * timeStep;
+            envelope.StoppingLimitAt(nearBoundaryPathS),
-        double firstTailPathS = input.TerminalPathS - firstTailDistance;
+            "near-boundary speed cap uses jerk-limited distance inversion");
-        Verification.NearlyEqual(deceleration * timeStep, envelope.StoppingLimitAt(firstTailPathS),
+        Verification.NearlyEqual(0d, envelope.StoppingLimitAt(input.StopBoundaryPathS),
-            "first stopping-tail station matches one discrete deceleration time step");
+            "real stop boundary keeps an exact zero stopping cap");
-        double jerk = configuration.Longitudinal.MaximumJerkMetersPerSecondCubed;
-        double firstJerkTailDistance = jerk * timeStep * timeStep * timeStep / 6d;
-        double firstJerkTailPathS = input.TerminalPathS - firstJerkTailDistance;
-        Verification.NearlyEqual(Math.Sqrt(2d * deceleration * firstJerkTailDistance),
-            envelope.StoppingLimitAt(firstJerkTailPathS),
-            "first stopping-tail station matches one discrete jerk-release time step");
     }
 
-    private static void VerifiesStoppingPrecheckBeforeQpAssembly()
+    private static void VerifiesStoppingPrecheckOnlyAppliesToRealStopBoundaries()
     {
         EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
         LateralPath shortPath = CreatePath(new[]
@@ -176,14 +205,22 @@ internal static class LongitudinalModelChecks
             new PathFixture(0d, 0d, 0d, 0d),
             new PathFixture(100d, 0.01d, 0d, 0d),
         });
-        var input = new LongitudinalPlanningInput(shortPath, TravelDirection.Forward, 0.20d, 0.20d,
+        var rolling = new LongitudinalPlanningInput(shortPath, TravelDirection.Forward, 0.20d, 0.20d,
             EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
             Array.Empty<double>(), Array.Empty<double>());
 
-        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
+        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(rolling, out PathSpeedLimit envelope,
             out string failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status,
+            "rolling windows do not require a stop inside their local PathS extent: " + failureReason);
+        Verification.True(envelope != null, "rolling speed envelope is created despite the short local window");
+
+        var approach = new LongitudinalPlanningInput(shortPath, TravelDirection.Forward, 0.20d, 0.20d,
+            EmTerminalType.Goal, EmLongitudinalMode.ApproachStopBoundary, configuration,
+            Array.Empty<double>(), Array.Empty<double>());
+        status = new PathSpeedLimitBuilder().Build(approach, out envelope, out failureReason);
         Verification.Equal(EmPlanningStatus.StoppingDistanceInsufficient, status,
-            "jerk/deceleration stopping precheck status");
+            "real stop-boundary jerk/deceleration stopping precheck status");
         Verification.True(envelope == null, "stopping-distance failure does not create a speed envelope");
         Verification.True(failureReason.Length != 0, "stopping-distance failure explains the rejection");
     }
@@ -375,6 +412,17 @@ internal static class LongitudinalModelChecks
         return configuration;
     }
 
+    private static double MaximumJerkLimitedStopSpeed(LongitudinalPlanningInput input, double pathS)
+    {
+        LongitudinalConfiguration limits = input.Configuration.Longitudinal;
+        return JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
+            Math.Max(0d, input.StopBoundaryPathS - pathS),
+            limits.MaximumAccelerationMetersPerSecondSquared,
+            limits.MaximumDecelerationMetersPerSecondSquared,
+            limits.MaximumJerkMetersPerSecondCubed,
+            input.DirectionMaximumSpeedMetersPerSecond);
+    }
+
     private static double MatrixValue(SparseCscMatrix matrix, int row, int column)
     {
         for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)