From c01d0d5b47683f1c085775e66764fa796e67722d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Tue, 4 Aug 2026 09:21:07 +0800 Subject: [PATCH] feat: assemble longitudinal ST quadratic programs --- .../Longitudinal/LongitudinalCandidate.cs | 124 ++++++++++++ .../LongitudinalConstraintBuilder.cs | 188 ++++++++++++++++++ .../LongitudinalObjectiveBuilder.cs | 68 +++++++ .../LongitudinalPlanningResult.cs | 29 +++ .../LongitudinalVariableLayout.cs | 52 +++++ .../LongitudinalModelChecks.cs | 181 +++++++++++++++++ 6 files changed, 642 insertions(+) create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalCandidate.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalObjectiveBuilder.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningResult.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalVariableLayout.cs diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalCandidate.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalCandidate.cs new file mode 100644 index 0000000..831db45 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalCandidate.cs @@ -0,0 +1,124 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Immutable time-knot s/u/a/j iterate used for ST linearization and strict validation. +public sealed class LongitudinalCandidate +{ + public LongitudinalCandidate(IReadOnlyList knotTimes, IReadOnlyList s, IReadOnlyList u, + IReadOnlyList a, IReadOnlyList j) + { + KnotTimes = CopyTimes(knotTimes); + S = CopyValues(s, KnotTimes.Count, nameof(s)); + U = CopyValues(u, KnotTimes.Count, nameof(u)); + A = CopyValues(a, KnotTimes.Count, nameof(a)); + J = CopyValues(j, KnotTimes.Count - 1, nameof(j)); + } + + public IReadOnlyList KnotTimes { get; } + + public IReadOnlyList S { get; } + + public IReadOnlyList U { get; } + + public IReadOnlyList A { get; } + + public IReadOnlyList J { get; } + + public static LongitudinalCandidate Integrate(IReadOnlyList knotTimes, double initialS, double initialU, + double initialA, IReadOnlyList jerk) + { + IReadOnlyList times = CopyTimes(knotTimes); + if (!IsFinite(initialS) || !IsFinite(initialU) || !IsFinite(initialA)) + throw new ArgumentOutOfRangeException(nameof(initialS)); + IReadOnlyList copiedJerk = CopyValues(jerk, times.Count - 1, nameof(jerk)); + var s = new double[times.Count]; + var u = new double[times.Count]; + var a = new double[times.Count]; + s[0] = initialS; + u[0] = initialU; + a[0] = initialA; + for (int index = 0; index < copiedJerk.Count; index++) + { + double dt = times[index + 1] - times[index]; + double currentJerk = copiedJerk[index]; + a[index + 1] = a[index] + dt * currentJerk; + u[index + 1] = u[index] + dt * a[index] + 0.5d * dt * dt * currentJerk; + s[index + 1] = s[index] + dt * u[index] + 0.5d * dt * dt * a[index] + + dt * dt * dt * currentJerk / 6d; + } + return new LongitudinalCandidate(times, s, u, a, copiedJerk); + } + + public bool SatisfiesExactDiscreteDynamics(double tolerance) + { + if (!IsFinite(tolerance) || tolerance < 0d) + throw new ArgumentOutOfRangeException(nameof(tolerance)); + for (int index = 0; index < J.Count; index++) + { + double dt = KnotTimes[index + 1] - KnotTimes[index]; + if (Math.Abs(A[index + 1] - (A[index] + dt * J[index])) > tolerance || + Math.Abs(U[index + 1] - (U[index] + dt * A[index] + 0.5d * dt * dt * J[index])) > tolerance || + Math.Abs(S[index + 1] - (S[index] + dt * U[index] + 0.5d * dt * dt * A[index] + + dt * dt * dt * J[index] / 6d)) > tolerance) + { + return false; + } + } + return true; + } + + public static IReadOnlyList CreateKnotTimes(double timeHorizonSeconds, double outputTimeStepSeconds) + { + if (!IsFinite(timeHorizonSeconds) || !IsFinite(outputTimeStepSeconds) || timeHorizonSeconds <= 0d || + outputTimeStepSeconds <= 0d) + { + throw new ArgumentOutOfRangeException(nameof(timeHorizonSeconds)); + } + int intervalCount = checked((int)Math.Ceiling(timeHorizonSeconds / outputTimeStepSeconds)); + var times = new double[intervalCount + 1]; + for (int index = 0; index < intervalCount; index++) + times[index] = index * outputTimeStepSeconds; + times[intervalCount] = timeHorizonSeconds; + return new ReadOnlyCollection(times); + } + + private static IReadOnlyList CopyTimes(IReadOnlyList source) + { + if (source == null || source.Count < 2) + throw new ArgumentException("At least two strictly increasing 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) + throw new ArgumentException("Time knots must be finite and strictly increasing.", nameof(source)); + if (index == 0 && Math.Abs(source[index]) > 1e-12d) + throw new ArgumentException("The first time knot must be exact zero.", nameof(source)); + copy.Add(source[index]); + previous = source[index]; + } + return new ReadOnlyCollection(copy); + } + + private static IReadOnlyList CopyValues(IReadOnlyList source, int expectedCount, string parameterName) + { + if (source == null || source.Count != expectedCount) + throw new ArgumentException("Longitudinal values do not match the time-knot layout.", parameterName); + var copy = new List(source.Count); + for (int index = 0; index < source.Count; index++) + { + if (!IsFinite(source[index])) + throw new ArgumentOutOfRangeException(parameterName); + copy.Add(source[index]); + } + return new ReadOnlyCollection(copy); + } + + private static bool IsFinite(double value) + { + return !double.IsNaN(value) && !double.IsInfinity(value); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs new file mode 100644 index 0000000..1ba12ce --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs @@ -0,0 +1,188 @@ +using System; +using System.Collections.Generic; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Builds one normalized ST QP with exact constant-jerk integration and hard terminal conditions. +public sealed class LongitudinalConstraintBuilder +{ + private readonly LongitudinalObjectiveBuilder _objectiveBuilder; + + public LongitudinalConstraintBuilder(LongitudinalObjectiveBuilder objectiveBuilder) + { + _objectiveBuilder = objectiveBuilder ?? throw new ArgumentNullException(nameof(objectiveBuilder)); + } + + public bool TryBuild(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate iterate, + out QuadraticProgram problem, out string failureReason) + { + problem = null; + failureReason = string.Empty; + try + { + 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."); + + IReadOnlyList expectedTimes = LongitudinalCandidate.CreateKnotTimes( + input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds); + if (!HasMatchingTimes(iterate.KnotTimes, expectedTimes)) + throw new ArgumentException("The ST iterate time knots do not match the configured horizon."); + 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) + { + throw new ArgumentException("The ST iterate does not match the configured knot layout."); + } + if (!PathSpeedLimitBuilder.TryGetLimits(input, out double directionMaximum, out double maximumAcceleration, + out double maximumDeceleration, out double maximumJerk, out _, out _, out failureReason)) + { + return false; + } + if (input.InitialProgressSpeedMetersPerSecond > speedLimit.MaximumSpeedAt(0d) + 1e-12d || + input.InitialAccelerationMetersPerSecondSquared < -maximumDeceleration - 1e-12d || + input.InitialAccelerationMetersPerSecondSquared > maximumAcceleration + 1e-12d) + { + failureReason = "The initial ST state violates hard bounds."; + return false; + } + + 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 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) + throw new InvalidOperationException("ST constraint row accounting is inconsistent."); + problem = new QuadraticProgram(hessian.Build(), linearCost, constraints.Build(), lower, upper); + return true; + } + catch (ArgumentException exception) + { + failureReason = exception.Message; + return false; + } + } + + private static void AddVariableBounds(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, + LongitudinalCandidate iterate, LongitudinalVariableLayout layout, double maximumAcceleration, + double maximumDeceleration, double maximumJerk, SparseTripletBuilder constraints, IList lower, + IList upper, ref int row) + { + for (int index = 0; index < layout.KnotCount; index++) + { + if (iterate.S[index] < 0d || iterate.S[index] > input.TerminalPathS) + 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.U(index), 0d, + Math.Min(input.DirectionMaximumSpeedMetersPerSecond, speedLimit.MaximumSpeedAt(iterate.S[index])), ref row); + AddSingleVariableRow(constraints, lower, upper, layout.A(index), -maximumDeceleration, maximumAcceleration, + ref row); + } + for (int index = 0; index < layout.KnotCount - 1; index++) + AddSingleVariableRow(constraints, lower, upper, layout.J(index), -maximumJerk, maximumJerk, ref row); + } + + private static void AddMonotonicProgress(LongitudinalVariableLayout layout, SparseTripletBuilder constraints, + IList lower, IList upper, ref int row) + { + for (int index = 0; index < layout.KnotCount - 1; index++) + { + AddRow(constraints, lower, upper, row, new[] + { + new Coefficient(layout.S(index + 1), 1d), new Coefficient(layout.S(index), -1d), + }, 0d, QuadraticProgram.MaximumFiniteBound); + row++; + } + } + + private static void AddExactDynamics(IReadOnlyList times, LongitudinalVariableLayout layout, + SparseTripletBuilder constraints, IList lower, IList upper, ref int row) + { + for (int index = 0; index < layout.KnotCount - 1; index++) + { + double dt = times[index + 1] - times[index]; + AddRow(constraints, lower, upper, row, new[] + { + new Coefficient(layout.A(index + 1), 1d), new Coefficient(layout.A(index), -1d), + new Coefficient(layout.J(index), -dt), + }, 0d, 0d); + row++; + AddRow(constraints, lower, upper, row, new[] + { + new Coefficient(layout.U(index + 1), 1d), new Coefficient(layout.U(index), -1d), + new Coefficient(layout.A(index), -dt), new Coefficient(layout.J(index), -0.5d * dt * dt), + }, 0d, 0d); + row++; + AddRow(constraints, lower, upper, row, new[] + { + new Coefficient(layout.S(index + 1), 1d), new Coefficient(layout.S(index), -1d), + new Coefficient(layout.U(index), -dt), new Coefficient(layout.A(index), -0.5d * dt * dt), + new Coefficient(layout.J(index), -dt * dt * dt / 6d), + }, 0d, 0d); + row++; + } + } + + private static void AddExactStartAndTerminal(LongitudinalPlanningInput input, LongitudinalVariableLayout layout, + SparseTripletBuilder constraints, IList lower, IList upper, ref int row) + { + AddSingleVariableRow(constraints, lower, upper, layout.S(0), 0d, 0d, ref row); + AddSingleVariableRow(constraints, lower, upper, layout.U(0), input.InitialProgressSpeedMetersPerSecond, + 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 AddSingleVariableRow(SparseTripletBuilder constraints, IList lower, IList upper, + int variable, double minimum, double maximum, ref int row) + { + AddRow(constraints, lower, upper, row, new[] { new Coefficient(variable, 1d) }, minimum, maximum); + row++; + } + + private static void AddRow(SparseTripletBuilder constraints, IList lower, IList upper, int row, + IReadOnlyList coefficients, double minimum, double maximum) + { + for (int index = 0; index < coefficients.Count; index++) + constraints.Add(row, coefficients[index].Variable, coefficients[index].Value); + lower.Add(minimum); + upper.Add(maximum); + } + + private static bool HasMatchingTimes(IReadOnlyList actual, IReadOnlyList expected) + { + if (actual.Count != expected.Count) + return false; + for (int index = 0; index < expected.Count; index++) + { + if (Math.Abs(actual[index] - expected[index]) > 1e-12d) + return false; + } + return true; + } + + private readonly struct Coefficient + { + public Coefficient(int variable, double value) + { + Variable = variable; + Value = value; + } + + public int Variable { get; } + + public double Value { get; } + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalObjectiveBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalObjectiveBuilder.cs new file mode 100644 index 0000000..0172702 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalObjectiveBuilder.cs @@ -0,0 +1,68 @@ +using System; +using System.Collections.Generic; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Normalized 0.5*x'Px+q'x ST objective terms with no terminal-progress reward. +public sealed class LongitudinalObjectiveBuilder +{ + public void AddTerms(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalVariableLayout layout, + LongitudinalCandidate iterate, SparseTripletBuilder hessian, IList linearCost) + { + if (input == null || speedLimit == null || layout == null || iterate == null || hessian == null || linearCost == null || + linearCost.Count != layout.VariableCount || iterate.KnotTimes.Count != layout.KnotCount) + { + throw new ArgumentException("ST objective inputs do not match the variable layout."); + } + + LongitudinalConfiguration configuration = input.Configuration.Longitudinal; + LongitudinalWeights weights = configuration.Weights ?? throw new ArgumentException("Longitudinal weights are required."); + double speedScale = RequirePositive(input.DirectionMaximumSpeedMetersPerSecond, nameof(speedScale)); + double accelerationScale = RequirePositive(Math.Max(configuration.MaximumAccelerationMetersPerSecondSquared, + configuration.MaximumDecelerationMetersPerSecondSquared), nameof(accelerationScale)); + double jerkScale = RequirePositive(configuration.MaximumJerkMetersPerSecondCubed, nameof(jerkScale)); + double progressScale = input.TerminalPathS > 0d ? input.TerminalPathS : 1d; + + for (int index = 0; index < layout.KnotCount; index++) + { + AddSquaredResidual(hessian, linearCost, layout.U(index), speedLimit.MaximumSpeedAt(iterate.S[index]), + weights.ReferenceSpeed, speedScale); + AddSquaredResidual(hessian, linearCost, layout.A(index), 0d, weights.Acceleration, accelerationScale); + if (index < layout.KnotCount - 1 && index < input.PreviousPathS.Count) + { + AddSquaredResidual(hessian, linearCost, layout.S(index), input.PreviousPathS[index], + weights.PreviousTrajectory, progressScale); + AddSquaredResidual(hessian, linearCost, layout.U(index), input.PreviousProgressSpeedMetersPerSecond[index], + weights.PreviousTrajectory, speedScale); + } + } + for (int index = 0; index < layout.KnotCount - 1; index++) + AddSquaredResidual(hessian, linearCost, layout.J(index), 0d, weights.Jerk, jerkScale); + AddSquaredResidual(hessian, linearCost, layout.A(layout.KnotCount - 1), 0d, weights.TerminalAcceleration, + accelerationScale); + } + + private static void AddSquaredResidual(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)); + if (weight == 0d) + return; + double coefficient = 2d * weight / (scale * scale); + hessian.Add(variable, variable, coefficient); + linearCost[variable] += -coefficient * reference; + } + + private static double RequirePositive(double value, string name) + { + if (!IsFinite(value) || value <= 0d) + throw new ArgumentOutOfRangeException(name); + return value; + } + + private static bool IsFinite(double value) + { + return !double.IsNaN(value) && !double.IsInfinity(value); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningResult.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningResult.cs new file mode 100644 index 0000000..24b08ee --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningResult.cs @@ -0,0 +1,29 @@ +using System; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Immutable longitudinal solve result; only successful statuses may expose a candidate profile. +public sealed class LongitudinalPlanningResult +{ + public LongitudinalPlanningResult(EmPlanningStatus status, LongitudinalCandidate candidate, string failureReason) + { + if (!Enum.IsDefined(typeof(EmPlanningStatus), status)) + throw new ArgumentOutOfRangeException(nameof(status)); + bool successful = status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback; + if (successful && candidate == null) + throw new ArgumentException("Successful longitudinal results require a candidate.", nameof(candidate)); + if (!successful && candidate != null) + throw new ArgumentException("Failed longitudinal results cannot expose a candidate.", nameof(candidate)); + + Status = status; + Candidate = candidate == null ? null : new LongitudinalCandidate(candidate.KnotTimes, candidate.S, candidate.U, + candidate.A, candidate.J); + FailureReason = failureReason ?? string.Empty; + } + + public EmPlanningStatus Status { get; } + + public LongitudinalCandidate Candidate { get; } + + public string FailureReason { get; } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalVariableLayout.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalVariableLayout.cs new file mode 100644 index 0000000..781a1b1 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalVariableLayout.cs @@ -0,0 +1,52 @@ +using System; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Deterministic contiguous variable ranges for one time-domain longitudinal QP. +public sealed class LongitudinalVariableLayout +{ + public LongitudinalVariableLayout(int knotCount) + { + if (knotCount < 2) + throw new ArgumentOutOfRangeException(nameof(knotCount), "At least two time knots are required."); + + KnotCount = knotCount; + SStart = 0; + UStart = knotCount; + AStart = 2 * knotCount; + JStart = 3 * knotCount; + VariableCount = 4 * knotCount - 1; + } + + public int KnotCount { get; } + + public int SStart { get; } + + public int UStart { get; } + + public int AStart { get; } + + public int JStart { get; } + + public int VariableCount { get; } + + public int S(int knotIndex) { return RequireKnotIndex(knotIndex, SStart); } + + public int U(int knotIndex) { return RequireKnotIndex(knotIndex, UStart); } + + public int A(int knotIndex) { return RequireKnotIndex(knotIndex, AStart); } + + public int J(int intervalIndex) + { + if (intervalIndex < 0 || intervalIndex >= KnotCount - 1) + throw new ArgumentOutOfRangeException(nameof(intervalIndex)); + return JStart + intervalIndex; + } + + private int RequireKnotIndex(int knotIndex, int start) + { + if (knotIndex < 0 || knotIndex >= KnotCount) + throw new ArgumentOutOfRangeException(nameof(knotIndex)); + return start + knotIndex; + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs index 879bf89..26dea78 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs @@ -13,6 +13,7 @@ internal static class LongitudinalModelChecks VerifiesFinitePathSIndexedSpeedEnvelope(); VerifiesStoppingPrecheckBeforeQpAssembly(); VerifiesReferenceHorizonSelectionKeepsTheCurrentSegmentBoundary(); + VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints(); } private static void VerifiesFinitePathSIndexedSpeedEnvelope() @@ -101,6 +102,105 @@ internal static class LongitudinalModelChecks "rolling horizon remains within the current segment"); } + private static void VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints() + { + var layout = new LongitudinalVariableLayout(5); + Verification.Equal(19, layout.VariableCount, "ST variable count"); + for (int index = 0; index < 5; index++) + { + Verification.Equal(index, layout.S(index), "s index " + index); + Verification.Equal(5 + index, layout.U(index), "u index " + index); + Verification.Equal(10 + index, layout.A(index), "a index " + index); + } + for (int index = 0; index < 4; index++) + Verification.Equal(15 + index, layout.J(index), "j index " + index); + + double[] times = { 0d, 0.05d, 0.10d, 0.15d, 0.20d }; + double[] jerk = { 0.30d, -0.10d, 0.20d, -0.20d }; + LongitudinalCandidate integrated = LongitudinalCandidate.Integrate(times, 0d, 0.10d, 0.02d, jerk); + for (int index = 0; index < jerk.Length; index++) + { + double dt = times[index + 1] - times[index]; + Verification.NearlyEqual(integrated.A[index] + dt * integrated.J[index], integrated.A[index + 1], + "exact ST acceleration dynamics " + index); + Verification.NearlyEqual(integrated.U[index] + dt * integrated.A[index] + 0.5d * dt * dt * integrated.J[index], + integrated.U[index + 1], "exact ST speed dynamics " + index); + Verification.NearlyEqual(integrated.S[index] + dt * integrated.U[index] + + 0.5d * dt * dt * integrated.A[index] + dt * dt * dt * integrated.J[index] / 6d, + integrated.S[index + 1], "exact ST progress dynamics " + index); + } + Verification.True(integrated.SatisfiesExactDiscreteDynamics(1e-12d), "integrated ST candidate validates dynamics"); + + EmPlannerConfiguration configuration = CreateUnitScaleConfiguration(); + LateralPath path = CreatePath(new[] + { + new PathFixture(0d, 0d, 0d, 0d), + new PathFixture(1d, 1d, 0d, 0d), + new PathFixture(2d, 2d, 0d, 0d), + }); + var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0.02d, + EmTerminalType.Goal, configuration, new[] { 0d, 0.10d, 0.20d, 0.30d, 0.40d }, + new[] { 0.20d, 0.20d, 0.20d, 0.20d, 0.20d }); + EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope, + out string speedFailure); + Verification.Equal(EmPlanningStatus.Success, speedStatus, "unit-scale speed envelope: " + speedFailure); + + Verification.True(new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()).TryBuild(input, envelope, + integrated, out QuadraticProgram problem, out string failureReason), "ST QP builds: " + failureReason); + 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)), + "normalized acceleration P coefficient"); + Verification.NearlyEqual(20d, MatrixValue(problem.UpperTriangularP, layout.J(0), layout.J(0)), + "normalized jerk P coefficient"); + Verification.NearlyEqual(2.5d, MatrixValue(problem.UpperTriangularP, layout.S(0), layout.S(0)), + "normalized previous-S P coefficient"); + Verification.NearlyEqual(0d, MatrixValue(problem.UpperTriangularP, layout.S(4), layout.S(4)), + "fixed terminal S has no progress-reward coefficient"); + + FindSingleVariableBounds(problem, layout.S(0), out double sLower, out double sUpper); + Verification.NearlyEqual(0d, sLower, "S lower bound"); + 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"); + 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"); + FindSingleVariableBounds(problem, layout.J(1), out double jLower, out double jUpper); + Verification.NearlyEqual(-1d, jLower, "jerk lower bound"); + Verification.NearlyEqual(1d, jUpper, "jerk upper bound"); + + Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary { { layout.S(0), 1d } }, 0d), + "exact initial S"); + Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary { { layout.U(0), 1d } }, 0.10d), + "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(1, CountBoundedRow(problem, new Dictionary + { + { layout.S(1), 1d }, { layout.S(0), -1d }, + }, 0d, QuadraticProgram.MaximumFiniteBound), "monotonic S hard constraint"); + Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary + { + { layout.A(1), 1d }, { layout.A(0), -1d }, { layout.J(0), -0.05d }, + }, 0d), "exact ST acceleration equation"); + Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary + { + { layout.U(1), 1d }, { layout.U(0), -1d }, { layout.A(0), -0.05d }, { layout.J(0), -0.00125d }, + }, 0d), "exact ST speed equation"); + Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary + { + { layout.S(1), 1d }, { layout.S(0), -1d }, { layout.U(0), -0.05d }, { layout.A(0), -0.00125d }, + { layout.J(0), -0.000020833333333333333d }, + }, 0d), "exact ST progress equation"); + } + private static LateralPath CreatePath(IReadOnlyList fixtures) { var points = new List(fixtures.Count); @@ -131,6 +231,87 @@ internal static class LongitudinalModelChecks SmoothedPathPointSource.Anchor); } + private static EmPlannerConfiguration CreateUnitScaleConfiguration() + { + EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault(); + configuration.Scheduling.TimeHorizonSeconds = 0.20d; + configuration.Scheduling.OutputTimeStepSeconds = 0.05d; + configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d; + configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d; + configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1d; + configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d; + configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1d; + configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d; + configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d; + return configuration; + } + + private static double MatrixValue(SparseCscMatrix matrix, int row, int column) + { + for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++) + { + if (matrix.RowIndices[index] == row) + return matrix.Values[index]; + } + return 0d; + } + + private static void FindSingleVariableBounds(QuadraticProgram problem, int variable, out double lower, out double upper) + { + for (int row = 0; row < problem.ConstraintCount; row++) + { + if (RowMatches(problem.ConstraintMatrix, row, new Dictionary { { variable, 1d } })) + { + lower = problem.LowerBounds[row]; + upper = problem.UpperBounds[row]; + return; + } + } + throw new InvalidOperationException("No single-variable bounds were found for variable " + variable + "."); + } + + private static int CountExactEqualityRows(QuadraticProgram problem, IReadOnlyDictionary expected, + double bound) + { + return CountBoundedRow(problem, expected, bound, bound); + } + + private static int CountBoundedRow(QuadraticProgram problem, IReadOnlyDictionary expected, + double lower, double upper) + { + int count = 0; + for (int row = 0; row < problem.ConstraintCount; row++) + { + if (Math.Abs(problem.LowerBounds[row] - lower) <= 1e-12d && + Math.Abs(problem.UpperBounds[row] - upper) <= 1e-12d && RowMatches(problem.ConstraintMatrix, row, expected)) + { + count++; + } + } + return count; + } + + private static bool RowMatches(SparseCscMatrix matrix, int targetRow, IReadOnlyDictionary expected) + { + var actual = new Dictionary(); + for (int column = 0; column < matrix.ColumnCount; column++) + { + for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++) + { + if (matrix.RowIndices[index] == targetRow) + actual[column] = matrix.Values[index]; + } + } + if (actual.Count != expected.Count) + return false; + foreach (KeyValuePair pair in expected) + { + if (!actual.TryGetValue(pair.Key, out double actualValue) || Math.Abs(actualValue - pair.Value) > 1e-12d) + return false; + } + return true; + } + private sealed class PathFixture { public PathFixture(double referenceS, double pathS, double curvature, double curvatureDerivative)