feat: derive adaptive full-segment ST schedule

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs

@@ -83,16 +83,34 @@ public sealed class EmPlanningService : IEmPlanningService
                 return Failure(lateral.Status, request, lateral.FailureReason);
             EmitDebug(request, "LS optimization and validation succeeded");
 
-            IReadOnlyList<double> knotTimes = LongitudinalCandidate.CreateKnotTimes(
+            EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(lateral.Path, segment.Direction,
-                configuration.Scheduling.TimeHorizonSeconds, configuration.Scheduling.OutputTimeStepSeconds);
+                initialProgressSpeed, horizon.TerminalType, configuration, out PathSpeedLimit speedLimit,
+                out string envelopeReason);
+            if (envelopeStatus != EmPlanningStatus.Success)
+                return Failure(envelopeStatus, request, envelopeReason);
+            LongitudinalKnotSchedule knotSchedule;
+            if (request.PlanningScope == EmPlanningScope.FullDirectionSegment)
+            {
+                EmPlanningStatus scheduleStatus = new FullDirectionSegmentScheduleBuilder().TryBuild(lateral.Path, speedLimit,
+                    initialProgressSpeed, initialAcceleration, DesiredSpeed(configuration, segment.Direction), configuration,
+                    out knotSchedule, out string scheduleReason);
+                if (scheduleStatus != EmPlanningStatus.Success)
+                    return Failure(scheduleStatus, request, scheduleReason);
+            }
+            else
+            {
+                knotSchedule = LongitudinalKnotSchedule.CreateRolling(configuration.Scheduling.TimeHorizonSeconds,
+                    configuration.Scheduling.OutputTimeStepSeconds);
+            }
             LongitudinalPreviousTrajectorySeed previousLongitudinalSeed =
                 new LongitudinalPreviousTrajectorySeedBuilder().Build(
-                    request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc, knotTimes,
+                    request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc, knotSchedule,
                     segment.SegmentIndex, segment.Direction);
             var longitudinalInput = new LongitudinalPlanningInput(lateral.Path, segment.Direction, initialProgressSpeed,
                 initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, configuration,
+                request.PlanningScope, knotSchedule,
                 previousLongitudinalSeed.PathS, previousLongitudinalSeed.ProgressSpeedMetersPerSecond);
-            EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out string envelopeReason);
+            envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out envelopeReason);
             if (envelopeStatus != EmPlanningStatus.Success)
                 return Failure(envelopeStatus, request, envelopeReason);
             EmitDebug(request, "PathS speed envelope succeeded");
@@ -195,6 +213,13 @@ public sealed class EmPlanningService : IEmPlanningService
             : state.SignedLongitudinalSpeedMetersPerSecond < 0d;
     }
 
