feat: derive adaptive full-segment ST schedule

2026-08-06 23:54:34 +08:00
parent dad4ff4b04
commit 3269d556b6
14 changed files with 1642 additions and 108 deletions
@@ -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(
-                configuration.Scheduling.TimeHorizonSeconds, configuration.Scheduling.OutputTimeStepSeconds);
+            EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(lateral.Path, segment.Direction,
+                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;
@@ -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;
+}
@@ -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(
-                input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds);
+            IReadOnlyList<double> expectedTimes = input.KnotSchedule.KnotTimes;
            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);
-            int stabilizationStart = input.Mode == EmLongitudinalMode.ExactStopAtBoundary
-                ? LongitudinalTerminalSchedule.GetStabilizationStartIndex(expectedTimes,
-                    input.Configuration.Scheduling.OutputTimeStepSeconds)
-                : layout.KnotCount;
+            if (useScheduleReferenceObjective)
+                AddInitialFeasibilityObjective(input, layout, hessian, linearCost);
+            else
+                _objectiveBuilder.AddTerms(input, speedLimit, layout, iterate, hessian, linearCost);
+            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)
@@ -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);
+
+}
@@ -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);
@@ -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)
@@ -23,12 +23,11 @@ public sealed class LongitudinalSolutionValidator
            {
                return false;
            }
-            IReadOnlyList<double> expectedTimes = LongitudinalCandidate.CreateKnotTimes(
-                input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds);
+            IReadOnlyList<double> expectedTimes = input.KnotSchedule.KnotTimes;
            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(
-                    candidate.KnotTimes, input.Configuration.Scheduling.OutputTimeStepSeconds);
+                stabilizationStart = GetStabilizationStart(input, candidate.KnotTimes);
                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;
@@ -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,
-                out double maximumJerk, out double maximumLateralAcceleration, out double maximumCurvatureRate,
-                out failureReason))
+        if (configuration.Longitudinal == null)
        {
+            failureReason = "Longitudinal configuration is required.";
            return EmPlanningStatus.InvalidInput;
        }
-        if (input.InitialProgressSpeedMetersPerSecond > directionMaximum + StopDistanceToleranceMeters ||
-            input.InitialAccelerationMetersPerSecondSquared < -maximumDeceleration - StopDistanceToleranceMeters ||
-            input.InitialAccelerationMetersPerSecondSquared > maximumAcceleration + StopDistanceToleranceMeters)
+        LongitudinalConfiguration longitudinal = configuration.Longitudinal;
+        double directionMaximum = direction == TravelDirection.Forward
+            ? 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,
-                    input.InitialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk,
+            if (!JerkLimitedStoppingMath.TryCalculate(initialProgressSpeedMetersPerSecond,
+                    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 upperPoint = input.Path.Points[segmentIndex + 1];
+            LateralPathPoint lowerPoint = path.Points[segmentIndex];
+            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,
-                    input.StopBoundaryPathS, directionMaximum, maximumAcceleration, maximumDeceleration,
+                AddLimitSample(samplePathS, curvature, curvatureDerivative, hasStopBoundary,
+                    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)
@@ -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,
-            input.Configuration.Scheduling.OutputTimeStepSeconds);
+        IReadOnlyList<double> times = input.KnotSchedule.KnotTimes;
        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
-            ? LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes,
-                input.Configuration.Scheduling.OutputTimeStepSeconds)
-            : candidate.S.Count;
+        int stabilizationStart = input.Mode != EmLongitudinalMode.ExactStopAtBoundary
+            ? candidate.S.Count
+            : input.PlanningScope == EmPlanningScope.FullDirectionSegment
+                ? 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 &&
@@ -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,
-            metadata.LongitudinalMode);
+        bool isFullDirectionSegment = metadata.PlanningScope == EmPlanningScope.FullDirectionSegment;
+        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;
@@ -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)
-                throw new ArgumentException("Trajectory PathS cannot decrease.", nameof(candidate));
-            if (candidate.U[index] < -ZeroTolerance)
-                throw new ArgumentException("Longitudinal progress speed cannot be negative.", nameof(candidate));
-
-            samples.Add(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));
-            previousPathS = candidate.S[index];
+            double finalTime = candidate.KnotTimes[candidate.KnotTimes.Count - 1];
+            int sourceInterval = 0;
+            for (double sampleTime = 0d; sampleTime < finalTime - ZeroTolerance;
+                sampleTime += outputTimeStepSeconds)
+            {
+                AddSample(Interpolate(candidate, sampleTime, ref sourceInterval), samples, ref previousPathS, candidate);
+            }
+            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,
-            outputTimeStepSeconds);
-        double stopPathS = candidate.S[stabilizationStart];
-        for (int index = stabilizationStart; index < candidate.S.Count; index++)
+        int sourceStabilizationStart = resampleMotion
+            ? FindTerminalStationaryTailStart(candidate)
+            : LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes, outputTimeStepSeconds);
+        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 ||
-                Math.Abs(candidate.U[index]) > ZeroTolerance || Math.Abs(candidate.A[index]) > ZeroTolerance ||
-                (index < candidate.J.Count && Math.Abs(candidate.J[index]) > ZeroTolerance))
+            stabilizationStart++;
+        }
+        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);