diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningInput.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningInput.cs new file mode 100644 index 0000000..08ca926 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningInput.cs @@ -0,0 +1,111 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; +using MultiWheelC.TrajectoryPlanning.CoarsePath; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Immutable longitudinal inputs derived only from an independently validated lateral PathS path. +public sealed class LongitudinalPlanningInput +{ + private const double PathSTolerance = 1e-12d; + + public LongitudinalPlanningInput(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond, + double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmPlannerConfiguration configuration, + IReadOnlyList previousPathS, IReadOnlyList previousProgressSpeedMetersPerSecond) + { + if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2) + throw new ArgumentException("Longitudinal planning requires an independently validated lateral path with at least two points.", + nameof(path)); + if (!Enum.IsDefined(typeof(TravelDirection), direction)) + throw new ArgumentOutOfRangeException(nameof(direction)); + if (!IsFinite(initialProgressSpeedMetersPerSecond) || initialProgressSpeedMetersPerSecond < 0d) + throw new ArgumentOutOfRangeException(nameof(initialProgressSpeedMetersPerSecond)); + if (!IsFinite(initialAccelerationMetersPerSecondSquared)) + throw new ArgumentOutOfRangeException(nameof(initialAccelerationMetersPerSecondSquared)); + if (!Enum.IsDefined(typeof(EmTerminalType), terminalType)) + throw new ArgumentOutOfRangeException(nameof(terminalType)); + if (configuration == null) + throw new ArgumentNullException(nameof(configuration)); + + Path = CopyAndValidatePath(path); + Direction = direction; + InitialProgressSpeedMetersPerSecond = initialProgressSpeedMetersPerSecond; + InitialAccelerationMetersPerSecondSquared = initialAccelerationMetersPerSecondSquared; + TerminalType = terminalType; + Configuration = configuration.Copy(); + PreviousPathS = CopyFiniteNonnegative(previousPathS, nameof(previousPathS)); + PreviousProgressSpeedMetersPerSecond = CopyFiniteNonnegative(previousProgressSpeedMetersPerSecond, + nameof(previousProgressSpeedMetersPerSecond)); + if (PreviousPathS.Count != PreviousProgressSpeedMetersPerSecond.Count) + throw new ArgumentException("Previous path-S and progress-speed samples must have matching counts.", + nameof(previousProgressSpeedMetersPerSecond)); + } + + public LateralPath Path { get; } + + public TravelDirection Direction { get; } + + public double InitialProgressSpeedMetersPerSecond { get; } + + public double InitialAccelerationMetersPerSecondSquared { get; } + + public EmTerminalType TerminalType { get; } + + public EmPlannerConfiguration Configuration { get; } + + public IReadOnlyList PreviousPathS { get; } + + public IReadOnlyList PreviousProgressSpeedMetersPerSecond { get; } + + public double TerminalPathS { get { return Path.Points[Path.Points.Count - 1].PathS; } } + + public double DirectionMaximumSpeedMetersPerSecond + { + get + { + return Direction == TravelDirection.Forward + ? Configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond + : Configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond; + } + } + + private static LateralPath CopyAndValidatePath(LateralPath source) + { + var copy = new List(source.Points.Count); + double previousPathS = double.NegativeInfinity; + for (int index = 0; index < source.Points.Count; index++) + { + LateralPathPoint point = source.Points[index]; + if (point == null || !IsFinite(point.PathS) || point.PathS <= previousPathS) + throw new ArgumentException("Lateral PathS must be finite and strictly increasing for ST planning.", nameof(source)); + if (index == 0 && Math.Abs(point.PathS) > PathSTolerance) + throw new ArgumentException("The lateral PathS supplied to ST must begin at zero.", nameof(source)); + copy.Add(new LateralPathPoint(point.ReferenceS, point.PathS, point.L, point.DL, point.DDL, point.DDDL, + point.X, point.Y, point.VehicleYaw, point.GeometricCurvature, point.VehicleCurvature, + point.VehicleCurvatureDerivative)); + previousPathS = point.PathS; + } + return new LateralPath(copy, true); + } + + private static IReadOnlyList CopyFiniteNonnegative(IReadOnlyList source, string parameterName) + { + var copy = new List(source == null ? 0 : source.Count); + if (source != null) + { + for (int index = 0; index < source.Count; index++) + { + if (!IsFinite(source[index]) || source[index] < 0d) + throw new ArgumentOutOfRangeException(parameterName); + copy.Add(source[index]); + } + } + return new ReadOnlyCollection(copy); + } + + private static bool IsFinite(double value) + { + return !double.IsNaN(value) && !double.IsInfinity(value); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimit.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimit.cs new file mode 100644 index 0000000..d558915 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimit.cs @@ -0,0 +1,123 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Finite piecewise-linear speed limits indexed exclusively by actual lateral PathS. +public sealed class PathSpeedLimit +{ + private const double StationTolerance = 1e-12d; + + internal PathSpeedLimit(IReadOnlyList pathS, IReadOnlyList maximumSpeed, + IReadOnlyList lateralAccelerationLimit, IReadOnlyList curvatureRateLimit, + IReadOnlyList stoppingLimit, double directionMaximumSpeedMetersPerSecond) + { + PathS = CopyStrictStations(pathS, nameof(pathS)); + MaximumSpeedMetersPerSecond = CopyFiniteNonnegative(maximumSpeed, PathS.Count, nameof(maximumSpeed)); + LateralAccelerationSpeedLimitsMetersPerSecond = CopyFiniteNonnegative(lateralAccelerationLimit, PathS.Count, + nameof(lateralAccelerationLimit)); + CurvatureRateSpeedLimitsMetersPerSecond = CopyFiniteNonnegative(curvatureRateLimit, PathS.Count, + nameof(curvatureRateLimit)); + StoppingSpeedLimitsMetersPerSecond = CopyFiniteNonnegative(stoppingLimit, PathS.Count, nameof(stoppingLimit)); + if (!IsFinite(directionMaximumSpeedMetersPerSecond) || directionMaximumSpeedMetersPerSecond <= 0d) + throw new ArgumentOutOfRangeException(nameof(directionMaximumSpeedMetersPerSecond)); + if (MaximumSpeedMetersPerSecond[MaximumSpeedMetersPerSecond.Count - 1] != 0d || + StoppingSpeedLimitsMetersPerSecond[StoppingSpeedLimitsMetersPerSecond.Count - 1] != 0d) + { + throw new ArgumentException("Terminal PathS speed limits must be exactly zero."); + } + + DirectionMaximumSpeedMetersPerSecond = directionMaximumSpeedMetersPerSecond; + } + + public IReadOnlyList PathS { get; } + + public IReadOnlyList MaximumSpeedMetersPerSecond { get; } + + public IReadOnlyList LateralAccelerationSpeedLimitsMetersPerSecond { get; } + + public IReadOnlyList CurvatureRateSpeedLimitsMetersPerSecond { get; } + + public IReadOnlyList StoppingSpeedLimitsMetersPerSecond { get; } + + public double DirectionMaximumSpeedMetersPerSecond { get; } + + public double TerminalPathS { get { return PathS[PathS.Count - 1]; } } + + public double MaximumSpeedAt(double pathS) + { + return Interpolate(MaximumSpeedMetersPerSecond, pathS); + } + + public double LateralAccelerationLimitAt(double pathS) + { + return Interpolate(LateralAccelerationSpeedLimitsMetersPerSecond, pathS); + } + + public double CurvatureRateLimitAt(double pathS) + { + return Interpolate(CurvatureRateSpeedLimitsMetersPerSecond, pathS); + } + + public double StoppingLimitAt(double pathS) + { + return Interpolate(StoppingSpeedLimitsMetersPerSecond, pathS); + } + + private double Interpolate(IReadOnlyList values, double pathS) + { + if (!IsFinite(pathS) || pathS < PathS[0] - StationTolerance || pathS > TerminalPathS + StationTolerance) + throw new ArgumentOutOfRangeException(nameof(pathS)); + if (pathS <= PathS[0]) + return values[0]; + if (pathS >= TerminalPathS) + return values[values.Count - 1]; + + for (int index = 1; index < PathS.Count; index++) + { + if (pathS <= PathS[index]) + { + double fraction = (pathS - PathS[index - 1]) / (PathS[index] - PathS[index - 1]); + return values[index - 1] + (values[index] - values[index - 1]) * fraction; + } + } + return values[values.Count - 1]; + } + + private static IReadOnlyList CopyStrictStations(IReadOnlyList source, string parameterName) + { + if (source == null || source.Count < 2) + throw new ArgumentException("At least two PathS stations are required.", parameterName); + var copy = new List(source.Count); + double previous = double.NegativeInfinity; + for (int index = 0; index < source.Count; index++) + { + if (!IsFinite(source[index]) || source[index] <= previous) + throw new ArgumentException("PathS stations must be finite and strictly increasing.", parameterName); + copy.Add(source[index]); + previous = source[index]; + } + return new ReadOnlyCollection(copy); + } + + private static IReadOnlyList CopyFiniteNonnegative(IReadOnlyList source, int expectedCount, + string parameterName) + { + if (source == null || source.Count != expectedCount) + throw new ArgumentException("Speed limit count must match PathS stations.", parameterName); + var copy = new List(source.Count); + for (int index = 0; index < source.Count; index++) + { + if (!IsFinite(source[index]) || source[index] < 0d) + throw new ArgumentOutOfRangeException(parameterName); + copy.Add(source[index]); + } + return new ReadOnlyCollection(copy); + } + + private static bool IsFinite(double value) + { + return !double.IsNaN(value) && !double.IsInfinity(value); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs new file mode 100644 index 0000000..6fa0d57 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs @@ -0,0 +1,188 @@ +using System; +using System.Collections.Generic; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Builds curvature-aware, stopping-aware speed limits over actual optimized PathS. +public sealed class PathSpeedLimitBuilder +{ + internal const double CurvatureEpsilon = 1e-10d; + private const double StopDistanceToleranceMeters = 1e-8d; + + public EmPlanningStatus Build(LongitudinalPlanningInput input, out PathSpeedLimit speedLimit, out string failureReason) + { + speedLimit = null; + failureReason = string.Empty; + if (input == null) + { + failureReason = "Longitudinal planning input is required."; + return EmPlanningStatus.InvalidInput; + } + + if (!TryGetLimits(input, out double directionMaximum, out double maximumAcceleration, out double maximumDeceleration, + out double maximumJerk, out double maximumLateralAcceleration, out double maximumCurvatureRate, + out failureReason)) + { + return EmPlanningStatus.InvalidInput; + } + if (input.InitialProgressSpeedMetersPerSecond > directionMaximum + StopDistanceToleranceMeters || + input.InitialAccelerationMetersPerSecondSquared < -maximumDeceleration - StopDistanceToleranceMeters || + input.InitialAccelerationMetersPerSecondSquared > maximumAcceleration + StopDistanceToleranceMeters) + { + failureReason = "The initial longitudinal state violates the configured hard bounds."; + return EmPlanningStatus.InvalidInput; + } + + LongitudinalStoppingProfile stopProfile = LongitudinalStoppingMath.Calculate(input.InitialProgressSpeedMetersPerSecond, + input.InitialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk); + if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > input.TerminalPathS) + { + failureReason = "The available actual PathS distance is insufficient for the jerk-limited stop."; + return EmPlanningStatus.StoppingDistanceInsufficient; + } + + 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]; + pathS[index] = point.PathS; + double lateralLimit = Math.Sqrt(maximumLateralAcceleration / + Math.Max(Math.Abs(point.VehicleCurvature), CurvatureEpsilon)); + double curvatureRateLimit = maximumCurvatureRate / + Math.Max(Math.Abs(point.VehicleCurvatureDerivative), CurvatureEpsilon); + double remainingDistance = Math.Max(0d, input.TerminalPathS - point.PathS); + double stoppingLimit = Math.Sqrt(2d * maximumDeceleration * remainingDistance); + lateral[index] = ClampFinite(lateralLimit, directionMaximum); + curvatureRate[index] = ClampFinite(curvatureRateLimit, directionMaximum); + stopping[index] = index == count - 1 ? 0d : ClampFinite(stoppingLimit, directionMaximum); + maximum[index] = index == count - 1 ? 0d : Math.Min(directionMaximum, + Math.Min(lateral[index], Math.Min(curvatureRate[index], stopping[index]))); + } + + try + { + speedLimit = new PathSpeedLimit(pathS, maximum, lateral, curvatureRate, stopping, directionMaximum); + return EmPlanningStatus.Success; + } + catch (ArgumentException exception) + { + failureReason = exception.Message; + return EmPlanningStatus.InvalidInput; + } + } + + internal static bool TryGetLimits(LongitudinalPlanningInput input, out double directionMaximum, + out double maximumAcceleration, out double maximumDeceleration, out double maximumJerk, + out double maximumLateralAcceleration, out double maximumCurvatureRate, out string failureReason) + { + directionMaximum = 0d; + maximumAcceleration = 0d; + maximumDeceleration = 0d; + maximumJerk = 0d; + maximumLateralAcceleration = 0d; + maximumCurvatureRate = 0d; + failureReason = string.Empty; + if (input.Configuration == null || input.Configuration.Longitudinal == null) + { + failureReason = "Longitudinal configuration is required."; + return false; + } + + LongitudinalConfiguration configuration = input.Configuration.Longitudinal; + directionMaximum = input.DirectionMaximumSpeedMetersPerSecond; + maximumAcceleration = configuration.MaximumAccelerationMetersPerSecondSquared; + maximumDeceleration = configuration.MaximumDecelerationMetersPerSecondSquared; + maximumJerk = configuration.MaximumJerkMetersPerSecondCubed; + maximumLateralAcceleration = configuration.MaximumLateralAccelerationMetersPerSecondSquared; + maximumCurvatureRate = configuration.MaximumCurvatureRatePerMeterPerSecond; + if (!IsPositiveFinite(directionMaximum) || !IsPositiveFinite(maximumAcceleration) || + !IsPositiveFinite(maximumDeceleration) || !IsPositiveFinite(maximumJerk) || + !IsPositiveFinite(maximumLateralAcceleration) || !IsPositiveFinite(maximumCurvatureRate)) + { + failureReason = "Longitudinal limits must be positive and finite."; + return false; + } + return true; + } + + private static double ClampFinite(double value, double maximum) + { + if (!IsFinite(value) || value < 0d) + throw new ArgumentOutOfRangeException(nameof(value)); + return Math.Min(maximum, value); + } + + private static bool IsPositiveFinite(double value) + { + return IsFinite(value) && value > 0d; + } + + private static bool IsFinite(double value) + { + 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/Segmentation/PlanningHorizonSelector.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs new file mode 100644 index 0000000..bc9d2c2 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs @@ -0,0 +1,156 @@ +using System; +using MultiWheelC.TrajectoryPlanning.CoarsePath; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Reference-distance terminal chosen before LS without crossing the current direction segment. +public sealed class PlanningHorizonSelection +{ + internal PlanningHorizonSelection(double terminalReferenceS, EmTerminalType terminalType) + { + TerminalReferenceS = terminalReferenceS; + TerminalType = terminalType; + } + + public double TerminalReferenceS { get; } + + public EmTerminalType TerminalType { get; } +} + +public sealed class PlanningHorizonSelector +{ + private const double BoundaryTolerance = 1e-8d; + + public EmPlanningStatus Select(DirectionSegmentView segment, double currentSegmentReferenceS, + double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared, + EmPlannerConfiguration configuration, out PlanningHorizonSelection selection, out string failureReason) + { + selection = null; + failureReason = string.Empty; + if (segment == null || configuration == null || configuration.Scheduling == null || configuration.Longitudinal == null || + !IsFinite(currentSegmentReferenceS) || currentSegmentReferenceS < 0d || + currentSegmentReferenceS > segment.LengthMeters + BoundaryTolerance || + !IsFinite(initialProgressSpeedMetersPerSecond) || initialProgressSpeedMetersPerSecond < 0d || + !IsFinite(initialAccelerationMetersPerSecondSquared)) + { + failureReason = "Planning horizon inputs are invalid."; + return EmPlanningStatus.InvalidInput; + } + + LongitudinalConfiguration longitudinal = configuration.Longitudinal; + SchedulingConfiguration scheduling = configuration.Scheduling; + double directionMaximum = segment.Direction == TravelDirection.Forward + ? longitudinal.MaximumForwardSpeedMetersPerSecond + : longitudinal.MaximumReverseSpeedMetersPerSecond; + if (!IsPositiveFinite(directionMaximum) || !IsPositiveFinite(longitudinal.MaximumAccelerationMetersPerSecondSquared) || + !IsPositiveFinite(longitudinal.MaximumDecelerationMetersPerSecondSquared) || + !IsPositiveFinite(longitudinal.MaximumJerkMetersPerSecondCubed) || + !IsPositiveFinite(longitudinal.ZeroSpeedHoldSeconds) || !IsPositiveFinite(scheduling.TimeHorizonSeconds) || + !IsPositiveFinite(scheduling.DistanceHorizonMeters)) + { + failureReason = "Planning horizon configuration is invalid."; + return EmPlanningStatus.InvalidInput; + } + if (initialProgressSpeedMetersPerSecond > directionMaximum + BoundaryTolerance || + initialAccelerationMetersPerSecondSquared < -longitudinal.MaximumDecelerationMetersPerSecondSquared - BoundaryTolerance || + initialAccelerationMetersPerSecondSquared > longitudinal.MaximumAccelerationMetersPerSecondSquared + BoundaryTolerance) + { + failureReason = "The initial state violates longitudinal bounds."; + return EmPlanningStatus.InvalidInput; + } + + double remainingSegment = Math.Max(0d, segment.LengthMeters - currentSegmentReferenceS); + LongitudinalStoppingProfile initialStop = LongitudinalStoppingMath.Calculate(initialProgressSpeedMetersPerSecond, + initialAccelerationMetersPerSecondSquared, longitudinal.MaximumDecelerationMetersPerSecondSquared, + longitudinal.MaximumJerkMetersPerSecondCubed); + if (initialStop.DistanceMeters + BoundaryTolerance > remainingSegment) + { + failureReason = "The current segment lacks the jerk-limited stopping distance."; + return EmPlanningStatus.StoppingDistanceInsufficient; + } + + double timeReachable = CalculateReachableDistance(initialProgressSpeedMetersPerSecond, + initialAccelerationMetersPerSecondSquared, directionMaximum, longitudinal, scheduling.TimeHorizonSeconds); + double terminalReferenceS = currentSegmentReferenceS + Math.Min(remainingSegment, + Math.Min(scheduling.DistanceHorizonMeters, timeReachable)); + if (terminalReferenceS >= segment.LengthMeters - BoundaryTolerance) + { + terminalReferenceS = segment.LengthMeters; + selection = new PlanningHorizonSelection(terminalReferenceS, ToTerminalType(segment.EndBoundary.BoundaryType)); + } + else + { + selection = new PlanningHorizonSelection(terminalReferenceS, EmTerminalType.RollingSafetyStop); + } + return EmPlanningStatus.Success; + } + + private static double CalculateReachableDistance(double initialSpeed, double initialAcceleration, double maximumSpeed, + LongitudinalConfiguration configuration, double timeHorizonSeconds) + { + LongitudinalStoppingProfile stopAtMaximumSpeed = LongitudinalStoppingMath.Calculate(maximumSpeed, 0d, + configuration.MaximumDecelerationMetersPerSecondSquared, configuration.MaximumJerkMetersPerSecondCubed); + double drivingDuration = timeHorizonSeconds - configuration.ZeroSpeedHoldSeconds - stopAtMaximumSpeed.DurationSeconds; + if (drivingDuration <= 0d) + return Math.Min(initialSpeed, maximumSpeed) * Math.Max(0d, timeHorizonSeconds - configuration.ZeroSpeedHoldSeconds); + + double speed = initialSpeed; + double acceleration = initialAcceleration; + double distance = 0d; + const double simulationStepSeconds = 0.001d; + while (drivingDuration > 0d) + { + double step = Math.Min(simulationStepSeconds, drivingDuration); + double jerk = ChooseAccelerationJerk(speed, acceleration, maximumSpeed, + configuration.MaximumAccelerationMetersPerSecondSquared, configuration.MaximumJerkMetersPerSecondCubed); + double nextSpeed = speed + acceleration * step + 0.5d * jerk * step * step; + if (nextSpeed > maximumSpeed) + { + nextSpeed = maximumSpeed; + acceleration = 0d; + jerk = 0d; + } + distance += speed * step + 0.5d * acceleration * step * step + jerk * step * step * step / 6d; + acceleration += jerk * step; + speed = Math.Max(0d, nextSpeed); + drivingDuration -= step; + } + return Math.Max(0d, distance + stopAtMaximumSpeed.DistanceMeters); + } + + private static double ChooseAccelerationJerk(double speed, double acceleration, double maximumSpeed, + double maximumAcceleration, double maximumJerk) + { + if (speed >= maximumSpeed - BoundaryTolerance) + { + if (acceleration > 0d) + return -maximumJerk; + return acceleration < 0d ? maximumJerk : 0d; + } + double speedToReduceAccelerationToZero = acceleration > 0d + ? acceleration * acceleration / (2d * maximumJerk) + : 0d; + if (speed + speedToReduceAccelerationToZero >= maximumSpeed - BoundaryTolerance) + return acceleration > 0d ? -maximumJerk : 0d; + return acceleration < maximumAcceleration - BoundaryTolerance ? maximumJerk : 0d; + } + + private static EmTerminalType ToTerminalType(EmBoundaryType boundaryType) + { + return boundaryType == EmBoundaryType.Goal + ? EmTerminalType.Goal + : boundaryType == EmBoundaryType.GearSwitchApproach || boundaryType == EmBoundaryType.GearSwitchDeparture + ? EmTerminalType.GearSwitch + : EmTerminalType.RollingSafetyStop; + } + + 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/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs new file mode 100644 index 0000000..879bf89 --- /dev/null +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs @@ -0,0 +1,149 @@ +using System; +using System.Collections.Generic; +using EMPlannerVerificationHost; +using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.PathSmoothing; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +internal static class LongitudinalModelChecks +{ + public static void Run() + { + VerifiesFinitePathSIndexedSpeedEnvelope(); + VerifiesStoppingPrecheckBeforeQpAssembly(); + VerifiesReferenceHorizonSelectionKeepsTheCurrentSegmentBoundary(); + } + + private static void VerifiesFinitePathSIndexedSpeedEnvelope() + { + LateralPath directionPath = CreatePath(new[] + { + new PathFixture(0d, 0d, 0d, 0d), + new PathFixture(1d, 1d, 0d, 0d), + }); + var directionInput = new LongitudinalPlanningInput(directionPath, TravelDirection.Forward, 0d, 0d, + EmTerminalType.Goal, EmPlannerConfiguration.CreateDefault(), Array.Empty(), Array.Empty()); + EmPlanningStatus directionStatus = new PathSpeedLimitBuilder().Build(directionInput, + out PathSpeedLimit directionEnvelope, out string directionFailureReason); + Verification.Equal(EmPlanningStatus.Success, directionStatus, "default direction speed limit status: " + + directionFailureReason); + Verification.NearlyEqual(0.20d, directionEnvelope.DirectionMaximumSpeedMetersPerSecond, + "default direction speed limit"); + + EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault(); + configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d; + configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d; + LateralPath path = CreatePath(new[] + { + new PathFixture(10d, 0d, 0d, 0d), + new PathFixture(20d, 2d, 2d, 4d), + new PathFixture(20.5d, 4d, 20d, 0d), + new PathFixture(21d, 5d, 0d, 0d), + }); + var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d, + EmTerminalType.Goal, configuration, Array.Empty(), Array.Empty()); + + EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope, + out string failureReason); + Verification.Equal(EmPlanningStatus.Success, status, "speed envelope status: " + failureReason); + Verification.NearlyEqual(1d, envelope.DirectionMaximumSpeedMetersPerSecond, "overridden direction speed limit"); + Verification.NearlyEqual(Math.Sqrt(0.20d / 2d), envelope.LateralAccelerationLimitAt(2d), + "curvature lateral-acceleration limit"); + 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), + "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, + envelope.MaximumSpeedAt(1d), "speed envelope interpolates by PathS rather than ReferenceS"); + Verification.NearlyEqual(0d, envelope.MaximumSpeedAt(5d), "terminal speed is exactly zero"); + } + + private static void VerifiesStoppingPrecheckBeforeQpAssembly() + { + EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault(); + LateralPath shortPath = CreatePath(new[] + { + new PathFixture(0d, 0d, 0d, 0d), + new PathFixture(100d, 0.01d, 0d, 0d), + }); + var input = new LongitudinalPlanningInput(shortPath, TravelDirection.Forward, 0.20d, 0.20d, + EmTerminalType.RollingSafetyStop, configuration, Array.Empty(), Array.Empty()); + + EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope, + out string failureReason); + Verification.Equal(EmPlanningStatus.StoppingDistanceInsufficient, status, + "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"); + } + + private static void VerifiesReferenceHorizonSelectionKeepsTheCurrentSegmentBoundary() + { + EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault(); + DirectionSegmentView shortGoal = CreateSegment(0.25d, EmBoundaryType.Goal); + EmPlanningStatus status = new PlanningHorizonSelector().Select(shortGoal, 0d, 0d, 0d, configuration, + out PlanningHorizonSelection goalSelection, out string goalFailure); + + Verification.Equal(EmPlanningStatus.Success, status, "goal horizon status: " + goalFailure); + Verification.Equal(EmTerminalType.Goal, goalSelection.TerminalType, "goal terminal type"); + Verification.NearlyEqual(0.25d, goalSelection.TerminalReferenceS, "goal terminal reference S"); + + DirectionSegmentView longSegment = CreateSegment(4d, EmBoundaryType.GearSwitchApproach); + status = new PlanningHorizonSelector().Select(longSegment, 0d, 0d, 0d, configuration, + out PlanningHorizonSelection rollingSelection, out string rollingFailure); + Verification.Equal(EmPlanningStatus.Success, status, "rolling horizon status: " + rollingFailure); + Verification.Equal(EmTerminalType.RollingSafetyStop, rollingSelection.TerminalType, "rolling terminal type"); + Verification.True(rollingSelection.TerminalReferenceS >= 0d && rollingSelection.TerminalReferenceS < 4d, + "rolling horizon remains within the current segment"); + } + + private static LateralPath CreatePath(IReadOnlyList fixtures) + { + var points = new List(fixtures.Count); + for (int index = 0; index < fixtures.Count; index++) + { + PathFixture fixture = fixtures[index]; + points.Add(new LateralPathPoint(fixture.ReferenceS, fixture.PathS, 0d, 0d, 0d, 0d, fixture.PathS, 0d, + 0d, fixture.Curvature, fixture.Curvature, fixture.CurvatureDerivative)); + } + return new LateralPath(points, true); + } + + private static DirectionSegmentView CreateSegment(double length, EmBoundaryType endBoundaryType) + { + var points = new List + { + Point(0d, 0d), + Point(length, length), + }; + return new DirectionSegmentView(0, TravelDirection.Forward, points, + new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d), + new ReferenceBoundary(0, length, endBoundaryType, length), 0d); + } + + private static SmoothedPathPoint Point(double x, double s) + { + return new SmoothedPathPoint(x, 0d, 0d, 0d, s, TravelDirection.Forward, 0d, 0d, 0d, 1d, false, + SmoothedPathPointSource.Anchor); + } + + private sealed class PathFixture + { + public PathFixture(double referenceS, double pathS, double curvature, double curvatureDerivative) + { + ReferenceS = referenceS; + PathS = pathS; + Curvature = curvature; + CurvatureDerivative = curvatureDerivative; + } + + public double ReferenceS { get; } + public double PathS { get; } + public double Curvature { get; } + public double CurvatureDerivative { get; } + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs index 2b5b2d6..107992e 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs @@ -9,7 +9,8 @@ internal static class Program if (args.Length != 1 || (args[0] != "foundation" && args[0] != "segmentation" && args[0] != "frenet" && args[0] != "corridor" && args[0] != "optimization" && args[0] != "osqp" && args[0] != "osqp-loader" && args[0] != "osqp-probe" && args[0] != "all-foundation" && args[0] != "lateral-model" && args[0] != "lateral-integration" && - args[0] != "lateral-real-osqp" && args[0] != "lateral-real-osqp-probe" && args[0] != "lateral-all")) + args[0] != "lateral-real-osqp" && args[0] != "lateral-real-osqp-probe" && args[0] != "lateral-all" && + args[0] != "longitudinal-model")) { Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration|lateral-real-osqp|lateral-all"); return 2; @@ -71,6 +72,11 @@ internal static class Program MultiWheelC.TrajectoryPlanning.EMPlanner.LateralIntegrationChecks.RunRealOsqpInCleanPluginBundle(); Console.WriteLine("PASS lateral-real-osqp"); } + if (args[0] == "longitudinal-model") + { + MultiWheelC.TrajectoryPlanning.EMPlanner.LongitudinalModelChecks.Run(); + Console.WriteLine("PASS longitudinal-model"); + } return 0; } catch (Exception exception)