+    private static double DesiredSpeed(EmPlannerConfiguration configuration, TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward
+            ? configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond
+            : configuration.Longitudinal.DesiredReverseSpeedMetersPerSecond;
+    }
+
     private static bool IsSuccess(EmPlanningStatus status)
     {
         return status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback;

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/FullDirectionSegmentScheduleBuilder.cs

@@ -0,0 +1,203 @@
+using System;
+using System.Collections.Generic;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Derives bounded full-direction ST knots from the physical PathS speed and stopping envelope.</summary>
+public sealed class FullDirectionSegmentScheduleBuilder
+{
+    private const double Tolerance = 1e-10d;
+
+    public EmPlanningStatus TryBuild(LateralPath path, PathSpeedLimit speedLimit,
+        double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
+        double desiredSpeedMetersPerSecond, EmPlannerConfiguration configuration,
+        out LongitudinalKnotSchedule schedule, out string failureReason)
+    {
+        schedule = null;
+        failureReason = string.Empty;
+        if (path == null || speedLimit == null || configuration == null || configuration.Scheduling == null ||
+            configuration.Longitudinal == null || !path.IsIndependentlyValidated || path.Points.Count < 2 ||
+            !IsFinite(initialProgressSpeedMetersPerSecond) || initialProgressSpeedMetersPerSecond < 0d ||
+            !IsFinite(initialAccelerationMetersPerSecondSquared) || !IsPositiveFinite(desiredSpeedMetersPerSecond))
+        {
+            failureReason = "Full-direction schedule inputs are invalid.";
+            return EmPlanningStatus.InvalidInput;
+        }
+        if (!speedLimit.HasStopBoundary || Math.Abs(speedLimit.PathUpperBoundS -
+                path.Points[path.Points.Count - 1].PathS) > Tolerance)
+        {
+            failureReason = "A full-direction schedule requires the matching real stop-boundary speed envelope.";
+            return EmPlanningStatus.InvalidInput;
+        }
+
+        SchedulingConfiguration scheduling = configuration.Scheduling;
+        LongitudinalConfiguration longitudinal = configuration.Longitudinal;
+        if (!IsPositiveFinite(scheduling.MaximumOptimizationTimeStepSeconds) ||
+            !IsPositiveFinite(scheduling.MaximumOptimizationSpatialStepMeters) ||
+            scheduling.MaximumOptimizationKnotCount < 3 ||
+            !IsPositiveFinite(longitudinal.MaximumAccelerationMetersPerSecondSquared) ||
+            !IsPositiveFinite(longitudinal.MaximumDecelerationMetersPerSecondSquared) ||
+            !IsPositiveFinite(longitudinal.MaximumJerkMetersPerSecondCubed))
+        {
+            failureReason = "Full-direction schedule limits are invalid.";
+            return EmPlanningStatus.InvalidInput;
+        }
+
+        int stationCount = speedLimit.PathS.Count;
+        var speeds = new double[stationCount];
+        double desired = Math.Min(desiredSpeedMetersPerSecond, speedLimit.DirectionMaximumSpeedMetersPerSecond);
+        speeds[0] = Math.Min(initialProgressSpeedMetersPerSecond, Math.Min(desired,
+            speedLimit.MaximumSpeedMetersPerSecond[0]));
+        for (int index = 1; index < stationCount; index++)
+        {
+            double distance = speedLimit.PathS[index] - speedLimit.PathS[index - 1];
+            double reachable = Math.Sqrt(Math.Max(0d, speeds[index - 1] * speeds[index - 1] +
+                2d * longitudinal.MaximumAccelerationMetersPerSecondSquared * distance));
+            speeds[index] = Math.Min(reachable, Math.Min(desired, speedLimit.MaximumSpeedMetersPerSecond[index]));
+        }
+        speeds[stationCount - 1] = 0d;
+        for (int index = stationCount - 2; index >= 0; index--)
+        {
+            double remainingDistance = speedLimit.PathUpperBoundS - speedLimit.PathS[index];
+            double stopCap = JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(remainingDistance,
+                Math.Max(0d, initialAccelerationMetersPerSecondSquared), longitudinal.MaximumDecelerationMetersPerSecondSquared,
+                longitudinal.MaximumJerkMetersPerSecondCubed, speedLimit.DirectionMaximumSpeedMetersPerSecond);
+            double distance = speedLimit.PathS[index + 1] - speedLimit.PathS[index];
+            double decelerationCap = Math.Sqrt(Math.Max(0d, speeds[index + 1] * speeds[index + 1] +
+                2d * longitudinal.MaximumDecelerationMetersPerSecondSquared * distance));
+            speeds[index] = Math.Min(speeds[index], Math.Min(stopCap, decelerationCap));
+        }
+
+        var times = new List<double> { 0d };
+        var pathS = new List<double> { 0d };
+        var referenceSpeeds = new List<double> { speeds[0] };
+        IReadOnlyList<int> scheduleStations = SelectScheduleStations(speedLimit.PathS, speeds);
+        int minimumIntervalsPerSegment = Math.Max(1,
+            (3 + scheduleStations.Count - 2) / (scheduleStations.Count - 1));
+        for (int stationIndex = 1; stationIndex < scheduleStations.Count; stationIndex++)
+        {
+            int startIndex = scheduleStations[stationIndex - 1];
+            int endIndex = scheduleStations[stationIndex];
+            double startS = speedLimit.PathS[startIndex];
+            double endS = speedLimit.PathS[endIndex];
+            double startSpeed = speeds[startIndex];
+            double endSpeed = speeds[endIndex];
+            double distance = endS - startS;
+            double denominator = startSpeed + endSpeed;
+            double duration = denominator > Tolerance ? 2d * distance / denominator :
+                Math.Sqrt(2d * distance / Math.Max(Tolerance, longitudinal.MaximumAccelerationMetersPerSecondSquared));
+            int subdivisionCount = Math.Max(minimumIntervalsPerSegment, Math.Max(
+                checked((int)Math.Ceiling(distance / scheduling.MaximumOptimizationSpatialStepMeters)),
+                checked((int)Math.Ceiling(duration / scheduling.MaximumOptimizationTimeStepSeconds))));
+            for (int subdivision = 1; subdivision <= subdivisionCount; subdivision++)
+            {
+                double fraction = (double)subdivision / subdivisionCount;
+                times.Add(times[times.Count - 1] + duration / subdivisionCount);
+                pathS.Add(startS + distance * fraction);
+                referenceSpeeds.Add(startSpeed + (endSpeed - startSpeed) * fraction);
+            }
+        }
+        referenceSpeeds[referenceSpeeds.Count - 1] = 0d;
+        pathS[pathS.Count - 1] = speedLimit.PathUpperBoundS;
+        EnsureJerkReachableReferenceTimes(times, referenceSpeeds, longitudinal);
+        EnsureMinimumExactStopDuration(times, speedLimit.PathUpperBoundS, initialProgressSpeedMetersPerSecond,
+            initialAccelerationMetersPerSecondSquared, longitudinal);
+        if (!IsFinite(longitudinal.ZeroSpeedHoldSeconds) || longitudinal.ZeroSpeedHoldSeconds < 0d)
+        {
+            failureReason = "The full-direction zero-speed hold duration is invalid.";
+            return EmPlanningStatus.InvalidInput;
+        }
+        int terminalHoldStartIndex = times.Count - 1;
+        double remainingHold = longitudinal.ZeroSpeedHoldSeconds;
+        while (remainingHold > Tolerance)
+        {
+            double holdStep = Math.Min(remainingHold, scheduling.MaximumOptimizationTimeStepSeconds);
+            times.Add(times[times.Count - 1] + holdStep);
+            pathS.Add(speedLimit.PathUpperBoundS);
+            referenceSpeeds.Add(0d);
+            remainingHold -= holdStep;
+        }
+        if (times.Count > scheduling.MaximumOptimizationKnotCount)
+        {
+            failureReason = "Full-direction schedule required knots=" + times.Count + ", configured maximum=" +
+                scheduling.MaximumOptimizationKnotCount + ".";
+            return EmPlanningStatus.FullSegmentResourceLimitExceeded;
+        }
+        try
+        {
+            schedule = LongitudinalKnotSchedule.CreateAdaptive(times, pathS, referenceSpeeds,
+                terminalHoldStartIndex);
+            return EmPlanningStatus.Success;
+        }
+        catch (ArgumentException exception)
+        {
+            failureReason = exception.Message;
+            return EmPlanningStatus.InvalidInput;
+        }
+    }
+
+    private static void EnsureMinimumExactStopDuration(IList<double> times, double stopBoundaryPathS,
+        double initialSpeed, double initialAcceleration, LongitudinalConfiguration configuration)
+    {
+        if (initialSpeed <= Tolerance || !JerkLimitedStoppingMath.TryCalculate(initialSpeed, initialAcceleration,
+                configuration.MaximumDecelerationMetersPerSecondSquared,
+                configuration.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile stop, out _))
+        {
+            return;
+        }
+        double cruiseDistance = Math.Max(0d, stopBoundaryPathS - stop.DistanceMeters);
+        double requiredDuration = stop.DurationSeconds + cruiseDistance / initialSpeed;
+        double stopSpeedTolerance = configuration.StopSpeedToleranceMetersPerSecond;
+        if (IsPositiveFinite(stopSpeedTolerance) && JerkLimitedStoppingMath.TryCalculate(stopSpeedTolerance, 0d,
+                configuration.MaximumDecelerationMetersPerSecondSquared,
+                configuration.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile settlingStop, out _))
+        {
+            double envelopeTraverseDuration = 2d * stopBoundaryPathS / (initialSpeed + stopSpeedTolerance);
+            requiredDuration = Math.Max(requiredDuration, envelopeTraverseDuration + settlingStop.DurationSeconds);
+        }
+        double currentDuration = times[times.Count - 1];
+        if (currentDuration + Tolerance >= requiredDuration)
+            return;
+        double scale = requiredDuration / currentDuration;
+        for (int index = 1; index < times.Count; index++)
+            times[index] *= scale;
+    }
+
+    private static void EnsureJerkReachableReferenceTimes(IList<double> times, IReadOnlyList<double> referenceSpeeds,
+        LongitudinalConfiguration configuration)
+    {
+        double adjustedTime = 0d;
+        for (int index = 1; index < times.Count; index++)
+        {
+            double requestedDuration = times[index] - times[index - 1];
+            double speedChange = Math.Abs(referenceSpeeds[index] - referenceSpeeds[index - 1]);
+            double accelerationLimit = referenceSpeeds[index] >= referenceSpeeds[index - 1]
+                ? configuration.MaximumAccelerationMetersPerSecondSquared
+                : configuration.MaximumDecelerationMetersPerSecondSquared;
+            double accelerationDuration = speedChange / accelerationLimit;
+            double triangularJerkDuration = speedChange <= Tolerance
+                ? 0d
+                : 2d * Math.Sqrt(speedChange / configuration.MaximumJerkMetersPerSecondCubed);
+            adjustedTime += Math.Max(requestedDuration, Math.Max(accelerationDuration, triangularJerkDuration));
+            times[index] = adjustedTime;
+        }
+    }
+
+    private static IReadOnlyList<int> SelectScheduleStations(IReadOnlyList<double> pathS,
+        IReadOnlyList<double> speeds)
+    {
+        var stations = new List<int> { 0 };
+        for (int index = 1; index < pathS.Count - 1; index++)
+        {
+            double previousSlope = (speeds[index] - speeds[index - 1]) / (pathS[index] - pathS[index - 1]);
+            double nextSlope = (speeds[index + 1] - speeds[index]) / (pathS[index + 1] - pathS[index]);
+            if (previousSlope * nextSlope < 0d)
+                stations.Add(index);
+        }
+        stations.Add(pathS.Count - 1);
+        return stations;
+    }
+
+    private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value);
+    private static bool IsPositiveFinite(double value) => IsFinite(value) && value > 0d;
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs

@@ -15,6 +15,35 @@ public sealed class LongitudinalConstraintBuilder
 
     public bool TryBuild(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
         out QuadraticProgram problem, out string failureReason)
+    {
+        return TryBuildCore(input, speedLimit, iterate, false, out problem, out failureReason);
+    }
+
+    /// <summary>Builds the bounded full-scope feasibility projection before objective optimization.</summary>
+    public bool TryBuildInitialFeasibilityProjection(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
+        out QuadraticProgram problem, out string failureReason)
+    {
+        return TryBuildInitialFeasibilityProjection(input, speedLimit,
+            input == null ? null : CreateScheduleReferenceIterate(input), out problem, out failureReason);
+    }
+
+    public bool TryBuildInitialFeasibilityProjection(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
+        LongitudinalCandidate linearizationIterate, out QuadraticProgram problem, out string failureReason)
+    {
+        problem = null;
+        failureReason = string.Empty;
+        if (input == null || input.PlanningScope != EmPlanningScope.FullDirectionSegment ||
+            input.Mode != EmLongitudinalMode.ExactStopAtBoundary || linearizationIterate == null)
+        {
+            failureReason = "Initial feasibility projection is only defined for full-direction exact-stop planning.";
+            return false;
+        }
+        return TryBuildCore(input, speedLimit, linearizationIterate, true, out problem,
+            out failureReason);
+    }
+
+    private bool TryBuildCore(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
+        bool useScheduleReferenceObjective, out QuadraticProgram problem, out string failureReason)
     {
         problem = null;
         failureReason = string.Empty;
@@ -25,10 +54,9 @@ public sealed class LongitudinalConstraintBuilder
             if (Math.Abs(speedLimit.PathUpperBoundS - input.PathUpperBoundS) > 1e-12d)
                 throw new ArgumentException("The speed envelope upper bound must match actual lateral PathS.");
 
-            IReadOnlyList<double> expectedTimes = LongitudinalCandidate.CreateKnotTimes(
+            IReadOnlyList<double> expectedTimes = input.KnotSchedule.KnotTimes;
-                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.");
+                throw new ArgumentException("The ST iterate time knots do not match the supplied knot schedule.");
             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)
@@ -50,13 +78,13 @@ public sealed class LongitudinalConstraintBuilder
 
             var hessian = new SparseTripletBuilder(layout.VariableCount, layout.VariableCount, true);
             var linearCost = new double[layout.VariableCount];
-            _objectiveBuilder.AddTerms(input, speedLimit, layout, iterate, hessian, linearCost);
+            if (useScheduleReferenceObjective)
-            int stabilizationStart = input.Mode == EmLongitudinalMode.ExactStopAtBoundary
+                AddInitialFeasibilityObjective(input, layout, hessian, linearCost);
-                ? LongitudinalTerminalSchedule.GetStabilizationStartIndex(expectedTimes,
+            else
-                    input.Configuration.Scheduling.OutputTimeStepSeconds)
+                _objectiveBuilder.AddTerms(input, speedLimit, layout, iterate, hessian, linearCost);
-                : layout.KnotCount;
+            int stabilizationStart = GetStabilizationStart(input, expectedTimes, layout.KnotCount);
             int stationaryKnotCount = layout.KnotCount - stabilizationStart;
-            int expectedRows = 8 * layout.KnotCount - 2 + 3 * stationaryKnotCount;
+            int expectedRows = 9 * layout.KnotCount - 3 + 3 * stationaryKnotCount;
             var constraints = new SparseTripletBuilder(expectedRows, layout.VariableCount);
             var lower = new List<double>(expectedRows);
             var upper = new List<double>(expectedRows);
@@ -80,6 +108,42 @@ public sealed class LongitudinalConstraintBuilder
         }
     }
 
