diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/JerkLimitedStoppingMath.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/JerkLimitedStoppingMath.cs new file mode 100644 index 0000000..2f4d03d --- /dev/null +++ b/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); +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs index 74dbf6c..d2bdcd5 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs @@ -34,8 +34,12 @@ public sealed class PathSpeedLimitBuilder return EmPlanningStatus.InvalidInput; } - LongitudinalStoppingProfile stopProfile = LongitudinalStoppingMath.Calculate(input.InitialProgressSpeedMetersPerSecond, - input.InitialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk); + if (!JerkLimitedStoppingMath.TryCalculate(input.InitialProgressSpeedMetersPerSecond, + 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); - } -} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs index 70b03eb..2c10d17 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs +++ b/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, - input.Configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared, - input.Configuration.Longitudinal.MaximumJerkMetersPerSecondCubed); + if (!JerkLimitedStoppingMath.TryCalculate(requestedSpeed, 0d, + input.Configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared, + 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++) { diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs index bc9d2c2..55d419a 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs +++ b/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, - initialAccelerationMetersPerSecondSquared, longitudinal.MaximumDecelerationMetersPerSecondSquared, - longitudinal.MaximumJerkMetersPerSecondCubed); + if (!JerkLimitedStoppingMath.TryCalculate(initialProgressSpeedMetersPerSecond, + initialAccelerationMetersPerSecondSquared, longitudinal.MaximumDecelerationMetersPerSecondSquared, + 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, - configuration.MaximumDecelerationMetersPerSecondSquared, configuration.MaximumJerkMetersPerSecondCubed); + if (!JerkLimitedStoppingMath.TryCalculate(maximumSpeed, 0d, + 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); diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs index f607bf9..6557fd3 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs +++ b/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[]