feat: derive adaptive full-segment ST schedule

2026-08-06 23:54:34 +08:00
parent dad4ff4b04
commit 3269d556b6
14 changed files with 1642 additions and 108 deletions
@@ -109,7 +109,9 @@ internal static class EmPlanningServiceChecks
            CreateReferencePath(TravelDirection.Forward, false, 0.0075d), null,
            EmPlanningScope.FullDirectionSegment);
        ConfigureExactStopServiceScenario(request.Configuration);
-        EmPlanningResult result = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(
+        double[] strictFullPrimal = CreateStrictFullScopePrimal(request.Configuration);
+        EmPlanningResult result = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success, null,
+            strictFullPrimal)).Plan(
            request, CancellationToken.None);
        VerifySuccess(result, request, EmTerminalType.Goal, "full scope publication");
        Verification.Equal(EmPlanningScope.FullDirectionSegment, result.Trajectory.Metadata.PlanningScope,
@@ -127,9 +129,9 @@ internal static class EmPlanningServiceChecks
        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.2d;
    }

-    private static void ConfigureExactStopServiceScenario(EmPlannerConfiguration configuration)
-    {
-        configuration.Scheduling.TimeHorizonSeconds = 0.40d;
+        private static void ConfigureExactStopServiceScenario(EmPlannerConfiguration configuration)
+        {
+            configuration.Scheduling.TimeHorizonSeconds = 0.40d;
        configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
        configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
@@ -137,8 +139,171 @@ internal static class EmPlanningServiceChecks
        configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
        configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 20d;
        configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
-        configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
-    }
+            configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
+        }
+
+        private static double[] CreateStrictFullScopePrimal(EmPlannerConfiguration configuration)
+        {
+            var path = new LateralPath(new[]
+            {
+                new LateralPathPoint(0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d),
+                new LateralPathPoint(0.0075d, 0.0075d, 0d, 0d, 0d, 0d, 0.0075d, 0d, 0d, 0d, 0d, 0d),
+            }, true);
+            EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.05d,
+                EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
+            Verification.Equal(EmPlanningStatus.Success, status, "full scope test envelope: " + failureReason);
+            status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.05d, 0d,
+                configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
+                out LongitudinalKnotSchedule schedule, out failureReason);
+            Verification.Equal(EmPlanningStatus.Success, status, "full scope test schedule: " + failureReason);
+            var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.05d, 0d,
+                EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
+                EmPlanningScope.FullDirectionSegment, schedule, Array.Empty<double>(), Array.Empty<double>());
+            int motionIntervalCount = schedule.TerminalHoldStartIndex;
+            Verification.True(motionIntervalCount >= 3, "full scope test schedule has three motion intervals");
+            int terminalFirstInterval = motionIntervalCount - 3;
+            var terminalTimes = new double[4];
+            for (int index = 1; index < terminalTimes.Length; index++)
+                terminalTimes[index] = terminalTimes[index - 1] +
+                    schedule.KnotTimes[terminalFirstInterval + index] -
+                    schedule.KnotTimes[terminalFirstInterval + index - 1];
+            var motionTimes = new double[motionIntervalCount + 1];
+            for (int index = 0; index < motionTimes.Length; index++)
+                motionTimes[index] = schedule.KnotTimes[index];
+            var influence = new double[3, 3];
+            for (int interval = 0; interval < 3; interval++)
+            {
+                var basis = new double[3];
+                basis[interval] = 1d;
+                LongitudinalCandidate response = LongitudinalCandidate.Integrate(terminalTimes, 0d, 0d, 0d, basis);
+                int terminalIndex = response.S.Count - 1;
+                influence[0, interval] = response.A[terminalIndex];
+                influence[1, interval] = response.U[terminalIndex];
+                influence[2, interval] = response.S[terminalIndex];
+            }
+            var validator = new LongitudinalSolutionValidator();
+            for (int firstJerkStep = -20; firstJerkStep <= 0; firstJerkStep++)
+            {
+                for (int secondJerkStep = terminalFirstInterval >= 2 ? -20 : 0;
+                    secondJerkStep <= (terminalFirstInterval >= 2 ? 20 : 0); secondJerkStep++)
+                {
+                    for (int thirdJerkStep = terminalFirstInterval >= 3 ? -20 : 0;
+                        thirdJerkStep <= (terminalFirstInterval >= 3 ? 20 : 0); thirdJerkStep++)
+                    {
+                        var jerk = new double[motionIntervalCount];
+                        jerk[0] = firstJerkStep;
+                        if (terminalFirstInterval >= 2)
+                            jerk[1] = secondJerkStep;
+                        if (terminalFirstInterval >= 3)
+                            jerk[2] = thirdJerkStep;
+                        LongitudinalCandidate baseline = LongitudinalCandidate.Integrate(motionTimes, 0d, 0.05d,
+                            0d, jerk);
+                        double[] target =
+                        {
+                            -baseline.A[baseline.A.Count - 1],
+                            -baseline.U[baseline.U.Count - 1],
+                            0.0075d - baseline.S[baseline.S.Count - 1],
+                        };
+                        if (!TrySolveThreeByThree(influence, target, out double[] terminalJerk))
+                            throw new InvalidOperationException("Full scope strict candidate terminal system is singular.");
+                        for (int interval = 0; interval < 3; interval++)
+                            jerk[terminalFirstInterval + interval] = terminalJerk[interval];
+                        LongitudinalCandidate motion = LongitudinalCandidate.Integrate(motionTimes, 0d, 0.05d, 0d,
+                            jerk);
+                        LongitudinalCandidate candidate = AppendFullStopTail(schedule.KnotTimes, motionIntervalCount,
+                            motion);
+                        if (!validator.TryValidate(input, speedLimit, candidate, out LongitudinalCandidate strict, out _))
+                            continue;
+                        return ToPrimal(strict);
+                    }
+                }
+            }
+            throw new InvalidOperationException("Unable to construct a strict full-scope test candidate: hold=" +
+                motionIntervalCount + ";times=" + string.Join(",", schedule.KnotTimes));
+        }
+
+        private static LongitudinalCandidate AppendFullStopTail(IReadOnlyList<double> times, int motionIntervalCount,
+            LongitudinalCandidate motion)
+        {
+            var pathS = new double[times.Count];
+            var speed = new double[times.Count];
+            var acceleration = new double[times.Count];
+            var jerk = new double[times.Count - 1];
+            for (int index = 0; index <= motionIntervalCount; index++)
+            {
+                pathS[index] = index == motionIntervalCount ? 0.0075d : motion.S[index];
+                speed[index] = index == motionIntervalCount ? 0d : motion.U[index];
+                acceleration[index] = index == motionIntervalCount ? 0d : motion.A[index];
+            }
+            for (int index = motionIntervalCount + 1; index < times.Count; index++)
+                pathS[index] = 0.0075d;
+            for (int index = 0; index < motion.J.Count; index++)
+                jerk[index] = motion.J[index];
+            return new LongitudinalCandidate(times, pathS, speed, acceleration, jerk);
+        }
+
+        private static double[] ToPrimal(LongitudinalCandidate candidate)
+        {
+            var layout = new LongitudinalVariableLayout(candidate.S.Count);
+            var primal = new double[layout.VariableCount];
+            for (int index = 0; index < candidate.S.Count; index++)
+            {
+                primal[layout.S(index)] = candidate.S[index];
+                primal[layout.U(index)] = candidate.U[index];
+                primal[layout.A(index)] = candidate.A[index];
+            }
+            for (int index = 0; index < candidate.J.Count; index++)
+                primal[layout.J(index)] = candidate.J[index];
+            return primal;
+        }
+
+        private static bool TrySolveThreeByThree(double[,] matrix, IReadOnlyList<double> rightHandSide,
+            out double[] solution)
+        {
+            var augmented = new double[3, 4];
+            for (int row = 0; row < 3; row++)
+            {
+                for (int column = 0; column < 3; column++)
+                    augmented[row, column] = matrix[row, column];
+                augmented[row, 3] = rightHandSide[row];
+            }
+            for (int column = 0; column < 3; column++)
+            {
+                int pivot = column;
+                for (int row = column + 1; row < 3; row++)
+                {
+                    if (Math.Abs(augmented[row, column]) > Math.Abs(augmented[pivot, column]))
+                        pivot = row;
+                }
+                if (Math.Abs(augmented[pivot, column]) < 1e-12d)
+                {
+                    solution = Array.Empty<double>();
+                    return false;
+                }
+                if (pivot != column)
+                {
+                    for (int index = column; index < 4; index++)
+                    {
+                        double temporary = augmented[column, index];
+                        augmented[column, index] = augmented[pivot, index];
+                        augmented[pivot, index] = temporary;
+                    }
+                }
+                double divisor = augmented[column, column];
+                for (int index = column; index < 4; index++)
+                    augmented[column, index] /= divisor;
+                for (int row = 0; row < 3; row++)
+                {
+                    if (row == column)
+                        continue;
+                    double factor = augmented[row, column];
+                    for (int index = column; index < 4; index++)
+                        augmented[row, index] -= factor * augmented[column, index];
+                }
+            }
+            solution = new[] { augmented[0, 3], augmented[1, 3], augmented[2, 3] };
+            return true;
+        }

    private static void AssertExactStopStabilization(EmTrajectory trajectory, EmBoundaryType boundaryType, string name)
    {
@@ -452,20 +617,23 @@ internal static class EmPlanningServiceChecks
    {
        private readonly PipelineSolverMode mode;
        private readonly PlanningGridMap? mapToCorrupt;
+        private readonly IReadOnlyList<double>? strictFullPrimal;
        private int longitudinalCallCount;

        public QuadraticProgram? LastLongitudinalProblem { get; private set; }

-        public ScriptedPipelineSolver(PipelineSolverMode mode, PlanningGridMap? mapToCorrupt = null)
+        public ScriptedPipelineSolver(PipelineSolverMode mode, PlanningGridMap? mapToCorrupt = null,
+            IReadOnlyList<double>? strictFullPrimal = null)
        {
            this.mode = mode;
            this.mapToCorrupt = mapToCorrupt;
+            this.strictFullPrimal = strictFullPrimal;
        }

        public QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings, IReadOnlyList<double> warmStart,
            CancellationToken cancellationToken)
        {
-            bool longitudinal = problem.VariableCount > 100;
+            bool longitudinal = IsLongitudinalProblem(problem);
            if (mode == PipelineSolverMode.SolverUnavailable)
                return Result(QpSolveStatus.SolverUnavailable, Array.Empty<double>());
            if (!longitudinal)
@@ -479,12 +647,18 @@ internal static class EmPlanningServiceChecks
            LastLongitudinalProblem = problem;
            if (mode == PipelineSolverMode.LongitudinalInfeasible)
                return Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>());
+            if (strictFullPrimal != null && strictFullPrimal.Count == problem.VariableCount)
+            {
+                longitudinalCallCount++;
+                return Result(QpSolveStatus.Solved, strictFullPrimal);
+            }
            if (mode == PipelineSolverMode.PublicationValidationFailure && longitudinalCallCount == 0)
                CorruptMapAtOrigin(mapToCorrupt);
            if (mode == PipelineSolverMode.TimeoutWithFallback && ++longitudinalCallCount > 1)
                return Result(QpSolveStatus.TimeLimit, Array.Empty<double>());
            longitudinalCallCount++;
-            return Result(QpSolveStatus.Solved, warmStart);
+            return Result(QpSolveStatus.Solved,
+                TryCreateStrictExactStopPrimal(problem, out double[] strictPrimal) ? strictPrimal : warmStart);
        }

        private static void CorruptMapAtOrigin(PlanningGridMap? map)