+    private static LongitudinalCandidate CreateScheduleReferenceIterate(LongitudinalPlanningInput input)
+    {
+        int knotCount = input.KnotSchedule.KnotTimes.Count;
+        return new LongitudinalCandidate(input.KnotSchedule.KnotTimes, input.KnotSchedule.ReferencePathS,
+            input.KnotSchedule.ReferenceSpeedMetersPerSecond, new double[knotCount], new double[knotCount - 1]);
+    }
+
+    private static void AddInitialFeasibilityObjective(LongitudinalPlanningInput input, LongitudinalVariableLayout layout,
+        SparseTripletBuilder hessian, IList<double> linearCost)
+    {
+        double progressScale = 1d;
+        double speedScale = 1d;
+        double accelerationScale = 1d;
+        double jerkScale = 1d;
+        for (int index = 0; index < layout.KnotCount; index++)
+        {
+            AddProjectionSquaredResidual(hessian, linearCost, layout.S(index), input.KnotSchedule.ReferencePathS[index],
+                1d, progressScale);
+            AddProjectionSquaredResidual(hessian, linearCost, layout.U(index),
+                input.KnotSchedule.ReferenceSpeedMetersPerSecond[index], 10d, speedScale);
+            AddProjectionSquaredResidual(hessian, linearCost, layout.A(index), 0d, 1e-3d, accelerationScale);
+        }
+        for (int index = 0; index < layout.KnotCount - 1; index++)
+            AddProjectionSquaredResidual(hessian, linearCost, layout.J(index), 0d, 1e-3d, jerkScale);
+    }
+
+    private static void AddProjectionSquaredResidual(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));
+        double coefficient = 2d * weight / (scale * scale);
+        hessian.Add(variable, variable, coefficient);
+        linearCost[variable] += -coefficient * reference;
+    }
+
     private static void AddVariableBounds(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
         LongitudinalCandidate iterate, LongitudinalVariableLayout layout, double maximumAcceleration,
         double maximumDeceleration, double maximumJerk, SparseTripletBuilder constraints, IList<double> lower,
@@ -92,8 +156,11 @@ public sealed class LongitudinalConstraintBuilder
             AddSingleVariableRow(constraints, lower, upper, layout.S(index), 0d, input.PathUpperBoundS, ref row);
             double maximumSpeed = index == 0
                 ? input.DirectionMaximumSpeedMetersPerSecond
-                : Math.Min(input.DirectionMaximumSpeedMetersPerSecond, speedLimit.MaximumSpeedAt(iterate.S[index]));
+                : input.DirectionMaximumSpeedMetersPerSecond;
             AddSingleVariableRow(constraints, lower, upper, layout.U(index), 0d, maximumSpeed, ref row);
+            if (index > 0)
+                AddLinearizedSpeedEnvelopeRow(speedLimit, iterate.S[index], layout.S(index), layout.U(index),
+                    constraints, lower, upper, ref row);
             AddSingleVariableRow(constraints, lower, upper, layout.A(index), -maximumDeceleration, maximumAcceleration,
                 ref row);
         }
@@ -101,6 +168,34 @@ public sealed class LongitudinalConstraintBuilder
             AddSingleVariableRow(constraints, lower, upper, layout.J(index), -maximumJerk, maximumJerk, ref row);
     }
 
+    private static void AddLinearizedSpeedEnvelopeRow(PathSpeedLimit speedLimit, double pathS, int pathSVariable,
+        int speedVariable, SparseTripletBuilder constraints, IList<double> lower, IList<double> upper, ref int row)
+    {
+        int segment = FindSpeedEnvelopeSegment(speedLimit, pathS);
+        double startS = speedLimit.PathS[segment];
+        double endS = speedLimit.PathS[segment + 1];
+        double startSpeed = speedLimit.MaximumSpeedMetersPerSecond[segment];
+        double endSpeed = speedLimit.MaximumSpeedMetersPerSecond[segment + 1];
+        double slope = (endSpeed - startSpeed) / (endS - startS);
+        double intercept = startSpeed - slope * startS;
+        AddRow(constraints, lower, upper, row, new[]
+        {
+            new Coefficient(speedVariable, 1d), new Coefficient(pathSVariable, -slope),
+        }, -QuadraticProgram.MaximumFiniteBound, intercept);
+        row++;
+    }
+
+    private static int FindSpeedEnvelopeSegment(PathSpeedLimit speedLimit, double pathS)
+    {
+        double clamped = Math.Max(speedLimit.PathS[0], Math.Min(speedLimit.PathUpperBoundS, pathS));
+        for (int index = 0; index < speedLimit.PathS.Count - 1; index++)
+        {
+            if (clamped <= speedLimit.PathS[index + 1])
+                return index;
+        }
+        return speedLimit.PathS.Count - 2;
+    }
+
     private static void AddMonotonicProgress(LongitudinalVariableLayout layout, SparseTripletBuilder constraints,
         IList<double> lower, IList<double> upper, ref int row)
     {
@@ -164,6 +259,24 @@ public sealed class LongitudinalConstraintBuilder
         }
     }
 
+    private static int GetStabilizationStart(LongitudinalPlanningInput input, IReadOnlyList<double> times,
+        int knotCount)
+    {
+        if (input.Mode != EmLongitudinalMode.ExactStopAtBoundary)
+            return knotCount;
+        if (input.PlanningScope == EmPlanningScope.FullDirectionSegment)
+        {
+            if (input.KnotSchedule.TerminalHoldStartIndex < 1 ||
+                input.KnotSchedule.TerminalHoldStartIndex >= knotCount)
+            {
+                throw new ArgumentException("Full-direction exact-stop schedules require an explicit terminal hold boundary.");
+            }
+            return input.KnotSchedule.TerminalHoldStartIndex;
+        }
+        return LongitudinalTerminalSchedule.GetStabilizationStartIndex(times,
+            input.Configuration.Scheduling.OutputTimeStepSeconds);
+    }
+
     private static void AddSingleVariableRow(SparseTripletBuilder constraints, IList<double> lower, IList<double> upper,
         int variable, double minimum, double maximum, ref int row)
     {
@@ -192,6 +305,11 @@ public sealed class LongitudinalConstraintBuilder
         return true;
     }
 
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+
     private readonly struct Coefficient
     {
         public Coefficient(int variable, double value)

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalKnotSchedule.cs

@@ -0,0 +1,118 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable ST optimization knots, separate from the trajectory publication cadence.</summary>
+public sealed class LongitudinalKnotSchedule
+{
+    public LongitudinalKnotSchedule(IReadOnlyList<double> knotTimes, IReadOnlyList<double> referencePathS,
+        IReadOnlyList<double> referenceSpeedMetersPerSecond, bool isAdaptive)
+        : this(knotTimes, referencePathS, referenceSpeedMetersPerSecond, isAdaptive, -1)
+    {
+    }
+
+    public LongitudinalKnotSchedule(IReadOnlyList<double> knotTimes, IReadOnlyList<double> referencePathS,
+        IReadOnlyList<double> referenceSpeedMetersPerSecond, bool isAdaptive,
+        int terminalHoldStartIndex)
+    {
+        KnotTimes = CopyTimes(knotTimes);
+        ReferencePathS = CopyNondecreasing(referencePathS, KnotTimes.Count, nameof(referencePathS));
+        ReferenceSpeedMetersPerSecond = CopyNonnegative(referenceSpeedMetersPerSecond, KnotTimes.Count,
+            nameof(referenceSpeedMetersPerSecond));
+        if (isAdaptive && ReferenceSpeedMetersPerSecond[ReferenceSpeedMetersPerSecond.Count - 1] != 0d)
+            throw new ArgumentException("An adaptive full-segment schedule must end at exact zero speed.",
+                nameof(referenceSpeedMetersPerSecond));
+
+        IsAdaptive = isAdaptive;
+        TotalDurationSeconds = KnotTimes[KnotTimes.Count - 1];
+        if (terminalHoldStartIndex != -1 &&
+            (!isAdaptive || terminalHoldStartIndex < 1 || terminalHoldStartIndex >= KnotTimes.Count))
+        {
+            throw new ArgumentOutOfRangeException(nameof(terminalHoldStartIndex));
+        }
+        TerminalHoldStartIndex = terminalHoldStartIndex;
+    }
+
+    public IReadOnlyList<double> KnotTimes { get; }
+    public IReadOnlyList<double> ReferencePathS { get; }
+    public IReadOnlyList<double> ReferenceSpeedMetersPerSecond { get; }
+    public double TotalDurationSeconds { get; }
+    public bool IsAdaptive { get; }
+    public int TerminalHoldStartIndex { get; }
+
+    internal static LongitudinalKnotSchedule CreateAdaptive(IReadOnlyList<double> knotTimes,
+        IReadOnlyList<double> referencePathS, IReadOnlyList<double> referenceSpeedMetersPerSecond,
+        int terminalHoldStartIndex)
+    {
+        return new LongitudinalKnotSchedule(knotTimes, referencePathS, referenceSpeedMetersPerSecond, true,
+            terminalHoldStartIndex);
+    }
+
+    internal LongitudinalKnotSchedule Copy()
+    {
+        return new LongitudinalKnotSchedule(KnotTimes, ReferencePathS, ReferenceSpeedMetersPerSecond, IsAdaptive,
+            TerminalHoldStartIndex);
+    }
+
+    public static LongitudinalKnotSchedule CreateRolling(double timeHorizonSeconds, double timeStepSeconds)
+    {
+        IReadOnlyList<double> times = LongitudinalCandidate.CreateKnotTimes(timeHorizonSeconds, timeStepSeconds);
+        var pathS = new double[times.Count];
+        var speeds = new double[times.Count];
+        return new LongitudinalKnotSchedule(times, pathS, speeds, false);
+    }
+
+    private static IReadOnlyList<double> CopyTimes(IReadOnlyList<double> source)
+    {
+        if (source == null || source.Count < 2)
+            throw new ArgumentException("At least two 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 || (index == 0 && source[index] != 0d))
+                throw new ArgumentException("Time knots must be finite, begin at exact zero, and strictly increase.",
+                    nameof(source));
+            copy.Add(source[index]);
+            previous = source[index];
+        }
+        return new ReadOnlyCollection<double>(copy);
+    }
+
+    private static IReadOnlyList<double> CopyNondecreasing(IReadOnlyList<double> source, int expectedCount,
+        string parameterName)
+    {
+        if (source == null || source.Count != expectedCount || source[0] != 0d)
+            throw new ArgumentException("Reference PathS must begin at exact zero and match the knot count.", parameterName);
+        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("Reference PathS must be finite and nondecreasing.", parameterName);
+            copy.Add(source[index]);
+            previous = source[index];
+        }
+        return new ReadOnlyCollection<double>(copy);
+    }
+
+    private static IReadOnlyList<double> CopyNonnegative(IReadOnlyList<double> source, int expectedCount,
+        string parameterName)
+    {
+        if (source == null || source.Count != expectedCount)
+            throw new ArgumentException("Reference speeds must match the knot count.", parameterName);
+        var copy = new List<double>(source.Count);
+        for (int index = 0; index < source.Count; index++)
+        {
+            if (!IsFinite(source[index]) || source[index] < 0d)
+                throw new ArgumentOutOfRangeException(parameterName);
+            copy.Add(source[index]);
+        }
+        return new ReadOnlyCollection<double>(copy);
+    }
+
+    private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value);
+
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningInput.cs

