From 3269d556b6a9f80c5a0f9bc95ad5b3d6cb1c0f52 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Thu, 6 Aug 2026 23:54:34 +0800 Subject: [PATCH] feat: derive adaptive full-segment ST schedule --- .../EMPlanner/Facade/EmPlanningService.cs | 33 +- .../FullDirectionSegmentScheduleBuilder.cs | 203 ++++++++ .../LongitudinalConstraintBuilder.cs | 138 +++++- .../Longitudinal/LongitudinalKnotSchedule.cs | 118 +++++ .../Longitudinal/LongitudinalPlanningInput.cs | 31 ++ ...ngitudinalPreviousTrajectorySeedBuilder.cs | 9 + .../LongitudinalSolutionValidator.cs | 23 +- .../Longitudinal/PathSpeedLimitBuilder.cs | 90 +++- .../SequentialLongitudinalOptimizer.cs | 224 ++++++++- .../Trajectory/EmTrajectoryAssembler.cs | 5 +- .../Trajectory/TrajectorySampleSchedule.cs | 102 +++- .../EmPlanningServiceChecks.cs | 435 ++++++++++++++++-- .../LongitudinalIntegrationChecks.cs | 94 ++++ .../LongitudinalModelChecks.cs | 245 +++++++++- 14 files changed, 1642 insertions(+), 108 deletions(-) create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/FullDirectionSegmentScheduleBuilder.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalKnotSchedule.cs diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs index 6fb2696..ca5a8b0 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs @@ -83,16 +83,34 @@ public sealed class EmPlanningService : IEmPlanningService return Failure(lateral.Status, request, lateral.FailureReason); EmitDebug(request, "LS optimization and validation succeeded"); - IReadOnlyList knotTimes = LongitudinalCandidate.CreateKnotTimes( - configuration.Scheduling.TimeHorizonSeconds, configuration.Scheduling.OutputTimeStepSeconds); + EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(lateral.Path, segment.Direction, + initialProgressSpeed, horizon.TerminalType, configuration, out PathSpeedLimit speedLimit, + out string envelopeReason); + if (envelopeStatus != EmPlanningStatus.Success) + return Failure(envelopeStatus, request, envelopeReason); + LongitudinalKnotSchedule knotSchedule; + if (request.PlanningScope == EmPlanningScope.FullDirectionSegment) + { + EmPlanningStatus scheduleStatus = new FullDirectionSegmentScheduleBuilder().TryBuild(lateral.Path, speedLimit, + initialProgressSpeed, initialAcceleration, DesiredSpeed(configuration, segment.Direction), configuration, + out knotSchedule, out string scheduleReason); + if (scheduleStatus != EmPlanningStatus.Success) + return Failure(scheduleStatus, request, scheduleReason); + } + else + { + knotSchedule = LongitudinalKnotSchedule.CreateRolling(configuration.Scheduling.TimeHorizonSeconds, + configuration.Scheduling.OutputTimeStepSeconds); + } LongitudinalPreviousTrajectorySeed previousLongitudinalSeed = new LongitudinalPreviousTrajectorySeedBuilder().Build( - request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc, knotTimes, + request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc, knotSchedule, segment.SegmentIndex, segment.Direction); var longitudinalInput = new LongitudinalPlanningInput(lateral.Path, segment.Direction, initialProgressSpeed, initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, configuration, + request.PlanningScope, knotSchedule, previousLongitudinalSeed.PathS, previousLongitudinalSeed.ProgressSpeedMetersPerSecond); - EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out string envelopeReason); + envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out envelopeReason); if (envelopeStatus != EmPlanningStatus.Success) return Failure(envelopeStatus, request, envelopeReason); EmitDebug(request, "PathS speed envelope succeeded"); @@ -195,6 +213,13 @@ public sealed class EmPlanningService : IEmPlanningService : state.SignedLongitudinalSpeedMetersPerSecond < 0d; } + private static double DesiredSpeed(EmPlannerConfiguration configuration, TravelDirection direction) + { + return direction == TravelDirection.Forward + ? configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond + : configuration.Longitudinal.DesiredReverseSpeedMetersPerSecond; + } + private static bool IsSuccess(EmPlanningStatus status) { return status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback; diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/FullDirectionSegmentScheduleBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/FullDirectionSegmentScheduleBuilder.cs new file mode 100644 index 0000000..dcf187c --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/FullDirectionSegmentScheduleBuilder.cs @@ -0,0 +1,203 @@ +using System; +using System.Collections.Generic; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Derives bounded full-direction ST knots from the physical PathS speed and stopping envelope. +public sealed class FullDirectionSegmentScheduleBuilder +{ + private const double Tolerance = 1e-10d; + + public EmPlanningStatus TryBuild(LateralPath path, PathSpeedLimit speedLimit, + double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared, + double desiredSpeedMetersPerSecond, EmPlannerConfiguration configuration, + out LongitudinalKnotSchedule schedule, out string failureReason) + { + schedule = null; + failureReason = string.Empty; + if (path == null || speedLimit == null || configuration == null || configuration.Scheduling == null || + configuration.Longitudinal == null || !path.IsIndependentlyValidated || path.Points.Count < 2 || + !IsFinite(initialProgressSpeedMetersPerSecond) || initialProgressSpeedMetersPerSecond < 0d || + !IsFinite(initialAccelerationMetersPerSecondSquared) || !IsPositiveFinite(desiredSpeedMetersPerSecond)) + { + failureReason = "Full-direction schedule inputs are invalid."; + return EmPlanningStatus.InvalidInput; + } + if (!speedLimit.HasStopBoundary || Math.Abs(speedLimit.PathUpperBoundS - + path.Points[path.Points.Count - 1].PathS) > Tolerance) + { + failureReason = "A full-direction schedule requires the matching real stop-boundary speed envelope."; + return EmPlanningStatus.InvalidInput; + } + + SchedulingConfiguration scheduling = configuration.Scheduling; + LongitudinalConfiguration longitudinal = configuration.Longitudinal; + if (!IsPositiveFinite(scheduling.MaximumOptimizationTimeStepSeconds) || + !IsPositiveFinite(scheduling.MaximumOptimizationSpatialStepMeters) || + scheduling.MaximumOptimizationKnotCount < 3 || + !IsPositiveFinite(longitudinal.MaximumAccelerationMetersPerSecondSquared) || + !IsPositiveFinite(longitudinal.MaximumDecelerationMetersPerSecondSquared) || + !IsPositiveFinite(longitudinal.MaximumJerkMetersPerSecondCubed)) + { + failureReason = "Full-direction schedule limits are invalid."; + return EmPlanningStatus.InvalidInput; + } + + int stationCount = speedLimit.PathS.Count; + var speeds = new double[stationCount]; + double desired = Math.Min(desiredSpeedMetersPerSecond, speedLimit.DirectionMaximumSpeedMetersPerSecond); + speeds[0] = Math.Min(initialProgressSpeedMetersPerSecond, Math.Min(desired, + speedLimit.MaximumSpeedMetersPerSecond[0])); + for (int index = 1; index < stationCount; index++) + { + double distance = speedLimit.PathS[index] - speedLimit.PathS[index - 1]; + double reachable = Math.Sqrt(Math.Max(0d, speeds[index - 1] * speeds[index - 1] + + 2d * longitudinal.MaximumAccelerationMetersPerSecondSquared * distance)); + speeds[index] = Math.Min(reachable, Math.Min(desired, speedLimit.MaximumSpeedMetersPerSecond[index])); + } + speeds[stationCount - 1] = 0d; + for (int index = stationCount - 2; index >= 0; index--) + { + double remainingDistance = speedLimit.PathUpperBoundS - speedLimit.PathS[index]; + double stopCap = JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(remainingDistance, + Math.Max(0d, initialAccelerationMetersPerSecondSquared), longitudinal.MaximumDecelerationMetersPerSecondSquared, + longitudinal.MaximumJerkMetersPerSecondCubed, speedLimit.DirectionMaximumSpeedMetersPerSecond); + double distance = speedLimit.PathS[index + 1] - speedLimit.PathS[index]; + double decelerationCap = Math.Sqrt(Math.Max(0d, speeds[index + 1] * speeds[index + 1] + + 2d * longitudinal.MaximumDecelerationMetersPerSecondSquared * distance)); + speeds[index] = Math.Min(speeds[index], Math.Min(stopCap, decelerationCap)); + } + + var times = new List { 0d }; + var pathS = new List { 0d }; + var referenceSpeeds = new List { speeds[0] }; + IReadOnlyList scheduleStations = SelectScheduleStations(speedLimit.PathS, speeds); + int minimumIntervalsPerSegment = Math.Max(1, + (3 + scheduleStations.Count - 2) / (scheduleStations.Count - 1)); + for (int stationIndex = 1; stationIndex < scheduleStations.Count; stationIndex++) + { + int startIndex = scheduleStations[stationIndex - 1]; + int endIndex = scheduleStations[stationIndex]; + double startS = speedLimit.PathS[startIndex]; + double endS = speedLimit.PathS[endIndex]; + double startSpeed = speeds[startIndex]; + double endSpeed = speeds[endIndex]; + double distance = endS - startS; + double denominator = startSpeed + endSpeed; + double duration = denominator > Tolerance ? 2d * distance / denominator : + Math.Sqrt(2d * distance / Math.Max(Tolerance, longitudinal.MaximumAccelerationMetersPerSecondSquared)); + int subdivisionCount = Math.Max(minimumIntervalsPerSegment, Math.Max( + checked((int)Math.Ceiling(distance / scheduling.MaximumOptimizationSpatialStepMeters)), + checked((int)Math.Ceiling(duration / scheduling.MaximumOptimizationTimeStepSeconds)))); + for (int subdivision = 1; subdivision <= subdivisionCount; subdivision++) + { + double fraction = (double)subdivision / subdivisionCount; + times.Add(times[times.Count - 1] + duration / subdivisionCount); + pathS.Add(startS + distance * fraction); + referenceSpeeds.Add(startSpeed + (endSpeed - startSpeed) * fraction); + } + } + referenceSpeeds[referenceSpeeds.Count - 1] = 0d; + pathS[pathS.Count - 1] = speedLimit.PathUpperBoundS; + EnsureJerkReachableReferenceTimes(times, referenceSpeeds, longitudinal); + EnsureMinimumExactStopDuration(times, speedLimit.PathUpperBoundS, initialProgressSpeedMetersPerSecond, + initialAccelerationMetersPerSecondSquared, longitudinal); + if (!IsFinite(longitudinal.ZeroSpeedHoldSeconds) || longitudinal.ZeroSpeedHoldSeconds < 0d) + { + failureReason = "The full-direction zero-speed hold duration is invalid."; + return EmPlanningStatus.InvalidInput; + } + int terminalHoldStartIndex = times.Count - 1; + double remainingHold = longitudinal.ZeroSpeedHoldSeconds; + while (remainingHold > Tolerance) + { + double holdStep = Math.Min(remainingHold, scheduling.MaximumOptimizationTimeStepSeconds); + times.Add(times[times.Count - 1] + holdStep); + pathS.Add(speedLimit.PathUpperBoundS); + referenceSpeeds.Add(0d); + remainingHold -= holdStep; + } + if (times.Count > scheduling.MaximumOptimizationKnotCount) + { + failureReason = "Full-direction schedule required knots=" + times.Count + ", configured maximum=" + + scheduling.MaximumOptimizationKnotCount + "."; + return EmPlanningStatus.FullSegmentResourceLimitExceeded; + } + try + { + schedule = LongitudinalKnotSchedule.CreateAdaptive(times, pathS, referenceSpeeds, + terminalHoldStartIndex); + return EmPlanningStatus.Success; + } + catch (ArgumentException exception) + { + failureReason = exception.Message; + return EmPlanningStatus.InvalidInput; + } + } + + private static void EnsureMinimumExactStopDuration(IList times, double stopBoundaryPathS, + double initialSpeed, double initialAcceleration, LongitudinalConfiguration configuration) + { + if (initialSpeed <= Tolerance || !JerkLimitedStoppingMath.TryCalculate(initialSpeed, initialAcceleration, + configuration.MaximumDecelerationMetersPerSecondSquared, + configuration.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile stop, out _)) + { + return; + } + double cruiseDistance = Math.Max(0d, stopBoundaryPathS - stop.DistanceMeters); + double requiredDuration = stop.DurationSeconds + cruiseDistance / initialSpeed; + double stopSpeedTolerance = configuration.StopSpeedToleranceMetersPerSecond; + if (IsPositiveFinite(stopSpeedTolerance) && JerkLimitedStoppingMath.TryCalculate(stopSpeedTolerance, 0d, + configuration.MaximumDecelerationMetersPerSecondSquared, + configuration.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile settlingStop, out _)) + { + double envelopeTraverseDuration = 2d * stopBoundaryPathS / (initialSpeed + stopSpeedTolerance); + requiredDuration = Math.Max(requiredDuration, envelopeTraverseDuration + settlingStop.DurationSeconds); + } + double currentDuration = times[times.Count - 1]; + if (currentDuration + Tolerance >= requiredDuration) + return; + double scale = requiredDuration / currentDuration; + for (int index = 1; index < times.Count; index++) + times[index] *= scale; + } + + private static void EnsureJerkReachableReferenceTimes(IList times, IReadOnlyList referenceSpeeds, + LongitudinalConfiguration configuration) + { + double adjustedTime = 0d; + for (int index = 1; index < times.Count; index++) + { + double requestedDuration = times[index] - times[index - 1]; + double speedChange = Math.Abs(referenceSpeeds[index] - referenceSpeeds[index - 1]); + double accelerationLimit = referenceSpeeds[index] >= referenceSpeeds[index - 1] + ? configuration.MaximumAccelerationMetersPerSecondSquared + : configuration.MaximumDecelerationMetersPerSecondSquared; + double accelerationDuration = speedChange / accelerationLimit; + double triangularJerkDuration = speedChange <= Tolerance + ? 0d + : 2d * Math.Sqrt(speedChange / configuration.MaximumJerkMetersPerSecondCubed); + adjustedTime += Math.Max(requestedDuration, Math.Max(accelerationDuration, triangularJerkDuration)); + times[index] = adjustedTime; + } + } + + private static IReadOnlyList SelectScheduleStations(IReadOnlyList pathS, + IReadOnlyList speeds) + { + var stations = new List { 0 }; + for (int index = 1; index < pathS.Count - 1; index++) + { + double previousSlope = (speeds[index] - speeds[index - 1]) / (pathS[index] - pathS[index - 1]); + double nextSlope = (speeds[index + 1] - speeds[index]) / (pathS[index + 1] - pathS[index]); + if (previousSlope * nextSlope < 0d) + stations.Add(index); + } + stations.Add(pathS.Count - 1); + return stations; + } + + private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value); + private static bool IsPositiveFinite(double value) => IsFinite(value) && value > 0d; +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs index 49c5a5c..f8f1fb7 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs @@ -15,6 +15,35 @@ public sealed class LongitudinalConstraintBuilder public bool TryBuild(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate iterate, out QuadraticProgram problem, out string failureReason) + { + return TryBuildCore(input, speedLimit, iterate, false, out problem, out failureReason); + } + + /// Builds the bounded full-scope feasibility projection before objective optimization. + public bool TryBuildInitialFeasibilityProjection(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, + out QuadraticProgram problem, out string failureReason) + { + return TryBuildInitialFeasibilityProjection(input, speedLimit, + input == null ? null : CreateScheduleReferenceIterate(input), out problem, out failureReason); + } + + public bool TryBuildInitialFeasibilityProjection(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, + LongitudinalCandidate linearizationIterate, out QuadraticProgram problem, out string failureReason) + { + problem = null; + failureReason = string.Empty; + if (input == null || input.PlanningScope != EmPlanningScope.FullDirectionSegment || + input.Mode != EmLongitudinalMode.ExactStopAtBoundary || linearizationIterate == null) + { + failureReason = "Initial feasibility projection is only defined for full-direction exact-stop planning."; + return false; + } + return TryBuildCore(input, speedLimit, linearizationIterate, true, out problem, + out failureReason); + } + + private bool TryBuildCore(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate iterate, + bool useScheduleReferenceObjective, out QuadraticProgram problem, out string failureReason) { problem = null; failureReason = string.Empty; @@ -25,10 +54,9 @@ public sealed class LongitudinalConstraintBuilder if (Math.Abs(speedLimit.PathUpperBoundS - input.PathUpperBoundS) > 1e-12d) throw new ArgumentException("The speed envelope upper bound must match actual lateral PathS."); - IReadOnlyList expectedTimes = LongitudinalCandidate.CreateKnotTimes( - input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds); + IReadOnlyList expectedTimes = input.KnotSchedule.KnotTimes; if (!HasMatchingTimes(iterate.KnotTimes, expectedTimes)) - throw new ArgumentException("The ST iterate time knots do not match the configured horizon."); + throw new ArgumentException("The ST iterate time knots do not match the supplied knot schedule."); var layout = new LongitudinalVariableLayout(expectedTimes.Count); if (iterate.S.Count != layout.KnotCount || iterate.U.Count != layout.KnotCount || iterate.A.Count != layout.KnotCount || iterate.J.Count != layout.KnotCount - 1) @@ -50,13 +78,13 @@ public sealed class LongitudinalConstraintBuilder var hessian = new SparseTripletBuilder(layout.VariableCount, layout.VariableCount, true); var linearCost = new double[layout.VariableCount]; - _objectiveBuilder.AddTerms(input, speedLimit, layout, iterate, hessian, linearCost); - int stabilizationStart = input.Mode == EmLongitudinalMode.ExactStopAtBoundary - ? LongitudinalTerminalSchedule.GetStabilizationStartIndex(expectedTimes, - input.Configuration.Scheduling.OutputTimeStepSeconds) - : layout.KnotCount; + if (useScheduleReferenceObjective) + AddInitialFeasibilityObjective(input, layout, hessian, linearCost); + else + _objectiveBuilder.AddTerms(input, speedLimit, layout, iterate, hessian, linearCost); + int stabilizationStart = GetStabilizationStart(input, expectedTimes, layout.KnotCount); int stationaryKnotCount = layout.KnotCount - stabilizationStart; - int expectedRows = 8 * layout.KnotCount - 2 + 3 * stationaryKnotCount; + int expectedRows = 9 * layout.KnotCount - 3 + 3 * stationaryKnotCount; var constraints = new SparseTripletBuilder(expectedRows, layout.VariableCount); var lower = new List(expectedRows); var upper = new List(expectedRows); @@ -80,6 +108,42 @@ public sealed class LongitudinalConstraintBuilder } } + private static LongitudinalCandidate CreateScheduleReferenceIterate(LongitudinalPlanningInput input) + { + int knotCount = input.KnotSchedule.KnotTimes.Count; + return new LongitudinalCandidate(input.KnotSchedule.KnotTimes, input.KnotSchedule.ReferencePathS, + input.KnotSchedule.ReferenceSpeedMetersPerSecond, new double[knotCount], new double[knotCount - 1]); + } + + private static void AddInitialFeasibilityObjective(LongitudinalPlanningInput input, LongitudinalVariableLayout layout, + SparseTripletBuilder hessian, IList linearCost) + { + double progressScale = 1d; + double speedScale = 1d; + double accelerationScale = 1d; + double jerkScale = 1d; + for (int index = 0; index < layout.KnotCount; index++) + { + AddProjectionSquaredResidual(hessian, linearCost, layout.S(index), input.KnotSchedule.ReferencePathS[index], + 1d, progressScale); + AddProjectionSquaredResidual(hessian, linearCost, layout.U(index), + input.KnotSchedule.ReferenceSpeedMetersPerSecond[index], 10d, speedScale); + AddProjectionSquaredResidual(hessian, linearCost, layout.A(index), 0d, 1e-3d, accelerationScale); + } + for (int index = 0; index < layout.KnotCount - 1; index++) + AddProjectionSquaredResidual(hessian, linearCost, layout.J(index), 0d, 1e-3d, jerkScale); + } + + private static void AddProjectionSquaredResidual(SparseTripletBuilder hessian, IList linearCost, + int variable, double reference, double weight, double scale) + { + if (!IsFinite(reference) || !IsFinite(weight) || weight <= 0d || !IsFinite(scale) || scale <= 0d) + throw new ArgumentOutOfRangeException(nameof(reference)); + double coefficient = 2d * weight / (scale * scale); + hessian.Add(variable, variable, coefficient); + linearCost[variable] += -coefficient * reference; + } + private static void AddVariableBounds(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate iterate, LongitudinalVariableLayout layout, double maximumAcceleration, double maximumDeceleration, double maximumJerk, SparseTripletBuilder constraints, IList lower, @@ -92,8 +156,11 @@ public sealed class LongitudinalConstraintBuilder AddSingleVariableRow(constraints, lower, upper, layout.S(index), 0d, input.PathUpperBoundS, ref row); double maximumSpeed = index == 0 ? input.DirectionMaximumSpeedMetersPerSecond - : Math.Min(input.DirectionMaximumSpeedMetersPerSecond, speedLimit.MaximumSpeedAt(iterate.S[index])); + : input.DirectionMaximumSpeedMetersPerSecond; AddSingleVariableRow(constraints, lower, upper, layout.U(index), 0d, maximumSpeed, ref row); + if (index > 0) + AddLinearizedSpeedEnvelopeRow(speedLimit, iterate.S[index], layout.S(index), layout.U(index), + constraints, lower, upper, ref row); AddSingleVariableRow(constraints, lower, upper, layout.A(index), -maximumDeceleration, maximumAcceleration, ref row); } @@ -101,6 +168,34 @@ public sealed class LongitudinalConstraintBuilder AddSingleVariableRow(constraints, lower, upper, layout.J(index), -maximumJerk, maximumJerk, ref row); } + private static void AddLinearizedSpeedEnvelopeRow(PathSpeedLimit speedLimit, double pathS, int pathSVariable, + int speedVariable, SparseTripletBuilder constraints, IList lower, IList upper, ref int row) + { + int segment = FindSpeedEnvelopeSegment(speedLimit, pathS); + double startS = speedLimit.PathS[segment]; + double endS = speedLimit.PathS[segment + 1]; + double startSpeed = speedLimit.MaximumSpeedMetersPerSecond[segment]; + double endSpeed = speedLimit.MaximumSpeedMetersPerSecond[segment + 1]; + double slope = (endSpeed - startSpeed) / (endS - startS); + double intercept = startSpeed - slope * startS; + AddRow(constraints, lower, upper, row, new[] + { + new Coefficient(speedVariable, 1d), new Coefficient(pathSVariable, -slope), + }, -QuadraticProgram.MaximumFiniteBound, intercept); + row++; + } + + private static int FindSpeedEnvelopeSegment(PathSpeedLimit speedLimit, double pathS) + { + double clamped = Math.Max(speedLimit.PathS[0], Math.Min(speedLimit.PathUpperBoundS, pathS)); + for (int index = 0; index < speedLimit.PathS.Count - 1; index++) + { + if (clamped <= speedLimit.PathS[index + 1]) + return index; + } + return speedLimit.PathS.Count - 2; + } + private static void AddMonotonicProgress(LongitudinalVariableLayout layout, SparseTripletBuilder constraints, IList lower, IList upper, ref int row) { @@ -164,6 +259,24 @@ public sealed class LongitudinalConstraintBuilder } } + private static int GetStabilizationStart(LongitudinalPlanningInput input, IReadOnlyList times, + int knotCount) + { + if (input.Mode != EmLongitudinalMode.ExactStopAtBoundary) + return knotCount; + if (input.PlanningScope == EmPlanningScope.FullDirectionSegment) + { + if (input.KnotSchedule.TerminalHoldStartIndex < 1 || + input.KnotSchedule.TerminalHoldStartIndex >= knotCount) + { + throw new ArgumentException("Full-direction exact-stop schedules require an explicit terminal hold boundary."); + } + return input.KnotSchedule.TerminalHoldStartIndex; + } + return LongitudinalTerminalSchedule.GetStabilizationStartIndex(times, + input.Configuration.Scheduling.OutputTimeStepSeconds); + } + private static void AddSingleVariableRow(SparseTripletBuilder constraints, IList lower, IList upper, int variable, double minimum, double maximum, ref int row) { @@ -192,6 +305,11 @@ public sealed class LongitudinalConstraintBuilder return true; } + private static bool IsFinite(double value) + { + return !double.IsNaN(value) && !double.IsInfinity(value); + } + private readonly struct Coefficient { public Coefficient(int variable, double value) diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalKnotSchedule.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalKnotSchedule.cs new file mode 100644 index 0000000..77de1b3 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalKnotSchedule.cs @@ -0,0 +1,118 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Immutable ST optimization knots, separate from the trajectory publication cadence. +public sealed class LongitudinalKnotSchedule +{ + public LongitudinalKnotSchedule(IReadOnlyList knotTimes, IReadOnlyList referencePathS, + IReadOnlyList referenceSpeedMetersPerSecond, bool isAdaptive) + : this(knotTimes, referencePathS, referenceSpeedMetersPerSecond, isAdaptive, -1) + { + } + + public LongitudinalKnotSchedule(IReadOnlyList knotTimes, IReadOnlyList referencePathS, + IReadOnlyList referenceSpeedMetersPerSecond, bool isAdaptive, + int terminalHoldStartIndex) + { + KnotTimes = CopyTimes(knotTimes); + ReferencePathS = CopyNondecreasing(referencePathS, KnotTimes.Count, nameof(referencePathS)); + ReferenceSpeedMetersPerSecond = CopyNonnegative(referenceSpeedMetersPerSecond, KnotTimes.Count, + nameof(referenceSpeedMetersPerSecond)); + if (isAdaptive && ReferenceSpeedMetersPerSecond[ReferenceSpeedMetersPerSecond.Count - 1] != 0d) + throw new ArgumentException("An adaptive full-segment schedule must end at exact zero speed.", + nameof(referenceSpeedMetersPerSecond)); + + IsAdaptive = isAdaptive; + TotalDurationSeconds = KnotTimes[KnotTimes.Count - 1]; + if (terminalHoldStartIndex != -1 && + (!isAdaptive || terminalHoldStartIndex < 1 || terminalHoldStartIndex >= KnotTimes.Count)) + { + throw new ArgumentOutOfRangeException(nameof(terminalHoldStartIndex)); + } + TerminalHoldStartIndex = terminalHoldStartIndex; + } + + public IReadOnlyList KnotTimes { get; } + public IReadOnlyList ReferencePathS { get; } + public IReadOnlyList ReferenceSpeedMetersPerSecond { get; } + public double TotalDurationSeconds { get; } + public bool IsAdaptive { get; } + public int TerminalHoldStartIndex { get; } + + internal static LongitudinalKnotSchedule CreateAdaptive(IReadOnlyList knotTimes, + IReadOnlyList referencePathS, IReadOnlyList referenceSpeedMetersPerSecond, + int terminalHoldStartIndex) + { + return new LongitudinalKnotSchedule(knotTimes, referencePathS, referenceSpeedMetersPerSecond, true, + terminalHoldStartIndex); + } + + internal LongitudinalKnotSchedule Copy() + { + return new LongitudinalKnotSchedule(KnotTimes, ReferencePathS, ReferenceSpeedMetersPerSecond, IsAdaptive, + TerminalHoldStartIndex); + } + + public static LongitudinalKnotSchedule CreateRolling(double timeHorizonSeconds, double timeStepSeconds) + { + IReadOnlyList times = LongitudinalCandidate.CreateKnotTimes(timeHorizonSeconds, timeStepSeconds); + var pathS = new double[times.Count]; + var speeds = new double[times.Count]; + return new LongitudinalKnotSchedule(times, pathS, speeds, false); + } + + private static IReadOnlyList CopyTimes(IReadOnlyList source) + { + if (source == null || source.Count < 2) + throw new ArgumentException("At least two time knots are required.", nameof(source)); + 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 || (index == 0 && source[index] != 0d)) + throw new ArgumentException("Time knots must be finite, begin at exact zero, and strictly increase.", + nameof(source)); + copy.Add(source[index]); + previous = source[index]; + } + return new ReadOnlyCollection(copy); + } + + private static IReadOnlyList CopyNondecreasing(IReadOnlyList source, int expectedCount, + string parameterName) + { + if (source == null || source.Count != expectedCount || source[0] != 0d) + throw new ArgumentException("Reference PathS must begin at exact zero and match the knot count.", 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("Reference PathS must be finite and nondecreasing.", parameterName); + copy.Add(source[index]); + previous = source[index]; + } + return new ReadOnlyCollection(copy); + } + + private static IReadOnlyList CopyNonnegative(IReadOnlyList source, int expectedCount, + string parameterName) + { + if (source == null || source.Count != expectedCount) + throw new ArgumentException("Reference speeds must match the knot count.", 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) => !double.IsNaN(value) && !double.IsInfinity(value); + +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningInput.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningInput.cs index e00179b..877a2d0 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningInput.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningInput.cs @@ -14,6 +14,16 @@ public sealed class LongitudinalPlanningInput double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmLongitudinalMode mode, EmPlannerConfiguration configuration, IReadOnlyList previousPathS, IReadOnlyList previousProgressSpeedMetersPerSecond) + : this(path, direction, initialProgressSpeedMetersPerSecond, initialAccelerationMetersPerSecondSquared, + terminalType, mode, configuration, EmPlanningScope.RollingHorizon, + CreateRollingSchedule(configuration), previousPathS, previousProgressSpeedMetersPerSecond) + { + } + + public LongitudinalPlanningInput(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond, + double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmLongitudinalMode mode, + EmPlannerConfiguration configuration, EmPlanningScope planningScope, LongitudinalKnotSchedule knotSchedule, + 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.", @@ -34,6 +44,13 @@ public sealed class LongitudinalPlanningInput throw new ArgumentException("Stop-boundary modes require Goal or GearSwitch."); if (configuration == null) throw new ArgumentNullException(nameof(configuration)); + if (!Enum.IsDefined(typeof(EmPlanningScope), planningScope)) + throw new ArgumentOutOfRangeException(nameof(planningScope)); + if (knotSchedule == null) + throw new ArgumentNullException(nameof(knotSchedule)); + if ((planningScope == EmPlanningScope.FullDirectionSegment) != knotSchedule.IsAdaptive) + throw new ArgumentException("Full-direction planning requires an adaptive schedule and rolling planning requires a rolling schedule.", + nameof(knotSchedule)); Path = CopyAndValidatePath(path); Direction = direction; @@ -42,6 +59,8 @@ public sealed class LongitudinalPlanningInput TerminalType = terminalType; Mode = mode; Configuration = configuration.Copy(); + PlanningScope = planningScope; + KnotSchedule = knotSchedule.Copy(); PreviousPathS = CopyFiniteNonnegative(previousPathS, nameof(previousPathS)); PreviousProgressSpeedMetersPerSecond = CopyFiniteNonnegative(previousProgressSpeedMetersPerSecond, nameof(previousProgressSpeedMetersPerSecond)); @@ -75,6 +94,10 @@ public sealed class LongitudinalPlanningInput public EmPlannerConfiguration Configuration { get; } + public EmPlanningScope PlanningScope { get; } + + public LongitudinalKnotSchedule KnotSchedule { get; } + public IReadOnlyList PreviousPathS { get; } public IReadOnlyList PreviousProgressSpeedMetersPerSecond { get; } @@ -143,6 +166,14 @@ public sealed class LongitudinalPlanningInput return new ReadOnlyCollection(copy); } + private static LongitudinalKnotSchedule CreateRollingSchedule(EmPlannerConfiguration configuration) + { + if (configuration == null || configuration.Scheduling == null) + throw new ArgumentNullException(nameof(configuration)); + return LongitudinalKnotSchedule.CreateRolling(configuration.Scheduling.TimeHorizonSeconds, + configuration.Scheduling.OutputTimeStepSeconds); + } + private static bool IsFinite(double value) { return !double.IsNaN(value) && !double.IsInfinity(value); diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPreviousTrajectorySeedBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPreviousTrajectorySeedBuilder.cs index dfbdebc..4aadeba 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPreviousTrajectorySeedBuilder.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPreviousTrajectorySeedBuilder.cs @@ -55,6 +55,15 @@ public sealed class LongitudinalPreviousTrajectorySeedBuilder { private const double ProjectionTolerance = 1e-10d; + public LongitudinalPreviousTrajectorySeed Build(EmTrajectory previous, LateralPath currentPath, + DateTimeOffset newEffectiveAtUtc, LongitudinalKnotSchedule knotSchedule, int segmentIndex, + TravelDirection direction) + { + if (knotSchedule == null) + return LongitudinalPreviousTrajectorySeed.Empty; + return Build(previous, currentPath, newEffectiveAtUtc, knotSchedule.KnotTimes, segmentIndex, direction); + } + public LongitudinalPreviousTrajectorySeed Build(EmTrajectory previous, LateralPath currentPath, DateTimeOffset newEffectiveAtUtc, IReadOnlyList newKnotTimes, int segmentIndex, TravelDirection direction) diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs index 3ff71c1..64ab5d7 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs @@ -23,12 +23,11 @@ public sealed class LongitudinalSolutionValidator { return false; } - IReadOnlyList expectedTimes = LongitudinalCandidate.CreateKnotTimes( - input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds); + IReadOnlyList expectedTimes = input.KnotSchedule.KnotTimes; double tolerance = RequireNonnegative(input.Configuration.Validation.KinematicTolerance, nameof(tolerance)); if (!HasMatchingTimes(candidate.KnotTimes, expectedTimes, tolerance)) { - failureReason = "ST candidate knot times do not match the configured horizon."; + failureReason = "ST candidate knot times do not match the supplied knot schedule."; return false; } if (candidate.S.Count != expectedTimes.Count || candidate.U.Count != expectedTimes.Count || @@ -88,8 +87,7 @@ public sealed class LongitudinalSolutionValidator int stabilizationStart = candidate.S.Count; if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary) { - stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex( - candidate.KnotTimes, input.Configuration.Scheduling.OutputTimeStepSeconds); + stabilizationStart = GetStabilizationStart(input, candidate.KnotTimes); for (int index = stabilizationStart; index < candidate.S.Count; index++) { if (!AreClose(candidate.S[index], input.StopBoundaryPathS, tolerance) || @@ -166,6 +164,21 @@ public sealed class LongitudinalSolutionValidator return true; } + private static int GetStabilizationStart(LongitudinalPlanningInput input, IReadOnlyList times) + { + if (input.PlanningScope == EmPlanningScope.FullDirectionSegment) + { + if (input.KnotSchedule.TerminalHoldStartIndex < 1 || + input.KnotSchedule.TerminalHoldStartIndex >= times.Count) + { + throw new ArgumentException("Full-direction exact-stop schedules require an explicit terminal hold boundary."); + } + return input.KnotSchedule.TerminalHoldStartIndex; + } + return LongitudinalTerminalSchedule.GetStabilizationStartIndex(times, + input.Configuration.Scheduling.OutputTimeStepSeconds); + } + private static bool AreClose(double actual, double expected, double tolerance) { return Math.Abs(actual - expected) <= tolerance; diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs index 3b8b9f3..b8e5ed9 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs @@ -1,5 +1,6 @@ using System; using System.Collections.Generic; +using MultiWheelC.TrajectoryPlanning.CoarsePath; namespace MultiWheelC.TrajectoryPlanning.EMPlanner; @@ -11,67 +12,112 @@ public sealed class PathSpeedLimitBuilder private const double StationMergeToleranceMeters = 1e-12d; public EmPlanningStatus Build(LongitudinalPlanningInput input, out PathSpeedLimit speedLimit, out string failureReason) + { + if (input == null) + { + speedLimit = null; + failureReason = "Longitudinal planning input is required."; + return EmPlanningStatus.InvalidInput; + } + return BuildCore(input.Path, input.Direction, input.InitialProgressSpeedMetersPerSecond, + input.InitialAccelerationMetersPerSecondSquared, input.TerminalType, input.Configuration, + out speedLimit, out failureReason); + } + + public EmPlanningStatus Build(LateralPath path, TravelDirection direction, + double initialProgressSpeedMetersPerSecond, EmTerminalType terminalType, + EmPlannerConfiguration configuration, out PathSpeedLimit speedLimit, out string failureReason) + { + return BuildCore(path, direction, initialProgressSpeedMetersPerSecond, 0d, terminalType, configuration, + out speedLimit, out failureReason); + } + + private EmPlanningStatus BuildCore(LateralPath path, TravelDirection direction, + double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared, + EmTerminalType terminalType, EmPlannerConfiguration configuration, out PathSpeedLimit speedLimit, + out string failureReason) { speedLimit = null; failureReason = string.Empty; - if (input == null) + if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2 || + !Enum.IsDefined(typeof(TravelDirection), direction) || !Enum.IsDefined(typeof(EmTerminalType), terminalType) || + configuration == null || !IsFinite(initialProgressSpeedMetersPerSecond) || + initialProgressSpeedMetersPerSecond < 0d || !IsFinite(initialAccelerationMetersPerSecondSquared)) { 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)) + if (configuration.Longitudinal == null) { + failureReason = "Longitudinal configuration is required."; return EmPlanningStatus.InvalidInput; } - if (input.InitialProgressSpeedMetersPerSecond > directionMaximum + StopDistanceToleranceMeters || - input.InitialAccelerationMetersPerSecondSquared < -maximumDeceleration - StopDistanceToleranceMeters || - input.InitialAccelerationMetersPerSecondSquared > maximumAcceleration + StopDistanceToleranceMeters) + LongitudinalConfiguration longitudinal = configuration.Longitudinal; + double directionMaximum = direction == TravelDirection.Forward + ? longitudinal.MaximumForwardSpeedMetersPerSecond + : longitudinal.MaximumReverseSpeedMetersPerSecond; + double maximumAcceleration = longitudinal.MaximumAccelerationMetersPerSecondSquared; + double maximumDeceleration = longitudinal.MaximumDecelerationMetersPerSecondSquared; + double maximumJerk = longitudinal.MaximumJerkMetersPerSecondCubed; + double maximumLateralAcceleration = longitudinal.MaximumLateralAccelerationMetersPerSecondSquared; + double maximumCurvatureRate = longitudinal.MaximumCurvatureRatePerMeterPerSecond; + if (!IsPositiveFinite(directionMaximum) || !IsPositiveFinite(maximumAcceleration) || + !IsPositiveFinite(maximumDeceleration) || !IsPositiveFinite(maximumJerk) || + !IsPositiveFinite(maximumLateralAcceleration) || !IsPositiveFinite(maximumCurvatureRate)) + { + failureReason = "Longitudinal limits must be positive and finite."; + return EmPlanningStatus.InvalidInput; + } + if (initialProgressSpeedMetersPerSecond > directionMaximum + StopDistanceToleranceMeters || + initialAccelerationMetersPerSecondSquared < -maximumDeceleration - StopDistanceToleranceMeters || + initialAccelerationMetersPerSecondSquared > maximumAcceleration + StopDistanceToleranceMeters) { failureReason = "The initial longitudinal state violates the configured hard bounds."; return EmPlanningStatus.InvalidInput; } - if (input.HasStopBoundary) + bool hasStopBoundary = terminalType != EmTerminalType.RollingSafetyStop; + double stopBoundaryPathS = path.Points[path.Points.Count - 1].PathS; + if (hasStopBoundary) { - if (!JerkLimitedStoppingMath.TryCalculate(input.InitialProgressSpeedMetersPerSecond, - input.InitialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk, + if (!JerkLimitedStoppingMath.TryCalculate(initialProgressSpeedMetersPerSecond, + initialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stopProfile, out failureReason)) { return EmPlanningStatus.InvalidInput; } - if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > input.StopBoundaryPathS) + if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > 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)) + if (configuration.Scheduling == null || + !IsPositiveFinite(configuration.Scheduling.MaximumOptimizationSpatialStepMeters)) { - failureReason = "The output time step required to refine the PathS speed envelope is invalid."; + failureReason = "The optimization spatial step required to refine the PathS speed envelope is invalid."; return EmPlanningStatus.InvalidInput; } - double maximumStationSpacing = directionMaximum * input.Configuration.Scheduling.OutputTimeStepSeconds; + double maximumStationSpacing = configuration.Scheduling.MaximumOptimizationSpatialStepMeters; var pathS = new List(); var maximum = new List(); var lateral = new List(); var curvatureRate = new List(); var stopping = new List(); - for (int segmentIndex = 0; segmentIndex < input.Path.Points.Count - 1; segmentIndex++) + for (int segmentIndex = 0; segmentIndex < path.Points.Count - 1; segmentIndex++) { - LateralPathPoint lowerPoint = input.Path.Points[segmentIndex]; - LateralPathPoint upperPoint = input.Path.Points[segmentIndex + 1]; + LateralPathPoint lowerPoint = path.Points[segmentIndex]; + LateralPathPoint upperPoint = 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(subdivisions + 16); for (int subdivision = segmentIndex == 0 ? 0 : 1; subdivision <= subdivisions; subdivision++) segmentStations.Add(Interpolate(lowerPoint.PathS, upperPoint.PathS, (double)subdivision / subdivisions)); - if (input.HasStopBoundary) + if (hasStopBoundary) { - AddJerkLimitedStoppingStations(lowerPoint.PathS, upperPoint.PathS, input.StopBoundaryPathS, + AddJerkLimitedStoppingStations(lowerPoint.PathS, upperPoint.PathS, stopBoundaryPathS, directionMaximum, maximumAcceleration, maximumDeceleration, maximumJerk, segmentIndex == 0, segmentStations); } @@ -87,8 +133,8 @@ 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.HasStopBoundary, - input.StopBoundaryPathS, directionMaximum, maximumAcceleration, maximumDeceleration, + AddLimitSample(samplePathS, curvature, curvatureDerivative, hasStopBoundary, + stopBoundaryPathS, directionMaximum, maximumAcceleration, maximumDeceleration, maximumJerk, maximumLateralAcceleration, maximumCurvatureRate, pathS, maximum, lateral, curvatureRate, stopping); } @@ -97,7 +143,7 @@ public sealed class PathSpeedLimitBuilder try { speedLimit = new PathSpeedLimit(pathS, maximum, lateral, curvatureRate, stopping, directionMaximum, - input.HasStopBoundary); + hasStopBoundary); return EmPlanningStatus.Success; } catch (ArgumentException exception) diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs index 05dfb7c..fb4361d 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs @@ -47,18 +47,40 @@ public sealed class SequentialLongitudinalOptimizer if (speedStatus != EmPlanningStatus.Success) return Failed(speedStatus, speedFailure); - LongitudinalCandidate iterate = CreateInitialIterate(input, speedLimit); + var stopwatch = Stopwatch.StartNew(); + LongitudinalCandidate iterate; + LongitudinalCandidate lastStrictCandidate = null; + int remainingObjectiveIterations = iterationLimit; + if (input.PlanningScope == EmPlanningScope.FullDirectionSegment && + input.Mode == EmLongitudinalMode.ExactStopAtBoundary) + { + if (!TryCreateInitialFeasibleCandidate(input, speedLimit, settings, totalBudget, convergenceTolerance, + iterationLimit, stopwatch, cancellationToken, out iterate, out int projectionSolveCount, + out EmPlanningStatus projectionStatus, out string projectionFailure)) + { + return Failed(projectionStatus, projectionFailure); + } + lastStrictCandidate = CopyCandidate(iterate); + remainingObjectiveIterations -= projectionSolveCount; + if (remainingObjectiveIterations <= 0) + { + return new LongitudinalPlanningResult(EmPlanningStatus.SuccessWithFallback, lastStrictCandidate, + "The strict initial feasibility projection consumed the configured outer-iteration budget."); + } + } + else + { + iterate = CreateInitialIterate(input, speedLimit); + if (!_solutionValidator.TryValidate(input, speedLimit, iterate, out lastStrictCandidate, out _)) + lastStrictCandidate = null; + } double[] warmStart = ToPrimal(iterate); bool hasDynamicsConsistentInitialWarmStart = iterate.SatisfiesExactDiscreteDynamics(1e-12d); - LongitudinalCandidate lastStrictCandidate; - if (!_solutionValidator.TryValidate(input, speedLimit, iterate, out lastStrictCandidate, out _)) - lastStrictCandidate = null; string lastCandidateRejection = string.Empty; bool hasPreviousObjective = false; double previousObjective = 0d; - var stopwatch = Stopwatch.StartNew(); - for (int iteration = 0; iteration < iterationLimit; iteration++) + for (int iteration = 0; iteration < remainingObjectiveIterations; iteration++) { if (cancellationToken.IsCancellationRequested) return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled."); @@ -191,10 +213,132 @@ public sealed class SequentialLongitudinalOptimizer } } + private bool TryCreateInitialFeasibleCandidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, + QpSolverSettings settings, TimeSpan totalBudget, double convergenceTolerance, int iterationLimit, + Stopwatch stopwatch, CancellationToken cancellationToken, out LongitudinalCandidate candidate, + out int projectionSolveCount, out EmPlanningStatus failureStatus, out string failureReason) + { + candidate = null; + projectionSolveCount = 0; + failureStatus = EmPlanningStatus.LongitudinalInfeasible; + failureReason = string.Empty; + LongitudinalCandidate linearizationIterate = CreateScheduleReferenceIterate(input); + string lastRejection = string.Empty; + for (int iteration = 0; iteration < iterationLimit; iteration++) + { + if (cancellationToken.IsCancellationRequested) + { + failureStatus = EmPlanningStatus.Cancelled; + failureReason = "Initial full-direction feasibility projection was cancelled."; + return false; + } + TimeSpan remainingBudget = totalBudget - stopwatch.Elapsed; + if (remainingBudget <= TimeSpan.Zero) + { + failureStatus = EmPlanningStatus.SolverTimedOut; + failureReason = "Initial full-direction feasibility projection exhausted the shared solve budget."; + return false; + } + if (!_constraintBuilder.TryBuildInitialFeasibilityProjection(input, speedLimit, linearizationIterate, + out QuadraticProgram problem, out string buildFailure)) + { + failureStatus = EmPlanningStatus.LongitudinalInfeasible; + failureReason = "Initial full-direction feasibility constraints are infeasible: " + buildFailure; + return false; + } + + double projectionTolerance = Math.Min(settings.AbsoluteTolerance, + input.Configuration.Validation.KinematicTolerance * 0.1d); + QpSolveResult solved = _qpSolver.Solve(problem, + new QpSolverSettings(settings.MaximumIterations, projectionTolerance, projectionTolerance, + remainingBudget, settings.EnableWarmStart && linearizationIterate.SatisfiesExactDiscreteDynamics(1e-12d), + settings.EnablePolishing, settings.EnableNativeVerboseOutput), + ToPrimal(linearizationIterate), cancellationToken); + projectionSolveCount++; + if (solved == null) + { + failureStatus = EmPlanningStatus.Failed; + failureReason = "The initial full-direction feasibility solver returned no result."; + return false; + } + if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations) + { + failureStatus = EmPlanningStatus.SolverTimedOut; + failureReason = "Initial full-direction feasibility projection timed out (status=" + solved.NativeStatus + + ", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual + ", dual=" + + solved.DualResidual + "): " + solved.Diagnostic; + return false; + } + if (solved.Status == QpSolveStatus.Cancelled) + { + failureStatus = EmPlanningStatus.Cancelled; + failureReason = "Initial full-direction feasibility projection was cancelled: " + solved.Diagnostic; + return false; + } + if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible) + { + failureStatus = EmPlanningStatus.LongitudinalInfeasible; + failureReason = "Initial full-direction feasibility projection is infeasible: " + solved.Diagnostic; + return false; + } + if (solved.Status == QpSolveStatus.SolverUnavailable) + { + failureStatus = EmPlanningStatus.SolverUnavailable; + failureReason = "Initial full-direction feasibility solver is unavailable: " + solved.Diagnostic; + return false; + } + if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate) + { + failureStatus = EmPlanningStatus.Failed; + failureReason = "Initial full-direction feasibility solver failed: " + solved.Diagnostic; + return false; + } + if (!TryCreateCandidate(input.KnotSchedule.KnotTimes, solved.Primal, out LongitudinalCandidate projected)) + { + failureStatus = EmPlanningStatus.LongitudinalInfeasible; + failureReason = "Initial full-direction feasibility solver primal does not match the ST layout."; + return false; + } + if (solved.Status == QpSolveStatus.Solved || HasStrictResiduals(solved, convergenceTolerance)) + { + if (_solutionValidator.TryValidate(input, speedLimit, projected, out LongitudinalCandidate strict, + out string validationFailure)) + { + candidate = strict; + return true; + } + lastRejection = validationFailure; + } + + if (!TryCreateFeasibilityEnvelopeIterate(input, projected, + out LongitudinalCandidate nextLinearization)) + { + failureStatus = EmPlanningStatus.LongitudinalInfeasible; + failureReason = "Initial full-direction feasibility candidate could not be relinearized against the PathS envelope."; + return false; + } + linearizationIterate = nextLinearization; + if (solved.Status == QpSolveStatus.SolvedInaccurate) + lastRejection = "Initial feasibility projection residuals exceed the strict acceptance tolerance."; + else if (string.IsNullOrEmpty(lastRejection)) + lastRejection = "Initial feasibility projection violated the strict physical validator."; + } + failureStatus = EmPlanningStatus.LongitudinalInfeasible; + failureReason = "Initial full-direction feasibility projection exhausted the configured outer iterations. " + + lastRejection; + return false; + } + + private static LongitudinalCandidate CreateScheduleReferenceIterate(LongitudinalPlanningInput input) + { + int knotCount = input.KnotSchedule.KnotTimes.Count; + return new LongitudinalCandidate(input.KnotSchedule.KnotTimes, input.KnotSchedule.ReferencePathS, + input.KnotSchedule.ReferenceSpeedMetersPerSecond, new double[knotCount], new double[knotCount - 1]); + } + private LongitudinalCandidate CreateInitialIterate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit) { - IReadOnlyList times = LongitudinalCandidate.CreateKnotTimes(input.Configuration.Scheduling.TimeHorizonSeconds, - input.Configuration.Scheduling.OutputTimeStepSeconds); + IReadOnlyList times = input.KnotSchedule.KnotTimes; switch (input.Mode) { case EmLongitudinalMode.RollingContinuation: @@ -269,6 +413,10 @@ public sealed class SequentialLongitudinalOptimizer private LongitudinalCandidate CreateExactStopSeed(LongitudinalPlanningInput input, IReadOnlyList times, PathSpeedLimit speedLimit) { + if (input.PlanningScope == EmPlanningScope.FullDirectionSegment) + { + throw new InvalidOperationException("Full-direction exact-stop planning requires the initial feasibility projection."); + } int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(times, input.Configuration.Scheduling.OutputTimeStepSeconds); var motionTimes = new double[stabilizationStart + 1]; @@ -364,7 +512,7 @@ public sealed class SequentialLongitudinalOptimizer for (int index = 0; index < motionTimes.Length; index++) motionTimes[index] = times[index]; - LongitudinalCandidate baseline = CreateApproachSeed(input, motionTimes, speedLimit); + LongitudinalCandidate baseline = CreateScheduleReferenceSeed(input, motionTimes, speedLimit); var influence = new double[3, intervalCount]; for (int interval = 0; interval < intervalCount; interval++) { @@ -461,6 +609,30 @@ public sealed class SequentialLongitudinalOptimizer return AppendExactStopTail(times, stabilizationStart, input.StopBoundaryPathS, motion); } + private static LongitudinalCandidate CreateScheduleReferenceSeed(LongitudinalPlanningInput input, + IReadOnlyList times, PathSpeedLimit speedLimit) + { + LongitudinalConfiguration configuration = input.Configuration.Longitudinal; + var jerk = new double[times.Count - 1]; + double speed = input.InitialProgressSpeedMetersPerSecond; + double acceleration = input.InitialAccelerationMetersPerSecondSquared; + for (int index = 0; index < jerk.Length; index++) + { + double dt = times[index + 1] - times[index]; + double targetSpeed = input.KnotSchedule.ReferenceSpeedMetersPerSecond[index + 1]; + double lowerJerk = Math.Max(-configuration.MaximumJerkMetersPerSecondCubed, + (-configuration.MaximumDecelerationMetersPerSecondSquared - acceleration) / dt); + double upperJerk = Math.Min(configuration.MaximumJerkMetersPerSecondCubed, + (configuration.MaximumAccelerationMetersPerSecondSquared - acceleration) / dt); + double requestedJerk = 2d * (targetSpeed - speed - acceleration * dt) / (dt * dt); + double selectedJerk = Clamp(requestedJerk, lowerJerk, upperJerk); + jerk[index] = selectedJerk; + IntegrateStep(0d, speed, acceleration, selectedJerk, dt, out _, out speed, out acceleration); + } + return LongitudinalCandidate.Integrate(times, 0d, input.InitialProgressSpeedMetersPerSecond, + input.InitialAccelerationMetersPerSecondSquared, jerk); + } + private static double[] CreateEndpointNullspaceDirection(double[,] influence, double[,] gram, int basisIndex) { int intervalCount = influence.GetLength(1); @@ -679,10 +851,12 @@ public sealed class SequentialLongitudinalOptimizer nextIterate = null; if (candidate.S.Count != previous.S.Count) return false; - int stabilizationStart = input.Mode == EmLongitudinalMode.ExactStopAtBoundary - ? LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes, - input.Configuration.Scheduling.OutputTimeStepSeconds) - : candidate.S.Count; + int stabilizationStart = input.Mode != EmLongitudinalMode.ExactStopAtBoundary + ? candidate.S.Count + : input.PlanningScope == EmPlanningScope.FullDirectionSegment + ? input.KnotSchedule.TerminalHoldStartIndex + : LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes, + input.Configuration.Scheduling.OutputTimeStepSeconds); var candidateProgressSamples = new double[candidate.S.Count]; double priorProgress = double.NegativeInfinity; double priorPreviousProgress = double.NegativeInfinity; @@ -737,6 +911,30 @@ public sealed class SequentialLongitudinalOptimizer return true; } + private static bool TryCreateFeasibilityEnvelopeIterate(LongitudinalPlanningInput input, + LongitudinalCandidate candidate, out LongitudinalCandidate nextIterate) + { + nextIterate = null; + int stabilizationStart = input.KnotSchedule.TerminalHoldStartIndex; + var pathS = new double[candidate.S.Count]; + double previousPathS = double.NegativeInfinity; + double tolerance = input.Configuration.Validation.KinematicTolerance; + for (int index = 0; index < pathS.Length; index++) + { + double value = candidate.S[index]; + if (!IsFinite(value) || value < -tolerance || value > input.PathUpperBoundS + tolerance || + value < previousPathS - tolerance) + { + return false; + } + value = Math.Max(0d, Math.Min(input.PathUpperBoundS, value)); + pathS[index] = index >= stabilizationStart ? input.StopBoundaryPathS : Math.Max(previousPathS, value); + previousPathS = pathS[index]; + } + nextIterate = new LongitudinalCandidate(candidate.KnotTimes, pathS, candidate.U, candidate.A, candidate.J); + return true; + } + private static bool HasStrictResiduals(QpSolveResult result, double tolerance) { return IsPositiveFinite(tolerance) && result.PrimalResidual >= 0d && result.DualResidual >= 0d && diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs index bbbe95d..42c47a6 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs @@ -39,8 +39,9 @@ public sealed class EmTrajectoryAssembler throw new ArgumentNullException(nameof(metadata)); var interpolator = new LateralPathInterpolator(path); - var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds, zeroSpeedHoldSeconds, - metadata.LongitudinalMode); + bool isFullDirectionSegment = metadata.PlanningScope == EmPlanningScope.FullDirectionSegment; + var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds, + isFullDirectionSegment ? 0d : zeroSpeedHoldSeconds, metadata.LongitudinalMode, isFullDirectionSegment); double terminalPathS = path.Points[path.Points.Count - 1].PathS; var points = new List(schedule.Samples.Count); double directionSign = metadata.Direction == TravelDirection.Forward ? 1d : -1d; diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs index fa10678..3b5b9c4 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs @@ -9,7 +9,7 @@ internal sealed class TrajectorySampleSchedule private const double ZeroTolerance = 1e-12d; public TrajectorySampleSchedule(LongitudinalCandidate candidate, double outputTimeStepSeconds, double holdDurationSeconds, - EmLongitudinalMode mode) + EmLongitudinalMode mode, bool resampleMotion) { if (candidate == null) throw new ArgumentNullException(nameof(candidate)); @@ -22,16 +22,25 @@ internal sealed class TrajectorySampleSchedule var samples = new List(candidate.KnotTimes.Count + 4); double previousPathS = double.NegativeInfinity; - for (int index = 0; index < candidate.KnotTimes.Count; index++) + if (resampleMotion) { - if (candidate.S[index] < previousPathS) - throw new ArgumentException("Trajectory PathS cannot decrease.", nameof(candidate)); - if (candidate.U[index] < -ZeroTolerance) - throw new ArgumentException("Longitudinal progress speed cannot be negative.", nameof(candidate)); - - samples.Add(new TrajectorySample(candidate.KnotTimes[index], candidate.S[index], Math.Max(0d, candidate.U[index]), - candidate.A[index], index < candidate.J.Count ? candidate.J[index] : 0d, false)); - previousPathS = candidate.S[index]; + double finalTime = candidate.KnotTimes[candidate.KnotTimes.Count - 1]; + int sourceInterval = 0; + for (double sampleTime = 0d; sampleTime < finalTime - ZeroTolerance; + sampleTime += outputTimeStepSeconds) + { + AddSample(Interpolate(candidate, sampleTime, ref sourceInterval), samples, ref previousPathS, candidate); + } + AddSample(Interpolate(candidate, finalTime, ref sourceInterval), samples, ref previousPathS, candidate); + } + else + { + for (int index = 0; index < candidate.KnotTimes.Count; index++) + { + AddSample(new TrajectorySample(candidate.KnotTimes[index], candidate.S[index], + Math.Max(0d, candidate.U[index]), candidate.A[index], index < candidate.J.Count ? candidate.J[index] : 0d, + false), samples, ref previousPathS, candidate); + } } if (mode != EmLongitudinalMode.ExactStopAtBoundary) @@ -41,14 +50,22 @@ internal sealed class TrajectorySampleSchedule return; } - int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes, - outputTimeStepSeconds); - double stopPathS = candidate.S[stabilizationStart]; - for (int index = stabilizationStart; index < candidate.S.Count; index++) + int sourceStabilizationStart = resampleMotion + ? FindTerminalStationaryTailStart(candidate) + : LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes, outputTimeStepSeconds); + double stabilizationStartTime = candidate.KnotTimes[sourceStabilizationStart]; + int stabilizationStart = 0; + while (stabilizationStart < samples.Count - 1 && + samples[stabilizationStart].TimeFromStart < stabilizationStartTime - ZeroTolerance) { - if (Math.Abs(candidate.S[index] - stopPathS) > ZeroTolerance || - Math.Abs(candidate.U[index]) > ZeroTolerance || Math.Abs(candidate.A[index]) > ZeroTolerance || - (index < candidate.J.Count && Math.Abs(candidate.J[index]) > ZeroTolerance)) + stabilizationStart++; + } + double stopPathS = samples[stabilizationStart].PathS; + for (int index = stabilizationStart; index < samples.Count; index++) + { + if (Math.Abs(samples[index].PathS - stopPathS) > ZeroTolerance || + Math.Abs(samples[index].ProgressSpeed) > ZeroTolerance || Math.Abs(samples[index].Acceleration) > ZeroTolerance || + Math.Abs(samples[index].Jerk) > ZeroTolerance) { throw new ArgumentException("An exact stop requires a stationary S/U/A/J tail.", nameof(candidate)); } @@ -69,6 +86,57 @@ internal sealed class TrajectorySampleSchedule public IReadOnlyList Samples { get; } public int TerminalAnchorSampleIndex { get; } + private static void AddSample(TrajectorySample sample, ICollection samples, + ref double previousPathS, LongitudinalCandidate candidate) + { + if (sample.PathS < previousPathS) + throw new ArgumentException("Trajectory PathS cannot decrease.", nameof(candidate)); + if (sample.ProgressSpeed < -ZeroTolerance) + throw new ArgumentException("Longitudinal progress speed cannot be negative.", nameof(candidate)); + samples.Add(sample); + previousPathS = sample.PathS; + } + + private static TrajectorySample Interpolate(LongitudinalCandidate candidate, double sampleTime, ref int sourceInterval) + { + int lastKnot = candidate.KnotTimes.Count - 1; + if (sampleTime >= candidate.KnotTimes[lastKnot] - ZeroTolerance) + { + return new TrajectorySample(candidate.KnotTimes[lastKnot], candidate.S[lastKnot], + Math.Max(0d, candidate.U[lastKnot]), candidate.A[lastKnot], 0d, false); + } + while (sourceInterval < lastKnot - 1 && + sampleTime >= candidate.KnotTimes[sourceInterval + 1] - ZeroTolerance) + { + sourceInterval++; + } + if (Math.Abs(sampleTime - candidate.KnotTimes[sourceInterval]) <= ZeroTolerance) + { + return new TrajectorySample(candidate.KnotTimes[sourceInterval], candidate.S[sourceInterval], + Math.Max(0d, candidate.U[sourceInterval]), candidate.A[sourceInterval], candidate.J[sourceInterval], false); + } + double dt = sampleTime - candidate.KnotTimes[sourceInterval]; + double jerk = candidate.J[sourceInterval]; + double acceleration = candidate.A[sourceInterval] + jerk * dt; + double speed = candidate.U[sourceInterval] + candidate.A[sourceInterval] * dt + 0.5d * jerk * dt * dt; + double pathS = candidate.S[sourceInterval] + candidate.U[sourceInterval] * dt + + 0.5d * candidate.A[sourceInterval] * dt * dt + jerk * dt * dt * dt / 6d; + return new TrajectorySample(sampleTime, pathS, Math.Max(0d, speed), acceleration, jerk, false); + } + + private static int FindTerminalStationaryTailStart(LongitudinalCandidate candidate) + { + int start = candidate.KnotTimes.Count - 1; + double terminalPathS = candidate.S[start]; + while (start > 0 && Math.Abs(candidate.S[start - 1] - terminalPathS) <= ZeroTolerance && + Math.Abs(candidate.U[start - 1]) <= ZeroTolerance && Math.Abs(candidate.A[start - 1]) <= ZeroTolerance && + Math.Abs(candidate.J[start - 1]) <= ZeroTolerance) + { + start--; + } + return start; + } + private static bool IsFinite(double value) { return !double.IsNaN(value) && !double.IsInfinity(value); diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs index 167af5a..1f8082e 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs @@ -109,7 +109,9 @@ internal static class EmPlanningServiceChecks CreateReferencePath(TravelDirection.Forward, false, 0.0075d), null, EmPlanningScope.FullDirectionSegment); ConfigureExactStopServiceScenario(request.Configuration); - EmPlanningResult result = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan( + double[] strictFullPrimal = CreateStrictFullScopePrimal(request.Configuration); + EmPlanningResult result = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success, null, + strictFullPrimal)).Plan( request, CancellationToken.None); VerifySuccess(result, request, EmTerminalType.Goal, "full scope publication"); Verification.Equal(EmPlanningScope.FullDirectionSegment, result.Trajectory.Metadata.PlanningScope, @@ -127,9 +129,9 @@ internal static class EmPlanningServiceChecks configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.2d; } - private static void ConfigureExactStopServiceScenario(EmPlannerConfiguration configuration) - { - configuration.Scheduling.TimeHorizonSeconds = 0.40d; + private static void ConfigureExactStopServiceScenario(EmPlannerConfiguration configuration) + { + configuration.Scheduling.TimeHorizonSeconds = 0.40d; configuration.Scheduling.OutputTimeStepSeconds = 0.10d; configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d; configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d; @@ -137,8 +139,171 @@ internal static class EmPlanningServiceChecks configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d; configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 20d; configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d; - configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d; - } + configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d; + } + + private static double[] CreateStrictFullScopePrimal(EmPlannerConfiguration configuration) + { + var path = new LateralPath(new[] + { + new LateralPathPoint(0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d), + new LateralPathPoint(0.0075d, 0.0075d, 0d, 0d, 0d, 0d, 0.0075d, 0d, 0d, 0d, 0d, 0d), + }, true); + EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.05d, + EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason); + Verification.Equal(EmPlanningStatus.Success, status, "full scope test envelope: " + failureReason); + status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.05d, 0d, + configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration, + out LongitudinalKnotSchedule schedule, out failureReason); + Verification.Equal(EmPlanningStatus.Success, status, "full scope test schedule: " + failureReason); + var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.05d, 0d, + EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration, + EmPlanningScope.FullDirectionSegment, schedule, Array.Empty(), Array.Empty()); + int motionIntervalCount = schedule.TerminalHoldStartIndex; + Verification.True(motionIntervalCount >= 3, "full scope test schedule has three motion intervals"); + int terminalFirstInterval = motionIntervalCount - 3; + var terminalTimes = new double[4]; + for (int index = 1; index < terminalTimes.Length; index++) + terminalTimes[index] = terminalTimes[index - 1] + + schedule.KnotTimes[terminalFirstInterval + index] - + schedule.KnotTimes[terminalFirstInterval + index - 1]; + var motionTimes = new double[motionIntervalCount + 1]; + for (int index = 0; index < motionTimes.Length; index++) + motionTimes[index] = schedule.KnotTimes[index]; + var influence = new double[3, 3]; + for (int interval = 0; interval < 3; interval++) + { + var basis = new double[3]; + basis[interval] = 1d; + LongitudinalCandidate response = LongitudinalCandidate.Integrate(terminalTimes, 0d, 0d, 0d, basis); + int terminalIndex = response.S.Count - 1; + influence[0, interval] = response.A[terminalIndex]; + influence[1, interval] = response.U[terminalIndex]; + influence[2, interval] = response.S[terminalIndex]; + } + var validator = new LongitudinalSolutionValidator(); + for (int firstJerkStep = -20; firstJerkStep <= 0; firstJerkStep++) + { + for (int secondJerkStep = terminalFirstInterval >= 2 ? -20 : 0; + secondJerkStep <= (terminalFirstInterval >= 2 ? 20 : 0); secondJerkStep++) + { + for (int thirdJerkStep = terminalFirstInterval >= 3 ? -20 : 0; + thirdJerkStep <= (terminalFirstInterval >= 3 ? 20 : 0); thirdJerkStep++) + { + var jerk = new double[motionIntervalCount]; + jerk[0] = firstJerkStep; + if (terminalFirstInterval >= 2) + jerk[1] = secondJerkStep; + if (terminalFirstInterval >= 3) + jerk[2] = thirdJerkStep; + LongitudinalCandidate baseline = LongitudinalCandidate.Integrate(motionTimes, 0d, 0.05d, + 0d, jerk); + double[] target = + { + -baseline.A[baseline.A.Count - 1], + -baseline.U[baseline.U.Count - 1], + 0.0075d - baseline.S[baseline.S.Count - 1], + }; + if (!TrySolveThreeByThree(influence, target, out double[] terminalJerk)) + throw new InvalidOperationException("Full scope strict candidate terminal system is singular."); + for (int interval = 0; interval < 3; interval++) + jerk[terminalFirstInterval + interval] = terminalJerk[interval]; + LongitudinalCandidate motion = LongitudinalCandidate.Integrate(motionTimes, 0d, 0.05d, 0d, + jerk); + LongitudinalCandidate candidate = AppendFullStopTail(schedule.KnotTimes, motionIntervalCount, + motion); + if (!validator.TryValidate(input, speedLimit, candidate, out LongitudinalCandidate strict, out _)) + continue; + return ToPrimal(strict); + } + } + } + throw new InvalidOperationException("Unable to construct a strict full-scope test candidate: hold=" + + motionIntervalCount + ";times=" + string.Join(",", schedule.KnotTimes)); + } + + private static LongitudinalCandidate AppendFullStopTail(IReadOnlyList times, int motionIntervalCount, + LongitudinalCandidate motion) + { + var pathS = new double[times.Count]; + var speed = new double[times.Count]; + var acceleration = new double[times.Count]; + var jerk = new double[times.Count - 1]; + for (int index = 0; index <= motionIntervalCount; index++) + { + pathS[index] = index == motionIntervalCount ? 0.0075d : motion.S[index]; + speed[index] = index == motionIntervalCount ? 0d : motion.U[index]; + acceleration[index] = index == motionIntervalCount ? 0d : motion.A[index]; + } + for (int index = motionIntervalCount + 1; index < times.Count; index++) + pathS[index] = 0.0075d; + for (int index = 0; index < motion.J.Count; index++) + jerk[index] = motion.J[index]; + return new LongitudinalCandidate(times, pathS, speed, acceleration, jerk); + } + + private static double[] ToPrimal(LongitudinalCandidate candidate) + { + var layout = new LongitudinalVariableLayout(candidate.S.Count); + var primal = new double[layout.VariableCount]; + for (int index = 0; index < candidate.S.Count; index++) + { + primal[layout.S(index)] = candidate.S[index]; + primal[layout.U(index)] = candidate.U[index]; + primal[layout.A(index)] = candidate.A[index]; + } + for (int index = 0; index < candidate.J.Count; index++) + primal[layout.J(index)] = candidate.J[index]; + return primal; + } + + private static bool TrySolveThreeByThree(double[,] matrix, IReadOnlyList rightHandSide, + out double[] solution) + { + var augmented = new double[3, 4]; + for (int row = 0; row < 3; row++) + { + for (int column = 0; column < 3; column++) + augmented[row, column] = matrix[row, column]; + augmented[row, 3] = rightHandSide[row]; + } + for (int column = 0; column < 3; column++) + { + int pivot = column; + for (int row = column + 1; row < 3; row++) + { + if (Math.Abs(augmented[row, column]) > Math.Abs(augmented[pivot, column])) + pivot = row; + } + if (Math.Abs(augmented[pivot, column]) < 1e-12d) + { + solution = Array.Empty(); + return false; + } + if (pivot != column) + { + for (int index = column; index < 4; index++) + { + double temporary = augmented[column, index]; + augmented[column, index] = augmented[pivot, index]; + augmented[pivot, index] = temporary; + } + } + double divisor = augmented[column, column]; + for (int index = column; index < 4; index++) + augmented[column, index] /= divisor; + for (int row = 0; row < 3; row++) + { + if (row == column) + continue; + double factor = augmented[row, column]; + for (int index = column; index < 4; index++) + augmented[row, index] -= factor * augmented[column, index]; + } + } + solution = new[] { augmented[0, 3], augmented[1, 3], augmented[2, 3] }; + return true; + } private static void AssertExactStopStabilization(EmTrajectory trajectory, EmBoundaryType boundaryType, string name) { @@ -452,20 +617,23 @@ internal static class EmPlanningServiceChecks { private readonly PipelineSolverMode mode; private readonly PlanningGridMap? mapToCorrupt; + private readonly IReadOnlyList? strictFullPrimal; private int longitudinalCallCount; public QuadraticProgram? LastLongitudinalProblem { get; private set; } - public ScriptedPipelineSolver(PipelineSolverMode mode, PlanningGridMap? mapToCorrupt = null) + public ScriptedPipelineSolver(PipelineSolverMode mode, PlanningGridMap? mapToCorrupt = null, + IReadOnlyList? strictFullPrimal = null) { this.mode = mode; this.mapToCorrupt = mapToCorrupt; + this.strictFullPrimal = strictFullPrimal; } public QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings, IReadOnlyList warmStart, CancellationToken cancellationToken) { - bool longitudinal = problem.VariableCount > 100; + bool longitudinal = IsLongitudinalProblem(problem); if (mode == PipelineSolverMode.SolverUnavailable) return Result(QpSolveStatus.SolverUnavailable, Array.Empty()); if (!longitudinal) @@ -479,12 +647,18 @@ internal static class EmPlanningServiceChecks LastLongitudinalProblem = problem; if (mode == PipelineSolverMode.LongitudinalInfeasible) return Result(QpSolveStatus.PrimalInfeasible, Array.Empty()); + if (strictFullPrimal != null && strictFullPrimal.Count == problem.VariableCount) + { + longitudinalCallCount++; + return Result(QpSolveStatus.Solved, strictFullPrimal); + } if (mode == PipelineSolverMode.PublicationValidationFailure && longitudinalCallCount == 0) CorruptMapAtOrigin(mapToCorrupt); if (mode == PipelineSolverMode.TimeoutWithFallback && ++longitudinalCallCount > 1) return Result(QpSolveStatus.TimeLimit, Array.Empty()); longitudinalCallCount++; - return Result(QpSolveStatus.Solved, warmStart); + return Result(QpSolveStatus.Solved, + TryCreateStrictExactStopPrimal(problem, out double[] strictPrimal) ? strictPrimal : warmStart); } private static void CorruptMapAtOrigin(PlanningGridMap? map) @@ -502,41 +676,234 @@ internal static class EmPlanningServiceChecks distances[index] = 0d; } - private static double[] CreateStrictLongitudinalPrimal(QuadraticProgram problem) + private static bool TryCreateStrictExactStopPrimal(QuadraticProgram problem, out double[] primal) { + primal = Array.Empty(); int variableCount = problem.VariableCount; int knotCount = (variableCount + 1) / 4; var layout = new LongitudinalVariableLayout(knotCount); - var jerk = new double[knotCount - 1]; - const int rampIntervals = 5; - for (int index = 0; index < rampIntervals; index++) jerk[index] = 1d; - for (int index = rampIntervals; index < 3 * rampIntervals; index++) jerk[index] = -1d; - for (int index = 3 * rampIntervals; index < 4 * rampIntervals; index++) jerk[index] = 1d; + int stabilizationStart = FindExactStopTailStart(problem, layout); + if (stabilizationStart < 3) + return false; var times = new double[knotCount]; - for (int index = 0; index < times.Length; index++) times[index] = index * 0.05d; - LongitudinalCandidate baseCandidate = LongitudinalCandidate.Integrate(times, 0d, 0d, 0d, jerk); - double terminalPathS = ReadFixedVariable(problem, layout.S(knotCount - 1)); - double scale = terminalPathS / baseCandidate.S[baseCandidate.S.Count - 1]; - for (int index = 0; index < jerk.Length; index++) jerk[index] *= scale; - LongitudinalCandidate candidate = LongitudinalCandidate.Integrate(times, 0d, 0d, 0d, jerk); + for (int index = 0; index < knotCount - 1; index++) + { + if (!TryReadDynamicsDuration(problem, layout, index, out double duration)) + return false; + times[index + 1] = times[index] + duration; + } + + var motionTimes = new double[stabilizationStart + 1]; + Array.Copy(times, motionTimes, motionTimes.Length); + double initialPathS = ReadFixedVariable(problem, layout.S(0)); + double initialSpeed = ReadFixedVariable(problem, layout.U(0)); + double initialAcceleration = ReadFixedVariable(problem, layout.A(0)); + double terminalPathS = ReadFixedVariable(problem, layout.S(stabilizationStart)); + var preferredJerk = new double[stabilizationStart]; + LongitudinalCandidate baseline = LongitudinalCandidate.Integrate(motionTimes, initialPathS, initialSpeed, + initialAcceleration, preferredJerk); + var influence = new double[3, stabilizationStart]; + for (int interval = 0; interval < stabilizationStart; interval++) + { + var basis = new double[stabilizationStart]; + basis[interval] = 1d; + LongitudinalCandidate response = LongitudinalCandidate.Integrate(motionTimes, 0d, 0d, 0d, basis); + int terminalIndex = response.S.Count - 1; + influence[0, interval] = response.A[terminalIndex]; + influence[1, interval] = response.U[terminalIndex]; + influence[2, interval] = response.S[terminalIndex]; + } + double[] target = + { + -baseline.A[baseline.A.Count - 1], + -baseline.U[baseline.U.Count - 1], + terminalPathS - baseline.S[baseline.S.Count - 1], + }; + var jerk = new double[knotCount - 1]; + if (stabilizationStart >= 4) + { + int terminalFirstInterval = stabilizationStart - 3; + var terminalInfluence = new double[3, 3]; + for (int row = 0; row < 3; row++) + { + for (int column = 0; column < 3; column++) + terminalInfluence[row, column] = influence[row, terminalFirstInterval + column]; + } + if (!TrySolveThreeByThree(terminalInfluence, target, out double[] terminalJerk)) + return false; + for (int interval = 0; interval < 3; interval++) + jerk[terminalFirstInterval + interval] = terminalJerk[interval]; + } + else + { + var gram = new double[3, 3]; + for (int row = 0; row < 3; row++) + { + for (int column = 0; column < 3; column++) + { + for (int interval = 0; interval < stabilizationStart; interval++) + gram[row, column] += influence[row, interval] * influence[column, interval]; + } + } + if (!TrySolveThreeByThree(gram, target, out double[] multipliers)) + return false; + for (int interval = 0; interval < stabilizationStart; interval++) + { + jerk[interval] = preferredJerk[interval]; + for (int row = 0; row < 3; row++) + jerk[interval] += influence[row, interval] * multipliers[row]; + } + } + var motionJerk = new double[stabilizationStart]; + Array.Copy(jerk, motionJerk, motionJerk.Length); + LongitudinalCandidate candidate = LongitudinalCandidate.Integrate(motionTimes, initialPathS, initialSpeed, + initialAcceleration, motionJerk); + primal = CreateExactStopPrimal(layout, knotCount, stabilizationStart, terminalPathS, candidate); + return true; + } + + private static double[] CreateExactStopPrimal(LongitudinalVariableLayout layout, int knotCount, + int stabilizationStart, double terminalPathS, LongitudinalCandidate candidate) + { var primal = new double[layout.VariableCount]; for (int index = 0; index < knotCount; index++) { - primal[layout.S(index)] = candidate.S[index]; - primal[layout.U(index)] = candidate.U[index]; - primal[layout.A(index)] = candidate.A[index]; - } - for (int index = 0; index < jerk.Length; index++) primal[layout.J(index)] = candidate.J[index]; - for (int index = 4 * rampIntervals; index < knotCount; index++) - { - primal[layout.S(index)] = terminalPathS; - primal[layout.U(index)] = 0d; - primal[layout.A(index)] = 0d; + bool isTerminalTail = index >= stabilizationStart; + primal[layout.S(index)] = isTerminalTail ? terminalPathS : candidate.S[index]; + primal[layout.U(index)] = isTerminalTail ? 0d : candidate.U[index]; + primal[layout.A(index)] = isTerminalTail ? 0d : candidate.A[index]; } + for (int index = 0; index < candidate.J.Count; index++) + primal[layout.J(index)] = candidate.J[index]; return primal; } + private static int FindExactStopTailStart(QuadraticProgram problem, LongitudinalVariableLayout layout) + { + for (int index = 1; index < layout.KnotCount; index++) + { + if (TryReadFixedVariable(problem, layout.S(index), out _) && + TryReadFixedVariable(problem, layout.U(index), out _) && + TryReadFixedVariable(problem, layout.A(index), out _)) + { + return index; + } + } + return -1; + } + + private static bool TryReadDynamicsDuration(QuadraticProgram problem, LongitudinalVariableLayout layout, + int interval, out double duration) + { + duration = 0d; + for (int row = 0; row < problem.ConstraintCount; row++) + { + if (Math.Abs(problem.LowerBounds[row]) > 1e-12d || Math.Abs(problem.UpperBounds[row]) > 1e-12d || + CountRowEntries(problem, row) != 3 || + Math.Abs(ReadCoefficient(problem, row, layout.A(interval + 1)) - 1d) > 1e-12d || + Math.Abs(ReadCoefficient(problem, row, layout.A(interval)) + 1d) > 1e-12d) + { + continue; + } + double jerkCoefficient = ReadCoefficient(problem, row, layout.J(interval)); + if (jerkCoefficient >= -1e-12d) + continue; + duration = -jerkCoefficient; + return true; + } + return false; + } + + private static int CountRowEntries(QuadraticProgram problem, int row) + { + int count = 0; + for (int column = 0; column < problem.ConstraintMatrix.ColumnCount; column++) + { + for (int index = problem.ConstraintMatrix.ColumnPointers[column]; + index < problem.ConstraintMatrix.ColumnPointers[column + 1]; index++) + { + if (problem.ConstraintMatrix.RowIndices[index] == row) + count++; + } + } + return count; + } + + private static double ReadCoefficient(QuadraticProgram problem, int row, int column) + { + for (int index = problem.ConstraintMatrix.ColumnPointers[column]; + index < problem.ConstraintMatrix.ColumnPointers[column + 1]; index++) + { + if (problem.ConstraintMatrix.RowIndices[index] == row) + return problem.ConstraintMatrix.Values[index]; + } + return 0d; + } + + private static bool TrySolveThreeByThree(double[,] matrix, IReadOnlyList rightHandSide, + out double[] solution) + { + var augmented = new double[3, 4]; + for (int row = 0; row < 3; row++) + { + for (int column = 0; column < 3; column++) + augmented[row, column] = matrix[row, column]; + augmented[row, 3] = rightHandSide[row]; + } + for (int column = 0; column < 3; column++) + { + int pivot = column; + for (int row = column + 1; row < 3; row++) + { + if (Math.Abs(augmented[row, column]) > Math.Abs(augmented[pivot, column])) + pivot = row; + } + if (Math.Abs(augmented[pivot, column]) < 1e-12d) + { + solution = Array.Empty(); + return false; + } + if (pivot != column) + { + for (int index = column; index < 4; index++) + { + double temporary = augmented[column, index]; + augmented[column, index] = augmented[pivot, index]; + augmented[pivot, index] = temporary; + } + } + double divisor = augmented[column, column]; + for (int index = column; index < 4; index++) + augmented[column, index] /= divisor; + for (int row = 0; row < 3; row++) + { + if (row == column) + continue; + double factor = augmented[row, column]; + for (int index = column; index < 4; index++) + augmented[row, index] -= factor * augmented[column, index]; + } + } + solution = new[] { augmented[0, 3], augmented[1, 3], augmented[2, 3] }; + return true; + } + + private static bool IsLongitudinalProblem(QuadraticProgram problem) + { + if (problem.VariableCount < 7 || (problem.VariableCount + 1) % 4 != 0) + return false; + int knotCount = (problem.VariableCount + 1) / 4; + return problem.ConstraintCount >= 8 * knotCount - 2; + } + private static double ReadFixedVariable(QuadraticProgram problem, int variable) + { + if (TryReadFixedVariable(problem, variable, out double value)) + return value; + throw new InvalidOperationException("Expected a fixed ST variable constraint."); + } + + private static bool TryReadFixedVariable(QuadraticProgram problem, int variable, out double value) { for (int row = 0; row < problem.ConstraintCount; row++) { @@ -557,10 +924,12 @@ internal static class EmPlanningServiceChecks if (entryCount == 1 && Math.Abs(coefficient) > 1e-12d && Math.Abs(problem.LowerBounds[row] - problem.UpperBounds[row]) <= 1e-12d) { - return problem.LowerBounds[row] / coefficient; + value = problem.LowerBounds[row] / coefficient; + return true; } } - throw new InvalidOperationException("Expected a fixed ST variable constraint."); + value = 0d; + return false; } private static QpSolveResult Result(QpSolveStatus status, IReadOnlyList primal) diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs index 3202c3a..fb42496 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs @@ -13,6 +13,8 @@ internal static class LongitudinalIntegrationChecks public static void Run() { VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed(); + VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence(); + VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold(); VerifiesExactStopIncludesAStabilizationTail(); VerifiesLastStrictCandidateSurvivesLaterTimeout(); VerifiesInvalidAndInaccurateCandidatesNeverBecomeFallbacks(); @@ -58,6 +60,82 @@ internal static class LongitudinalIntegrationChecks "rolling ST keeps nonzero terminal speed"); } + private static void VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence() + { + EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault(); + configuration.Scheduling.TimeHorizonSeconds = 10d; + configuration.Scheduling.OutputTimeStepSeconds = 0.10d; + configuration.Scheduling.MaximumOptimizationTimeStepSeconds = 0.20d; + configuration.Scheduling.MaximumOptimizationSpatialStepMeters = 0.10d; + configuration.Scheduling.MaximumOptimizationKnotCount = 401; + LateralPath path = new LateralPath(new[] + { + Point(0d, 0d, 0d), + Point(1d, 1d, 0d), + Point(2d, 2d, 0d), + }, true); + + EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.10d, + EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason); + Verification.Equal(EmPlanningStatus.Success, status, "full schedule envelope: " + failureReason); + status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.10d, 0d, + configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration, + out LongitudinalKnotSchedule coarsePublication, out failureReason); + Verification.Equal(EmPlanningStatus.Success, status, "full schedule with 0.10 s publication: " + failureReason); + + EmPlannerConfiguration densePublicationConfiguration = configuration.Copy(); + densePublicationConfiguration.Scheduling.OutputTimeStepSeconds = 0.05d; + status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.10d, 0d, + densePublicationConfiguration.Longitudinal.DesiredForwardSpeedMetersPerSecond, densePublicationConfiguration, + out LongitudinalKnotSchedule densePublication, out failureReason); + Verification.Equal(EmPlanningStatus.Success, status, "full schedule with 0.05 s publication: " + failureReason); + Verification.Equal(coarsePublication.KnotTimes.Count, densePublication.KnotTimes.Count, + "publication cadence does not determine full-segment optimization knot count"); + + var publicationCandidate = new LongitudinalCandidate(new[] { 0d, 0.50d, 1d }, + new[] { 0d, 1d / 60d, 1d / 60d }, new[] { 0.10d, 0d, 0d }, new[] { -0.40d, 0d, 0d }, + new[] { 0.80d, 0d }); + var publicationResult = new LongitudinalPlanningResult(EmPlanningStatus.Success, publicationCandidate, string.Empty); + DateTimeOffset now = DateTimeOffset.UtcNow; + var metadata = new EmTrajectoryMetadata("publication-cadence", now, now, 1L, "publication-path", 1L, + string.Empty, 0, TravelDirection.Forward, EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, + EmPlanningScope.FullDirectionSegment); + configuration.Longitudinal.ZeroSpeedHoldSeconds = 0d; + densePublicationConfiguration.Longitudinal.ZeroSpeedHoldSeconds = 0d; + EmTrajectory coarseTrajectory = new EmTrajectoryAssembler(configuration).Assemble(path, publicationResult, metadata); + EmTrajectory denseTrajectory = new EmTrajectoryAssembler(densePublicationConfiguration).Assemble(path, + publicationResult, metadata); + Verification.Equal(2 * (coarseTrajectory.Points.Count - 1), denseTrajectory.Points.Count - 1, + "halving publication cadence doubles emitted trajectory intervals without changing optimization knots"); + } + + private static void VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold() + { + EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault(); + configuration.Scheduling.OutputTimeStepSeconds = 0.10d; + configuration.Longitudinal.ZeroSpeedHoldSeconds = 0.20d; + LateralPath path = new LateralPath(new[] + { + Point(0d, 0d, 0d), + Point(1d, 0.0075d, 0d), + }, true); + var candidate = new LongitudinalCandidate(new[] { 0d, 0.10d, 0.20d, 0.40d }, + new[] { 0d, 0.005d, 0.0075d, 0.0075d }, new[] { 0.05d, 0.025d, 0d, 0d }, + new[] { 0d, -0.5d, 0d, 0d }, new[] { -5d, 5d, 0d }); + var result = new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty); + DateTimeOffset now = DateTimeOffset.UtcNow; + var metadata = new EmTrajectoryMetadata("full-hold", now, now, 1L, "hold-path", 1L, string.Empty, 0, + TravelDirection.Forward, EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, + EmPlanningScope.FullDirectionSegment); + + EmTrajectory trajectory = new EmTrajectoryAssembler(configuration).Assemble(path, result, metadata); + + Verification.NearlyEqual(0.40d, trajectory.Points[trajectory.Points.Count - 1].TimeFromStart, + "full-scope publication reuses its candidate hold instead of appending a second hold"); + Verification.Equal(3, CountStationaryTerminalPoints(trajectory), + "full-scope publication emits the candidate's single nonzero terminal hold"); + } + private static void VerifiesExactStopIncludesAStabilizationTail() { EmPlannerConfiguration configuration = CreateExactStopSeedConfiguration(); @@ -97,6 +175,22 @@ internal static class LongitudinalIntegrationChecks } } + private static int CountStationaryTerminalPoints(EmTrajectory trajectory) + { + double terminalPathS = trajectory.Points[trajectory.Points.Count - 1].PathS; + int count = 0; + for (int index = 0; index < trajectory.Points.Count; index++) + { + EmTrajectoryPoint point = trajectory.Points[index]; + if (Math.Abs(point.PathS - terminalPathS) <= 1e-12d && + Math.Abs(point.SignedLongitudinalVelocity) <= 1e-12d) + { + count++; + } + } + return count; + } + public static void RunRealOsqp() { foreach (LongitudinalScenario scenario in CreateRealOsqpScenarios()) diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs index 82b7b28..ddbabf9 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs @@ -1,5 +1,6 @@ using System; using System.Collections.Generic; +using System.Threading; using EMPlannerVerificationHost; using MultiWheelC.TrajectoryPlanning.CoarsePath; using MultiWheelC.TrajectoryPlanning.PathSmoothing; @@ -19,6 +20,8 @@ internal static class LongitudinalModelChecks VerifiesStoppingPrecheckOnlyAppliesToRealStopBoundaries(); VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime(); VerifiesFullDirectionScopeSelectsActualSegmentBoundary(); + VerifiesFullDirectionScheduleDerivesDurationAndAdaptiveBreakpoints(); + VerifiesFullDirectionInitialFeasibilityProjectionAndFallbackSemantics(); VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints(); VerifiesModeSpecificSolutionValidation(); VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically(); @@ -297,6 +300,231 @@ internal static class LongitudinalModelChecks Verification.NearlyEqual(10d, gear.WindowEndReferenceS, "gear full selection stops before the next segment"); } + private static void VerifiesFullDirectionScheduleDerivesDurationAndAdaptiveBreakpoints() + { + EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault(); + configuration.Scheduling.TimeHorizonSeconds = 10d; + configuration.Scheduling.DistanceHorizonMeters = 0.25d; + configuration.Scheduling.OutputTimeStepSeconds = 0.10d; + configuration.Scheduling.MaximumOptimizationTimeStepSeconds = 0.20d; + configuration.Scheduling.MaximumOptimizationSpatialStepMeters = 0.10d; + configuration.Scheduling.MaximumOptimizationKnotCount = 401; + configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d; + configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond = 1d; + configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 0.50d; + configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 0.50d; + configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1d; + + LateralPath shortPath = CreateStraightPath(0.50d); + EmPlanningStatus status = new PathSpeedLimitBuilder().Build(shortPath, TravelDirection.Forward, 0.10d, + EmTerminalType.Goal, configuration, out PathSpeedLimit shortLimit, out string failureReason); + Verification.Equal(EmPlanningStatus.Success, status, "short full-segment envelope: " + failureReason); + status = new FullDirectionSegmentScheduleBuilder().TryBuild(shortPath, shortLimit, 0.10d, 0d, + configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration, + out LongitudinalKnotSchedule shortSchedule, out failureReason); + Verification.Equal(EmPlanningStatus.Success, status, "short full-segment schedule: " + failureReason); + Verification.True(shortSchedule.TotalDurationSeconds < 10d, "short segment derives its own T_end"); + + LateralPath longPath = CreatePath(new[] + { + new PathFixture(0d, 0d, 0d, 0d), + new PathFixture(1.50d, 1.50d, 2d, 0d), + new PathFixture(3d, 3d, 0d, 0d), + }); + status = new PathSpeedLimitBuilder().Build(longPath, TravelDirection.Forward, 0.10d, + EmTerminalType.Goal, configuration, out PathSpeedLimit longLimit, out failureReason); + Verification.Equal(EmPlanningStatus.Success, status, "long full-segment envelope: " + failureReason); + status = new FullDirectionSegmentScheduleBuilder().TryBuild(longPath, longLimit, 0.10d, 0d, + configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration, + out LongitudinalKnotSchedule longSchedule, out failureReason); + Verification.Equal(EmPlanningStatus.Success, status, "long full-segment schedule: " + failureReason); + Verification.True(longSchedule.TotalDurationSeconds > shortSchedule.TotalDurationSeconds, + "duration grows from s_end and limits"); + Verification.True(longSchedule.KnotTimes.Count <= configuration.Scheduling.MaximumOptimizationKnotCount, + "adaptive schedule respects knot cap"); + Verification.True(longSchedule.IsAdaptive, "full segment produces an adaptive knot schedule"); + Verification.True(longSchedule.ReferencePathS.Count > longPath.Points.Count, + "curvature and stopping envelopes add schedule breakpoints"); + Verification.NearlyEqual(longPath.Points[longPath.Points.Count - 1].PathS, + longSchedule.ReferencePathS[longSchedule.ReferencePathS.Count - 1], "schedule reaches s_end"); + Verification.NearlyEqual(0d, + longSchedule.ReferenceSpeedMetersPerSecond[longSchedule.ReferenceSpeedMetersPerSecond.Count - 1], + "schedule stops at s_end"); + + EmPlannerConfiguration constrained = configuration.Copy(); + constrained.Scheduling.MaximumOptimizationKnotCount = 4; + status = new FullDirectionSegmentScheduleBuilder().TryBuild(longPath, longLimit, 0.10d, 0d, + constrained.Longitudinal.DesiredForwardSpeedMetersPerSecond, constrained, + out LongitudinalKnotSchedule rejected, out failureReason); + Verification.Equal(EmPlanningStatus.FullSegmentResourceLimitExceeded, status, + "undersized full-segment knot cap rejects rather than truncates"); + Verification.True(rejected == null, "resource rejection produces no partial schedule"); + Verification.True(failureReason.IndexOf("required", StringComparison.OrdinalIgnoreCase) >= 0 && + failureReason.IndexOf("configured", StringComparison.OrdinalIgnoreCase) >= 0, + "resource rejection reports required and configured knots"); + } + + private static void VerifiesFullDirectionInitialFeasibilityProjectionAndFallbackSemantics() + { + EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault(); + configuration.Scheduling.MaximumOptimizationTimeStepSeconds = 0.20d; + configuration.Scheduling.MaximumOptimizationSpatialStepMeters = 0.10d; + configuration.Scheduling.MaximumOptimizationKnotCount = 401; + configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d; + configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d; + configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 20d; + LateralPath path = CreateStraightPath(0.0075d); + EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.05d, + EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason); + Verification.Equal(EmPlanningStatus.Success, status, "feasible-reference envelope: " + failureReason); + status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.05d, 0d, + configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration, + out LongitudinalKnotSchedule schedule, out failureReason); + Verification.Equal(EmPlanningStatus.Success, status, "feasible-reference schedule: " + failureReason); + + Verification.True(typeof(LongitudinalKnotSchedule).GetProperty("ReferenceCandidate") == null, + "adaptive schedule is only a knot/reference/hold contract"); + Verification.True(schedule.TerminalHoldStartIndex > 0 && + schedule.TerminalHoldStartIndex < schedule.KnotTimes.Count, + "adaptive reference explicitly identifies its terminal hold boundary"); + Verification.True(schedule.TerminalHoldStartIndex >= 3, + "adaptive exact-stop schedule reserves three independent motion jerk intervals"); + LongitudinalCandidate strictProjection = CreateStrictNonuniformExactStopCandidate(); + var projectionSchedule = new LongitudinalKnotSchedule(strictProjection.KnotTimes, + new[] { 0d, 0.003d, 0.006d, 0.0075d, 0.0075d }, new[] { 0.05d, 0.025d, 0.01d, 0d, 0d }, true, 3); + var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.05d, 0d, + EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration, + EmPlanningScope.FullDirectionSegment, projectionSchedule, Array.Empty(), Array.Empty()); + Verification.True(new LongitudinalSolutionValidator().TryValidate(input, speedLimit, strictProjection, + out _, out failureReason), "nonuniform strict projection fixture is physically feasible: " + failureReason); + + var constraintBuilder = new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()); + Verification.True(constraintBuilder.TryBuildInitialFeasibilityProjection(input, speedLimit, + out QuadraticProgram projectionProblem, out failureReason), + "full exact-stop feasibility projection builds: " + failureReason); + var layout = new LongitudinalVariableLayout(projectionSchedule.KnotTimes.Count); + Verification.True(Math.Abs(projectionProblem.LinearCost[layout.S(1)]) > 1e-12d, + "feasibility projection tracks scheduled PathS"); + Verification.True(Math.Abs(projectionProblem.LinearCost[layout.U(1)]) > 1e-12d, + "feasibility projection tracks scheduled speed"); + Verification.Equal(9 * layout.KnotCount - 3 + + 3 * (layout.KnotCount - projectionSchedule.TerminalHoldStartIndex), projectionProblem.ConstraintCount, + "feasibility projection carries a PathS-linearized speed-envelope row for each motion knot"); + + var initialTimeoutSolver = new FakeQpSolver(new QpSolveResult(QpSolveStatus.TimeLimit, Array.Empty(), 0d, 0d, + 0d, 0, TimeSpan.Zero, "time limit", string.Empty)); + LongitudinalPlanningResult initialTimeout = new SequentialLongitudinalOptimizer(initialTimeoutSolver).Optimize(input, + CancellationToken.None); + Verification.Equal(EmPlanningStatus.SolverTimedOut, initialTimeout.Status, + "initial feasibility timeout cannot publish a fallback"); + Verification.True(initialTimeout.Candidate == null, "initial feasibility timeout publishes no candidate"); + + var solver = new FakeQpSolver(new[] + { + new QpSolveResult(QpSolveStatus.Solved, ToPrimal(strictProjection), 0d, 0d, 0d, 1, + TimeSpan.Zero, "solved", string.Empty), + new QpSolveResult(QpSolveStatus.TimeLimit, Array.Empty(), 0d, 0d, 0d, 0, + TimeSpan.Zero, "time limit", string.Empty), + }); + LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input, + CancellationToken.None); + Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status, + "strict feasibility projection permits a later exact-stop fallback: " + result.FailureReason); + Verification.Equal(2, solver.SolveCallCount, + "full scope consumes strict feasibility projection before the objective timeout"); + Verification.True(new LongitudinalSolutionValidator().TryValidate(input, speedLimit, + result.Candidate ?? throw new InvalidOperationException("Adaptive fallback was missing."), out _, + out failureReason), "adaptive fallback is strict-feasible: " + failureReason); + + EmPlannerConfiguration denserPublication = configuration.Copy(); + denserPublication.Scheduling.OutputTimeStepSeconds = 0.05d; + status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.05d, 0d, + denserPublication.Longitudinal.DesiredForwardSpeedMetersPerSecond, denserPublication, + out LongitudinalKnotSchedule sameOptimizationSchedule, out failureReason); + Verification.Equal(EmPlanningStatus.Success, status, "independent-cadence schedule: " + failureReason); + Verification.Equal(schedule.KnotTimes.Count, sameOptimizationSchedule.KnotTimes.Count, + "publication cadence does not change adaptive knot count"); + Verification.Equal(schedule.TerminalHoldStartIndex, sameOptimizationSchedule.TerminalHoldStartIndex, + "publication cadence does not change the terminal hold boundary"); + } + + private static LongitudinalCandidate CreateStrictNonuniformExactStopCandidate() + { + double[] times = { 0d, 0.09d, 0.19d, 0.30d, 0.50d }; + double[] motionTimes = { 0d, 0.09d, 0.19d, 0.30d }; + var influence = new double[3, 3]; + for (int interval = 0; interval < 3; interval++) + { + var basis = new double[3]; + basis[interval] = 1d; + LongitudinalCandidate response = LongitudinalCandidate.Integrate(motionTimes, 0d, 0d, 0d, basis); + int last = response.S.Count - 1; + influence[0, interval] = response.A[last]; + influence[1, interval] = response.U[last]; + influence[2, interval] = response.S[last]; + } + double[] jerkMotion = SolveThreeByThree(influence, new[] { 0d, -0.05d, -0.0075d }); + var jerk = new[] { jerkMotion[0], jerkMotion[1], jerkMotion[2], 0d }; + LongitudinalCandidate integrated = LongitudinalCandidate.Integrate(times, 0d, 0.05d, 0d, jerk); + var pathS = new[] { integrated.S[0], integrated.S[1], integrated.S[2], 0.0075d, 0.0075d }; + var speed = new[] { integrated.U[0], integrated.U[1], integrated.U[2], 0d, 0d }; + var acceleration = new[] { integrated.A[0], integrated.A[1], integrated.A[2], 0d, 0d }; + return new LongitudinalCandidate(times, pathS, speed, acceleration, jerk); + } + + private static double[] ToPrimal(LongitudinalCandidate candidate) + { + var layout = new LongitudinalVariableLayout(candidate.KnotTimes.Count); + var primal = new double[layout.VariableCount]; + for (int index = 0; index < layout.KnotCount; index++) + { + primal[layout.S(index)] = candidate.S[index]; + primal[layout.U(index)] = candidate.U[index]; + primal[layout.A(index)] = candidate.A[index]; + } + for (int index = 0; index < layout.KnotCount - 1; index++) + primal[layout.J(index)] = candidate.J[index]; + return primal; + } + + private static double[] SolveThreeByThree(double[,] matrix, IReadOnlyList rightHandSide) + { + var augmented = new double[3, 4]; + for (int row = 0; row < 3; row++) + { + for (int column = 0; column < 3; column++) + augmented[row, column] = matrix[row, column]; + augmented[row, 3] = rightHandSide[row]; + } + for (int pivot = 0; pivot < 3; pivot++) + { + int bestRow = pivot; + for (int row = pivot + 1; row < 3; row++) + { + if (Math.Abs(augmented[row, pivot]) > Math.Abs(augmented[bestRow, pivot])) + bestRow = row; + } + for (int column = pivot; column < 4; column++) + { + double temporary = augmented[pivot, column]; + augmented[pivot, column] = augmented[bestRow, column]; + augmented[bestRow, column] = temporary; + } + double divisor = augmented[pivot, pivot]; + for (int column = pivot; column < 4; column++) + augmented[pivot, column] /= divisor; + for (int row = 0; row < 3; row++) + { + if (row == pivot) + continue; + double factor = augmented[row, pivot]; + for (int column = pivot; column < 4; column++) + augmented[row, column] -= factor * augmented[pivot, column]; + } + } + return new[] { augmented[0, 3], augmented[1, 3], augmented[2, 3] }; + } + private static void VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints() { var layout = new LongitudinalVariableLayout(5); @@ -370,8 +598,21 @@ internal static class LongitudinalModelChecks Verification.NearlyEqual(2d, sUpper, "S upper bound"); FindSingleVariableBounds(problem, layout.U(1), out double uLower, out double uUpper); Verification.NearlyEqual(0d, uLower, "U nonnegative bound"); - Verification.NearlyEqual(envelope.MaximumSpeedAt(integrated.S[1]), uUpper, - "U upper bound samples envelope at current S iterate"); + Verification.NearlyEqual(input.DirectionMaximumSpeedMetersPerSecond, uUpper, + "U retains its direction hard bound alongside the PathS envelope"); + int envelopeSegment = 0; + while (envelopeSegment < envelope.PathS.Count - 2 && integrated.S[1] > envelope.PathS[envelopeSegment + 1]) + envelopeSegment++; + double envelopeSlope = (envelope.MaximumSpeedMetersPerSecond[envelopeSegment + 1] - + envelope.MaximumSpeedMetersPerSecond[envelopeSegment]) / + (envelope.PathS[envelopeSegment + 1] - envelope.PathS[envelopeSegment]); + double envelopeIntercept = envelope.MaximumSpeedMetersPerSecond[envelopeSegment] - + envelopeSlope * envelope.PathS[envelopeSegment]; + Verification.Equal(1, CountBoundedRow(problem, new Dictionary + { + { layout.U(1), 1d }, { layout.S(1), -envelopeSlope }, + }, -QuadraticProgram.MaximumFiniteBound, envelopeIntercept), + "U upper bound linearly re-evaluates the actual PathS envelope"); FindSingleVariableBounds(problem, layout.A(1), out double aLower, out double aUpper); Verification.NearlyEqual(-1d, aLower, "deceleration lower bound"); Verification.NearlyEqual(1d, aUpper, "acceleration upper bound");