feat: add complete jerk-limited stopping math

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/JerkLimitedStoppingMath.cs

@@ -0,0 +1,203 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class JerkLimitedStoppingProfile
+{
+    internal JerkLimitedStoppingProfile(double distanceMeters, double durationSeconds,
+        double finalSpeedMetersPerSecond, double finalAccelerationMetersPerSecondSquared)
+    {
+        DistanceMeters = distanceMeters;
+        DurationSeconds = durationSeconds;
+        FinalSpeedMetersPerSecond = finalSpeedMetersPerSecond;
+        FinalAccelerationMetersPerSecondSquared = finalAccelerationMetersPerSecondSquared;
+    }
+
+    public double DistanceMeters { get; }
+    public double DurationSeconds { get; }
+    public double FinalSpeedMetersPerSecond { get; }
+    public double FinalAccelerationMetersPerSecondSquared { get; }
+}
+
+public static class JerkLimitedStoppingMath
+{
+    private const double NumericTolerance = 1e-12d;
+
+    public static bool TryCalculate(double speed, double acceleration,
+        double maximumDeceleration, double maximumJerk,
+        out JerkLimitedStoppingProfile profile, out string failureReason)
+    {
+        profile = null;
+        failureReason = string.Empty;
+        if (!IsFinite(speed) || speed < 0d || !IsFinite(acceleration) ||
+            !IsPositiveFinite(maximumDeceleration) || !IsPositiveFinite(maximumJerk) ||
+            acceleration < -maximumDeceleration - NumericTolerance)
+        {
+            failureReason = "Stopping inputs are outside finite longitudinal bounds.";
+            return false;
+        }
+        if (speed <= NumericTolerance && Math.Abs(acceleration) <= NumericTolerance)
+        {
+            profile = new JerkLimitedStoppingProfile(0d, 0d, 0d, 0d);
+            return true;
+        }
+
+        double unavoidableReleaseLoss = acceleration < 0d
+            ? acceleration * acceleration / (2d * maximumJerk)
+            : 0d;
+        if (speed + NumericTolerance < unavoidableReleaseLoss)
+        {
+            failureReason = "The current negative acceleration cannot be released before speed crosses zero.";
+            return false;
+        }
+
+        double peakDeceleration = Math.Sqrt(maximumJerk * speed +
+            0.5d * acceleration * acceleration);
+        double downDuration;
+        double plateauDuration;
+        double upDuration;
+        if (peakDeceleration <= maximumDeceleration + NumericTolerance)
+        {
+            peakDeceleration = Math.Min(peakDeceleration, maximumDeceleration);
+            downDuration = (acceleration + peakDeceleration) / maximumJerk;
+            plateauDuration = 0d;
+            upDuration = peakDeceleration / maximumJerk;
+        }
+        else
+        {
+            peakDeceleration = maximumDeceleration;
+            downDuration = (acceleration + peakDeceleration) / maximumJerk;
+            upDuration = peakDeceleration / maximumJerk;
+            double speedAfterDown = speed + acceleration * downDuration -
+                0.5d * maximumJerk * downDuration * downDuration;
+            double releaseLoss = peakDeceleration * peakDeceleration /
+                (2d * maximumJerk);
+            plateauDuration = (speedAfterDown - releaseLoss) / peakDeceleration;
+        }
+        if (downDuration < -NumericTolerance || plateauDuration < -NumericTolerance)
+        {
+            failureReason = "No monotone three-phase jerk-limited stop exists for the current state.";
+            return false;
+        }
+
+        downDuration = Math.Max(0d, downDuration);
+        plateauDuration = Math.Max(0d, plateauDuration);
+        double s = 0d;
+        double u = speed;
+        double a = acceleration;
+        Integrate(ref s, ref u, ref a, -maximumJerk, downDuration);
+        Integrate(ref s, ref u, ref a, 0d, plateauDuration);
+        Integrate(ref s, ref u, ref a, maximumJerk, upDuration);
+        if (Math.Abs(u) > 1e-9d || Math.Abs(a) > 1e-9d || s < -NumericTolerance)
+        {
+            failureReason = "The jerk-limited stop did not end at zero speed and acceleration.";
+            return false;
+        }
+
+        profile = new JerkLimitedStoppingProfile(Math.Max(0d, s),
+            downDuration + plateauDuration + upDuration, 0d, 0d);
+        return true;
+    }
+
+    public static double MaximumInitialSpeedForDistance(double availableDistance,
+        double conservativeInitialAcceleration, double maximumDeceleration,
+        double maximumJerk, double directionMaximumSpeed)
+    {
+        if (!IsFinite(availableDistance) || availableDistance < 0d ||
+            !IsFinite(conservativeInitialAcceleration) ||
+            !IsPositiveFinite(maximumDeceleration) || !IsPositiveFinite(maximumJerk) ||
+            !IsPositiveFinite(directionMaximumSpeed))
+            throw new ArgumentOutOfRangeException(nameof(availableDistance));
+
+        double lower = 0d;
+        double upper = directionMaximumSpeed;
+        for (int iteration = 0; iteration < 64; iteration++)
+        {
+            double candidate = 0.5d * (lower + upper);
+            bool fits = TryCalculate(candidate, conservativeInitialAcceleration,
+                maximumDeceleration, maximumJerk,
+                out JerkLimitedStoppingProfile stop, out _) &&
+                stop.DistanceMeters <= availableDistance + NumericTolerance;
+            if (fits)
+                lower = candidate;
+            else
+                upper = candidate;
+        }
+        return lower;
+    }
+
+    public static double CalculateMaximumStoppedDistance(double initialSpeed,
+        double initialAcceleration, double maximumSpeed, double maximumAcceleration,
+        double maximumDeceleration, double maximumJerk, double timeHorizon)
+    {
+        if (!IsFinite(initialSpeed) || initialSpeed < 0d ||
+            !IsFinite(initialAcceleration) || !IsPositiveFinite(maximumSpeed) ||
+            !IsPositiveFinite(maximumAcceleration) || !IsPositiveFinite(maximumDeceleration) ||
+            !IsPositiveFinite(maximumJerk) || !IsPositiveFinite(timeHorizon))
+            throw new ArgumentOutOfRangeException(nameof(timeHorizon));
+
+        double lower = 0d;
+        double upper = timeHorizon;
+        double bestDistance = 0d;
+        for (int iteration = 0; iteration < 64; iteration++)
+        {
+            double driveDuration = 0.5d * (lower + upper);
+            AdvanceTowardMaximumSpeed(initialSpeed, initialAcceleration,
+                maximumSpeed, maximumAcceleration, maximumJerk, driveDuration,
+                out double driveDistance, out double speed, out double acceleration);
+            bool fits = TryCalculate(speed, acceleration, maximumDeceleration,
+                maximumJerk, out JerkLimitedStoppingProfile stop, out _) &&
+                driveDuration + stop.DurationSeconds <= timeHorizon + NumericTolerance;
+            if (fits)
+            {
+                lower = driveDuration;
+                bestDistance = Math.Max(bestDistance, driveDistance + stop.DistanceMeters);
+            }
+            else
+            {
+                upper = driveDuration;
+            }
+        }
+        return bestDistance;
+    }
+
+    private static void AdvanceTowardMaximumSpeed(double initialSpeed, double initialAcceleration,
+        double maximumSpeed, double maximumAcceleration, double maximumJerk, double duration,
+        out double distance, out double speed, out double acceleration)
+    {
+        distance = 0d;
+        speed = initialSpeed;
+        acceleration = initialAcceleration;
+        double remaining = duration;
+        while (remaining > NumericTolerance)
+        {
+            double dt = Math.Min(0.001d, remaining);
+            double speedNeededToReleaseAcceleration = acceleration > 0d
+                ? acceleration * acceleration / (2d * maximumJerk)
+                : 0d;
+            double jerk = speed + speedNeededToReleaseAcceleration >= maximumSpeed
+                ? (acceleration > 0d ? -maximumJerk : 0d)
+                : (acceleration < maximumAcceleration ? maximumJerk : 0d);
+            Integrate(ref distance, ref speed, ref acceleration, jerk, dt);
+            if (speed > maximumSpeed && speed - maximumSpeed <= 1e-6d)
+            {
+                speed = maximumSpeed;
+                acceleration = 0d;
+            }
+            remaining -= dt;
+        }
+    }
+
+    private static void Integrate(ref double s, ref double u, ref double a,
+        double jerk, double duration)
+    {
+        s += u * duration + 0.5d * a * duration * duration +
+            jerk * duration * duration * duration / 6d;
+        u += a * duration + 0.5d * jerk * duration * duration;
+        a += jerk * duration;
+    }
+
+    private static bool IsPositiveFinite(double value) => IsFinite(value) && value > 0d;
+
+    private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value);
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs

@@ -34,8 +34,12 @@ public sealed class PathSpeedLimitBuilder
             return EmPlanningStatus.InvalidInput;
         }
 
-        LongitudinalStoppingProfile stopProfile = LongitudinalStoppingMath.Calculate(input.InitialProgressSpeedMetersPerSecond,
+        if (!JerkLimitedStoppingMath.TryCalculate(input.InitialProgressSpeedMetersPerSecond,
-            input.InitialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk);
+                input.InitialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk,
+                out JerkLimitedStoppingProfile stopProfile, out failureReason))
+        {
+            return EmPlanningStatus.InvalidInput;
+        }
         if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > input.TerminalPathS)
         {
             failureReason = "The available actual PathS distance is insufficient for the jerk-limited stop.";
@@ -206,62 +210,3 @@ public sealed class PathSpeedLimitBuilder
         return !double.IsNaN(value) && !double.IsInfinity(value);
     }
 }
-
-internal sealed class LongitudinalStoppingProfile
-{
-    public LongitudinalStoppingProfile(double distanceMeters, double durationSeconds)
-    {
-        DistanceMeters = distanceMeters;
-        DurationSeconds = durationSeconds;
-    }
-
-    public double DistanceMeters { get; }
-
-    public double DurationSeconds { get; }
-}
-
-internal static class LongitudinalStoppingMath
-{
-    public static LongitudinalStoppingProfile Calculate(double speedMetersPerSecond, double accelerationMetersPerSecondSquared,
-        double maximumDecelerationMetersPerSecondSquared, double maximumJerkMetersPerSecondCubed)
-    {
-        if (!IsFinite(speedMetersPerSecond) || !IsFinite(accelerationMetersPerSecondSquared) ||
-            !IsPositiveFinite(maximumDecelerationMetersPerSecondSquared) || !IsPositiveFinite(maximumJerkMetersPerSecondCubed))
-        {
-            throw new ArgumentOutOfRangeException(nameof(speedMetersPerSecond));
-        }
-        if (speedMetersPerSecond <= 0d)
-            return new LongitudinalStoppingProfile(0d, 0d);
-
-        double acceleration = Math.Max(-maximumDecelerationMetersPerSecondSquared, accelerationMetersPerSecondSquared);
-        double rampDuration = (acceleration + maximumDecelerationMetersPerSecondSquared) / maximumJerkMetersPerSecondCubed;
-        double speedAfterRamp = speedMetersPerSecond + acceleration * rampDuration -
-            0.5d * maximumJerkMetersPerSecondCubed * rampDuration * rampDuration;
-        if (speedAfterRamp <= 0d)
-        {
-            double root = (acceleration + Math.Sqrt(acceleration * acceleration + 2d * maximumJerkMetersPerSecondCubed *
-                speedMetersPerSecond)) / maximumJerkMetersPerSecondCubed;
-            double distance = speedMetersPerSecond * root + 0.5d * acceleration * root * root -
-                maximumJerkMetersPerSecondCubed * root * root * root / 6d;
-            return new LongitudinalStoppingProfile(Math.Max(0d, distance), root);
-        }
-
-        double rampDistance = speedMetersPerSecond * rampDuration + 0.5d * acceleration * rampDuration * rampDuration -
-            maximumJerkMetersPerSecondCubed * rampDuration * rampDuration * rampDuration / 6d;
-        double constantDecelerationDuration = speedAfterRamp / maximumDecelerationMetersPerSecondSquared;
-        double constantDecelerationDistance = speedAfterRamp * speedAfterRamp /
-            (2d * maximumDecelerationMetersPerSecondSquared);
-        return new LongitudinalStoppingProfile(rampDistance + constantDecelerationDistance,
-            rampDuration + constantDecelerationDuration);
-    }
-
-    private static bool IsPositiveFinite(double value)
-    {
-        return IsFinite(value) && value > 0d;
-    }
-
-    private static bool IsFinite(double value)
-    {
-        return !double.IsNaN(value) && !double.IsInfinity(value);
-    }
-}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs

@@ -205,9 +205,13 @@ public sealed class SequentialLongitudinalOptimizer
         double horizon = times[knotCount - 1];
         double requestedSpeed = Math.Min(input.DirectionMaximumSpeedMetersPerSecond,
             Math.Max(0d, input.TerminalPathS / horizon));
-        LongitudinalStoppingProfile terminalStop = LongitudinalStoppingMath.Calculate(requestedSpeed, 0d,
+        if (!JerkLimitedStoppingMath.TryCalculate(requestedSpeed, 0d,
-            input.Configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared,
+                input.Configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared,
-            input.Configuration.Longitudinal.MaximumJerkMetersPerSecondCubed);
+                input.Configuration.Longitudinal.MaximumJerkMetersPerSecondCubed,
+                out JerkLimitedStoppingProfile terminalStop, out _))
+        {
+            throw new ArgumentOutOfRangeException(nameof(input));
+        }
         double cruiseDistance = Math.Max(0d, input.TerminalPathS - terminalStop.DistanceMeters);
         for (int index = 0; index < knotCount; index++)
         {

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs

@@ -60,9 +60,13 @@ public sealed class PlanningHorizonSelector
         }
 
         double remainingSegment = Math.Max(0d, segment.LengthMeters - currentSegmentReferenceS);
