feat: seed rolling and exact-stop ST profiles

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalObjectiveBuilder.cs

@@ -21,11 +21,12 @@ public sealed class LongitudinalObjectiveBuilder
         double accelerationScale = RequirePositive(Math.Max(configuration.MaximumAccelerationMetersPerSecondSquared,
             configuration.MaximumDecelerationMetersPerSecondSquared), nameof(accelerationScale));
         double jerkScale = RequirePositive(configuration.MaximumJerkMetersPerSecondCubed, nameof(jerkScale));
-        double progressScale = input.TerminalPathS > 0d ? input.TerminalPathS : 1d;
+        double progressScale = input.PathUpperBoundS > 0d ? input.PathUpperBoundS : 1d;
 
         for (int index = 0; index < layout.KnotCount; index++)
         {
-            AddSquaredResidual(hessian, linearCost, layout.U(index), speedLimit.MaximumSpeedAt(iterate.S[index]),
+            double iteratePathS = Math.Max(0d, Math.Min(input.PathUpperBoundS, iterate.S[index]));
+            AddSquaredResidual(hessian, linearCost, layout.U(index), speedLimit.MaximumSpeedAt(iteratePathS),
                 weights.ReferenceSpeed, speedScale);
             AddSquaredResidual(hessian, linearCost, layout.A(index), 0d, weights.Acceleration, accelerationScale);
             if (index < layout.KnotCount - 1 && index < input.PreviousPathS.Count)
@@ -38,8 +39,11 @@ public sealed class LongitudinalObjectiveBuilder
         }
         for (int index = 0; index < layout.KnotCount - 1; index++)
             AddSquaredResidual(hessian, linearCost, layout.J(index), 0d, weights.Jerk, jerkScale);
-        AddSquaredResidual(hessian, linearCost, layout.A(layout.KnotCount - 1), 0d, weights.TerminalAcceleration,
-            accelerationScale);
+        if (input.Mode != EmLongitudinalMode.ExactStopAtBoundary)
+        {
+            AddSquaredResidual(hessian, linearCost, layout.A(layout.KnotCount - 1), 0d,
+                weights.TerminalAcceleration, accelerationScale);
+        }
     }
 
     private static void AddSquaredResidual(SparseTripletBuilder hessian, IList<double> linearCost, int variable,

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs

@@ -47,7 +47,7 @@ public sealed class SequentialLongitudinalOptimizer
         if (speedStatus != EmPlanningStatus.Success)
             return Failed(speedStatus, speedFailure);
 
-        LongitudinalCandidate iterate = CreateInitialIterate(input);
+        LongitudinalCandidate iterate = CreateInitialIterate(input, speedLimit);
         double[] warmStart = ToPrimal(iterate);
         bool hasDynamicsConsistentInitialWarmStart = iterate.SatisfiesExactDiscreteDynamics(1e-12d);
         LongitudinalCandidate lastStrictCandidate;
@@ -191,44 +191,111 @@ public sealed class SequentialLongitudinalOptimizer
         }
     }
 
