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)