feat: assemble longitudinal ST quadratic programs

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalCandidate.cs

@@ -0,0 +1,124 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable time-knot s/u/a/j iterate used for ST linearization and strict validation.</summary>
+public sealed class LongitudinalCandidate
+{
+    public LongitudinalCandidate(IReadOnlyList<double> knotTimes, IReadOnlyList<double> s, IReadOnlyList<double> u,
+        IReadOnlyList<double> a, IReadOnlyList<double> 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<double> KnotTimes { get; }
+
+    public IReadOnlyList<double> S { get; }
+
+    public IReadOnlyList<double> U { get; }
+
+    public IReadOnlyList<double> A { get; }
+
+    public IReadOnlyList<double> J { get; }
+
+    public static LongitudinalCandidate Integrate(IReadOnlyList<double> knotTimes, double initialS, double initialU,
+        double initialA, IReadOnlyList<double> jerk)
+    {
+        IReadOnlyList<double> times = CopyTimes(knotTimes);
+        if (!IsFinite(initialS) || !IsFinite(initialU) || !IsFinite(initialA))
+            throw new ArgumentOutOfRangeException(nameof(initialS));
+        IReadOnlyList<double> 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<double> 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<double>(times);
+    }
+
+    private static IReadOnlyList<double> CopyTimes(IReadOnlyList<double> 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<double>(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<double>(copy);
+    }
+
+    private static IReadOnlyList<double> CopyValues(IReadOnlyList<double> 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<double>(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<double>(copy);
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs

@@ -0,0 +1,188 @@
+using System;
+using System.Collections.Generic;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Builds one normalized ST QP with exact constant-jerk integration and hard terminal conditions.</summary>
+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<double> 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<double>(8 * layout.KnotCount);
+            var upper = new List<double>(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<double> lower,
+        IList<double> 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<double> lower, IList<double> 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<double> times, LongitudinalVariableLayout layout,
+        SparseTripletBuilder constraints, IList<double> lower, IList<double> 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<double> lower, IList<double> 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<double> lower, IList<double> 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<double> lower, IList<double> upper, int row,
+        IReadOnlyList<Coefficient> 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<double> actual, IReadOnlyList<double> 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; }
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalObjectiveBuilder.cs

@@ -0,0 +1,68 @@
+using System;
+using System.Collections.Generic;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Normalized 0.5*x'Px+q'x ST objective terms with no terminal-progress reward.</summary>
+public sealed class LongitudinalObjectiveBuilder
+{
+    public void AddTerms(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalVariableLayout layout,
+        LongitudinalCandidate iterate, SparseTripletBuilder hessian, IList<double> 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<double> 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);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningResult.cs

@@ -0,0 +1,29 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable longitudinal solve result; only successful statuses may expose a candidate profile.</summary>
+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; }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalVariableLayout.cs

@@ -0,0 +1,52 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Deterministic contiguous variable ranges for one time-domain longitudinal QP.</summary>
+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;
+    }
+}

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<int, double> { { layout.S(0), 1d } }, 0d),
+            "exact initial S");
+        Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.U(0), 1d } }, 0.10d),
+            "exact initial U");
+        Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.A(0), 1d } }, 0.02d),
+            "exact initial A");
+        Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.S(4), 1d } }, 2d),
+            "exact terminal S");
+        Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double> { { layout.U(4), 1d } }, 0d),
+            "exact terminal U");
+        Verification.Equal(1, CountBoundedRow(problem, new Dictionary<int, double>
+        {
+            { layout.S(1), 1d }, { layout.S(0), -1d },
+        }, 0d, QuadraticProgram.MaximumFiniteBound), "monotonic S hard constraint");
+        Verification.Equal(1, CountExactEqualityRows(problem, new Dictionary<int, double>
+        {
+            { 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<int, double>
+        {
+            { 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<int, double>
+        {
+            { 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<PathFixture> fixtures)
     {
         var points = new List<LateralPathPoint>(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<int, double> { { 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<int, double> expected,
+        double bound)
+    {
+        return CountBoundedRow(problem, expected, bound, bound);
+    }
+
+    private static int CountBoundedRow(QuadraticProgram problem, IReadOnlyDictionary<int, double> 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<int, double> expected)
+    {
+        var actual = new Dictionary<int, double>();
+        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<int, double> 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)