-        LongitudinalStoppingProfile initialStop = LongitudinalStoppingMath.Calculate(initialProgressSpeedMetersPerSecond,
+        if (!JerkLimitedStoppingMath.TryCalculate(initialProgressSpeedMetersPerSecond,
-            initialAccelerationMetersPerSecondSquared, longitudinal.MaximumDecelerationMetersPerSecondSquared,
+                initialAccelerationMetersPerSecondSquared, longitudinal.MaximumDecelerationMetersPerSecondSquared,
-            longitudinal.MaximumJerkMetersPerSecondCubed);
+                longitudinal.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile initialStop,
+                out failureReason))
+        {
+            return EmPlanningStatus.InvalidInput;
+        }
         if (initialStop.DistanceMeters + BoundaryTolerance > remainingSegment)
         {
             failureReason = "The current segment lacks the jerk-limited stopping distance.";
@@ -88,8 +92,13 @@ public sealed class PlanningHorizonSelector
     private static double CalculateReachableDistance(double initialSpeed, double initialAcceleration, double maximumSpeed,
         LongitudinalConfiguration configuration, double timeHorizonSeconds)
     {
-        LongitudinalStoppingProfile stopAtMaximumSpeed = LongitudinalStoppingMath.Calculate(maximumSpeed, 0d,
+        if (!JerkLimitedStoppingMath.TryCalculate(maximumSpeed, 0d,
-            configuration.MaximumDecelerationMetersPerSecondSquared, configuration.MaximumJerkMetersPerSecondCubed);
+                configuration.MaximumDecelerationMetersPerSecondSquared,
+                configuration.MaximumJerkMetersPerSecondCubed,
+                out JerkLimitedStoppingProfile stopAtMaximumSpeed, out _))
+        {
+            throw new ArgumentOutOfRangeException(nameof(configuration));
+        }
         double drivingDuration = timeHorizonSeconds - configuration.ZeroSpeedHoldSeconds - stopAtMaximumSpeed.DurationSeconds;
         if (drivingDuration <= 0d)
             return Math.Min(initialSpeed, maximumSpeed) * Math.Max(0d, timeHorizonSeconds - configuration.ZeroSpeedHoldSeconds);

ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs

@@ -10,6 +10,8 @@ internal static class LongitudinalModelChecks
 {
     public static void Run()
     {
+        VerifiesJerkLimitedStoppingProfileEndsAtRest();
+        VerifiesStoppedReachabilityUsesTheSameJerkModel();
         VerifiesFinitePathSIndexedSpeedEnvelope();
         VerifiesStoppingEnvelopeIsRefinedOnActualPathS();
         VerifiesStoppingEnvelopeUsesDiscreteTimeTailStations();
@@ -18,6 +20,42 @@ internal static class LongitudinalModelChecks
         VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
     }
 
+    private static void VerifiesJerkLimitedStoppingProfileEndsAtRest()
+    {
+        Verification.True(JerkLimitedStoppingMath.TryCalculate(
+            0.20d, 0d, 0.30d, 0.50d,
+            out JerkLimitedStoppingProfile profile, out string failure),
+            "jerk-limited stop builds: " + failure);
+        Verification.True(profile.DistanceMeters > 0d, "stop distance is positive");
+        Verification.True(profile.DurationSeconds > 0d, "stop duration is positive");
+        Verification.NearlyEqual(0d, profile.FinalSpeedMetersPerSecond,
+            "stop ends at zero speed");
+        Verification.NearlyEqual(0d, profile.FinalAccelerationMetersPerSecondSquared,
+            "stop releases acceleration to zero");
+
+        Verification.True(JerkLimitedStoppingMath.TryCalculate(
+            0.20d, 0.20d, 0.30d, 0.50d,
+            out JerkLimitedStoppingProfile accelerating, out failure),
+            "positive-acceleration stop builds: " + failure);
+        Verification.True(accelerating.DistanceMeters > profile.DistanceMeters,
+            "positive initial acceleration needs more stopping distance");
+        Verification.NearlyEqual(0d, accelerating.FinalAccelerationMetersPerSecondSquared,
+            "positive-acceleration stop also releases acceleration");
+    }
+
+    private static void VerifiesStoppedReachabilityUsesTheSameJerkModel()
+    {
+        double maximumDistance = JerkLimitedStoppingMath.CalculateMaximumStoppedDistance(
+            0d, 0d, 0.20d, 0.20d, 0.30d, 0.50d, 2d);
+        Verification.True(maximumDistance > 0d && maximumDistance < 0.40d,
+            "two-second stopped reach is finite and below unconstrained cruise distance");
+
+        double cap = JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
+            maximumDistance, 0.20d, 0.30d, 0.50d, 0.20d);
+        Verification.True(cap >= 0d && cap <= 0.20d,
+            "distance inversion stays inside the direction speed range");
+    }
+
     private static void VerifiesFinitePathSIndexedSpeedEnvelope()
     {
         LateralPath directionPath = CreatePath(new[]