@@ -502,41 +676,234 @@ internal static class EmPlanningServiceChecks
            distances[index] = 0d;
        }

-        private static double[] CreateStrictLongitudinalPrimal(QuadraticProgram problem)
+        private static bool TryCreateStrictExactStopPrimal(QuadraticProgram problem, out double[] primal)
        {
+            primal = Array.Empty<double>();
            int variableCount = problem.VariableCount;
            int knotCount = (variableCount + 1) / 4;
            var layout = new LongitudinalVariableLayout(knotCount);
-            var jerk = new double[knotCount - 1];
-            const int rampIntervals = 5;
-            for (int index = 0; index < rampIntervals; index++) jerk[index] = 1d;
-            for (int index = rampIntervals; index < 3 * rampIntervals; index++) jerk[index] = -1d;
-            for (int index = 3 * rampIntervals; index < 4 * rampIntervals; index++) jerk[index] = 1d;
+            int stabilizationStart = FindExactStopTailStart(problem, layout);
+            if (stabilizationStart < 3)
+                return false;
            var times = new double[knotCount];
-            for (int index = 0; index < times.Length; index++) times[index] = index * 0.05d;
-            LongitudinalCandidate baseCandidate = LongitudinalCandidate.Integrate(times, 0d, 0d, 0d, jerk);
-            double terminalPathS = ReadFixedVariable(problem, layout.S(knotCount - 1));
-            double scale = terminalPathS / baseCandidate.S[baseCandidate.S.Count - 1];
-            for (int index = 0; index < jerk.Length; index++) jerk[index] *= scale;
-            LongitudinalCandidate candidate = LongitudinalCandidate.Integrate(times, 0d, 0d, 0d, jerk);
+            for (int index = 0; index < knotCount - 1; index++)
+            {
+                if (!TryReadDynamicsDuration(problem, layout, index, out double duration))
+                    return false;
+                times[index + 1] = times[index] + duration;
+            }
+
+            var motionTimes = new double[stabilizationStart + 1];
+            Array.Copy(times, motionTimes, motionTimes.Length);
+            double initialPathS = ReadFixedVariable(problem, layout.S(0));
+            double initialSpeed = ReadFixedVariable(problem, layout.U(0));
+            double initialAcceleration = ReadFixedVariable(problem, layout.A(0));
+            double terminalPathS = ReadFixedVariable(problem, layout.S(stabilizationStart));
+            var preferredJerk = new double[stabilizationStart];
+            LongitudinalCandidate baseline = LongitudinalCandidate.Integrate(motionTimes, initialPathS, initialSpeed,
+                initialAcceleration, preferredJerk);
+            var influence = new double[3, stabilizationStart];
+            for (int interval = 0; interval < stabilizationStart; interval++)
+            {
+                var basis = new double[stabilizationStart];
+                basis[interval] = 1d;
+                LongitudinalCandidate response = LongitudinalCandidate.Integrate(motionTimes, 0d, 0d, 0d, basis);
+                int terminalIndex = response.S.Count - 1;
+                influence[0, interval] = response.A[terminalIndex];
+                influence[1, interval] = response.U[terminalIndex];
+                influence[2, interval] = response.S[terminalIndex];
+            }
+            double[] target =
+            {
+                -baseline.A[baseline.A.Count - 1],
+                -baseline.U[baseline.U.Count - 1],
+                terminalPathS - baseline.S[baseline.S.Count - 1],
+            };
+            var jerk = new double[knotCount - 1];
+            if (stabilizationStart >= 4)
+            {
+                int terminalFirstInterval = stabilizationStart - 3;
+                var terminalInfluence = new double[3, 3];
+                for (int row = 0; row < 3; row++)
+                {
+                    for (int column = 0; column < 3; column++)
+                        terminalInfluence[row, column] = influence[row, terminalFirstInterval + column];
+                }
+                if (!TrySolveThreeByThree(terminalInfluence, target, out double[] terminalJerk))
+                    return false;
+                for (int interval = 0; interval < 3; interval++)
+                    jerk[terminalFirstInterval + interval] = terminalJerk[interval];
+            }
+            else
+            {
+                var gram = new double[3, 3];
+                for (int row = 0; row < 3; row++)
+                {
+                    for (int column = 0; column < 3; column++)
+                    {
+                        for (int interval = 0; interval < stabilizationStart; interval++)
+                            gram[row, column] += influence[row, interval] * influence[column, interval];
+                    }
+                }
+                if (!TrySolveThreeByThree(gram, target, out double[] multipliers))
+                    return false;
+                for (int interval = 0; interval < stabilizationStart; interval++)
+                {
+                    jerk[interval] = preferredJerk[interval];
+                    for (int row = 0; row < 3; row++)
+                        jerk[interval] += influence[row, interval] * multipliers[row];
+                }
+            }
+            var motionJerk = new double[stabilizationStart];
+            Array.Copy(jerk, motionJerk, motionJerk.Length);
+            LongitudinalCandidate candidate = LongitudinalCandidate.Integrate(motionTimes, initialPathS, initialSpeed,
+                initialAcceleration, motionJerk);
+            primal = CreateExactStopPrimal(layout, knotCount, stabilizationStart, terminalPathS, candidate);
+            return true;
+        }
+
+        private static double[] CreateExactStopPrimal(LongitudinalVariableLayout layout, int knotCount,
+            int stabilizationStart, double terminalPathS, LongitudinalCandidate candidate)
+        {
            var primal = new double[layout.VariableCount];
            for (int index = 0; index < knotCount; index++)
            {
-                primal[layout.S(index)] = candidate.S[index];
-                primal[layout.U(index)] = candidate.U[index];
-                primal[layout.A(index)] = candidate.A[index];
-            }
-            for (int index = 0; index < jerk.Length; index++) primal[layout.J(index)] = candidate.J[index];
-            for (int index = 4 * rampIntervals; index < knotCount; index++)
-            {
-                primal[layout.S(index)] = terminalPathS;
-                primal[layout.U(index)] = 0d;
-                primal[layout.A(index)] = 0d;
+                bool isTerminalTail = index >= stabilizationStart;
+                primal[layout.S(index)] = isTerminalTail ? terminalPathS : candidate.S[index];
+                primal[layout.U(index)] = isTerminalTail ? 0d : candidate.U[index];
+                primal[layout.A(index)] = isTerminalTail ? 0d : candidate.A[index];
            }
+            for (int index = 0; index < candidate.J.Count; index++)
+                primal[layout.J(index)] = candidate.J[index];
            return primal;
        }