-    private static LongitudinalCandidate CreateInitialIterate(LongitudinalPlanningInput input)
+    private LongitudinalCandidate CreateInitialIterate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit)
     {
         IReadOnlyList<double> times = LongitudinalCandidate.CreateKnotTimes(input.Configuration.Scheduling.TimeHorizonSeconds,
             input.Configuration.Scheduling.OutputTimeStepSeconds);
-        if (TryCreateCruiseThenBrakeSeed(input, times, out LongitudinalCandidate brakingSeed))
-            return brakingSeed;
-        int knotCount = times.Count;
-        var s = new double[knotCount];
-        var u = new double[knotCount];
-        var a = new double[knotCount];
-        var j = new double[knotCount - 1];
-        double horizon = times[knotCount - 1];
-        double requestedSpeed = Math.Min(input.DirectionMaximumSpeedMetersPerSecond,
-            Math.Max(0d, input.TerminalPathS / horizon));
-        if (!JerkLimitedStoppingMath.TryCalculate(requestedSpeed, 0d,
-                input.Configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared,
-                input.Configuration.Longitudinal.MaximumJerkMetersPerSecondCubed,
-                out JerkLimitedStoppingProfile terminalStop, out _))
+        switch (input.Mode)
         {
-            throw new ArgumentOutOfRangeException(nameof(input));
+            case EmLongitudinalMode.RollingContinuation:
+                return CreateRollingSeed(input, times, speedLimit);
+            case EmLongitudinalMode.ApproachStopBoundary:
+                return CreateApproachSeed(input, times, speedLimit);
+            case EmLongitudinalMode.ExactStopAtBoundary:
+                return CreateExactStopSeed(input, times, speedLimit);
+            default:
+                throw new ArgumentOutOfRangeException(nameof(input.Mode));
         }
-        double cruiseDistance = Math.Max(0d, input.TerminalPathS - terminalStop.DistanceMeters);
-        for (int index = 0; index < knotCount; index++)
+    }
+
+    private static LongitudinalCandidate CreateRollingSeed(LongitudinalPlanningInput input,
+        IReadOnlyList<double> times, PathSpeedLimit speedLimit)
+    {
+        return CreateEnvelopeSeed(input, times, speedLimit);
+    }
+
+    private static LongitudinalCandidate CreateApproachSeed(LongitudinalPlanningInput input,
+        IReadOnlyList<double> times, PathSpeedLimit speedLimit)
+    {
+        return CreateEnvelopeSeed(input, times, speedLimit);
+    }
+
+    private static LongitudinalCandidate CreateEnvelopeSeed(LongitudinalPlanningInput input,
+        IReadOnlyList<double> times, PathSpeedLimit speedLimit)
+    {
+        LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
+        var jerk = new double[times.Count - 1];
+        double s = 0d;
+        double u = input.InitialProgressSpeedMetersPerSecond;
+        double a = input.InitialAccelerationMetersPerSecondSquared;
+        for (int index = 0; index < jerk.Length; index++)
         {
-            double fraction = (double)index / (knotCount - 1);
-            s[index] = Math.Min(input.TerminalPathS, cruiseDistance * fraction + terminalStop.DistanceMeters * fraction * fraction);
-            u[index] = index == 0 ? input.InitialProgressSpeedMetersPerSecond : requestedSpeed;
-            a[index] = index == 0 ? input.InitialAccelerationMetersPerSecondSquared : 0d;
+            double dt = times[index + 1] - times[index];
+            double speedLimitAtS = speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.PathUpperBoundS, s)));
+            double targetSpeed = Math.Min(input.InitialProgressSpeedMetersPerSecond, speedLimitAtS);
+            double lowerJerk = Math.Max(-configuration.MaximumJerkMetersPerSecondCubed,
+                (-configuration.MaximumDecelerationMetersPerSecondSquared - a) / dt);
+            lowerJerk = Math.Max(lowerJerk, -2d * (u + a * dt) / (dt * dt));
+            double upperJerk = Math.Min(configuration.MaximumJerkMetersPerSecondCubed,
+                (configuration.MaximumAccelerationMetersPerSecondSquared - a) / dt);
+            double requestedJerk = 2d * (targetSpeed - u - a * dt) / (dt * dt);
+            double selectedJerk = Clamp(requestedJerk, lowerJerk, upperJerk);
+            IntegrateStep(s, u, a, selectedJerk, dt, out double nextS, out double nextU, out double nextA);
+            if (nextU > speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.PathUpperBoundS, nextS))) + 1e-12d)
+            {
+                double lower = lowerJerk;
+                double upper = selectedJerk;
+                for (int iteration = 0; iteration < 48; iteration++)
+                {
+                    double midpoint = 0.5d * (lower + upper);
+                    IntegrateStep(s, u, a, midpoint, dt, out double probeS, out double probeU, out _);
+                    if (probeU <= speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.PathUpperBoundS, probeS))))
+                        lower = midpoint;
+                    else
+                        upper = midpoint;
+                }
+                selectedJerk = lower;
+                IntegrateStep(s, u, a, selectedJerk, dt, out nextS, out nextU, out nextA);
+            }
+            jerk[index] = selectedJerk;
+            s = nextS;
+            u = nextU;
+            a = nextA;
         }