@@ -14,6 +14,16 @@ public sealed class LongitudinalPlanningInput
         double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmLongitudinalMode mode,
         EmPlannerConfiguration configuration,
         IReadOnlyList<double> previousPathS, IReadOnlyList<double> previousProgressSpeedMetersPerSecond)
+        : this(path, direction, initialProgressSpeedMetersPerSecond, initialAccelerationMetersPerSecondSquared,
+            terminalType, mode, configuration, EmPlanningScope.RollingHorizon,
+            CreateRollingSchedule(configuration), previousPathS, previousProgressSpeedMetersPerSecond)
+    {
+    }
+
+    public LongitudinalPlanningInput(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond,
+        double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmLongitudinalMode mode,
+        EmPlannerConfiguration configuration, EmPlanningScope planningScope, LongitudinalKnotSchedule knotSchedule,
+        IReadOnlyList<double> previousPathS, IReadOnlyList<double> previousProgressSpeedMetersPerSecond)
     {
         if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2)
             throw new ArgumentException("Longitudinal planning requires an independently validated lateral path with at least two points.",
@@ -34,6 +44,13 @@ public sealed class LongitudinalPlanningInput
             throw new ArgumentException("Stop-boundary modes require Goal or GearSwitch.");
         if (configuration == null)
             throw new ArgumentNullException(nameof(configuration));
+        if (!Enum.IsDefined(typeof(EmPlanningScope), planningScope))
+            throw new ArgumentOutOfRangeException(nameof(planningScope));
+        if (knotSchedule == null)
+            throw new ArgumentNullException(nameof(knotSchedule));
+        if ((planningScope == EmPlanningScope.FullDirectionSegment) != knotSchedule.IsAdaptive)
+            throw new ArgumentException("Full-direction planning requires an adaptive schedule and rolling planning requires a rolling schedule.",
+                nameof(knotSchedule));
 
         Path = CopyAndValidatePath(path);
         Direction = direction;
@@ -42,6 +59,8 @@ public sealed class LongitudinalPlanningInput
         TerminalType = terminalType;
         Mode = mode;
         Configuration = configuration.Copy();
+        PlanningScope = planningScope;
+        KnotSchedule = knotSchedule.Copy();
         PreviousPathS = CopyFiniteNonnegative(previousPathS, nameof(previousPathS));
         PreviousProgressSpeedMetersPerSecond = CopyFiniteNonnegative(previousProgressSpeedMetersPerSecond,
             nameof(previousProgressSpeedMetersPerSecond));
@@ -75,6 +94,10 @@ public sealed class LongitudinalPlanningInput
 
     public EmPlannerConfiguration Configuration { get; }
 
+    public EmPlanningScope PlanningScope { get; }
+
+    public LongitudinalKnotSchedule KnotSchedule { get; }
+
     public IReadOnlyList<double> PreviousPathS { get; }
 
     public IReadOnlyList<double> PreviousProgressSpeedMetersPerSecond { get; }
@@ -143,6 +166,14 @@ public sealed class LongitudinalPlanningInput
         return new ReadOnlyCollection<double>(copy);
     }
 