+        private static int FindExactStopTailStart(QuadraticProgram problem, LongitudinalVariableLayout layout)
+        {
+            for (int index = 1; index < layout.KnotCount; index++)
+            {
+                if (TryReadFixedVariable(problem, layout.S(index), out _) &&
+                    TryReadFixedVariable(problem, layout.U(index), out _) &&
+                    TryReadFixedVariable(problem, layout.A(index), out _))
+                {
+                    return index;
+                }
+            }
+            return -1;
+        }
+
+        private static bool TryReadDynamicsDuration(QuadraticProgram problem, LongitudinalVariableLayout layout,
+            int interval, out double duration)
+        {
+            duration = 0d;
+            for (int row = 0; row < problem.ConstraintCount; row++)
+            {
+                if (Math.Abs(problem.LowerBounds[row]) > 1e-12d || Math.Abs(problem.UpperBounds[row]) > 1e-12d ||
+                    CountRowEntries(problem, row) != 3 ||
+                    Math.Abs(ReadCoefficient(problem, row, layout.A(interval + 1)) - 1d) > 1e-12d ||
+                    Math.Abs(ReadCoefficient(problem, row, layout.A(interval)) + 1d) > 1e-12d)
+                {
+                    continue;
+                }
+                double jerkCoefficient = ReadCoefficient(problem, row, layout.J(interval));
+                if (jerkCoefficient >= -1e-12d)
+                    continue;
+                duration = -jerkCoefficient;
+                return true;
+            }
+            return false;
+        }
+
+        private static int CountRowEntries(QuadraticProgram problem, int row)
+        {
+            int count = 0;
+            for (int column = 0; column < problem.ConstraintMatrix.ColumnCount; column++)
+            {
+                for (int index = problem.ConstraintMatrix.ColumnPointers[column];
+                    index < problem.ConstraintMatrix.ColumnPointers[column + 1]; index++)
+                {
+                    if (problem.ConstraintMatrix.RowIndices[index] == row)
+                        count++;
+                }
+            }
+            return count;
+        }
+
+        private static double ReadCoefficient(QuadraticProgram problem, int row, int column)
+        {
+            for (int index = problem.ConstraintMatrix.ColumnPointers[column];
+                index < problem.ConstraintMatrix.ColumnPointers[column + 1]; index++)
+            {
+                if (problem.ConstraintMatrix.RowIndices[index] == row)
+                    return problem.ConstraintMatrix.Values[index];
+            }
+            return 0d;
+        }
+
+        private static bool TrySolveThreeByThree(double[,] matrix, IReadOnlyList<double> rightHandSide,
+            out double[] solution)
+        {
+            var augmented = new double[3, 4];
+            for (int row = 0; row < 3; row++)
+            {
+                for (int column = 0; column < 3; column++)
+                    augmented[row, column] = matrix[row, column];
+                augmented[row, 3] = rightHandSide[row];
+            }
+            for (int column = 0; column < 3; column++)
+            {
+                int pivot = column;
+                for (int row = column + 1; row < 3; row++)
+                {
+                    if (Math.Abs(augmented[row, column]) > Math.Abs(augmented[pivot, column]))
+                        pivot = row;
+                }
+                if (Math.Abs(augmented[pivot, column]) < 1e-12d)
+                {
+                    solution = Array.Empty<double>();
+                    return false;
+                }
+                if (pivot != column)
+                {
+                    for (int index = column; index < 4; index++)
+                    {
+                        double temporary = augmented[column, index];
+                        augmented[column, index] = augmented[pivot, index];
+                        augmented[pivot, index] = temporary;
+                    }
+                }
+                double divisor = augmented[column, column];
+                for (int index = column; index < 4; index++)
+                    augmented[column, index] /= divisor;
+                for (int row = 0; row < 3; row++)
+                {
+                    if (row == column)
+                        continue;
+                    double factor = augmented[row, column];
+                    for (int index = column; index < 4; index++)
+                        augmented[row, index] -= factor * augmented[column, index];
+                }
+            }
+            solution = new[] { augmented[0, 3], augmented[1, 3], augmented[2, 3] };
+            return true;
+        }
+
+        private static bool IsLongitudinalProblem(QuadraticProgram problem)
+        {
+            if (problem.VariableCount < 7 || (problem.VariableCount + 1) % 4 != 0)
+                return false;
+            int knotCount = (problem.VariableCount + 1) / 4;
+            return problem.ConstraintCount >= 8 * knotCount - 2;
+        }
+
        private static double ReadFixedVariable(QuadraticProgram problem, int variable)