-        s[0] = 0d;
-        s[knotCount - 1] = input.TerminalPathS;
-        u[knotCount - 1] = 0d;
-        a[knotCount - 1] = 0d;
-        return new LongitudinalCandidate(times, s, u, a, j);
+        return LongitudinalCandidate.Integrate(times, 0d, input.InitialProgressSpeedMetersPerSecond,
+            input.InitialAccelerationMetersPerSecondSquared, jerk);
+    }
+
+    private LongitudinalCandidate CreateExactStopSeed(LongitudinalPlanningInput input,
+        IReadOnlyList<double> times, PathSpeedLimit speedLimit)
+    {
+        int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(times,
+            input.Configuration.Scheduling.OutputTimeStepSeconds);
+        var motionTimes = new double[stabilizationStart + 1];
+        for (int index = 0; index < motionTimes.Length; index++)
+            motionTimes[index] = times[index];
+
+        if (TryCreateCruiseThenBrakeSeed(input, motionTimes, input.StopBoundaryPathS,
+                out LongitudinalCandidate cruiseThenBrake))
+        {
+            LongitudinalCandidate candidate = AppendExactStopTail(times, stabilizationStart,
+                input.StopBoundaryPathS, cruiseThenBrake);
+            if (_solutionValidator.TryValidate(input, speedLimit, candidate,
+                    out LongitudinalCandidate validated, out _))
+            {
+                return validated;
+            }
+        }
+
+        if (TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit,
+                out LongitudinalCandidate exactSeed))
+            return exactSeed;
+
+        return CreateApproachSeed(input, times, speedLimit);
     }
 
     private static bool TryCreateCruiseThenBrakeSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
-        out LongitudinalCandidate candidate)
+        double stopBoundaryPathS, out LongitudinalCandidate candidate)
     {
         candidate = null;
         double initialSpeed = input.InitialProgressSpeedMetersPerSecond;
@@ -260,11 +327,11 @@ public sealed class SequentialLongitudinalOptimizer
                 }
                 int brakingIntervals = 2 * rampIntervals + plateauIntervals;
                 double brakingDistance = 0.5d * initialSpeed * brakingIntervals * timeStep;
-                if (brakingDistance > input.TerminalPathS + 1e-12d)
+                if (brakingDistance > stopBoundaryPathS + 1e-12d)
                     continue;
                 int maximumCruiseIntervals = intervalCount - brakingIntervals;
                 int cruiseIntervals = Math.Min(maximumCruiseIntervals, Math.Max(0, checked((int)Math.Floor(
-                    (input.TerminalPathS - brakingDistance) / (initialSpeed * timeStep) + 1e-12d))));
+                    (stopBoundaryPathS - brakingDistance) / (initialSpeed * timeStep) + 1e-12d))));
                 var jerk = new double[intervalCount];
                 int cursor = cruiseIntervals;
                 for (int index = 0; index < rampIntervals; index++)
@@ -273,7 +340,9 @@ public sealed class SequentialLongitudinalOptimizer
                 for (int index = 0; index < rampIntervals; index++)
                     jerk[cursor++] = jerkMagnitude;
                 LongitudinalCandidate integrated = LongitudinalCandidate.Integrate(times, 0d, initialSpeed, 0d, jerk);
-                if (integrated.S[integrated.S.Count - 1] <= input.TerminalPathS + 1e-12d)
+                int lastIndex = integrated.S.Count - 1;
+                if (Math.Abs(integrated.S[lastIndex] - stopBoundaryPathS) <= 1e-10d &&
+                    Math.Abs(integrated.U[lastIndex]) <= 1e-10d && Math.Abs(integrated.A[lastIndex]) <= 1e-10d)
                 {
                     candidate = integrated;
                     return true;
@@ -283,6 +352,280 @@ public sealed class SequentialLongitudinalOptimizer
         return false;
     }
 
