fix: refine EM stopping speed limits

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs

@@ -8,6 +8,7 @@ public sealed class PathSpeedLimitBuilder
 {
     internal const double CurvatureEpsilon = 1e-10d;
     private const double StopDistanceToleranceMeters = 1e-8d;
+    private const double StationMergeToleranceMeters = 1e-12d;
 
     public EmPlanningStatus Build(LongitudinalPlanningInput input, out PathSpeedLimit speedLimit, out string failureReason)
     {
@@ -40,28 +41,46 @@ public sealed class PathSpeedLimitBuilder
             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))
-        int count = input.Path.Points.Count;
-        var pathS = new double[count];
-        var maximum = new double[count];
-        var lateral = new double[count];
-        var curvatureRate = new double[count];
-        var stopping = new double[count];
-        for (int index = 0; index < count; index++)
         {
-            LateralPathPoint point = input.Path.Points[index];
+            failureReason = "The output time step required to refine the PathS speed envelope is invalid.";
-            pathS[index] = point.PathS;
+            return EmPlanningStatus.InvalidInput;
-            double lateralLimit = Math.Sqrt(maximumLateralAcceleration /
+        }
-                Math.Max(Math.Abs(point.VehicleCurvature), CurvatureEpsilon));
+
-            double curvatureRateLimit = maximumCurvatureRate /
+        double maximumStationSpacing = directionMaximum * input.Configuration.Scheduling.OutputTimeStepSeconds;
-                Math.Max(Math.Abs(point.VehicleCurvatureDerivative), CurvatureEpsilon);
+        var pathS = new List<double>();
-            double remainingDistance = Math.Max(0d, input.TerminalPathS - point.PathS);
+        var maximum = new List<double>();
-            double stoppingLimit = Math.Sqrt(2d * maximumDeceleration * remainingDistance);
+        var lateral = new List<double>();
-            lateral[index] = ClampFinite(lateralLimit, directionMaximum);
+        var curvatureRate = new List<double>();
-            curvatureRate[index] = ClampFinite(curvatureRateLimit, directionMaximum);
+        var stopping = new List<double>();
-            stopping[index] = index == count - 1 ? 0d : ClampFinite(stoppingLimit, directionMaximum);
+        for (int segmentIndex = 0; segmentIndex < input.Path.Points.Count - 1; segmentIndex++)
-            maximum[index] = index == count - 1 ? 0d : Math.Min(directionMaximum,
+        {
-                Math.Min(lateral[index], Math.Min(curvatureRate[index], stopping[index])));
+            LateralPathPoint lowerPoint = input.Path.Points[segmentIndex];
+            LateralPathPoint upperPoint = input.Path.Points[segmentIndex + 1];
+            double span = upperPoint.PathS - lowerPoint.PathS;
+            int subdivisions = Math.Max(1, checked((int)Math.Ceiling(span / maximumStationSpacing)));
+            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,
+                maximumDeceleration, maximumJerk, input.Configuration.Scheduling.OutputTimeStepSeconds,
+                segmentIndex == 0, segmentStations);
+            segmentStations.Sort();
+            double previousStation = double.NegativeInfinity;
+            for (int stationIndex = 0; stationIndex < segmentStations.Count; stationIndex++)
+            {
+                double samplePathS = segmentStations[stationIndex];
+                if (samplePathS <= previousStation + StationMergeToleranceMeters)
+                    continue;
+                previousStation = samplePathS;
+                double fraction = (samplePathS - lowerPoint.PathS) / span;
+                double curvature = Interpolate(lowerPoint.VehicleCurvature, upperPoint.VehicleCurvature, fraction);
+                double curvatureDerivative = Interpolate(lowerPoint.VehicleCurvatureDerivative,
+                    upperPoint.VehicleCurvatureDerivative, fraction);
+                AddLimitSample(samplePathS, curvature, curvatureDerivative, input.TerminalPathS, directionMaximum,
+                    maximumDeceleration, maximumLateralAcceleration, maximumCurvatureRate, pathS, maximum, lateral,
+                    curvatureRate, stopping);
+            }
         }
 
         try
@@ -110,6 +129,66 @@ public sealed class PathSpeedLimitBuilder
         return true;
     }
 