+        {
+            if (TryReadFixedVariable(problem, variable, out double value))
+                return value;
+            throw new InvalidOperationException("Expected a fixed ST variable constraint.");
+        }
+
+        private static bool TryReadFixedVariable(QuadraticProgram problem, int variable, out double value)
        {
            for (int row = 0; row < problem.ConstraintCount; row++)
            {
@@ -557,10 +924,12 @@ internal static class EmPlanningServiceChecks
                if (entryCount == 1 && Math.Abs(coefficient) > 1e-12d &&
                    Math.Abs(problem.LowerBounds[row] - problem.UpperBounds[row]) <= 1e-12d)
                {
-                    return problem.LowerBounds[row] / coefficient;
+                    value = problem.LowerBounds[row] / coefficient;
+                    return true;
                }
            }
-            throw new InvalidOperationException("Expected a fixed ST variable constraint.");
+            value = 0d;
+            return false;
        }

        private static QpSolveResult Result(QpSolveStatus status, IReadOnlyList<double> primal)
@@ -13,6 +13,8 @@ internal static class LongitudinalIntegrationChecks
    public static void Run()
    {
        VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed();
+        VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence();
+        VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold();
        VerifiesExactStopIncludesAStabilizationTail();
        VerifiesLastStrictCandidateSurvivesLaterTimeout();
        VerifiesInvalidAndInaccurateCandidatesNeverBecomeFallbacks();
@@ -58,6 +60,82 @@ internal static class LongitudinalIntegrationChecks
            "rolling ST keeps nonzero terminal speed");
    }

