diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs index 1ba12ce..040fc17 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs @@ -3,7 +3,7 @@ using System.Collections.Generic; namespace MultiWheelC.TrajectoryPlanning.EMPlanner; -/// Builds one normalized ST QP with exact constant-jerk integration and hard terminal conditions. +/// Builds one normalized ST QP with exact constant-jerk integration and mode-specific stop conditions. public sealed class LongitudinalConstraintBuilder { private readonly LongitudinalObjectiveBuilder _objectiveBuilder; @@ -22,8 +22,8 @@ public sealed class LongitudinalConstraintBuilder { if (input == null || speedLimit == null || iterate == null) throw new ArgumentException("ST input, speed envelope, and iterate are required."); - if (Math.Abs(speedLimit.TerminalPathS - input.TerminalPathS) > 1e-12d) - throw new ArgumentException("The speed envelope terminal must match actual lateral PathS."); + 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); @@ -51,16 +51,24 @@ 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); - var constraints = new SparseTripletBuilder(8 * layout.KnotCount, layout.VariableCount); - var lower = new List(8 * layout.KnotCount); - var upper = new List(8 * layout.KnotCount); + int stabilizationStart = input.Mode == EmLongitudinalMode.ExactStopAtBoundary + ? LongitudinalTerminalSchedule.GetStabilizationStartIndex(expectedTimes, + input.Configuration.Scheduling.OutputTimeStepSeconds) + : layout.KnotCount; + int stationaryKnotCount = layout.KnotCount - stabilizationStart; + int expectedRows = 8 * layout.KnotCount - 2 + 3 * stationaryKnotCount; + var constraints = new SparseTripletBuilder(expectedRows, layout.VariableCount); + var lower = new List(expectedRows); + var upper = new List(expectedRows); int row = 0; AddVariableBounds(input, speedLimit, iterate, layout, maximumAcceleration, maximumDeceleration, maximumJerk, constraints, lower, upper, ref row); AddMonotonicProgress(layout, constraints, lower, upper, ref row); AddExactDynamics(expectedTimes, layout, constraints, lower, upper, ref row); - AddExactStartAndTerminal(input, layout, constraints, lower, upper, ref row); - if (row != 8 * layout.KnotCount) + AddExactStart(input, layout, constraints, lower, upper, ref row); + if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary) + AddExactStopTail(input, layout, stabilizationStart, constraints, lower, upper, ref row); + if (row != expectedRows) throw new InvalidOperationException("ST constraint row accounting is inconsistent."); problem = new QuadraticProgram(hessian.Build(), linearCost, constraints.Build(), lower, upper); return true; @@ -79,9 +87,9 @@ public sealed class LongitudinalConstraintBuilder { for (int index = 0; index < layout.KnotCount; index++) { - if (iterate.S[index] < 0d || iterate.S[index] > input.TerminalPathS) + if (iterate.S[index] < 0d || iterate.S[index] > input.PathUpperBoundS) throw new ArgumentException("The ST iterate progress lies outside actual PathS bounds."); - AddSingleVariableRow(constraints, lower, upper, layout.S(index), 0d, input.TerminalPathS, ref row); + AddSingleVariableRow(constraints, lower, upper, layout.S(index), 0d, input.PathUpperBoundS, ref row); AddSingleVariableRow(constraints, lower, upper, layout.U(index), 0d, Math.Min(input.DirectionMaximumSpeedMetersPerSecond, speedLimit.MaximumSpeedAt(iterate.S[index])), ref row); AddSingleVariableRow(constraints, lower, upper, layout.A(index), -maximumDeceleration, maximumAcceleration, @@ -132,7 +140,7 @@ public sealed class LongitudinalConstraintBuilder } } - private static void AddExactStartAndTerminal(LongitudinalPlanningInput input, LongitudinalVariableLayout layout, + private static void AddExactStart(LongitudinalPlanningInput input, LongitudinalVariableLayout layout, SparseTripletBuilder constraints, IList lower, IList upper, ref int row) { AddSingleVariableRow(constraints, lower, upper, layout.S(0), 0d, 0d, ref row); @@ -140,9 +148,18 @@ public sealed class LongitudinalConstraintBuilder input.InitialProgressSpeedMetersPerSecond, ref row); AddSingleVariableRow(constraints, lower, upper, layout.A(0), input.InitialAccelerationMetersPerSecondSquared, input.InitialAccelerationMetersPerSecondSquared, ref row); - AddSingleVariableRow(constraints, lower, upper, layout.S(layout.KnotCount - 1), input.TerminalPathS, - input.TerminalPathS, ref row); - AddSingleVariableRow(constraints, lower, upper, layout.U(layout.KnotCount - 1), 0d, 0d, ref row); + } + + private static void AddExactStopTail(LongitudinalPlanningInput input, LongitudinalVariableLayout layout, + int stabilizationStart, SparseTripletBuilder constraints, IList lower, IList upper, ref int row) + { + for (int index = stabilizationStart; index < layout.KnotCount; index++) + { + AddSingleVariableRow(constraints, lower, upper, layout.S(index), input.StopBoundaryPathS, + input.StopBoundaryPathS, ref row); + AddSingleVariableRow(constraints, lower, upper, layout.U(index), 0d, 0d, ref row); + AddSingleVariableRow(constraints, lower, upper, layout.A(index), 0d, 0d, ref row); + } } private static void AddSingleVariableRow(SparseTripletBuilder constraints, IList lower, IList upper, diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs index eec8022..c365e57 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs @@ -56,13 +56,14 @@ public sealed class LongitudinalSolutionValidator double speed = candidate.U[index]; double acceleration = candidate.A[index]; if (!IsFinite(progress) || !IsFinite(speed) || !IsFinite(acceleration) || progress < -tolerance || - progress > input.TerminalPathS + tolerance || speed < -tolerance || + progress > input.PathUpperBoundS + tolerance || speed < -tolerance || acceleration < -maximumDeceleration - tolerance || acceleration > maximumAcceleration + tolerance) { failureReason = "ST candidate violates physical bounds at knot " + index + "."; return false; } - double speedLimitAtProgress = speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.TerminalPathS, progress))); + double speedLimitAtProgress = speedLimit.MaximumSpeedAt( + Math.Max(0d, Math.Min(input.PathUpperBoundS, progress))); if (speed > speedLimitAtProgress + tolerance) { failureReason = "ST candidate violates the actual-PathS speed envelope at knot " + index + @@ -83,12 +84,35 @@ public sealed class LongitudinalSolutionValidator return false; } } - int terminalIndex = candidate.S.Count - 1; - if (!AreClose(candidate.S[terminalIndex], input.TerminalPathS, tolerance) || - !AreClose(candidate.U[terminalIndex], 0d, tolerance)) + int stabilizationStart = candidate.S.Count; + if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary) { - failureReason = "ST candidate does not satisfy the exact zero-speed terminal."; - return false; + stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex( + candidate.KnotTimes, input.Configuration.Scheduling.OutputTimeStepSeconds); + for (int index = stabilizationStart; index < candidate.S.Count; index++) + { + if (!AreClose(candidate.S[index], input.StopBoundaryPathS, tolerance) || + !AreClose(candidate.U[index], 0d, tolerance) || + !AreClose(candidate.A[index], 0d, tolerance)) + { + failureReason = "ST candidate does not satisfy the exact stabilized S/U/A stop tail at knot " + + index + "."; + return false; + } + } + } + if (input.Mode != EmLongitudinalMode.RollingContinuation) + { + for (int index = 0; index < candidate.S.Count; index++) + { + if (!JerkLimitedStoppingMath.TryCalculate(candidate.U[index], candidate.A[index], + maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stop, out _) || + candidate.S[index] + stop.DistanceMeters > input.StopBoundaryPathS + tolerance) + { + failureReason = "ST candidate leaves the jerk-limited stoppable set at knot " + index + "."; + return false; + } + } } var canonicalS = new double[candidate.S.Count]; @@ -103,8 +127,15 @@ public sealed class LongitudinalSolutionValidator canonicalS[0] = 0d; canonicalU[0] = input.InitialProgressSpeedMetersPerSecond; canonicalA[0] = input.InitialAccelerationMetersPerSecondSquared; - canonicalS[terminalIndex] = input.TerminalPathS; - canonicalU[terminalIndex] = 0d; + if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary) + { + for (int index = stabilizationStart; index < candidate.S.Count; index++) + { + canonicalS[index] = input.StopBoundaryPathS; + canonicalU[index] = 0d; + canonicalA[index] = 0d; + } + } var canonicalCandidate = new LongitudinalCandidate(candidate.KnotTimes, canonicalS, canonicalU, canonicalA, candidate.J); if (!canonicalCandidate.SatisfiesExactDiscreteDynamics(tolerance)) diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs index 1942b1b..25bca36 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs @@ -19,6 +19,7 @@ internal static class LongitudinalModelChecks VerifiesStoppingPrecheckOnlyAppliesToRealStopBoundaries(); VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime(); VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints(); + VerifiesModeSpecificSolutionValidation(); } private static void VerifiesJerkLimitedStoppingProfileEndsAtRest() @@ -313,6 +314,17 @@ internal static class LongitudinalModelChecks Verification.True(new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()).TryBuild(input, envelope, integrated, out QuadraticProgram problem, out string failureReason), "ST QP builds: " + failureReason); + var rollingInput = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0.02d, + EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration, + new[] { 0d, 0.10d, 0.20d, 0.30d, 0.40d }, + new[] { 0.20d, 0.20d, 0.20d, 0.20d, 0.20d }); + EmPlanningStatus rollingSpeedStatus = new PathSpeedLimitBuilder().Build(rollingInput, + out PathSpeedLimit rollingEnvelope, out string rollingSpeedFailure); + Verification.Equal(EmPlanningStatus.Success, rollingSpeedStatus, + "unit-scale rolling speed envelope: " + rollingSpeedFailure); + Verification.True(new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()).TryBuild( + rollingInput, rollingEnvelope, integrated, out QuadraticProgram rollingProblem, out string rollingFailure), + "rolling ST QP builds: " + rollingFailure); Verification.NearlyEqual(30d, MatrixValue(problem.UpperTriangularP, layout.U(0), layout.U(0)), "normalized speed and previous-U P coefficient"); Verification.NearlyEqual(2d, MatrixValue(problem.UpperTriangularP, layout.A(0), layout.A(0)), @@ -344,10 +356,29 @@ internal static class LongitudinalModelChecks "exact initial U"); Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary { { layout.A(0), 1d } }, 0.02d), "exact initial A"); - Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary { { layout.S(4), 1d } }, 2d), - "exact terminal S"); - Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary { { layout.U(4), 1d } }, 0d), - "exact terminal U"); + Verification.Equal(0, CountExactEqualityRows(rollingProblem, + new Dictionary { { layout.S(4), 1d } }, rollingInput.PathUpperBoundS), + "rolling has no exact terminal S"); + Verification.Equal(0, CountExactEqualityRows(rollingProblem, + new Dictionary { { layout.U(4), 1d } }, 0d), + "rolling has no exact terminal U"); + Verification.Equal(0, CountExactEqualityRows(rollingProblem, + new Dictionary { { layout.A(4), 1d } }, 0d), + "rolling has no exact terminal A"); + int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex( + integrated.KnotTimes, configuration.Scheduling.OutputTimeStepSeconds); + for (int index = stabilizationStart; index < layout.KnotCount; index++) + { + Verification.Equal(1, CountExactEqualityRows(problem, + new Dictionary { { layout.S(index), 1d } }, input.StopBoundaryPathS), + "stop tail exact S " + index); + Verification.Equal(1, CountExactEqualityRows(problem, + new Dictionary { { layout.U(index), 1d } }, 0d), + "stop tail exact U " + index); + Verification.Equal(1, CountExactEqualityRows(problem, + new Dictionary { { layout.A(index), 1d } }, 0d), + "stop tail exact A " + index); + } Verification.Equal(1, CountBoundedRow(problem, new Dictionary { { layout.S(1), 1d }, { layout.S(0), -1d }, @@ -367,6 +398,62 @@ internal static class LongitudinalModelChecks }, 0d), "exact ST progress equation"); } + private static void VerifiesModeSpecificSolutionValidation() + { + EmPlannerConfiguration configuration = CreateTaskFourConfiguration(); + IReadOnlyList exactTimes = LongitudinalCandidate.CreateKnotTimes(0.30d, 0.10d); + LongitudinalCandidate nonstationaryExact = LongitudinalCandidate.Integrate( + exactTimes, 0d, 0.10d, 0d, new[] { -4d, 0d, 0d }); + LateralPath exactPath = CreateStraightPath(nonstationaryExact.S[nonstationaryExact.S.Count - 1]); + var exactInput = new LongitudinalPlanningInput(exactPath, TravelDirection.Forward, 0.10d, 0d, + EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration, + Array.Empty(), Array.Empty()); + var rollingForExact = new LongitudinalPlanningInput(exactPath, TravelDirection.Forward, 0.10d, 0d, + EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration, + Array.Empty(), Array.Empty()); + EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(rollingForExact, + out PathSpeedLimit rollingEnvelope, out string speedFailure); + Verification.Equal(EmPlanningStatus.Success, speedStatus, "exact-validation rolling envelope: " + speedFailure); + + var validator = new LongitudinalSolutionValidator(); + Verification.True(!validator.TryValidate(exactInput, rollingEnvelope, nonstationaryExact, + out _, out string exactFailure), "exact stops reject a nonstationary internal tail"); + Verification.True(exactFailure.IndexOf("exact stabilized", StringComparison.Ordinal) >= 0, + "exact-stop failure identifies the stabilized tail: " + exactFailure); + + LateralPath rollingPath = CreateStraightPath(0.10d); + var rollingInput = new LongitudinalPlanningInput(rollingPath, TravelDirection.Forward, 0.10d, 0d, + EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration, + Array.Empty(), Array.Empty()); + speedStatus = new PathSpeedLimitBuilder().Build(rollingInput, out PathSpeedLimit openEnvelope, out speedFailure); + Verification.Equal(EmPlanningStatus.Success, speedStatus, "rolling-validation envelope: " + speedFailure); + LongitudinalCandidate rollingCandidate = LongitudinalCandidate.Integrate( + exactTimes, 0d, 0.10d, 0d, new[] { 0d, 0d, 0d }); + Verification.True(validator.TryValidate(rollingInput, openEnvelope, rollingCandidate, + out _, out string rollingFailure), "rolling nonzero terminal speed validates: " + rollingFailure); + + EmPlannerConfiguration approachConfiguration = CreateTaskFourConfiguration(); + approachConfiguration.Scheduling.TimeHorizonSeconds = 0.15d; + approachConfiguration.Scheduling.OutputTimeStepSeconds = 0.05d; + IReadOnlyList approachTimes = LongitudinalCandidate.CreateKnotTimes(0.15d, 0.05d); + LongitudinalCandidate unstoppablyFastApproach = LongitudinalCandidate.Integrate( + approachTimes, 0d, 0.20d, 0d, new[] { 0d, -10d, 10d }); + LateralPath approachPath = CreateStraightPath(0.035d); + var approachInput = new LongitudinalPlanningInput(approachPath, TravelDirection.Forward, 0.20d, 0d, + EmTerminalType.Goal, EmLongitudinalMode.ApproachStopBoundary, approachConfiguration, + Array.Empty(), Array.Empty()); + var rollingForApproach = new LongitudinalPlanningInput(approachPath, TravelDirection.Forward, 0.20d, 0d, + EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, approachConfiguration, + Array.Empty(), Array.Empty()); + speedStatus = new PathSpeedLimitBuilder().Build(rollingForApproach, + out PathSpeedLimit approachEnvelope, out speedFailure); + Verification.Equal(EmPlanningStatus.Success, speedStatus, "approach-validation rolling envelope: " + speedFailure); + Verification.True(!validator.TryValidate(approachInput, approachEnvelope, unstoppablyFastApproach, + out _, out string approachFailure), "approach candidates outside the stoppable set are rejected"); + Verification.True(approachFailure.IndexOf("stoppable set", StringComparison.Ordinal) >= 0, + "approach failure identifies the jerk-limited stoppable set"); + } + private static LateralPath CreatePath(IReadOnlyList fixtures) { var points = new List(fixtures.Count); @@ -412,6 +499,30 @@ internal static class LongitudinalModelChecks return configuration; } + private static EmPlannerConfiguration CreateTaskFourConfiguration() + { + EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault(); + configuration.Scheduling.TimeHorizonSeconds = 0.30d; + configuration.Scheduling.OutputTimeStepSeconds = 0.10d; + configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d; + configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d; + configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d; + configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d; + configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 10d; + configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d; + configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d; + return configuration; + } + + private static LateralPath CreateStraightPath(double pathUpperBoundS) + { + return CreatePath(new[] + { + new PathFixture(0d, 0d, 0d, 0d), + new PathFixture(pathUpperBoundS, pathUpperBoundS, 0d, 0d), + }); + } + private static double MaximumJerkLimitedStopSpeed(LongitudinalPlanningInput input, double pathS) { LongitudinalConfiguration limits = input.Configuration.Longitudinal;