+    private bool TryCreateExactJerkSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
+        int stabilizationStart, PathSpeedLimit speedLimit, out LongitudinalCandidate candidate)
+    {
+        candidate = null;
+        int intervalCount = stabilizationStart;
+        if (intervalCount < 3)
+            return false;
+
+        var motionTimes = new double[intervalCount + 1];
+        for (int index = 0; index < motionTimes.Length; index++)
+            motionTimes[index] = times[index];
+
+        LongitudinalCandidate baseline = CreateApproachSeed(input, motionTimes, speedLimit);
+        var influence = new double[3, intervalCount];
+        for (int interval = 0; interval < intervalCount; interval++)
+        {
+            var basis = new double[intervalCount];
+            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[] target =
+        {
+            -baseline.A[baseline.A.Count - 1],
+            -baseline.U[baseline.U.Count - 1],
+            input.StopBoundaryPathS - baseline.S[baseline.S.Count - 1],
+        };
+        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 < intervalCount; interval++)
+                    gram[row, column] += influence[row, interval] * influence[column, interval];
+            }
+        }
+        if (!TrySolveThreeByThree(gram, target, out double[] multipliers))
+            return false;
+
+        var jerk = new double[intervalCount];
+        for (int interval = 0; interval < intervalCount; interval++)
+        {
+            jerk[interval] = baseline.J[interval];
+            for (int row = 0; row < 3; row++)
+                jerk[interval] += influence[row, interval] * multipliers[row];
+        }
+
+        if (TryValidateExactSeed(input, times, stabilizationStart, speedLimit, jerk, out candidate))
+            return true;
+
+        double currentViolation = CalculateExactSeedViolation(input, speedLimit,
+            CreateExactCandidate(input, times, stabilizationStart, jerk));
+        double maximumJerk = input.Configuration.Longitudinal.MaximumJerkMetersPerSecondCubed;
+        for (int pass = 0; pass < 4; pass++)
+        {
+            for (int basisIndex = 0; basisIndex < intervalCount; basisIndex++)
+            {
+                double[] direction = CreateEndpointNullspaceDirection(influence, gram, basisIndex);
+                if (direction == null)
+                    continue;
+
+                double[] bestJerk = jerk;
+                double bestViolation = currentViolation;
+                for (int sample = -256; sample <= 256; sample++)
+                {
+                    double scale = maximumJerk * sample / 256d;
+                    var probeJerk = new double[intervalCount];
+                    for (int interval = 0; interval < intervalCount; interval++)
+                        probeJerk[interval] = jerk[interval] + scale * direction[interval];
+                    LongitudinalCandidate probe = CreateExactCandidate(input, times, stabilizationStart, probeJerk);
+                    double violation = CalculateExactSeedViolation(input, speedLimit, probe);
+                    if (violation < bestViolation)
+                    {
+                        bestViolation = violation;
+                        bestJerk = probeJerk;
+                    }
+                }
+                jerk = bestJerk;
+                currentViolation = bestViolation;
+                if (TryValidateExactSeed(input, times, stabilizationStart, speedLimit, jerk, out candidate))
+                    return true;
+            }
+        }
+        return false;
+    }
+
+    private bool TryValidateExactSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
+        int stabilizationStart, PathSpeedLimit speedLimit, IReadOnlyList<double> jerk,
+        out LongitudinalCandidate candidate)
+    {
+        LongitudinalCandidate probe = CreateExactCandidate(input, times, stabilizationStart, jerk);
+        return _solutionValidator.TryValidate(input, speedLimit, probe, out candidate, out _);
+    }
+
+    private static LongitudinalCandidate CreateExactCandidate(LongitudinalPlanningInput input,
+        IReadOnlyList<double> times, int stabilizationStart, IReadOnlyList<double> jerk)
+    {
+        var motionTimes = new double[stabilizationStart + 1];
+        for (int index = 0; index < motionTimes.Length; index++)
+            motionTimes[index] = times[index];
+        LongitudinalCandidate motion = LongitudinalCandidate.Integrate(motionTimes, 0d,
+            input.InitialProgressSpeedMetersPerSecond, input.InitialAccelerationMetersPerSecondSquared, jerk);
+        return AppendExactStopTail(times, stabilizationStart, input.StopBoundaryPathS, motion);
+    }
+
+    private static double[] CreateEndpointNullspaceDirection(double[,] influence, double[,] gram, int basisIndex)
+    {
+        int intervalCount = influence.GetLength(1);
+        double[] rightHandSide = { influence[0, basisIndex], influence[1, basisIndex], influence[2, basisIndex] };
+        if (!TrySolveThreeByThree(gram, rightHandSide, out double[] multipliers))
+            return null;
+        var direction = new double[intervalCount];
+        double magnitude = 0d;
+        for (int interval = 0; interval < intervalCount; interval++)
+        {
+            direction[interval] = interval == basisIndex ? 1d : 0d;
+            for (int row = 0; row < 3; row++)
+                direction[interval] -= influence[row, interval] * multipliers[row];
+            magnitude = Math.Max(magnitude, Math.Abs(direction[interval]));
+        }
+        if (magnitude <= 1e-12d)
+            return null;
+        for (int interval = 0; interval < intervalCount; interval++)
+            direction[interval] /= magnitude;
+        return direction;
+    }
+
+    private static double CalculateExactSeedViolation(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
+        LongitudinalCandidate candidate)
+    {
+        double tolerance = input.Configuration.Validation.KinematicTolerance;
+        double maximumAcceleration = input.Configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared;
+        double maximumDeceleration = input.Configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared;
+        double maximumJerk = input.Configuration.Longitudinal.MaximumJerkMetersPerSecondCubed;
+        double violation = 0d;
+        double previousS = double.NegativeInfinity;
+        for (int index = 0; index < candidate.S.Count; index++)
+        {
+            double s = candidate.S[index];
+            double u = candidate.U[index];
+            double a = candidate.A[index];
+            if (!IsFinite(s) || !IsFinite(u) || !IsFinite(a))
+                return double.PositiveInfinity;
+            violation += SquaredExcess(-s, tolerance);
+            violation += SquaredExcess(s - input.PathUpperBoundS, tolerance);
+            violation += SquaredExcess(previousS - s, tolerance);
+            violation += SquaredExcess(-u, tolerance);
+            violation += SquaredExcess(a - maximumAcceleration, tolerance);
+            violation += SquaredExcess(-maximumDeceleration - a, tolerance);
+            double speedLimitAtS = speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.PathUpperBoundS, s)));
+            violation += SquaredExcess(u - speedLimitAtS, tolerance);
+            if (!JerkLimitedStoppingMath.TryCalculate(u, a, maximumDeceleration, maximumJerk,
+                    out JerkLimitedStoppingProfile stop, out _))
+            {
+                return double.PositiveInfinity;
+            }
+            violation += SquaredExcess(s + stop.DistanceMeters - input.StopBoundaryPathS, tolerance);
+            previousS = s;
+        }
+        for (int index = 0; index < candidate.J.Count; index++)
+        {
+            if (!IsFinite(candidate.J[index]))
+                return double.PositiveInfinity;
+            violation += SquaredExcess(Math.Abs(candidate.J[index]) - maximumJerk, tolerance);
+        }
+        return violation;
+    }
+
+    private static double SquaredExcess(double actual, double tolerance)
+    {
+        double excess = Math.Max(0d, actual - tolerance);
+        return excess * excess;
+    }
+
+    private static LongitudinalCandidate AppendExactStopTail(IReadOnlyList<double> times, int stabilizationStart,
+        double stopBoundaryPathS, LongitudinalCandidate motion)
+    {
+        var s = new double[times.Count];
+        var u = new double[times.Count];
+        var a = new double[times.Count];
+        var jerk = new double[times.Count - 1];
+        int motionCount = Math.Min(stabilizationStart + 1, motion.S.Count);
+        for (int index = 0; index < motionCount; index++)
+        {
+            s[index] = motion.S[index];
+            u[index] = motion.U[index];
+            a[index] = motion.A[index];
+        }
+        for (int index = 0; index < Math.Min(stabilizationStart, motion.J.Count); index++)
+            jerk[index] = motion.J[index];
+        for (int index = stabilizationStart; index < times.Count; index++)
+        {
+            s[index] = stopBoundaryPathS;
+            u[index] = 0d;
+            a[index] = 0d;
+        }
+        return new LongitudinalCandidate(times, s, u, a, jerk);
+    }
+
+    private static bool SatisfiesLongitudinalBounds(LongitudinalPlanningInput input, LongitudinalCandidate candidate)
+    {
+        LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
+        double previousS = double.NegativeInfinity;
+        for (int index = 0; index < candidate.S.Count; index++)
+        {
+            if (candidate.S[index] < -1e-10d || candidate.S[index] > input.StopBoundaryPathS + 1e-10d ||
+                candidate.S[index] < previousS - 1e-10d || candidate.U[index] < -1e-10d ||
+                candidate.U[index] > input.DirectionMaximumSpeedMetersPerSecond + 1e-10d ||
+                candidate.A[index] < -configuration.MaximumDecelerationMetersPerSecondSquared - 1e-10d ||
+                candidate.A[index] > configuration.MaximumAccelerationMetersPerSecondSquared + 1e-10d)
+            {
+                return false;
+            }
+            previousS = candidate.S[index];
+        }
+        for (int index = 0; index < candidate.J.Count; index++)
+        {
+            if (Math.Abs(candidate.J[index]) > configuration.MaximumJerkMetersPerSecondCubed + 1e-10d)
+                return false;
+        }
+        return true;
+    }
+
+    private static bool TrySolveThreeByThree(double[,] matrix, IReadOnlyList<double> rightHandSide,
+        out double[] solution)
+    {
+        solution = new double[3];
+        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;
+            }
+            if (Math.Abs(augmented[bestRow, pivot]) <= 1e-14d)
+                return false;
+            if (bestRow != pivot)
+            {
+                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];
+            }
+        }
+        for (int row = 0; row < 3; row++)
+            solution[row] = augmented[row, 3];
+        return true;
+    }
+
     private static bool TryCreateCandidate(IReadOnlyList<double> times, IReadOnlyList<double> primal,
         out LongitudinalCandidate candidate)
     {
@@ -336,6 +679,10 @@ 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;
         var candidateProgressSamples = new double[candidate.S.Count];
         double priorProgress = double.NegativeInfinity;
         double priorPreviousProgress = double.NegativeInfinity;
@@ -343,7 +690,7 @@ public sealed class SequentialLongitudinalOptimizer
         {
             double candidateProgress = candidate.S[index];
             double previousProgress = previous.S[index];
-            if (!IsFinite(previousProgress) || previousProgress < 0d || previousProgress > input.TerminalPathS ||
+            if (!IsFinite(previousProgress) || previousProgress < 0d || previousProgress > input.PathUpperBoundS ||
                 previousProgress < priorPreviousProgress)
             {
                 return false;
@@ -352,9 +699,9 @@ public sealed class SequentialLongitudinalOptimizer
             {
                 candidateProgress = previousProgress;
             }
-            candidateProgress = Math.Max(0d, Math.Min(input.TerminalPathS, candidateProgress));
-            if (index == candidate.S.Count - 1)
-                candidateProgress = input.TerminalPathS;
+            candidateProgress = Math.Max(0d, Math.Min(input.PathUpperBoundS, candidateProgress));
+            if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary && index >= stabilizationStart)
+                candidateProgress = input.StopBoundaryPathS;
             candidateProgress = Math.Max(priorProgress, candidateProgress);
             candidateProgressSamples[index] = candidateProgress;
             priorProgress = candidateProgress;
@@ -365,7 +712,8 @@ public sealed class SequentialLongitudinalOptimizer
         for (int index = 0; index < progress.Length; index++)
         {
             double candidateProgress = candidateProgressSamples[index];
-            if (index == 0 || index == progress.Length - 1 || candidateProgress >= input.TerminalPathS)
+            if (index == 0 || index == progress.Length - 1 || (input.Mode == EmLongitudinalMode.ExactStopAtBoundary &&
+                index >= stabilizationStart) || candidateProgress >= input.PathUpperBoundS)
             {
                 progress[index] = candidateProgress;
             }
@@ -378,7 +726,7 @@ public sealed class SequentialLongitudinalOptimizer
                     ? OrdinaryEnvelopeProbeLookaheadSteps * candidateSpeed * timeStep
                     : 0d;
                 double terminalLimitedAdvance = OrdinaryTerminalProbeFraction *
-                    (input.TerminalPathS - candidateProgress);
+                    (input.PathUpperBoundS - candidateProgress);
                 double advance = Math.Min(Math.Max(iterationAdvance, lookaheadAdvance), terminalLimitedAdvance);
                 progress[index] = candidateProgress + advance;
             }
@@ -404,7 +752,7 @@ public sealed class SequentialLongitudinalOptimizer
         {
             if (!IsFinite(candidate.S[index]) || !IsFinite(candidate.U[index]))
                 continue;
-            double candidateProgress = Math.Max(0d, Math.Min(speedLimit.TerminalPathS, candidate.S[index]));
+            double candidateProgress = Math.Max(0d, Math.Min(speedLimit.PathUpperBoundS, candidate.S[index]));
             double limit = speedLimit.MaximumSpeedAt(candidateProgress);
             double excess = candidate.U[index] - limit;
             if (excess > worstExcess)
@@ -462,4 +810,18 @@ public sealed class SequentialLongitudinalOptimizer
     {
         return !double.IsNaN(value) && !double.IsInfinity(value);
     }
+
+    private static double Clamp(double value, double minimum, double maximum)
+    {
+        return Math.Max(minimum, Math.Min(maximum, value));
+    }
+
+    private static void IntegrateStep(double s, double u, double a, double jerk, double duration,
+        out double nextS, out double nextU, out double nextA)
+    {
+        nextS = s + u * duration + 0.5d * a * duration * duration +
+            jerk * duration * duration * duration / 6d;
+        nextU = u + a * duration + 0.5d * jerk * duration * duration;
+        nextA = a + jerk * duration;
+    }
 }