+    private static void VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Scheduling.TimeHorizonSeconds = 10d;
+        configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
+        configuration.Scheduling.MaximumOptimizationTimeStepSeconds = 0.20d;
+        configuration.Scheduling.MaximumOptimizationSpatialStepMeters = 0.10d;
+        configuration.Scheduling.MaximumOptimizationKnotCount = 401;
+        LateralPath path = new LateralPath(new[]
+        {
+            Point(0d, 0d, 0d),
+            Point(1d, 1d, 0d),
+            Point(2d, 2d, 0d),
+        }, true);
+
+        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.10d,
+            EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "full schedule envelope: " + failureReason);
+        status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.10d, 0d,
+            configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
+            out LongitudinalKnotSchedule coarsePublication, out failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "full schedule with 0.10 s publication: " + failureReason);
+
+        EmPlannerConfiguration densePublicationConfiguration = configuration.Copy();
+        densePublicationConfiguration.Scheduling.OutputTimeStepSeconds = 0.05d;
+        status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.10d, 0d,
+            densePublicationConfiguration.Longitudinal.DesiredForwardSpeedMetersPerSecond, densePublicationConfiguration,
+            out LongitudinalKnotSchedule densePublication, out failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "full schedule with 0.05 s publication: " + failureReason);
+        Verification.Equal(coarsePublication.KnotTimes.Count, densePublication.KnotTimes.Count,
+            "publication cadence does not determine full-segment optimization knot count");
+
+        var publicationCandidate = new LongitudinalCandidate(new[] { 0d, 0.50d, 1d },
+            new[] { 0d, 1d / 60d, 1d / 60d }, new[] { 0.10d, 0d, 0d }, new[] { -0.40d, 0d, 0d },
+            new[] { 0.80d, 0d });
+        var publicationResult = new LongitudinalPlanningResult(EmPlanningStatus.Success, publicationCandidate, string.Empty);
+        DateTimeOffset now = DateTimeOffset.UtcNow;
+        var metadata = new EmTrajectoryMetadata("publication-cadence", now, now, 1L, "publication-path", 1L,
+            string.Empty, 0, TravelDirection.Forward, EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary,
+            EmPlanningScope.FullDirectionSegment);
+        configuration.Longitudinal.ZeroSpeedHoldSeconds = 0d;
+        densePublicationConfiguration.Longitudinal.ZeroSpeedHoldSeconds = 0d;
+        EmTrajectory coarseTrajectory = new EmTrajectoryAssembler(configuration).Assemble(path, publicationResult, metadata);
+        EmTrajectory denseTrajectory = new EmTrajectoryAssembler(densePublicationConfiguration).Assemble(path,
+            publicationResult, metadata);
+        Verification.Equal(2 * (coarseTrajectory.Points.Count - 1), denseTrajectory.Points.Count - 1,
+            "halving publication cadence doubles emitted trajectory intervals without changing optimization knots");
+    }
+
+    private static void VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
+        configuration.Longitudinal.ZeroSpeedHoldSeconds = 0.20d;
+        LateralPath path = new LateralPath(new[]
+        {
+            Point(0d, 0d, 0d),
+            Point(1d, 0.0075d, 0d),
+        }, true);
+        var candidate = new LongitudinalCandidate(new[] { 0d, 0.10d, 0.20d, 0.40d },
+            new[] { 0d, 0.005d, 0.0075d, 0.0075d }, new[] { 0.05d, 0.025d, 0d, 0d },
+            new[] { 0d, -0.5d, 0d, 0d }, new[] { -5d, 5d, 0d });
+        var result = new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty);
+        DateTimeOffset now = DateTimeOffset.UtcNow;
+        var metadata = new EmTrajectoryMetadata("full-hold", now, now, 1L, "hold-path", 1L, string.Empty, 0,
+            TravelDirection.Forward, EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary,
+            EmPlanningScope.FullDirectionSegment);
+
+        EmTrajectory trajectory = new EmTrajectoryAssembler(configuration).Assemble(path, result, metadata);
+
+        Verification.NearlyEqual(0.40d, trajectory.Points[trajectory.Points.Count - 1].TimeFromStart,
+            "full-scope publication reuses its candidate hold instead of appending a second hold");
+        Verification.Equal(3, CountStationaryTerminalPoints(trajectory),
+            "full-scope publication emits the candidate's single nonzero terminal hold");
+    }
+
    private static void VerifiesExactStopIncludesAStabilizationTail()
    {
        EmPlannerConfiguration configuration = CreateExactStopSeedConfiguration();
@@ -97,6 +175,22 @@ internal static class LongitudinalIntegrationChecks
        }
    }