+    private static LongitudinalKnotSchedule CreateRollingSchedule(EmPlannerConfiguration configuration)
+    {
+        if (configuration == null || configuration.Scheduling == null)
+            throw new ArgumentNullException(nameof(configuration));
+        return LongitudinalKnotSchedule.CreateRolling(configuration.Scheduling.TimeHorizonSeconds,
+            configuration.Scheduling.OutputTimeStepSeconds);
+    }
+
     private static bool IsFinite(double value)
     {
         return !double.IsNaN(value) && !double.IsInfinity(value);

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPreviousTrajectorySeedBuilder.cs

@@ -55,6 +55,15 @@ public sealed class LongitudinalPreviousTrajectorySeedBuilder
 {
     private const double ProjectionTolerance = 1e-10d;
 
+    public LongitudinalPreviousTrajectorySeed Build(EmTrajectory previous, LateralPath currentPath,
+        DateTimeOffset newEffectiveAtUtc, LongitudinalKnotSchedule knotSchedule, int segmentIndex,
+        TravelDirection direction)
+    {
+        if (knotSchedule == null)
+            return LongitudinalPreviousTrajectorySeed.Empty;
+        return Build(previous, currentPath, newEffectiveAtUtc, knotSchedule.KnotTimes, segmentIndex, direction);
+    }
+
     public LongitudinalPreviousTrajectorySeed Build(EmTrajectory previous, LateralPath currentPath,
         DateTimeOffset newEffectiveAtUtc, IReadOnlyList<double> newKnotTimes, int segmentIndex,
         TravelDirection direction)

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs

@@ -23,12 +23,11 @@ public sealed class LongitudinalSolutionValidator
             {
                 return false;
             }
-            IReadOnlyList<double> expectedTimes = LongitudinalCandidate.CreateKnotTimes(
+            IReadOnlyList<double> expectedTimes = input.KnotSchedule.KnotTimes;
-                input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds);
             double tolerance = RequireNonnegative(input.Configuration.Validation.KinematicTolerance, nameof(tolerance));
             if (!HasMatchingTimes(candidate.KnotTimes, expectedTimes, tolerance))
             {
-                failureReason = "ST candidate knot times do not match the configured horizon.";
+                failureReason = "ST candidate knot times do not match the supplied knot schedule.";
                 return false;
             }
             if (candidate.S.Count != expectedTimes.Count || candidate.U.Count != expectedTimes.Count ||
@@ -88,8 +87,7 @@ public sealed class LongitudinalSolutionValidator
             int stabilizationStart = candidate.S.Count;
             if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
             {
-                stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
+                stabilizationStart = GetStabilizationStart(input, candidate.KnotTimes);
-                    candidate.KnotTimes, input.Configuration.Scheduling.OutputTimeStepSeconds);
                 for (int index = stabilizationStart; index < candidate.S.Count; index++)
                 {
                     if (!AreClose(candidate.S[index], input.StopBoundaryPathS, tolerance) ||
@@ -166,6 +164,21 @@ public sealed class LongitudinalSolutionValidator
         return true;
     }
 
+    private static int GetStabilizationStart(LongitudinalPlanningInput input, IReadOnlyList<double> times)
+    {
+        if (input.PlanningScope == EmPlanningScope.FullDirectionSegment)
+        {
+            if (input.KnotSchedule.TerminalHoldStartIndex < 1 ||
+                input.KnotSchedule.TerminalHoldStartIndex >= times.Count)
+            {
+                throw new ArgumentException("Full-direction exact-stop schedules require an explicit terminal hold boundary.");
+            }
+            return input.KnotSchedule.TerminalHoldStartIndex;
+        }
+        return LongitudinalTerminalSchedule.GetStabilizationStartIndex(times,
+            input.Configuration.Scheduling.OutputTimeStepSeconds);
+    }
+
     private static bool AreClose(double actual, double expected, double tolerance)
     {
         return Math.Abs(actual - expected) <= tolerance;

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs

@@ -1,5 +1,6 @@
 using System;
 using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
@@ -11,67 +12,112 @@ public sealed class PathSpeedLimitBuilder
     private const double StationMergeToleranceMeters = 1e-12d;
 
     public EmPlanningStatus Build(LongitudinalPlanningInput input, out PathSpeedLimit speedLimit, out string failureReason)
+    {
+        if (input == null)
+        {
+            speedLimit = null;
+            failureReason = "Longitudinal planning input is required.";
+            return EmPlanningStatus.InvalidInput;
+        }
+        return BuildCore(input.Path, input.Direction, input.InitialProgressSpeedMetersPerSecond,
+            input.InitialAccelerationMetersPerSecondSquared, input.TerminalType, input.Configuration,
+            out speedLimit, out failureReason);
+    }
+
+    public EmPlanningStatus Build(LateralPath path, TravelDirection direction,
+        double initialProgressSpeedMetersPerSecond, EmTerminalType terminalType,
+        EmPlannerConfiguration configuration, out PathSpeedLimit speedLimit, out string failureReason)
+    {
+        return BuildCore(path, direction, initialProgressSpeedMetersPerSecond, 0d, terminalType, configuration,
+            out speedLimit, out failureReason);
+    }
+
+    private EmPlanningStatus BuildCore(LateralPath path, TravelDirection direction,
+        double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
+        EmTerminalType terminalType, EmPlannerConfiguration configuration, out PathSpeedLimit speedLimit,
+        out string failureReason)
     {
         speedLimit = null;
         failureReason = string.Empty;
-        if (input == null)
+        if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2 ||
+            !Enum.IsDefined(typeof(TravelDirection), direction) || !Enum.IsDefined(typeof(EmTerminalType), terminalType) ||
+            configuration == null || !IsFinite(initialProgressSpeedMetersPerSecond) ||
+            initialProgressSpeedMetersPerSecond < 0d || !IsFinite(initialAccelerationMetersPerSecondSquared))
         {
             failureReason = "Longitudinal planning input is required.";
             return EmPlanningStatus.InvalidInput;
         }
 
-        if (!TryGetLimits(input, out double directionMaximum, out double maximumAcceleration, out double maximumDeceleration,
+        if (configuration.Longitudinal == null)
-                out double maximumJerk, out double maximumLateralAcceleration, out double maximumCurvatureRate,
-                out failureReason))
         {
+            failureReason = "Longitudinal configuration is required.";
             return EmPlanningStatus.InvalidInput;
         }
-        if (input.InitialProgressSpeedMetersPerSecond > directionMaximum + StopDistanceToleranceMeters ||
+        LongitudinalConfiguration longitudinal = configuration.Longitudinal;
-            input.InitialAccelerationMetersPerSecondSquared < -maximumDeceleration - StopDistanceToleranceMeters ||
+        double directionMaximum = direction == TravelDirection.Forward
-            input.InitialAccelerationMetersPerSecondSquared > maximumAcceleration + StopDistanceToleranceMeters)
+            ? longitudinal.MaximumForwardSpeedMetersPerSecond
+            : longitudinal.MaximumReverseSpeedMetersPerSecond;
+        double maximumAcceleration = longitudinal.MaximumAccelerationMetersPerSecondSquared;
+        double maximumDeceleration = longitudinal.MaximumDecelerationMetersPerSecondSquared;
+        double maximumJerk = longitudinal.MaximumJerkMetersPerSecondCubed;
+        double maximumLateralAcceleration = longitudinal.MaximumLateralAccelerationMetersPerSecondSquared;
+        double maximumCurvatureRate = longitudinal.MaximumCurvatureRatePerMeterPerSecond;
+        if (!IsPositiveFinite(directionMaximum) || !IsPositiveFinite(maximumAcceleration) ||
+            !IsPositiveFinite(maximumDeceleration) || !IsPositiveFinite(maximumJerk) ||
+            !IsPositiveFinite(maximumLateralAcceleration) || !IsPositiveFinite(maximumCurvatureRate))
+        {
+            failureReason = "Longitudinal limits must be positive and finite.";
+            return EmPlanningStatus.InvalidInput;
+        }
+        if (initialProgressSpeedMetersPerSecond > directionMaximum + StopDistanceToleranceMeters ||
+            initialAccelerationMetersPerSecondSquared < -maximumDeceleration - StopDistanceToleranceMeters ||
+            initialAccelerationMetersPerSecondSquared > maximumAcceleration + StopDistanceToleranceMeters)
         {
             failureReason = "The initial longitudinal state violates the configured hard bounds.";
             return EmPlanningStatus.InvalidInput;
         }
 
-        if (input.HasStopBoundary)
+        bool hasStopBoundary = terminalType != EmTerminalType.RollingSafetyStop;
+        double stopBoundaryPathS = path.Points[path.Points.Count - 1].PathS;
+        if (hasStopBoundary)
         {
-            if (!JerkLimitedStoppingMath.TryCalculate(input.InitialProgressSpeedMetersPerSecond,
+            if (!JerkLimitedStoppingMath.TryCalculate(initialProgressSpeedMetersPerSecond,
-                    input.InitialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk,
+                    initialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk,
                     out JerkLimitedStoppingProfile stopProfile, out failureReason))
             {
                 return EmPlanningStatus.InvalidInput;
             }
-            if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > input.StopBoundaryPathS)
+            if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > stopBoundaryPathS)
             {
                 failureReason = "The available actual PathS distance is insufficient for the jerk-limited stop.";
                 return EmPlanningStatus.StoppingDistanceInsufficient;
             }
         }
-        if (input.Configuration.Scheduling == null || !IsPositiveFinite(input.Configuration.Scheduling.OutputTimeStepSeconds))
+        if (configuration.Scheduling == null ||
+            !IsPositiveFinite(configuration.Scheduling.MaximumOptimizationSpatialStepMeters))
         {
-            failureReason = "The output time step required to refine the PathS speed envelope is invalid.";
+            failureReason = "The optimization spatial step required to refine the PathS speed envelope is invalid.";
             return EmPlanningStatus.InvalidInput;
         }
 
-        double maximumStationSpacing = directionMaximum * input.Configuration.Scheduling.OutputTimeStepSeconds;
+        double maximumStationSpacing = configuration.Scheduling.MaximumOptimizationSpatialStepMeters;
         var pathS = new List<double>();
         var maximum = new List<double>();
         var lateral = new List<double>();
         var curvatureRate = new List<double>();
         var stopping = new List<double>();
-        for (int segmentIndex = 0; segmentIndex < input.Path.Points.Count - 1; segmentIndex++)
+        for (int segmentIndex = 0; segmentIndex < path.Points.Count - 1; segmentIndex++)
         {
-            LateralPathPoint lowerPoint = input.Path.Points[segmentIndex];
+            LateralPathPoint lowerPoint = path.Points[segmentIndex];
-            LateralPathPoint upperPoint = input.Path.Points[segmentIndex + 1];
+            LateralPathPoint upperPoint = path.Points[segmentIndex + 1];
             double span = upperPoint.PathS - lowerPoint.PathS;
             int subdivisions = Math.Max(1, checked((int)Math.Ceiling(span / maximumStationSpacing)));
             var segmentStations = new List<double>(subdivisions + 16);
             for (int subdivision = segmentIndex == 0 ? 0 : 1; subdivision <= subdivisions; subdivision++)
                 segmentStations.Add(Interpolate(lowerPoint.PathS, upperPoint.PathS, (double)subdivision / subdivisions));
-            if (input.HasStopBoundary)
+            if (hasStopBoundary)
             {
-                AddJerkLimitedStoppingStations(lowerPoint.PathS, upperPoint.PathS, input.StopBoundaryPathS,
+                AddJerkLimitedStoppingStations(lowerPoint.PathS, upperPoint.PathS, stopBoundaryPathS,
                     directionMaximum, maximumAcceleration, maximumDeceleration, maximumJerk,
                     segmentIndex == 0, segmentStations);
             }
@@ -87,8 +133,8 @@ public sealed class PathSpeedLimitBuilder
                 double curvature = Interpolate(lowerPoint.VehicleCurvature, upperPoint.VehicleCurvature, fraction);
                 double curvatureDerivative = Interpolate(lowerPoint.VehicleCurvatureDerivative,
                     upperPoint.VehicleCurvatureDerivative, fraction);
-                AddLimitSample(samplePathS, curvature, curvatureDerivative, input.HasStopBoundary,
+                AddLimitSample(samplePathS, curvature, curvatureDerivative, hasStopBoundary,
-                    input.StopBoundaryPathS, directionMaximum, maximumAcceleration, maximumDeceleration,
+                    stopBoundaryPathS, directionMaximum, maximumAcceleration, maximumDeceleration,
                     maximumJerk, maximumLateralAcceleration, maximumCurvatureRate, pathS, maximum, lateral,
                     curvatureRate, stopping);
             }
@@ -97,7 +143,7 @@ public sealed class PathSpeedLimitBuilder
         try
         {
             speedLimit = new PathSpeedLimit(pathS, maximum, lateral, curvatureRate, stopping, directionMaximum,
-                input.HasStopBoundary);
+                hasStopBoundary);
             return EmPlanningStatus.Success;
         }
         catch (ArgumentException exception)

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs

@@ -47,18 +47,40 @@ public sealed class SequentialLongitudinalOptimizer
         if (speedStatus != EmPlanningStatus.Success)
             return Failed(speedStatus, speedFailure);
 
-        LongitudinalCandidate iterate = CreateInitialIterate(input, speedLimit);
+        var stopwatch = Stopwatch.StartNew();
+        LongitudinalCandidate iterate;
+        LongitudinalCandidate lastStrictCandidate = null;
+        int remainingObjectiveIterations = iterationLimit;
+        if (input.PlanningScope == EmPlanningScope.FullDirectionSegment &&
+            input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
+        {
+            if (!TryCreateInitialFeasibleCandidate(input, speedLimit, settings, totalBudget, convergenceTolerance,
+                    iterationLimit, stopwatch, cancellationToken, out iterate, out int projectionSolveCount,
+                    out EmPlanningStatus projectionStatus, out string projectionFailure))
+            {
+                return Failed(projectionStatus, projectionFailure);
+            }
+            lastStrictCandidate = CopyCandidate(iterate);
+            remainingObjectiveIterations -= projectionSolveCount;
+            if (remainingObjectiveIterations <= 0)
+            {
+                return new LongitudinalPlanningResult(EmPlanningStatus.SuccessWithFallback, lastStrictCandidate,
+                    "The strict initial feasibility projection consumed the configured outer-iteration budget.");
+            }
+        }
+        else
+        {
+            iterate = CreateInitialIterate(input, speedLimit);
+            if (!_solutionValidator.TryValidate(input, speedLimit, iterate, out lastStrictCandidate, out _))
+                lastStrictCandidate = null;
+        }
         double[] warmStart = ToPrimal(iterate);
         bool hasDynamicsConsistentInitialWarmStart = iterate.SatisfiesExactDiscreteDynamics(1e-12d);
-        LongitudinalCandidate lastStrictCandidate;
-        if (!_solutionValidator.TryValidate(input, speedLimit, iterate, out lastStrictCandidate, out _))
-            lastStrictCandidate = null;
         string lastCandidateRejection = string.Empty;
         bool hasPreviousObjective = false;
         double previousObjective = 0d;
-        var stopwatch = Stopwatch.StartNew();
 
-        for (int iteration = 0; iteration < iterationLimit; iteration++)
+        for (int iteration = 0; iteration < remainingObjectiveIterations; iteration++)
         {
             if (cancellationToken.IsCancellationRequested)
                 return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled.");
@@ -191,10 +213,132 @@ public sealed class SequentialLongitudinalOptimizer
         }
     }
 
+    private bool TryCreateInitialFeasibleCandidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
+        QpSolverSettings settings, TimeSpan totalBudget, double convergenceTolerance, int iterationLimit,
+        Stopwatch stopwatch, CancellationToken cancellationToken, out LongitudinalCandidate candidate,
+        out int projectionSolveCount, out EmPlanningStatus failureStatus, out string failureReason)
+    {
+        candidate = null;
+        projectionSolveCount = 0;
+        failureStatus = EmPlanningStatus.LongitudinalInfeasible;
+        failureReason = string.Empty;
+        LongitudinalCandidate linearizationIterate = CreateScheduleReferenceIterate(input);
+        string lastRejection = string.Empty;
+        for (int iteration = 0; iteration < iterationLimit; iteration++)
+        {
+            if (cancellationToken.IsCancellationRequested)
+            {
+                failureStatus = EmPlanningStatus.Cancelled;
+                failureReason = "Initial full-direction feasibility projection was cancelled.";
+                return false;
+            }
+            TimeSpan remainingBudget = totalBudget - stopwatch.Elapsed;
+            if (remainingBudget <= TimeSpan.Zero)
+            {
+                failureStatus = EmPlanningStatus.SolverTimedOut;
+                failureReason = "Initial full-direction feasibility projection exhausted the shared solve budget.";
+                return false;
+            }
+            if (!_constraintBuilder.TryBuildInitialFeasibilityProjection(input, speedLimit, linearizationIterate,
+                    out QuadraticProgram problem, out string buildFailure))
+            {
+                failureStatus = EmPlanningStatus.LongitudinalInfeasible;
+                failureReason = "Initial full-direction feasibility constraints are infeasible: " + buildFailure;
+                return false;
+            }
+
+            double projectionTolerance = Math.Min(settings.AbsoluteTolerance,
+                input.Configuration.Validation.KinematicTolerance * 0.1d);
+            QpSolveResult solved = _qpSolver.Solve(problem,
+                new QpSolverSettings(settings.MaximumIterations, projectionTolerance, projectionTolerance,
+                    remainingBudget, settings.EnableWarmStart && linearizationIterate.SatisfiesExactDiscreteDynamics(1e-12d),
+                    settings.EnablePolishing, settings.EnableNativeVerboseOutput),
+                ToPrimal(linearizationIterate), cancellationToken);
+            projectionSolveCount++;
+            if (solved == null)
+            {
+                failureStatus = EmPlanningStatus.Failed;
+                failureReason = "The initial full-direction feasibility solver returned no result.";
+                return false;
+            }
+            if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
+            {
+                failureStatus = EmPlanningStatus.SolverTimedOut;
+                failureReason = "Initial full-direction feasibility projection timed out (status=" + solved.NativeStatus +
+                    ", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual + ", dual=" +
+                    solved.DualResidual + "): " + solved.Diagnostic;
+                return false;
+            }
+            if (solved.Status == QpSolveStatus.Cancelled)
+            {
+                failureStatus = EmPlanningStatus.Cancelled;
+                failureReason = "Initial full-direction feasibility projection was cancelled: " + solved.Diagnostic;
+                return false;
+            }
+            if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
+            {
+                failureStatus = EmPlanningStatus.LongitudinalInfeasible;
+                failureReason = "Initial full-direction feasibility projection is infeasible: " + solved.Diagnostic;
+                return false;
+            }
+            if (solved.Status == QpSolveStatus.SolverUnavailable)
+            {
+                failureStatus = EmPlanningStatus.SolverUnavailable;
+                failureReason = "Initial full-direction feasibility solver is unavailable: " + solved.Diagnostic;
+                return false;
+            }
+            if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
+            {
+                failureStatus = EmPlanningStatus.Failed;
+                failureReason = "Initial full-direction feasibility solver failed: " + solved.Diagnostic;
+                return false;
+            }
+            if (!TryCreateCandidate(input.KnotSchedule.KnotTimes, solved.Primal, out LongitudinalCandidate projected))
+            {
+                failureStatus = EmPlanningStatus.LongitudinalInfeasible;
+                failureReason = "Initial full-direction feasibility solver primal does not match the ST layout.";
+                return false;
+            }
+            if (solved.Status == QpSolveStatus.Solved || HasStrictResiduals(solved, convergenceTolerance))
+            {
+                if (_solutionValidator.TryValidate(input, speedLimit, projected, out LongitudinalCandidate strict,
+                        out string validationFailure))
+                {
+                    candidate = strict;
+                    return true;
+                }
+                lastRejection = validationFailure;
+            }
+
+            if (!TryCreateFeasibilityEnvelopeIterate(input, projected,
+                    out LongitudinalCandidate nextLinearization))
+            {
+                failureStatus = EmPlanningStatus.LongitudinalInfeasible;
+                failureReason = "Initial full-direction feasibility candidate could not be relinearized against the PathS envelope.";
+                return false;
+            }
+            linearizationIterate = nextLinearization;
+            if (solved.Status == QpSolveStatus.SolvedInaccurate)
+                lastRejection = "Initial feasibility projection residuals exceed the strict acceptance tolerance.";
+            else if (string.IsNullOrEmpty(lastRejection))
+                lastRejection = "Initial feasibility projection violated the strict physical validator.";
+        }
+        failureStatus = EmPlanningStatus.LongitudinalInfeasible;
+        failureReason = "Initial full-direction feasibility projection exhausted the configured outer iterations. " +
+            lastRejection;
+        return false;
+    }
+
+    private static LongitudinalCandidate CreateScheduleReferenceIterate(LongitudinalPlanningInput input)
+    {
+        int knotCount = input.KnotSchedule.KnotTimes.Count;
+        return new LongitudinalCandidate(input.KnotSchedule.KnotTimes, input.KnotSchedule.ReferencePathS,
+            input.KnotSchedule.ReferenceSpeedMetersPerSecond, new double[knotCount], new double[knotCount - 1]);
+    }
+
     private LongitudinalCandidate CreateInitialIterate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit)
     {
-        IReadOnlyList<double> times = LongitudinalCandidate.CreateKnotTimes(input.Configuration.Scheduling.TimeHorizonSeconds,
+        IReadOnlyList<double> times = input.KnotSchedule.KnotTimes;
-            input.Configuration.Scheduling.OutputTimeStepSeconds);
         switch (input.Mode)
         {
             case EmLongitudinalMode.RollingContinuation:
@@ -269,6 +413,10 @@ public sealed class SequentialLongitudinalOptimizer
     private LongitudinalCandidate CreateExactStopSeed(LongitudinalPlanningInput input,
         IReadOnlyList<double> times, PathSpeedLimit speedLimit)
     {
+        if (input.PlanningScope == EmPlanningScope.FullDirectionSegment)
+        {
+            throw new InvalidOperationException("Full-direction exact-stop planning requires the initial feasibility projection.");
+        }
         int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(times,
             input.Configuration.Scheduling.OutputTimeStepSeconds);
         var motionTimes = new double[stabilizationStart + 1];
@@ -364,7 +512,7 @@ public sealed class SequentialLongitudinalOptimizer
         for (int index = 0; index < motionTimes.Length; index++)
             motionTimes[index] = times[index];
 
-        LongitudinalCandidate baseline = CreateApproachSeed(input, motionTimes, speedLimit);
+        LongitudinalCandidate baseline = CreateScheduleReferenceSeed(input, motionTimes, speedLimit);
         var influence = new double[3, intervalCount];
         for (int interval = 0; interval < intervalCount; interval++)
         {
@@ -461,6 +609,30 @@ public sealed class SequentialLongitudinalOptimizer
         return AppendExactStopTail(times, stabilizationStart, input.StopBoundaryPathS, motion);
     }
 
+    private static LongitudinalCandidate CreateScheduleReferenceSeed(LongitudinalPlanningInput input,
+        IReadOnlyList<double> times, PathSpeedLimit speedLimit)
+    {
+        LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
+        var jerk = new double[times.Count - 1];
+        double speed = input.InitialProgressSpeedMetersPerSecond;
+        double acceleration = input.InitialAccelerationMetersPerSecondSquared;
+        for (int index = 0; index < jerk.Length; index++)
+        {
+            double dt = times[index + 1] - times[index];
+            double targetSpeed = input.KnotSchedule.ReferenceSpeedMetersPerSecond[index + 1];
+            double lowerJerk = Math.Max(-configuration.MaximumJerkMetersPerSecondCubed,
+                (-configuration.MaximumDecelerationMetersPerSecondSquared - acceleration) / dt);
+            double upperJerk = Math.Min(configuration.MaximumJerkMetersPerSecondCubed,
+                (configuration.MaximumAccelerationMetersPerSecondSquared - acceleration) / dt);
+            double requestedJerk = 2d * (targetSpeed - speed - acceleration * dt) / (dt * dt);
+            double selectedJerk = Clamp(requestedJerk, lowerJerk, upperJerk);
+            jerk[index] = selectedJerk;
+            IntegrateStep(0d, speed, acceleration, selectedJerk, dt, out _, out speed, out acceleration);
+        }
+        return LongitudinalCandidate.Integrate(times, 0d, input.InitialProgressSpeedMetersPerSecond,
+            input.InitialAccelerationMetersPerSecondSquared, jerk);
+    }
+
     private static double[] CreateEndpointNullspaceDirection(double[,] influence, double[,] gram, int basisIndex)
     {
         int intervalCount = influence.GetLength(1);
@@ -679,10 +851,12 @@ public sealed class SequentialLongitudinalOptimizer
         nextIterate = null;
         if (candidate.S.Count != previous.S.Count)
             return false;
-        int stabilizationStart = input.Mode == EmLongitudinalMode.ExactStopAtBoundary
+        int stabilizationStart = input.Mode != EmLongitudinalMode.ExactStopAtBoundary
-            ? LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes,
+            ? candidate.S.Count
-                input.Configuration.Scheduling.OutputTimeStepSeconds)
+            : input.PlanningScope == EmPlanningScope.FullDirectionSegment
-            : candidate.S.Count;
+                ? input.KnotSchedule.TerminalHoldStartIndex
+                : LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes,
+                    input.Configuration.Scheduling.OutputTimeStepSeconds);
         var candidateProgressSamples = new double[candidate.S.Count];
         double priorProgress = double.NegativeInfinity;
         double priorPreviousProgress = double.NegativeInfinity;
@@ -737,6 +911,30 @@ public sealed class SequentialLongitudinalOptimizer
         return true;
     }
 
+    private static bool TryCreateFeasibilityEnvelopeIterate(LongitudinalPlanningInput input,
+        LongitudinalCandidate candidate, out LongitudinalCandidate nextIterate)
+    {
+        nextIterate = null;
+        int stabilizationStart = input.KnotSchedule.TerminalHoldStartIndex;
+        var pathS = new double[candidate.S.Count];
+        double previousPathS = double.NegativeInfinity;
+        double tolerance = input.Configuration.Validation.KinematicTolerance;
+        for (int index = 0; index < pathS.Length; index++)
+        {
+            double value = candidate.S[index];
+            if (!IsFinite(value) || value < -tolerance || value > input.PathUpperBoundS + tolerance ||
+                value < previousPathS - tolerance)
+            {
+                return false;
+            }
+            value = Math.Max(0d, Math.Min(input.PathUpperBoundS, value));
+            pathS[index] = index >= stabilizationStart ? input.StopBoundaryPathS : Math.Max(previousPathS, value);
+            previousPathS = pathS[index];
+        }
+        nextIterate = new LongitudinalCandidate(candidate.KnotTimes, pathS, candidate.U, candidate.A, candidate.J);
+        return true;
+    }
+
     private static bool HasStrictResiduals(QpSolveResult result, double tolerance)
     {
         return IsPositiveFinite(tolerance) && result.PrimalResidual >= 0d && result.DualResidual >= 0d &&

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs

@@ -39,8 +39,9 @@ public sealed class EmTrajectoryAssembler
             throw new ArgumentNullException(nameof(metadata));
 
         var interpolator = new LateralPathInterpolator(path);
-        var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds, zeroSpeedHoldSeconds,
+        bool isFullDirectionSegment = metadata.PlanningScope == EmPlanningScope.FullDirectionSegment;
-            metadata.LongitudinalMode);
+        var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds,
+            isFullDirectionSegment ? 0d : zeroSpeedHoldSeconds, metadata.LongitudinalMode, isFullDirectionSegment);
         double terminalPathS = path.Points[path.Points.Count - 1].PathS;
         var points = new List<EmTrajectoryPoint>(schedule.Samples.Count);
         double directionSign = metadata.Direction == TravelDirection.Forward ? 1d : -1d;

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs

@@ -9,7 +9,7 @@ internal sealed class TrajectorySampleSchedule
     private const double ZeroTolerance = 1e-12d;
 
     public TrajectorySampleSchedule(LongitudinalCandidate candidate, double outputTimeStepSeconds, double holdDurationSeconds,
-        EmLongitudinalMode mode)
+        EmLongitudinalMode mode, bool resampleMotion)
     {
         if (candidate == null)
             throw new ArgumentNullException(nameof(candidate));
@@ -22,16 +22,25 @@ internal sealed class TrajectorySampleSchedule
 
         var samples = new List<TrajectorySample>(candidate.KnotTimes.Count + 4);
         double previousPathS = double.NegativeInfinity;
-        for (int index = 0; index < candidate.KnotTimes.Count; index++)
+        if (resampleMotion)
         {
-            if (candidate.S[index] < previousPathS)
+            double finalTime = candidate.KnotTimes[candidate.KnotTimes.Count - 1];
-                throw new ArgumentException("Trajectory PathS cannot decrease.", nameof(candidate));
+            int sourceInterval = 0;
-            if (candidate.U[index] < -ZeroTolerance)
+            for (double sampleTime = 0d; sampleTime < finalTime - ZeroTolerance;
-                throw new ArgumentException("Longitudinal progress speed cannot be negative.", nameof(candidate));
+                sampleTime += outputTimeStepSeconds)
-
+            {
-            samples.Add(new TrajectorySample(candidate.KnotTimes[index], candidate.S[index], Math.Max(0d, candidate.U[index]),
+                AddSample(Interpolate(candidate, sampleTime, ref sourceInterval), samples, ref previousPathS, candidate);
-                candidate.A[index], index < candidate.J.Count ? candidate.J[index] : 0d, false));
+            }
-            previousPathS = candidate.S[index];
+            AddSample(Interpolate(candidate, finalTime, ref sourceInterval), samples, ref previousPathS, candidate);
+        }
+        else
+        {
+            for (int index = 0; index < candidate.KnotTimes.Count; index++)
+            {
+                AddSample(new TrajectorySample(candidate.KnotTimes[index], candidate.S[index],
+                    Math.Max(0d, candidate.U[index]), candidate.A[index], index < candidate.J.Count ? candidate.J[index] : 0d,
+                    false), samples, ref previousPathS, candidate);
+            }
         }
 
         if (mode != EmLongitudinalMode.ExactStopAtBoundary)
@@ -41,14 +50,22 @@ internal sealed class TrajectorySampleSchedule
             return;
         }
 
-        int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes,
+        int sourceStabilizationStart = resampleMotion
-            outputTimeStepSeconds);
+            ? FindTerminalStationaryTailStart(candidate)
-        double stopPathS = candidate.S[stabilizationStart];
+            : LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes, outputTimeStepSeconds);
-        for (int index = stabilizationStart; index < candidate.S.Count; index++)
+        double stabilizationStartTime = candidate.KnotTimes[sourceStabilizationStart];
+        int stabilizationStart = 0;
+        while (stabilizationStart < samples.Count - 1 &&
+            samples[stabilizationStart].TimeFromStart < stabilizationStartTime - ZeroTolerance)
         {
-            if (Math.Abs(candidate.S[index] - stopPathS) > ZeroTolerance ||
+            stabilizationStart++;
-                Math.Abs(candidate.U[index]) > ZeroTolerance || Math.Abs(candidate.A[index]) > ZeroTolerance ||
+        }
-                (index < candidate.J.Count && Math.Abs(candidate.J[index]) > ZeroTolerance))
+        double stopPathS = samples[stabilizationStart].PathS;
+        for (int index = stabilizationStart; index < samples.Count; index++)
+        {
+            if (Math.Abs(samples[index].PathS - stopPathS) > ZeroTolerance ||
+                Math.Abs(samples[index].ProgressSpeed) > ZeroTolerance || Math.Abs(samples[index].Acceleration) > ZeroTolerance ||
+                Math.Abs(samples[index].Jerk) > ZeroTolerance)
             {
                 throw new ArgumentException("An exact stop requires a stationary S/U/A/J tail.", nameof(candidate));
             }
@@ -69,6 +86,57 @@ internal sealed class TrajectorySampleSchedule
     public IReadOnlyList<TrajectorySample> Samples { get; }
     public int TerminalAnchorSampleIndex { get; }
 
+    private static void AddSample(TrajectorySample sample, ICollection<TrajectorySample> samples,
+        ref double previousPathS, LongitudinalCandidate candidate)
+    {
+        if (sample.PathS < previousPathS)
+            throw new ArgumentException("Trajectory PathS cannot decrease.", nameof(candidate));
+        if (sample.ProgressSpeed < -ZeroTolerance)
+            throw new ArgumentException("Longitudinal progress speed cannot be negative.", nameof(candidate));
+        samples.Add(sample);
+        previousPathS = sample.PathS;
+    }
+
+    private static TrajectorySample Interpolate(LongitudinalCandidate candidate, double sampleTime, ref int sourceInterval)
+    {
+        int lastKnot = candidate.KnotTimes.Count - 1;
+        if (sampleTime >= candidate.KnotTimes[lastKnot] - ZeroTolerance)
+        {
+            return new TrajectorySample(candidate.KnotTimes[lastKnot], candidate.S[lastKnot],
+                Math.Max(0d, candidate.U[lastKnot]), candidate.A[lastKnot], 0d, false);
+        }
+        while (sourceInterval < lastKnot - 1 &&
+            sampleTime >= candidate.KnotTimes[sourceInterval + 1] - ZeroTolerance)
+        {
+            sourceInterval++;
+        }
+        if (Math.Abs(sampleTime - candidate.KnotTimes[sourceInterval]) <= ZeroTolerance)
+        {
+            return new TrajectorySample(candidate.KnotTimes[sourceInterval], candidate.S[sourceInterval],
+                Math.Max(0d, candidate.U[sourceInterval]), candidate.A[sourceInterval], candidate.J[sourceInterval], false);
+        }
+        double dt = sampleTime - candidate.KnotTimes[sourceInterval];
+        double jerk = candidate.J[sourceInterval];
+        double acceleration = candidate.A[sourceInterval] + jerk * dt;
+        double speed = candidate.U[sourceInterval] + candidate.A[sourceInterval] * dt + 0.5d * jerk * dt * dt;
+        double pathS = candidate.S[sourceInterval] + candidate.U[sourceInterval] * dt +
+            0.5d * candidate.A[sourceInterval] * dt * dt + jerk * dt * dt * dt / 6d;
+        return new TrajectorySample(sampleTime, pathS, Math.Max(0d, speed), acceleration, jerk, false);
+    }
+
+    private static int FindTerminalStationaryTailStart(LongitudinalCandidate candidate)
+    {
+        int start = candidate.KnotTimes.Count - 1;
+        double terminalPathS = candidate.S[start];
+        while (start > 0 && Math.Abs(candidate.S[start - 1] - terminalPathS) <= ZeroTolerance &&
+            Math.Abs(candidate.U[start - 1]) <= ZeroTolerance && Math.Abs(candidate.A[start - 1]) <= ZeroTolerance &&
+            Math.Abs(candidate.J[start - 1]) <= ZeroTolerance)
+        {
+            start--;
+        }
+        return start;
+    }
+
     private static bool IsFinite(double value)
     {
         return !double.IsNaN(value) && !double.IsInfinity(value);

ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs

@@ -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)
+        private static void ConfigureExactStopServiceScenario(EmPlannerConfiguration configuration)
-    {
+        {
-        configuration.Scheduling.TimeHorizonSeconds = 0.40d;
+            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];
+            int stabilizationStart = FindExactStopTailStart(problem, layout);
-            const int rampIntervals = 5;
+            if (stabilizationStart < 3)
-            for (int index = 0; index < rampIntervals; index++) jerk[index] = 1d;
+                return false;
-            for (int index = rampIntervals; index < 3 * rampIntervals; index++) jerk[index] = -1d;
-            for (int index = 3 * rampIntervals; index < 4 * rampIntervals; index++) jerk[index] = 1d;
             var times = new double[knotCount];
-            for (int index = 0; index < times.Length; index++) times[index] = index * 0.05d;
+            for (int index = 0; index < knotCount - 1; index++)
-            LongitudinalCandidate baseCandidate = LongitudinalCandidate.Integrate(times, 0d, 0d, 0d, jerk);
+            {
-            double terminalPathS = ReadFixedVariable(problem, layout.S(knotCount - 1));
+                if (!TryReadDynamicsDuration(problem, layout, index, out double duration))
-            double scale = terminalPathS / baseCandidate.S[baseCandidate.S.Count - 1];
+                    return false;
-            for (int index = 0; index < jerk.Length; index++) jerk[index] *= scale;
+                times[index + 1] = times[index] + duration;
-            LongitudinalCandidate candidate = LongitudinalCandidate.Integrate(times, 0d, 0d, 0d, jerk);
+            }
+
+            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];
+                bool isTerminalTail = index >= stabilizationStart;
-                primal[layout.U(index)] = candidate.U[index];
+                primal[layout.S(index)] = isTerminalTail ? terminalPathS : candidate.S[index];
-                primal[layout.A(index)] = candidate.A[index];
+                primal[layout.U(index)] = isTerminalTail ? 0d : candidate.U[index];
-            }
+                primal[layout.A(index)] = isTerminalTail ? 0d : 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;
             }
+            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)

ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs

@@ -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())

ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs

@@ -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,
+        Verification.NearlyEqual(input.DirectionMaximumSpeedMetersPerSecond, uUpper,
-            "U upper bound samples envelope at current S iterate");
+            "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");