+    private static void AddDiscreteStoppingTailStations(double lowerPathS, double upperPathS, double terminalPathS,
+        double directionMaximum, double maximumDeceleration, double maximumJerk, double timeStep, bool includeLower,
+        IList<double> stations)
+    {
+        double speedIncrement = maximumDeceleration * timeStep;
+        int tailStationCount = Math.Max(1, checked((int)Math.Ceiling(directionMaximum / speedIncrement)));
+        for (int step = 1; step <= tailStationCount; step++)
+        {
+            double stopDuration = step * timeStep;
+            double remainingDistance = 0.5d * maximumDeceleration * stopDuration * stopDuration;
+            if (remainingDistance >= terminalPathS)
+                break;
+            double station = terminalPathS - remainingDistance;
+            bool aboveLower = includeLower
+                ? station >= lowerPathS - StationMergeToleranceMeters
+                : 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;
+            if (aboveLower && station <= upperPathS + StationMergeToleranceMeters)
+                stations.Add(Math.Max(lowerPathS, Math.Min(upperPathS, station)));
+        }
+    }
+
+    private static void AddLimitSample(double samplePathS, double curvature, double curvatureDerivative, double terminalPathS,
+        double directionMaximum, double maximumDeceleration, double maximumLateralAcceleration, double maximumCurvatureRate,
+        IList<double> pathS, IList<double> maximum, IList<double> lateral, IList<double> curvatureRate, IList<double> stopping)
+    {
+        bool terminal = samplePathS >= terminalPathS;
+        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 lateralValue = ClampFinite(lateralLimit, directionMaximum);
+        double curvatureRateValue = ClampFinite(curvatureRateLimit, directionMaximum);
+        double stoppingValue = terminal ? 0d : ClampFinite(stoppingLimit, directionMaximum);
+        pathS.Add(samplePathS);
+        lateral.Add(lateralValue);
+        curvatureRate.Add(curvatureRateValue);
+        stopping.Add(stoppingValue);
+        maximum.Add(terminal ? 0d : Math.Min(directionMaximum,
+            Math.Min(lateralValue, Math.Min(curvatureRateValue, stoppingValue))));
+    }
+
+    private static double Interpolate(double lower, double upper, double fraction)
+    {
+        return lower + (upper - lower) * fraction;
+    }
+
     private static double ClampFinite(double value, double maximum)
     {
         if (!IsFinite(value) || value < 0d)

ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs

@@ -11,6 +11,8 @@ internal static class LongitudinalModelChecks
     public static void Run()
     {
         VerifiesFinitePathSIndexedSpeedEnvelope();
+        VerifiesStoppingEnvelopeIsRefinedOnActualPathS();
+        VerifiesStoppingEnvelopeUsesDiscreteTimeTailStations();
         VerifiesStoppingPrecheckBeforeQpAssembly();
         VerifiesReferenceHorizonSelectionKeepsTheCurrentSegmentBoundary();
         VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
@@ -58,11 +60,72 @@ internal static class LongitudinalModelChecks
             "stopping speed limit");
         Verification.NearlyEqual(Math.Sqrt(0.20d / 20d), envelope.MaximumSpeedAt(4d),
             "combined limit chooses the finite minimum");
-        Verification.NearlyEqual((envelope.MaximumSpeedAt(0d) + envelope.MaximumSpeedAt(2d)) / 2d,
+        double interpolationQueryPathS = 1.013d;
-            envelope.MaximumSpeedAt(1d), "speed envelope interpolates by PathS rather than ReferenceS");
+        int upperStation = 1;
+        while (envelope.PathS[upperStation] < interpolationQueryPathS)
+            upperStation++;
+        double lowerPathS = envelope.PathS[upperStation - 1];
+        double upperPathS = envelope.PathS[upperStation];
+        double fraction = (interpolationQueryPathS - lowerPathS) / (upperPathS - lowerPathS);
+        double expectedInterpolatedSpeed = envelope.MaximumSpeedMetersPerSecond[upperStation - 1] +
+            (envelope.MaximumSpeedMetersPerSecond[upperStation] - envelope.MaximumSpeedMetersPerSecond[upperStation - 1]) *
+            fraction;
+        Verification.NearlyEqual(expectedInterpolatedSpeed, envelope.MaximumSpeedAt(interpolationQueryPathS),
+            "speed envelope interpolates by PathS rather than ReferenceS");
         Verification.NearlyEqual(0d, envelope.MaximumSpeedAt(5d), "terminal speed is exactly zero");
     }
 
+    private static void VerifiesStoppingEnvelopeIsRefinedOnActualPathS()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
+        configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
+        LateralPath path = CreatePath(new[]
+        {
+            new PathFixture(0d, 0d, 0d, 0d),
+            new PathFixture(100d, 2d, 0d, 0d),
+        });
+        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
+            EmTerminalType.Goal, configuration, Array.Empty<double>(), Array.Empty<double>());
+
+        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
+            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),
+            "refined stopping envelope avoids a sparse terminal chord");
+    }
+
+    private static void VerifiesStoppingEnvelopeUsesDiscreteTimeTailStations()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
+        configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
+        LateralPath path = CreatePath(new[]
+        {
+            new PathFixture(0d, 0d, 0d, 0d),
+            new PathFixture(100d, 2d, 0d, 0d),
+        });
+        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
+            EmTerminalType.Goal, configuration, Array.Empty<double>(), Array.Empty<double>());
+
+        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
+            out string failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "discrete stopping-tail status: " + failureReason);
+        double timeStep = configuration.Scheduling.OutputTimeStepSeconds;
+        double deceleration = configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared;
+        double firstTailDistance = 0.5d * deceleration * timeStep * timeStep;
+        double firstTailPathS = input.TerminalPathS - firstTailDistance;
+        Verification.NearlyEqual(deceleration * timeStep, envelope.StoppingLimitAt(firstTailPathS),
+            "first stopping-tail station matches one discrete deceleration time step");
+        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()
     {
         EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();