+    private static int CountStationaryTerminalPoints(EmTrajectory trajectory)
+    {
+        double terminalPathS = trajectory.Points[trajectory.Points.Count - 1].PathS;
+        int count = 0;
+        for (int index = 0; index < trajectory.Points.Count; index++)
+        {
+            EmTrajectoryPoint point = trajectory.Points[index];
+            if (Math.Abs(point.PathS - terminalPathS) <= 1e-12d &&
+                Math.Abs(point.SignedLongitudinalVelocity) <= 1e-12d)
+            {
+                count++;
+            }
+        }
+        return count;
+    }
+
    public static void RunRealOsqp()
    {
        foreach (LongitudinalScenario scenario in CreateRealOsqpScenarios())
@@ -1,5 +1,6 @@
 using System;
 using System.Collections.Generic;
+using System.Threading;
 using EMPlannerVerificationHost;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;
 using MultiWheelC.TrajectoryPlanning.PathSmoothing;
@@ -19,6 +20,8 @@ internal static class LongitudinalModelChecks
        VerifiesStoppingPrecheckOnlyAppliesToRealStopBoundaries();
        VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime();
        VerifiesFullDirectionScopeSelectsActualSegmentBoundary();
+        VerifiesFullDirectionScheduleDerivesDurationAndAdaptiveBreakpoints();
+        VerifiesFullDirectionInitialFeasibilityProjectionAndFallbackSemantics();
        VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
        VerifiesModeSpecificSolutionValidation();
        VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically();
@@ -297,6 +300,231 @@ internal static class LongitudinalModelChecks
        Verification.NearlyEqual(10d, gear.WindowEndReferenceS, "gear full selection stops before the next segment");
    }

