From 0bba8d7e618be4fe46f79ed41dd67b4085ad1ea2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Fri, 7 Aug 2026 07:51:43 +0800 Subject: [PATCH] fix: accelerate EM trajectories from rest --- .../EMPlanner/Facade/EmPlanningService.cs | 2 +- .../FullDirectionSegmentScheduleBuilder.cs | 10 +- .../LongitudinalObjectiveBuilder.cs | 5 +- .../LongitudinalSolutionValidator.cs | 20 +++- .../Longitudinal/PathSpeedLimitBuilder.cs | 22 +++-- .../SequentialLongitudinalOptimizer.cs | 97 ++++++++++++++++++- .../EmPlanningServiceChecks.cs | 20 ++++ .../LongitudinalIntegrationChecks.cs | 51 ++++++++++ .../LongitudinalModelChecks.cs | 66 +++++++++++-- 9 files changed, 273 insertions(+), 20 deletions(-) diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs index ca5a8b0..e47fb13 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs @@ -84,7 +84,7 @@ public sealed class EmPlanningService : IEmPlanningService EmitDebug(request, "LS optimization and validation succeeded"); EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(lateral.Path, segment.Direction, - initialProgressSpeed, horizon.TerminalType, configuration, out PathSpeedLimit speedLimit, + initialProgressSpeed, initialAcceleration, horizon.TerminalType, configuration, out PathSpeedLimit speedLimit, out string envelopeReason); if (envelopeStatus != EmPlanningStatus.Success) return Failure(envelopeStatus, request, envelopeReason); diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/FullDirectionSegmentScheduleBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/FullDirectionSegmentScheduleBuilder.cs index dcf187c..209f36c 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/FullDirectionSegmentScheduleBuilder.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/FullDirectionSegmentScheduleBuilder.cs @@ -167,9 +167,12 @@ public sealed class FullDirectionSegmentScheduleBuilder LongitudinalConfiguration configuration) { double adjustedTime = 0d; + double requestedPreviousTime = times[0]; for (int index = 1; index < times.Count; index++) { - double requestedDuration = times[index] - times[index - 1]; + double requestedTime = times[index]; + double requestedDuration = requestedTime - requestedPreviousTime; + requestedPreviousTime = requestedTime; double speedChange = Math.Abs(referenceSpeeds[index] - referenceSpeeds[index - 1]); double accelerationLimit = referenceSpeeds[index] >= referenceSpeeds[index - 1] ? configuration.MaximumAccelerationMetersPerSecondSquared @@ -191,7 +194,10 @@ public sealed class FullDirectionSegmentScheduleBuilder { 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) + bool changesDirection = previousSlope * nextSlope < 0d; + bool entersCruise = previousSlope > Tolerance && nextSlope <= Tolerance; + bool leavesCruise = previousSlope >= -Tolerance && nextSlope < -Tolerance; + if (changesDirection || entersCruise || leavesCruise) stations.Add(index); } stations.Add(pathS.Count - 1); diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalObjectiveBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalObjectiveBuilder.cs index 84814a7..2bafd61 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalObjectiveBuilder.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalObjectiveBuilder.cs @@ -26,7 +26,10 @@ public sealed class LongitudinalObjectiveBuilder for (int index = 0; index < layout.KnotCount; index++) { double iteratePathS = Math.Max(0d, Math.Min(input.PathUpperBoundS, iterate.S[index])); - AddSquaredResidual(hessian, linearCost, layout.U(index), speedLimit.MaximumSpeedAt(iteratePathS), + double referenceSpeed = input.PlanningScope == EmPlanningScope.FullDirectionSegment + ? input.KnotSchedule.ReferenceSpeedMetersPerSecond[index] + : speedLimit.MaximumSpeedAt(iteratePathS); + AddSquaredResidual(hessian, linearCost, layout.U(index), referenceSpeed, weights.ReferenceSpeed, speedScale); AddSquaredResidual(hessian, linearCost, layout.A(index), 0d, weights.Acceleration, accelerationScale); if (index < layout.KnotCount - 1 && index < input.PreviousPathS.Count) diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs index 64ab5d7..fc363fa 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs @@ -8,8 +8,15 @@ public sealed class LongitudinalSolutionValidator { public bool TryValidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate candidate, out LongitudinalCandidate validatedCandidate, out string failureReason) + { + return TryValidate(input, speedLimit, candidate, out validatedCandidate, out _, out failureReason); + } + + public bool TryValidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate candidate, + out LongitudinalCandidate validatedCandidate, out EmPlanningStatus failureStatus, out string failureReason) { validatedCandidate = null; + failureStatus = EmPlanningStatus.LongitudinalInfeasible; failureReason = string.Empty; if (input == null || speedLimit == null || candidate == null) { @@ -58,7 +65,8 @@ public sealed class LongitudinalSolutionValidator progress > input.PathUpperBoundS + tolerance || speed < -tolerance || acceleration < -maximumDeceleration - tolerance || acceleration > maximumAcceleration + tolerance) { - failureReason = "ST candidate violates physical bounds at knot " + index + "."; + failureReason = "ST candidate violates physical bounds at knot " + index + + " (S=" + progress + ", U=" + speed + ", A=" + acceleration + ")."; return false; } double speedLimitAtProgress = index == 0 @@ -84,6 +92,16 @@ public sealed class LongitudinalSolutionValidator return false; } } + bool requiresProgress = input.PathUpperBoundS > + input.Configuration.Validation.TerminalPositionToleranceMeters; + double achievedProgress = candidate.S[candidate.S.Count - 1] - candidate.S[0]; + if (input.PlanningScope == EmPlanningScope.FullDirectionSegment && requiresProgress && + achievedProgress <= input.Configuration.Validation.SpatialToleranceMeters) + { + failureStatus = EmPlanningStatus.NoProgress; + failureReason = "NoProgress: a nonterminal full direction segment produced zero progress."; + return false; + } int stabilizationStart = candidate.S.Count; if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary) { diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs index b8e5ed9..cdfdf06 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs @@ -32,6 +32,15 @@ public sealed class PathSpeedLimitBuilder out speedLimit, out failureReason); } + public EmPlanningStatus Build(LateralPath path, TravelDirection direction, + double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared, + EmTerminalType terminalType, EmPlannerConfiguration configuration, out PathSpeedLimit speedLimit, + out string failureReason) + { + return BuildCore(path, direction, initialProgressSpeedMetersPerSecond, + initialAccelerationMetersPerSecondSquared, terminalType, configuration, out speedLimit, out failureReason); + } + private EmPlanningStatus BuildCore(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmPlannerConfiguration configuration, out PathSpeedLimit speedLimit, @@ -62,6 +71,7 @@ public sealed class PathSpeedLimitBuilder double maximumJerk = longitudinal.MaximumJerkMetersPerSecondCubed; double maximumLateralAcceleration = longitudinal.MaximumLateralAccelerationMetersPerSecondSquared; double maximumCurvatureRate = longitudinal.MaximumCurvatureRatePerMeterPerSecond; + double stoppingAcceleration = Math.Max(0d, initialAccelerationMetersPerSecondSquared); if (!IsPositiveFinite(directionMaximum) || !IsPositiveFinite(maximumAcceleration) || !IsPositiveFinite(maximumDeceleration) || !IsPositiveFinite(maximumJerk) || !IsPositiveFinite(maximumLateralAcceleration) || !IsPositiveFinite(maximumCurvatureRate)) @@ -118,7 +128,7 @@ public sealed class PathSpeedLimitBuilder if (hasStopBoundary) { AddJerkLimitedStoppingStations(lowerPoint.PathS, upperPoint.PathS, stopBoundaryPathS, - directionMaximum, maximumAcceleration, maximumDeceleration, maximumJerk, + directionMaximum, stoppingAcceleration, maximumDeceleration, maximumJerk, segmentIndex == 0, segmentStations); } segmentStations.Sort(); @@ -134,7 +144,7 @@ public sealed class PathSpeedLimitBuilder double curvatureDerivative = Interpolate(lowerPoint.VehicleCurvatureDerivative, upperPoint.VehicleCurvatureDerivative, fraction); AddLimitSample(samplePathS, curvature, curvatureDerivative, hasStopBoundary, - stopBoundaryPathS, directionMaximum, maximumAcceleration, maximumDeceleration, + stopBoundaryPathS, directionMaximum, stoppingAcceleration, maximumDeceleration, maximumJerk, maximumLateralAcceleration, maximumCurvatureRate, pathS, maximum, lateral, curvatureRate, stopping); } @@ -188,14 +198,14 @@ public sealed class PathSpeedLimitBuilder } private static void AddJerkLimitedStoppingStations(double lowerPathS, double upperPathS, - double stopBoundaryPathS, double directionMaximum, double maximumAcceleration, double maximumDeceleration, + double stopBoundaryPathS, double directionMaximum, double stoppingAcceleration, double maximumDeceleration, double maximumJerk, bool includeLower, IList stations) { const int stoppingSpeedSampleCount = 64; for (int step = 0; step < stoppingSpeedSampleCount; step++) { double speed = directionMaximum * step / stoppingSpeedSampleCount; - if (!JerkLimitedStoppingMath.TryCalculate(speed, maximumAcceleration, + if (!JerkLimitedStoppingMath.TryCalculate(speed, stoppingAcceleration, maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stop, out _)) { throw new ArgumentException("The configured jerk-limited stop envelope cannot be sampled."); @@ -210,7 +220,7 @@ public sealed class PathSpeedLimitBuilder } private static void AddLimitSample(double samplePathS, double curvature, double curvatureDerivative, - bool hasStopBoundary, double stopBoundaryPathS, double directionMaximum, double maximumAcceleration, + bool hasStopBoundary, double stopBoundaryPathS, double directionMaximum, double stoppingAcceleration, double maximumDeceleration, double maximumJerk, double maximumLateralAcceleration, double maximumCurvatureRate, IList pathS, IList maximum, IList lateral, IList curvatureRate, IList stopping) @@ -220,7 +230,7 @@ public sealed class PathSpeedLimitBuilder double curvatureRateLimit = maximumCurvatureRate / Math.Max(Math.Abs(curvatureDerivative), CurvatureEpsilon); double stoppingLimit = hasStopBoundary ? JerkLimitedStoppingMath.MaximumInitialSpeedForDistance( - Math.Max(0d, stopBoundaryPathS - samplePathS), maximumAcceleration, + Math.Max(0d, stopBoundaryPathS - samplePathS), stoppingAcceleration, maximumDeceleration, maximumJerk, directionMaximum) : directionMaximum; double lateralValue = ClampFinite(lateralLimit, directionMaximum); diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs index fb4361d..ec04e82 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs @@ -2,6 +2,7 @@ using System; using System.Collections.Generic; using System.Diagnostics; using System.Threading; +using MultiWheelC.TrajectoryPlanning.CoarsePath; namespace MultiWheelC.TrajectoryPlanning.EMPlanner; @@ -222,6 +223,14 @@ public sealed class SequentialLongitudinalOptimizer projectionSolveCount = 0; failureStatus = EmPlanningStatus.LongitudinalInfeasible; failureReason = string.Empty; + if (input.InitialProgressSpeedMetersPerSecond <= input.Configuration.Validation.SpatialToleranceMeters && + Math.Abs(input.InitialAccelerationMetersPerSecondSquared) <= + input.Configuration.Validation.KinematicTolerance && + TryCreateStaticStartSeed(input, speedLimit, out LongitudinalCandidate staticStartSeed)) + { + candidate = staticStartSeed; + return true; + } LongitudinalCandidate linearizationIterate = CreateScheduleReferenceIterate(input); string lastRejection = string.Empty; for (int iteration = 0; iteration < iterationLimit; iteration++) @@ -302,11 +311,17 @@ public sealed class SequentialLongitudinalOptimizer if (solved.Status == QpSolveStatus.Solved || HasStrictResiduals(solved, convergenceTolerance)) { if (_solutionValidator.TryValidate(input, speedLimit, projected, out LongitudinalCandidate strict, - out string validationFailure)) + out EmPlanningStatus validationStatus, out string validationFailure)) { candidate = strict; return true; } + if (validationStatus == EmPlanningStatus.NoProgress) + { + failureStatus = validationStatus; + failureReason = validationFailure; + return false; + } lastRejection = validationFailure; } @@ -377,6 +392,14 @@ public sealed class SequentialLongitudinalOptimizer double dt = times[index + 1] - times[index]; double speedLimitAtS = speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.PathUpperBoundS, s))); double targetSpeed = Math.Min(input.InitialProgressSpeedMetersPerSecond, speedLimitAtS); + if (input.PlanningScope == EmPlanningScope.FullDirectionSegment) + { + double desiredSpeed = input.Direction == TravelDirection.Forward + ? configuration.DesiredForwardSpeedMetersPerSecond + : configuration.DesiredReverseSpeedMetersPerSecond; + double scheduleSpeed = input.KnotSchedule.ReferenceSpeedMetersPerSecond[index]; + targetSpeed = Math.Min(desiredSpeed, Math.Min(scheduleSpeed, speedLimitAtS)); + } double lowerJerk = Math.Max(-configuration.MaximumJerkMetersPerSecondCubed, (-configuration.MaximumDecelerationMetersPerSecondSquared - a) / dt); lowerJerk = Math.Max(lowerJerk, -2d * (u + a * dt) / (dt * dt)); @@ -442,6 +465,44 @@ public sealed class SequentialLongitudinalOptimizer return CreateApproachSeed(input, times, speedLimit); } + private bool TryCreateStaticStartSeed(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, + out LongitudinalCandidate candidate) + { + candidate = null; + int stabilizationStart = input.KnotSchedule.TerminalHoldStartIndex; + if (stabilizationStart < 5) + return false; + + IReadOnlyList times = input.KnotSchedule.KnotTimes; + if (TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit, out candidate)) + return true; + double firstDuration = times[1] - times[0]; + double secondDuration = times[2] - times[1]; + LongitudinalConfiguration configuration = input.Configuration.Longitudinal; + double maximumFirstJerk = Math.Min(configuration.MaximumJerkMetersPerSecondCubed, + Math.Min(configuration.MaximumAccelerationMetersPerSecondSquared / firstDuration, + configuration.MaximumJerkMetersPerSecondCubed * secondDuration / firstDuration)); + for (int sample = 1; sample <= 256; sample++) + { + double firstJerk = maximumFirstJerk * sample / 256d; + var jerk = new double[stabilizationStart]; + jerk[0] = firstJerk; + jerk[1] = -firstJerk * firstDuration / secondDuration; + if (!TryCloseExactStopEndpoint(input, times, stabilizationStart, jerk, + out LongitudinalCandidate probe)) + { + continue; + } + if (_solutionValidator.TryValidate(input, speedLimit, probe, + out LongitudinalCandidate strict, out _)) + { + candidate = strict; + return true; + } + } + return false; + } + private static bool TryCreateCruiseThenBrakeSeed(LongitudinalPlanningInput input, IReadOnlyList times, double stopBoundaryPathS, out LongitudinalCandidate candidate) { @@ -609,6 +670,40 @@ public sealed class SequentialLongitudinalOptimizer return AppendExactStopTail(times, stabilizationStart, input.StopBoundaryPathS, motion); } + private static bool TryCloseExactStopEndpoint(LongitudinalPlanningInput input, IReadOnlyList times, + int stabilizationStart, double[] jerk, out LongitudinalCandidate candidate) + { + candidate = null; + var motionTimes = new double[stabilizationStart + 1]; + for (int index = 0; index < motionTimes.Length; index++) + motionTimes[index] = times[index]; + LongitudinalCandidate motion = LongitudinalCandidate.Integrate(motionTimes, 0d, + input.InitialProgressSpeedMetersPerSecond, input.InitialAccelerationMetersPerSecondSquared, jerk); + int terminalIndex = motion.S.Count - 1; + double[] correction = + { + -motion.A[terminalIndex], + -motion.U[terminalIndex], + input.StopBoundaryPathS - motion.S[terminalIndex], + }; + var influence = new double[3, 3]; + for (int basisIndex = 0; basisIndex < 3; basisIndex++) + { + var basis = new double[stabilizationStart]; + basis[stabilizationStart - 3 + basisIndex] = 1d; + LongitudinalCandidate response = LongitudinalCandidate.Integrate(motionTimes, 0d, 0d, 0d, basis); + influence[0, basisIndex] = response.A[terminalIndex]; + influence[1, basisIndex] = response.U[terminalIndex]; + influence[2, basisIndex] = response.S[terminalIndex]; + } + if (!TrySolveThreeByThree(influence, correction, out double[] adjustment)) + return false; + for (int index = 0; index < adjustment.Length; index++) + jerk[stabilizationStart - 3 + index] += adjustment[index]; + candidate = CreateExactCandidate(input, times, stabilizationStart, jerk); + return true; + } + private static LongitudinalCandidate CreateScheduleReferenceSeed(LongitudinalPlanningInput input, IReadOnlyList times, PathSpeedLimit speedLimit) { diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs index 1f8082e..fd845ea 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs @@ -19,6 +19,7 @@ internal static class EmPlanningServiceChecks VerifiesFullScopePublishesItsRequestScope(); VerifiesRequestAndStateFailuresPublishNoTrajectory(); VerifiesProjectionCorridorAndOptimizationFailuresPublishNoTrajectory(); + VerifiesNoProgressPublishesNoTrajectory(); VerifiesTimeoutFallbackAndCancellationSemantics(); VerifiesPublicationFailureAndDebugIsolation(); } @@ -396,6 +397,22 @@ internal static class EmPlanningServiceChecks } } + private static void VerifiesNoProgressPublishesNoTrajectory() + { + EmPlanningRequest request = CreateRequest(TravelDirection.Forward, 0d, false, false, + CreateReferencePath(TravelDirection.Forward, false, 0.10d), null, + EmPlanningScope.FullDirectionSegment); + request.Configuration.Scheduling.MaximumOptimizationSpatialStepMeters = 1d; + request.Configuration.Scheduling.MaximumOptimizationTimeStepSeconds = 1d; + + EmPlanningResult result = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.NoProgress)).Plan( + request, CancellationToken.None); + + VerifyFailure(result, EmPlanningStatus.NoProgress, "full nonterminal no progress"); + Verification.True(result.FailureReason.IndexOf("NoProgress", StringComparison.Ordinal) >= 0, + "no-progress failure preserves its diagnostic"); + } + private static void VerifiesPublicationFailureAndDebugIsolation() { EmPlanningRequest validationFailure = CreateRequest(TravelDirection.Forward, 0d, false, false); @@ -610,6 +627,7 @@ internal static class EmPlanningServiceChecks SolverUnavailable, TimeoutWithoutFallback, TimeoutWithFallback, + NoProgress, PublicationValidationFailure, } @@ -647,6 +665,8 @@ internal static class EmPlanningServiceChecks LastLongitudinalProblem = problem; if (mode == PipelineSolverMode.LongitudinalInfeasible) return Result(QpSolveStatus.PrimalInfeasible, Array.Empty()); + if (mode == PipelineSolverMode.NoProgress) + return Result(QpSolveStatus.Solved, new double[problem.VariableCount]); if (strictFullPrimal != null && strictFullPrimal.Count == problem.VariableCount) { longitudinalCallCount++; diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs index fb42496..26f703d 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs @@ -14,6 +14,7 @@ internal static class LongitudinalIntegrationChecks { VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed(); VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence(); + VerifiesFullDirectionStaticStartMakesProgress(); VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold(); VerifiesExactStopIncludesAStabilizationTail(); VerifiesLastStrictCandidateSurvivesLaterTimeout(); @@ -109,6 +110,46 @@ internal static class LongitudinalIntegrationChecks "halving publication cadence doubles emitted trajectory intervals without changing optimization knots"); } + private static void VerifiesFullDirectionStaticStartMakesProgress() + { + EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault(); + configuration.Scheduling.SolverTimeoutSeconds = 1d; + configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond = 1d; + configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d; + configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1d; + configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d; + configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1d; + LateralPath path = new LateralPath(new[] + { + Point(0d, 0d, 0d), + Point(2.5d, 2.5d, 0d), + Point(5d, 5d, 0d), + }, true); + + EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0d, + EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason); + Verification.Equal(EmPlanningStatus.Success, status, "static-start envelope: " + failureReason); + status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0d, 0d, + configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration, + out LongitudinalKnotSchedule schedule, out failureReason); + Verification.Equal(EmPlanningStatus.Success, status, "static-start schedule: " + failureReason); + + var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d, + EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration, + EmPlanningScope.FullDirectionSegment, schedule, Array.Empty(), Array.Empty()); + LongitudinalPlanningResult result = new LongitudinalPlanner(new OsqpNativeSolver()).Plan(input, + CancellationToken.None); + + Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback, + "static start succeeds: " + result.FailureReason); + LongitudinalCandidate candidate = result.Candidate ?? + throw new InvalidOperationException("static start must expose a successful candidate."); + Verification.True(HasValueGreaterThan(candidate.S, 0.05d), "static start makes measurable progress"); + Verification.True(HasValueGreaterThan(candidate.U, 0.05d), "static start accelerates"); + Verification.NearlyEqual(0d, candidate.U[candidate.U.Count - 1], + "full segment stops at the terminal boundary"); + } + private static void VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold() { EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault(); @@ -191,6 +232,16 @@ internal static class LongitudinalIntegrationChecks return count; } + private static bool HasValueGreaterThan(IReadOnlyList values, double threshold) + { + for (int index = 0; index < values.Count; index++) + { + if (values[index] > threshold) + return true; + } + return false; + } + public static void RunRealOsqp() { foreach (LongitudinalScenario scenario in CreateRealOsqpScenarios()) diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs index ddbabf9..f4aa36d 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs @@ -24,6 +24,7 @@ internal static class LongitudinalModelChecks VerifiesFullDirectionInitialFeasibilityProjectionAndFallbackSemantics(); VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints(); VerifiesModeSpecificSolutionValidation(); + VerifiesFullDirectionNoProgressValidation(); VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically(); } @@ -196,9 +197,10 @@ internal static class LongitudinalModelChecks out string failureReason); Verification.Equal(EmPlanningStatus.Success, status, "jerk-limited stopping-tail status: " + failureReason); double nearBoundaryPathS = input.StopBoundaryPathS - 0.005d; - Verification.NearlyEqual(MaximumJerkLimitedStopSpeed(input, nearBoundaryPathS), - envelope.StoppingLimitAt(nearBoundaryPathS), - "near-boundary speed cap uses jerk-limited distance inversion"); + Verification.True(envelope.StoppingLimitAt(nearBoundaryPathS) > 0d && + envelope.StoppingLimitAt(nearBoundaryPathS) <= + MaximumJerkLimitedStopSpeed(input, nearBoundaryPathS) + 1e-12d, + "near-boundary speed cap conservatively interpolates jerk-limited distance inversion"); Verification.NearlyEqual(0d, envelope.StoppingLimitAt(input.StopBoundaryPathS), "real stop boundary keeps an exact zero stopping cap"); } @@ -608,10 +610,11 @@ internal static class LongitudinalModelChecks (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), + var envelopeRow = new Dictionary { { layout.U(1), 1d } }; + if (Math.Abs(envelopeSlope) > 1e-12d) + envelopeRow.Add(layout.S(1), -envelopeSlope); + Verification.Equal(1, CountBoundedRow(problem, envelopeRow, + -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"); @@ -724,6 +727,53 @@ internal static class LongitudinalModelChecks "approach failure identifies the jerk-limited stoppable set"); } + private static void VerifiesFullDirectionNoProgressValidation() + { + EmPlannerConfiguration configuration = CreateUnitScaleConfiguration(); + double[] times = { 0d, 0.10d, 0.20d, 0.30d, 0.50d }; + var fullSchedule = new LongitudinalKnotSchedule(times, + new[] { 0d, 0.04d, 0.08d, 0.10d, 0.10d }, new[] { 0d, 0.20d, 0.10d, 0d, 0d }, true, 3); + LateralPath path = CreateStraightPath(0.10d); + var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d, + EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration, + EmPlanningScope.FullDirectionSegment, fullSchedule, Array.Empty(), Array.Empty()); + EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit speedLimit, + out string speedFailure); + Verification.Equal(EmPlanningStatus.Success, speedStatus, "no-progress envelope: " + speedFailure); + LongitudinalCandidate stationary = LongitudinalCandidate.Integrate(times, 0d, 0d, 0d, + new[] { 0d, 0d, 0d, 0d }); + + var validator = new LongitudinalSolutionValidator(); + Verification.True(!validator.TryValidate(input, speedLimit, stationary, out _, out EmPlanningStatus failureStatus, + out string failureReason), "nonterminal stationary full segment is rejected"); + Verification.Equal(EmPlanningStatus.NoProgress, failureStatus, "stationary full segment has exact status"); + Verification.True(failureReason.IndexOf("NoProgress", StringComparison.Ordinal) >= 0, + "stationary full segment has exact diagnostic"); + Verification.True(new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()).TryBuild(input, + speedLimit, stationary, out QuadraticProgram fullProblem, out string buildFailure), + "full no-progress objective builds: " + buildFailure); + var fullLayout = new LongitudinalVariableLayout(times.Length); + double expectedReferenceLinearCost = -2d * configuration.Longitudinal.Weights.ReferenceSpeed * + fullSchedule.ReferenceSpeedMetersPerSecond[1] / + (input.DirectionMaximumSpeedMetersPerSecond * input.DirectionMaximumSpeedMetersPerSecond); + Verification.NearlyEqual(expectedReferenceLinearCost, fullProblem.LinearCost[fullLayout.U(1)], + "full direction objective tracks adaptive speed reference"); + + LateralPath nearTerminalPath = CreateStraightPath(0.02d); + var nearTerminalSchedule = new LongitudinalKnotSchedule(times, + new[] { 0d, 0.01d, 0.02d, 0.02d, 0.02d }, new[] { 0d, 0d, 0d, 0d, 0d }, true, 3); + var nearTerminalInput = new LongitudinalPlanningInput(nearTerminalPath, TravelDirection.Forward, 0d, 0d, + EmTerminalType.GearSwitch, EmLongitudinalMode.ExactStopAtBoundary, configuration, + EmPlanningScope.FullDirectionSegment, nearTerminalSchedule, Array.Empty(), Array.Empty()); + speedStatus = new PathSpeedLimitBuilder().Build(nearTerminalInput, out PathSpeedLimit nearTerminalLimit, + out speedFailure); + Verification.Equal(EmPlanningStatus.Success, speedStatus, "near-terminal no-progress envelope: " + speedFailure); + Verification.True(!validator.TryValidate(nearTerminalInput, nearTerminalLimit, stationary, out _, + out failureStatus, out _), "near-terminal fixture remains subject to its stop-tail contract"); + Verification.True(failureStatus != EmPlanningStatus.NoProgress, + "near-terminal gear-switch stop is not classified as no progress"); + } + private static void VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically() { LateralPath path = CreateStraightPath(1d); @@ -848,7 +898,7 @@ internal static class LongitudinalModelChecks LongitudinalConfiguration limits = input.Configuration.Longitudinal; return JerkLimitedStoppingMath.MaximumInitialSpeedForDistance( Math.Max(0d, input.StopBoundaryPathS - pathS), - limits.MaximumAccelerationMetersPerSecondSquared, + Math.Max(0d, input.InitialAccelerationMetersPerSecondSquared), limits.MaximumDecelerationMetersPerSecondSquared, limits.MaximumJerkMetersPerSecondCubed, input.DirectionMaximumSpeedMetersPerSecond);