+    private static void VerifiesFullDirectionScheduleDerivesDurationAndAdaptiveBreakpoints()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Scheduling.TimeHorizonSeconds = 10d;
+        configuration.Scheduling.DistanceHorizonMeters = 0.25d;
+        configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
+        configuration.Scheduling.MaximumOptimizationTimeStepSeconds = 0.20d;
+        configuration.Scheduling.MaximumOptimizationSpatialStepMeters = 0.10d;
+        configuration.Scheduling.MaximumOptimizationKnotCount = 401;
+        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
+        configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond = 1d;
+        configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 0.50d;
+        configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 0.50d;
+        configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1d;
+
+        LateralPath shortPath = CreateStraightPath(0.50d);
+        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(shortPath, TravelDirection.Forward, 0.10d,
+            EmTerminalType.Goal, configuration, out PathSpeedLimit shortLimit, out string failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "short full-segment envelope: " + failureReason);
+        status = new FullDirectionSegmentScheduleBuilder().TryBuild(shortPath, shortLimit, 0.10d, 0d,
+            configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
+            out LongitudinalKnotSchedule shortSchedule, out failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "short full-segment schedule: " + failureReason);
+        Verification.True(shortSchedule.TotalDurationSeconds < 10d, "short segment derives its own T_end");
+
+        LateralPath longPath = CreatePath(new[]
+        {
+            new PathFixture(0d, 0d, 0d, 0d),
+            new PathFixture(1.50d, 1.50d, 2d, 0d),
+            new PathFixture(3d, 3d, 0d, 0d),
+        });
+        status = new PathSpeedLimitBuilder().Build(longPath, TravelDirection.Forward, 0.10d,
+            EmTerminalType.Goal, configuration, out PathSpeedLimit longLimit, out failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "long full-segment envelope: " + failureReason);
+        status = new FullDirectionSegmentScheduleBuilder().TryBuild(longPath, longLimit, 0.10d, 0d,
+            configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
+            out LongitudinalKnotSchedule longSchedule, out failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "long full-segment schedule: " + failureReason);
+        Verification.True(longSchedule.TotalDurationSeconds > shortSchedule.TotalDurationSeconds,
+            "duration grows from s_end and limits");
+        Verification.True(longSchedule.KnotTimes.Count <= configuration.Scheduling.MaximumOptimizationKnotCount,
+            "adaptive schedule respects knot cap");
+        Verification.True(longSchedule.IsAdaptive, "full segment produces an adaptive knot schedule");
+        Verification.True(longSchedule.ReferencePathS.Count > longPath.Points.Count,
+            "curvature and stopping envelopes add schedule breakpoints");
+        Verification.NearlyEqual(longPath.Points[longPath.Points.Count - 1].PathS,
+            longSchedule.ReferencePathS[longSchedule.ReferencePathS.Count - 1], "schedule reaches s_end");
+        Verification.NearlyEqual(0d,
+            longSchedule.ReferenceSpeedMetersPerSecond[longSchedule.ReferenceSpeedMetersPerSecond.Count - 1],
+            "schedule stops at s_end");
+
+        EmPlannerConfiguration constrained = configuration.Copy();
+        constrained.Scheduling.MaximumOptimizationKnotCount = 4;
+        status = new FullDirectionSegmentScheduleBuilder().TryBuild(longPath, longLimit, 0.10d, 0d,
+            constrained.Longitudinal.DesiredForwardSpeedMetersPerSecond, constrained,
+            out LongitudinalKnotSchedule rejected, out failureReason);
+        Verification.Equal(EmPlanningStatus.FullSegmentResourceLimitExceeded, status,
+            "undersized full-segment knot cap rejects rather than truncates");
+        Verification.True(rejected == null, "resource rejection produces no partial schedule");
+        Verification.True(failureReason.IndexOf("required", StringComparison.OrdinalIgnoreCase) >= 0 &&
+            failureReason.IndexOf("configured", StringComparison.OrdinalIgnoreCase) >= 0,
+            "resource rejection reports required and configured knots");
+    }
+
+    private static void VerifiesFullDirectionInitialFeasibilityProjectionAndFallbackSemantics()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Scheduling.MaximumOptimizationTimeStepSeconds = 0.20d;
+        configuration.Scheduling.MaximumOptimizationSpatialStepMeters = 0.10d;
+        configuration.Scheduling.MaximumOptimizationKnotCount = 401;
+        configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d;
+        configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
+        configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 20d;
+        LateralPath path = CreateStraightPath(0.0075d);
+        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.05d,
+            EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "feasible-reference envelope: " + failureReason);
+        status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.05d, 0d,
+            configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
+            out LongitudinalKnotSchedule schedule, out failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "feasible-reference schedule: " + failureReason);
+
+        Verification.True(typeof(LongitudinalKnotSchedule).GetProperty("ReferenceCandidate") == null,
+            "adaptive schedule is only a knot/reference/hold contract");
+        Verification.True(schedule.TerminalHoldStartIndex > 0 &&
+            schedule.TerminalHoldStartIndex < schedule.KnotTimes.Count,
+            "adaptive reference explicitly identifies its terminal hold boundary");
+        Verification.True(schedule.TerminalHoldStartIndex >= 3,
+            "adaptive exact-stop schedule reserves three independent motion jerk intervals");
+        LongitudinalCandidate strictProjection = CreateStrictNonuniformExactStopCandidate();
+        var projectionSchedule = new LongitudinalKnotSchedule(strictProjection.KnotTimes,
+            new[] { 0d, 0.003d, 0.006d, 0.0075d, 0.0075d }, new[] { 0.05d, 0.025d, 0.01d, 0d, 0d }, true, 3);
+        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.05d, 0d,
+            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
+            EmPlanningScope.FullDirectionSegment, projectionSchedule, Array.Empty<double>(), Array.Empty<double>());
+        Verification.True(new LongitudinalSolutionValidator().TryValidate(input, speedLimit, strictProjection,
+            out _, out failureReason), "nonuniform strict projection fixture is physically feasible: " + failureReason);
+
+        var constraintBuilder = new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder());
+        Verification.True(constraintBuilder.TryBuildInitialFeasibilityProjection(input, speedLimit,
+            out QuadraticProgram projectionProblem, out failureReason),
+            "full exact-stop feasibility projection builds: " + failureReason);
+        var layout = new LongitudinalVariableLayout(projectionSchedule.KnotTimes.Count);
+        Verification.True(Math.Abs(projectionProblem.LinearCost[layout.S(1)]) > 1e-12d,
+            "feasibility projection tracks scheduled PathS");
+        Verification.True(Math.Abs(projectionProblem.LinearCost[layout.U(1)]) > 1e-12d,
+            "feasibility projection tracks scheduled speed");
+        Verification.Equal(9 * layout.KnotCount - 3 +
+            3 * (layout.KnotCount - projectionSchedule.TerminalHoldStartIndex), projectionProblem.ConstraintCount,
+            "feasibility projection carries a PathS-linearized speed-envelope row for each motion knot");
+
+        var initialTimeoutSolver = new FakeQpSolver(new QpSolveResult(QpSolveStatus.TimeLimit, Array.Empty<double>(), 0d, 0d,
+            0d, 0, TimeSpan.Zero, "time limit", string.Empty));
+        LongitudinalPlanningResult initialTimeout = new SequentialLongitudinalOptimizer(initialTimeoutSolver).Optimize(input,
+            CancellationToken.None);
+        Verification.Equal(EmPlanningStatus.SolverTimedOut, initialTimeout.Status,
+            "initial feasibility timeout cannot publish a fallback");
+        Verification.True(initialTimeout.Candidate == null, "initial feasibility timeout publishes no candidate");
+
+        var solver = new FakeQpSolver(new[]
+        {
+            new QpSolveResult(QpSolveStatus.Solved, ToPrimal(strictProjection), 0d, 0d, 0d, 1,
+                TimeSpan.Zero, "solved", string.Empty),
+            new QpSolveResult(QpSolveStatus.TimeLimit, Array.Empty<double>(), 0d, 0d, 0d, 0,
+                TimeSpan.Zero, "time limit", string.Empty),
+        });
+        LongitudinalPlanningResult result = new SequentialLongitudinalOptimizer(solver).Optimize(input,
+            CancellationToken.None);
+        Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
+            "strict feasibility projection permits a later exact-stop fallback: " + result.FailureReason);
+        Verification.Equal(2, solver.SolveCallCount,
+            "full scope consumes strict feasibility projection before the objective timeout");
+        Verification.True(new LongitudinalSolutionValidator().TryValidate(input, speedLimit,
+            result.Candidate ?? throw new InvalidOperationException("Adaptive fallback was missing."), out _,
+            out failureReason), "adaptive fallback is strict-feasible: " + failureReason);
+
+        EmPlannerConfiguration denserPublication = configuration.Copy();
+        denserPublication.Scheduling.OutputTimeStepSeconds = 0.05d;
+        status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.05d, 0d,
+            denserPublication.Longitudinal.DesiredForwardSpeedMetersPerSecond, denserPublication,
+            out LongitudinalKnotSchedule sameOptimizationSchedule, out failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "independent-cadence schedule: " + failureReason);
+        Verification.Equal(schedule.KnotTimes.Count, sameOptimizationSchedule.KnotTimes.Count,
+            "publication cadence does not change adaptive knot count");
+        Verification.Equal(schedule.TerminalHoldStartIndex, sameOptimizationSchedule.TerminalHoldStartIndex,
+            "publication cadence does not change the terminal hold boundary");
+    }
+
+    private static LongitudinalCandidate CreateStrictNonuniformExactStopCandidate()
+    {
+        double[] times = { 0d, 0.09d, 0.19d, 0.30d, 0.50d };
+        double[] motionTimes = { 0d, 0.09d, 0.19d, 0.30d };
+        var influence = new double[3, 3];
+        for (int interval = 0; interval < 3; interval++)
+        {
+            var basis = new double[3];
+            basis[interval] = 1d;
+            LongitudinalCandidate response = LongitudinalCandidate.Integrate(motionTimes, 0d, 0d, 0d, basis);
+            int last = response.S.Count - 1;
+            influence[0, interval] = response.A[last];
+            influence[1, interval] = response.U[last];
+            influence[2, interval] = response.S[last];
+        }
+        double[] jerkMotion = SolveThreeByThree(influence, new[] { 0d, -0.05d, -0.0075d });
+        var jerk = new[] { jerkMotion[0], jerkMotion[1], jerkMotion[2], 0d };
+        LongitudinalCandidate integrated = LongitudinalCandidate.Integrate(times, 0d, 0.05d, 0d, jerk);
+        var pathS = new[] { integrated.S[0], integrated.S[1], integrated.S[2], 0.0075d, 0.0075d };
+        var speed = new[] { integrated.U[0], integrated.U[1], integrated.U[2], 0d, 0d };
+        var acceleration = new[] { integrated.A[0], integrated.A[1], integrated.A[2], 0d, 0d };
+        return new LongitudinalCandidate(times, pathS, speed, acceleration, jerk);
+    }
+
+    private static double[] ToPrimal(LongitudinalCandidate candidate)
+    {
+        var layout = new LongitudinalVariableLayout(candidate.KnotTimes.Count);
+        var primal = new double[layout.VariableCount];
+        for (int index = 0; index < layout.KnotCount; index++)
+        {
+            primal[layout.S(index)] = candidate.S[index];
+            primal[layout.U(index)] = candidate.U[index];
+            primal[layout.A(index)] = candidate.A[index];
+        }
+        for (int index = 0; index < layout.KnotCount - 1; index++)
+            primal[layout.J(index)] = candidate.J[index];
+        return primal;
+    }
+
+    private static double[] SolveThreeByThree(double[,] matrix, IReadOnlyList<double> rightHandSide)
+    {
+        var augmented = new double[3, 4];
+        for (int row = 0; row < 3; row++)
+        {
+            for (int column = 0; column < 3; column++)
+                augmented[row, column] = matrix[row, column];
+            augmented[row, 3] = rightHandSide[row];
+        }
+        for (int pivot = 0; pivot < 3; pivot++)
+        {
+            int bestRow = pivot;
+            for (int row = pivot + 1; row < 3; row++)
+            {
+                if (Math.Abs(augmented[row, pivot]) > Math.Abs(augmented[bestRow, pivot]))
+                    bestRow = row;
+            }
+            for (int column = pivot; column < 4; column++)
+            {
+                double temporary = augmented[pivot, column];
+                augmented[pivot, column] = augmented[bestRow, column];
+                augmented[bestRow, column] = temporary;
+            }
+            double divisor = augmented[pivot, pivot];
+            for (int column = pivot; column < 4; column++)
+                augmented[pivot, column] /= divisor;
+            for (int row = 0; row < 3; row++)
+            {
+                if (row == pivot)
+                    continue;
+                double factor = augmented[row, pivot];
+                for (int column = pivot; column < 4; column++)
+                    augmented[row, column] -= factor * augmented[pivot, column];
+            }
+        }
+        return new[] { augmented[0, 3], augmented[1, 3], augmented[2, 3] };
+    }
+
    private static void VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints()
    {
        var layout = new LongitudinalVariableLayout(5);
@@ -370,8 +598,21 @@ internal static class LongitudinalModelChecks
        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");
+        Verification.NearlyEqual(input.DirectionMaximumSpeedMetersPerSecond, uUpper,
+            "U retains its direction hard bound alongside the PathS envelope");
+        int envelopeSegment = 0;
+        while (envelopeSegment < envelope.PathS.Count - 2 && integrated.S[1] > envelope.PathS[envelopeSegment + 1])
+            envelopeSegment++;
+        double envelopeSlope = (envelope.MaximumSpeedMetersPerSecond[envelopeSegment + 1] -
+            envelope.MaximumSpeedMetersPerSecond[envelopeSegment]) /
+            (envelope.PathS[envelopeSegment + 1] - envelope.PathS[envelopeSegment]);
+        double envelopeIntercept = envelope.MaximumSpeedMetersPerSecond[envelopeSegment] -
+            envelopeSlope * envelope.PathS[envelopeSegment];
+        Verification.Equal(1, CountBoundedRow(problem, new Dictionary<int, double>
+        {
+            { layout.U(1), 1d }, { layout.S(1), -envelopeSlope },
+        }, -QuadraticProgram.MaximumFiniteBound, envelopeIntercept),
+            "U upper bound linearly re-evaluates the actual PathS envelope");
        FindSingleVariableBounds(problem, layout.A(1), out double aLower, out double aUpper);
        Verification.NearlyEqual(-1d, aLower, "deceleration lower bound");
        Verification.NearlyEqual(1d, aUpper, "acceleration upper bound");