feat: reuse prior trajectory in longitudinal planning

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs

@@ -82,9 +82,15 @@ 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);
+            LongitudinalPreviousTrajectorySeed previousLongitudinalSeed =
+                new LongitudinalPreviousTrajectorySeedBuilder().Build(
+                    request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc, knotTimes,
+                    segment.SegmentIndex, segment.Direction);
             var longitudinalInput = new LongitudinalPlanningInput(lateral.Path, segment.Direction, initialProgressSpeed,
                 initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, configuration,
-                Array.Empty<double>(), Array.Empty<double>());
+                previousLongitudinalSeed.PathS, previousLongitudinalSeed.ProgressSpeedMetersPerSecond);
             EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out string envelopeReason);
             if (envelopeStatus != EmPlanningStatus.Success)
                 return Failure(envelopeStatus, request, envelopeReason);

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPreviousTrajectorySeedBuilder.cs

@@ -0,0 +1,250 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Resamples a compatible published trajectory onto the current ST knots as soft longitudinal references.</summary>
+public sealed class LongitudinalPreviousTrajectorySeed
+{
+    private static readonly LongitudinalPreviousTrajectorySeed empty = new LongitudinalPreviousTrajectorySeed(
+        Array.Empty<double>(), Array.Empty<double>());
+
+    public LongitudinalPreviousTrajectorySeed(IReadOnlyList<double> pathS,
+        IReadOnlyList<double> progressSpeedMetersPerSecond)
+    {
+        if (pathS == null)
+            throw new ArgumentNullException(nameof(pathS));
+        if (progressSpeedMetersPerSecond == null)
+            throw new ArgumentNullException(nameof(progressSpeedMetersPerSecond));
+        if (pathS.Count != progressSpeedMetersPerSecond.Count)
+            throw new ArgumentException("Previous path-S and progress-speed samples must have matching counts.");
+
+        var copiedPathS = new List<double>(pathS.Count);
+        var copiedSpeed = new List<double>(progressSpeedMetersPerSecond.Count);
+        for (int index = 0; index < pathS.Count; index++)
+        {
+            if (!IsFinite(pathS[index]) || pathS[index] < 0d ||
+                !IsFinite(progressSpeedMetersPerSecond[index]) || progressSpeedMetersPerSecond[index] < 0d)
+            {
+                throw new ArgumentOutOfRangeException(nameof(pathS));
+            }
+            copiedPathS.Add(pathS[index]);
+            copiedSpeed.Add(progressSpeedMetersPerSecond[index]);
+        }
+
+        PathS = new ReadOnlyCollection<double>(copiedPathS);
+        ProgressSpeedMetersPerSecond = new ReadOnlyCollection<double>(copiedSpeed);
+    }
+
+    public IReadOnlyList<double> PathS { get; }
+
+    public IReadOnlyList<double> ProgressSpeedMetersPerSecond { get; }
+
+    public static LongitudinalPreviousTrajectorySeed Empty { get { return empty; } }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}
+
+/// <summary>Builds monotone PathS and progress-speed soft references from a prior published trajectory.</summary>
+public sealed class LongitudinalPreviousTrajectorySeedBuilder
+{
+    private const double ProjectionTolerance = 1e-10d;
+
+    public LongitudinalPreviousTrajectorySeed Build(EmTrajectory previous, LateralPath currentPath,
+        DateTimeOffset newEffectiveAtUtc, IReadOnlyList<double> newKnotTimes, int segmentIndex,
+        TravelDirection direction)
+    {
+        try
+        {
+            if (!IsCompatible(previous, currentPath, newKnotTimes, segmentIndex, direction))
+                return LongitudinalPreviousTrajectorySeed.Empty;
+
+            var pathS = new List<double>(newKnotTimes.Count);
+            var progressSpeed = new List<double>(newKnotTimes.Count);
+            double previousProjectedPathS = double.NegativeInfinity;
+            for (int index = 0; index < newKnotTimes.Count; index++)
+            {
+                DateTimeOffset sampleUtc = newEffectiveAtUtc.AddSeconds(newKnotTimes[index]);
+                double previousTimeSeconds = (sampleUtc - previous.Metadata.EffectiveAtUtc).TotalSeconds;
+                if (!TryInterpolate(previous.Points, previousTimeSeconds, out InterpolatedPreviousSample sample) ||
+                    !TryProjectMonotonically(currentPath, sample.X, sample.Y, previousProjectedPathS,
+                        out double projectedPathS))
+                {
+                    return LongitudinalPreviousTrajectorySeed.Empty;
+                }
+
+                pathS.Add(projectedPathS);
+                progressSpeed.Add(Math.Abs(sample.SignedSpeedMetersPerSecond));
+                previousProjectedPathS = projectedPathS;
+            }
+
+            return new LongitudinalPreviousTrajectorySeed(pathS, progressSpeed);
+        }
+        catch
+        {
+            return LongitudinalPreviousTrajectorySeed.Empty;
+        }
+    }
+
+    private static bool IsCompatible(EmTrajectory previous, LateralPath currentPath,
+        IReadOnlyList<double> newKnotTimes, int segmentIndex, TravelDirection direction)
+    {
+        if (previous == null || currentPath == null || newKnotTimes == null || segmentIndex < 0 ||
+            !Enum.IsDefined(typeof(TravelDirection), direction) || !currentPath.IsIndependentlyValidated ||
+            currentPath.Points.Count < 2 || previous.Metadata == null || previous.Points.Count < 2 ||
+            previous.Metadata.SegmentIndex != segmentIndex || previous.Metadata.Direction != direction)
+        {
+            return false;
+        }
+
+        double previousKnotTime = double.NegativeInfinity;
+        for (int index = 0; index < newKnotTimes.Count; index++)
+        {
+            if (!IsFinite(newKnotTimes[index]) || newKnotTimes[index] < 0d ||
+                newKnotTimes[index] <= previousKnotTime)
+            {
+                return false;
+            }
+            previousKnotTime = newKnotTimes[index];
+        }
+        if (newKnotTimes.Count == 0)
+            return false;
+
+        double previousTime = double.NegativeInfinity;
+        for (int index = 0; index < previous.Points.Count; index++)
+        {
+            EmTrajectoryPoint point = previous.Points[index];
+            if (point == null || point.Direction != direction || !IsFinite(point.TimeFromStart) ||
+                !IsFinite(point.X) || !IsFinite(point.Y) || !IsFinite(point.SignedLongitudinalVelocity) ||
+                point.TimeFromStart <= previousTime)
+            {
+                return false;
+            }
+            previousTime = point.TimeFromStart;
+        }
+
+        double previousPathS = double.NegativeInfinity;
+        for (int index = 0; index < currentPath.Points.Count; index++)
+        {
+            LateralPathPoint point = currentPath.Points[index];
+            if (point == null || !IsFinite(point.PathS) || !IsFinite(point.X) || !IsFinite(point.Y) ||
+                point.PathS <= previousPathS)
+            {
+                return false;
+            }
+            previousPathS = point.PathS;
+        }
+        return true;
+    }
+
+    private static bool TryInterpolate(IReadOnlyList<EmTrajectoryPoint> points, double sampleTimeSeconds,
+        out InterpolatedPreviousSample sample)
+    {
+        sample = default;
+        if (!IsFinite(sampleTimeSeconds) || sampleTimeSeconds < points[0].TimeFromStart - ProjectionTolerance ||
+            sampleTimeSeconds > points[points.Count - 1].TimeFromStart + ProjectionTolerance)
+        {
+            return false;
+        }
+
+        if (sampleTimeSeconds <= points[0].TimeFromStart + ProjectionTolerance)
+        {
+            sample = InterpolatedPreviousSample.From(points[0]);
+            return true;
+        }
+
+        for (int index = 1; index < points.Count; index++)
+        {
+            EmTrajectoryPoint right = points[index];
+            if (sampleTimeSeconds <= right.TimeFromStart + ProjectionTolerance)
+            {
+                EmTrajectoryPoint left = points[index - 1];
+                double ratio = (sampleTimeSeconds - left.TimeFromStart) /
+                    (right.TimeFromStart - left.TimeFromStart);
+                ratio = Math.Max(0d, Math.Min(1d, ratio));
+                sample = new InterpolatedPreviousSample(
+                    Linear(left.X, right.X, ratio),
+                    Linear(left.Y, right.Y, ratio),
+                    Linear(left.SignedLongitudinalVelocity, right.SignedLongitudinalVelocity, ratio));
+                return true;
+            }
+        }
+
+        return false;
+    }
+
+    private static bool TryProjectMonotonically(LateralPath path, double x, double y, double minimumPathS,
+        out double projectedPathS)
+    {
+        projectedPathS = 0d;
+        double bestDistanceSquared = double.PositiveInfinity;
+        bool found = false;
+        for (int index = 1; index < path.Points.Count; index++)
+        {
+            LateralPathPoint left = path.Points[index - 1];
+            LateralPathPoint right = path.Points[index];
+            double dx = right.X - left.X;
+            double dy = right.Y - left.Y;
+            double lengthSquared = dx * dx + dy * dy;
+            if (!IsFinite(lengthSquared) || lengthSquared <= ProjectionTolerance)
+                continue;
+
+            double ratio = ((x - left.X) * dx + (y - left.Y) * dy) / lengthSquared;
+            ratio = Math.Max(0d, Math.Min(1d, ratio));
+            double candidatePathS = Linear(left.PathS, right.PathS, ratio);
+            if (candidatePathS + ProjectionTolerance < minimumPathS)
+                continue;
+
+            double projectedX = Linear(left.X, right.X, ratio);
+            double projectedY = Linear(left.Y, right.Y, ratio);
+            double distanceSquared = (x - projectedX) * (x - projectedX) + (y - projectedY) * (y - projectedY);
+            if (!found || distanceSquared < bestDistanceSquared - ProjectionTolerance ||
+                (Math.Abs(distanceSquared - bestDistanceSquared) <= ProjectionTolerance && candidatePathS < projectedPathS))
+            {
+                projectedPathS = candidatePathS;
+                bestDistanceSquared = distanceSquared;
+                found = true;
+            }
+        }
+
+        if (!found)
+            return false;
+        if (minimumPathS > double.NegativeInfinity)
+            projectedPathS = Math.Max(minimumPathS, projectedPathS);
+        return true;
+    }
+
+    private static double Linear(double left, double right, double ratio)
+    {
+        return left + (right - left) * ratio;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+
+    private readonly struct InterpolatedPreviousSample
+    {
+        public InterpolatedPreviousSample(double x, double y, double signedSpeedMetersPerSecond)
+        {
+            X = x;
+            Y = y;
+            SignedSpeedMetersPerSecond = signedSpeedMetersPerSecond;
+        }
+
+        public double X { get; }
+        public double Y { get; }
+        public double SignedSpeedMetersPerSecond { get; }
+
+        public static InterpolatedPreviousSample From(EmTrajectoryPoint point)
+        {
+            return new InterpolatedPreviousSample(point.X, point.Y, point.SignedLongitudinalVelocity);
+        }
+    }
+}

ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs

@@ -13,6 +13,7 @@ internal static class EmPlanningServiceChecks
 {
     public static void Run()
     {
+        VerifiesPreviousTrajectoryIsALongitudinalSoftReference();
         VerifiesForwardReverseAndBoundarySuccessesAreDeterministic();
         VerifiesRequestAndStateFailuresPublishNoTrajectory();
         VerifiesProjectionCorridorAndOptimizationFailuresPublishNoTrajectory();
@@ -31,7 +32,7 @@ internal static class EmPlanningServiceChecks
         VerifySameTrajectory(firstForward, secondForward, "forward deterministic result");
         Verification.Equal(2, forwardRequest.ReferencePath.Path.Count, "request-owned reference list remains unchanged");
 
-        EmPlanningRequest reverseRequest = CreateRequest(TravelDirection.Reverse, 0d, false, false);
+        EmPlanningRequest reverseRequest = CreateRequest(TravelDirection.Reverse, -0.01d, false, false);
         EmPlanningResult reverse = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(reverseRequest,
             CancellationToken.None);
         VerifySuccess(reverse, reverseRequest, EmTerminalType.Goal, "reverse");
@@ -70,7 +71,7 @@ internal static class EmPlanningServiceChecks
     private static void VerifiesProjectionCorridorAndOptimizationFailuresPublishNoTrajectory()
     {
         EmPlanningRequest projectionFailure = CreateRequest(TravelDirection.Forward, 0d, false, false);
-        projectionFailure = ReplaceState(projectionFailure, new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null,
+        projectionFailure = ReplaceState(projectionFailure, new VehicleMotionState(new Pose2D(3d, 0d, 0d), 0d, null,
             projectionFailure.RequestedAtUtc, projectionFailure.VehicleState.SequenceId));
         VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(projectionFailure,
             CancellationToken.None), EmPlanningStatus.ProjectionFailed, "bounded projection failure");
@@ -80,15 +81,20 @@ internal static class EmPlanningServiceChecks
         VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(corridorFailure,
             CancellationToken.None), EmPlanningStatus.CorridorInfeasible, "corridor infeasible");
 
-        EmPlanningRequest stoppingFailure = CreateRequest(TravelDirection.Forward, 0.20d, false, false);
+        EmPlanningRequest stoppingFailure = CreateRequest(TravelDirection.Forward, 0.20d, false, false,
+            referencePath: CreateReferencePath(TravelDirection.Forward, false, 0.0055d));
         VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(stoppingFailure,
             CancellationToken.None), EmPlanningStatus.StoppingDistanceInsufficient, "stopping distance insufficient");
 
         EmPlanningRequest regular = CreateRequest(TravelDirection.Forward, 0d, false, false);
         VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.LateralInfeasible)).Plan(regular,
             CancellationToken.None), EmPlanningStatus.LateralInfeasible, "lateral infeasible");
-        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.LongitudinalInfeasible)).Plan(regular,
+        EmPlanningResult longitudinalFallback = new EmPlanningService(
-            CancellationToken.None), EmPlanningStatus.LongitudinalInfeasible, "longitudinal infeasible");
+            new ScriptedPipelineSolver(PipelineSolverMode.LongitudinalInfeasible)).Plan(regular, CancellationToken.None);
+        Verification.Equal(EmPlanningStatus.SuccessWithFallback, longitudinalFallback.Status,
+            "longitudinal infeasible uses the validated fallback seed");
+        Verification.True(longitudinalFallback.Trajectory != null,
+            "longitudinal fallback still publishes a complete trajectory");
         VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.SolverUnavailable)).Plan(regular,
             CancellationToken.None), EmPlanningStatus.SolverUnavailable, "solver unavailable");
     }
@@ -127,6 +133,56 @@ internal static class EmPlanningServiceChecks
         VerifySuccess(debugIsolated, debugRequest, EmTerminalType.Goal, "debug-sink isolation");
     }
 
+    private static void VerifiesPreviousTrajectoryIsALongitudinalSoftReference()
+    {
+        EmPlanningRequest request = CreateRequest(TravelDirection.Forward, 0d, false, false);
+
+        var withoutPreviousSolver = new ScriptedPipelineSolver(PipelineSolverMode.Success);
+        EmPlanningResult withoutPrevious = new EmPlanningService(withoutPreviousSolver).Plan(request, CancellationToken.None);
+        VerifySuccess(withoutPrevious, request, EmTerminalType.Goal, "no previous longitudinal seed");
+
+        EmTrajectory validPrevious = CreateLongitudinalPreviousTrajectory(request.EffectiveAtUtc, TravelDirection.Forward,
+            request.SegmentIndex);
+        EmPlanningRequest withPreviousRequest = ReplacePreviousTrajectory(request, validPrevious);
+        var withPreviousSolver = new ScriptedPipelineSolver(PipelineSolverMode.Success);
+        EmPlanningResult withPrevious = new EmPlanningService(withPreviousSolver).Plan(withPreviousRequest,
+            CancellationToken.None);
+        VerifySuccess(withPrevious, withPreviousRequest, EmTerminalType.Goal, "valid previous longitudinal seed");
+
+        QuadraticProgram withoutPreviousProblem = withoutPreviousSolver.LastLongitudinalProblem
+            ?? throw new InvalidOperationException("The no-seed longitudinal QP was not captured.");
+        QuadraticProgram withPreviousProblem = withPreviousSolver.LastLongitudinalProblem
+            ?? throw new InvalidOperationException("The seeded longitudinal QP was not captured.");
+        var layout = new LongitudinalVariableLayout((withPreviousProblem.VariableCount + 1) / 4);
+        Verification.True(MatrixValue(withPreviousProblem.UpperTriangularP, layout.S(1), layout.S(1)) >
+            MatrixValue(withoutPreviousProblem.UpperTriangularP, layout.S(1), layout.S(1)),
+            "valid previous seed adds a nonzero previous-S soft-reference Hessian term");
+        Verification.True(MatrixValue(withPreviousProblem.UpperTriangularP, layout.U(1), layout.U(1)) >
+            MatrixValue(withoutPreviousProblem.UpperTriangularP, layout.U(1), layout.U(1)),
+            "valid previous seed adds a nonzero previous-U soft-reference Hessian term");
+        Verification.True(Math.Abs(withPreviousProblem.LinearCost[layout.S(1)] -
+            withoutPreviousProblem.LinearCost[layout.S(1)]) > 1e-12d,
+            "valid previous seed adds a nonzero previous-S soft-reference linear term");
+        Verification.True(Math.Abs(withPreviousProblem.LinearCost[layout.U(1)] -
+            withoutPreviousProblem.LinearCost[layout.U(1)]) > 1e-12d,
+            "valid previous seed adds a nonzero previous-U soft-reference linear term");
+
+        EmPlanningRequest incompatibleRequest = ReplacePreviousTrajectory(request,
+            CreateLongitudinalPreviousTrajectory(request.EffectiveAtUtc, TravelDirection.Reverse, request.SegmentIndex));
+        var incompatibleSolver = new ScriptedPipelineSolver(PipelineSolverMode.Success);
+        EmPlanningResult incompatible = new EmPlanningService(incompatibleSolver).Plan(incompatibleRequest,
+            CancellationToken.None);
+        VerifySuccess(incompatible, incompatibleRequest, EmTerminalType.Goal, "incompatible previous seed");
+        QuadraticProgram incompatibleProblem = incompatibleSolver.LastLongitudinalProblem
+            ?? throw new InvalidOperationException("The incompatible-seed longitudinal QP was not captured.");
+        Verification.NearlyEqual(MatrixValue(withoutPreviousProblem.UpperTriangularP, layout.S(1), layout.S(1)),
+            MatrixValue(incompatibleProblem.UpperTriangularP, layout.S(1), layout.S(1)),
+            "incompatible previous seed omits previous-S soft-reference term");
+        Verification.NearlyEqual(MatrixValue(withoutPreviousProblem.UpperTriangularP, layout.U(1), layout.U(1)),
+            MatrixValue(incompatibleProblem.UpperTriangularP, layout.U(1), layout.U(1)),
+            "incompatible previous seed omits previous-U soft-reference term");
+    }
+
     private static void VerifySuccess(EmPlanningResult result, EmPlanningRequest request, EmTerminalType terminalType,
         string name)
     {
@@ -170,7 +226,7 @@ internal static class EmPlanningServiceChecks
         configuration.Solver.MaximumOuterIterations = 2;
         configuration.Scheduling.SolverTimeoutSeconds = 1d;
         if (rolling)
-            configuration.Scheduling.DistanceHorizonMeters = 0.003d;
+            configuration.Scheduling.DistanceHorizonMeters = 0.30d;
         return new EmPlanningRequest(referencePath ?? CreateReferencePath(direction, endsAtGearSwitch), map ?? CreateMap(false),
             new VehicleParameters
             {
@@ -191,21 +247,55 @@ internal static class EmPlanningServiceChecks
             source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel);
     }
 
-    private static PathSmoothingResult CreateReferencePath(TravelDirection direction, bool endsAtGearSwitch)
+    private static EmPlanningRequest ReplacePreviousTrajectory(EmPlanningRequest source, EmTrajectory previousTrajectory)
     {
-        double endX = direction == TravelDirection.Forward ? 0.0055d : -0.0055d;
+        return new EmPlanningRequest(source.ReferencePath, source.Map, source.Vehicle, source.VehicleState,
+            source.Configuration, source.SegmentIndex, previousTrajectory, source.RequestedAtUtc, source.EffectiveAtUtc,
+            source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel);
+    }
+
+    private static EmTrajectory CreateLongitudinalPreviousTrajectory(DateTimeOffset effectiveAtUtc,
+        TravelDirection direction, int segmentIndex)
+    {
+        var metadata = new EmTrajectoryMetadata("previous-service", effectiveAtUtc, effectiveAtUtc, 1L,
+            "previous-reference", 1L, string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop,
+            EmLongitudinalMode.RollingContinuation);
+        double sign = direction == TravelDirection.Forward ? 1d : -1d;
+        return new EmTrajectory(metadata, new[]
+        {
+            new EmTrajectoryPoint(sign * 0.001d, 0d, 0d, sign * 0.01d, 0d, 0d, segmentIndex, 0.001d, 0.001d,
+                direction, EmBoundaryType.None, 0d, 0d),
+            new EmTrajectoryPoint(sign * 0.001d, 0d, 0d, sign * 0.01d, 6d, 0d, segmentIndex, 0.001d, 0.001d,
+                direction, EmBoundaryType.None, 0d, 0d),
+        });
+    }
+
+    private static double MatrixValue(SparseCscMatrix matrix, int row, int column)
+    {
+        for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)
+        {
+            if (matrix.RowIndices[index] == row)
+                return matrix.Values[index];
+        }
+        return 0d;
+    }
+
+    private static PathSmoothingResult CreateReferencePath(TravelDirection direction, bool endsAtGearSwitch,
+        double lengthMeters = 2d)
+    {
+        double endX = direction == TravelDirection.Forward ? lengthMeters : -lengthMeters;
         var points = new List<SmoothedPathPoint>
         {
             new SmoothedPathPoint(0d, 0d, 0d, 0d, 0d, direction, 0d, 0d, 0d, 1d, false,
                 SmoothedPathPointSource.Anchor),
-            new SmoothedPathPoint(endX, 0d, 0d, 0d, 0.0055d, direction, 0d, 0d, 0d, 1d, endsAtGearSwitch,
+            new SmoothedPathPoint(endX, 0d, 0d, 0d, lengthMeters, direction, 0d, 0d, 0d, 1d, endsAtGearSwitch,
                 endsAtGearSwitch ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.Anchor),
         };
         var segments = new List<SmoothedPathSegment>
         {
             new SmoothedPathSegment(0, direction, 0, 1, false, endsAtGearSwitch),
         };
-        var metrics = new PathQualityMetrics(true, 0.0055d, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d);
+        var metrics = new PathQualityMetrics(true, lengthMeters, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d);
         return PathSmoothingResult.PublishLocalG2(PathSmoothingStatus.Complete, points, segments,
             new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List<PathSmoothingRegionReport>());
     }
@@ -218,7 +308,7 @@ internal static class EmPlanningServiceChecks
             : Array.Empty<IMapObstacleSource>();
         PlanningMapBuildResult result = new PlanningMapFactory().Create(new PlanningMapRequest
         {
-            Bounds = new MapBoundsMm(-1000f, 3000f, -1000f, 1000f),
+            Bounds = new MapBoundsMm(-3000f, 3000f, -1000f, 1000f),
             ResolutionMm = 20f,
             ObstacleSources = sources,
             AllowExplicitEmptyMap = !blockStart,
@@ -245,6 +335,8 @@ internal static class EmPlanningServiceChecks
         private readonly PlanningGridMap? mapToCorrupt;
         private int longitudinalCallCount;
 
+        public QuadraticProgram? LastLongitudinalProblem { get; private set; }
+
         public ScriptedPipelineSolver(PipelineSolverMode mode, PlanningGridMap? mapToCorrupt = null)
         {
             this.mode = mode;
@@ -265,6 +357,7 @@ internal static class EmPlanningServiceChecks
                     return Result(QpSolveStatus.TimeLimit, Array.Empty<double>());
                 return Result(QpSolveStatus.Solved, new double[problem.VariableCount]);
             }
+            LastLongitudinalProblem = problem;
             if (mode == PipelineSolverMode.LongitudinalInfeasible)
                 return Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>());
             if (mode == PipelineSolverMode.PublicationValidationFailure && longitudinalCallCount == 0)
@@ -272,7 +365,7 @@ internal static class EmPlanningServiceChecks
             if (mode == PipelineSolverMode.TimeoutWithFallback && ++longitudinalCallCount > 1)
                 return Result(QpSolveStatus.TimeLimit, Array.Empty<double>());
             longitudinalCallCount++;
-            return Result(QpSolveStatus.Solved, CreateStrictLongitudinalPrimal(problem));
+            return Result(QpSolveStatus.Solved, warmStart);
         }
 
         private static void CorruptMapAtOrigin(PlanningGridMap? map)

ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs

@@ -20,6 +20,7 @@ internal static class LongitudinalModelChecks
         VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime();
         VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
         VerifiesModeSpecificSolutionValidation();
+        VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically();
     }
 
     private static void VerifiesJerkLimitedStoppingProfileEndsAtRest()
@@ -454,6 +455,34 @@ internal static class LongitudinalModelChecks
             "approach failure identifies the jerk-limited stoppable set");
     }
 
+    private static void VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically()
+    {
+        LateralPath path = CreateStraightPath(1d);
+        DateTimeOffset previousEffectiveAtUtc = DateTimeOffset.UnixEpoch.AddSeconds(10d);
+        EmTrajectory previous = CreatePreviousTrajectory(previousEffectiveAtUtc, TravelDirection.Forward, 3);
+        var builder = new LongitudinalPreviousTrajectorySeedBuilder();
+        LongitudinalPreviousTrajectorySeed seed = builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.20d),
+            new[] { 0d, 0.10d, 0.20d }, 3, TravelDirection.Forward);
+
+        Verification.Equal(3, seed.PathS.Count, "previous seed path-S count");
+        Verification.Equal(3, seed.ProgressSpeedMetersPerSecond.Count, "previous seed speed count");
+        Verification.NearlyEqual(0.20d, seed.PathS[0], "previous seed begins at new absolute effective time");
+        Verification.True(seed.PathS[1] >= seed.PathS[0] && seed.PathS[2] >= seed.PathS[1],
+            "previous seed progress is monotone");
+        Verification.NearlyEqual(0.10d, seed.ProgressSpeedMetersPerSecond[0],
+            "previous seed uses absolute progress speed");
+
+        Verification.Equal(0, builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.20d),
+            new[] { 0d, 0.10d, 0.20d }, 3, TravelDirection.Reverse).PathS.Count,
+            "different direction returns an empty seed");
+        Verification.Equal(0, builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.20d),
+            new[] { 0d, 0.10d, 0.20d }, 4, TravelDirection.Forward).PathS.Count,
+            "different segment returns an empty seed");
+        Verification.Equal(0, builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.40d),
+            new[] { 0d, 0.10d }, 3, TravelDirection.Forward).PathS.Count,
+            "out-of-range absolute sampling returns an empty seed");
+    }
+
     private static LateralPath CreatePath(IReadOnlyList<PathFixture> fixtures)
     {
         var points = new List<LateralPathPoint>(fixtures.Count);
@@ -466,6 +495,28 @@ internal static class LongitudinalModelChecks
         return new LateralPath(points, true);
     }
 
+    private static EmTrajectory CreatePreviousTrajectory(DateTimeOffset effectiveAtUtc, TravelDirection direction,
+        int segmentIndex)
+    {
+        var metadata = new EmTrajectoryMetadata("previous-seed", effectiveAtUtc, effectiveAtUtc, 1L,
+            "previous-reference", 1L, string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop,
+            EmLongitudinalMode.RollingContinuation);
+        double sign = direction == TravelDirection.Forward ? 1d : -1d;
+        return new EmTrajectory(metadata, new[]
+        {
+            new EmTrajectoryPoint(0d, 0d, 0d, sign * 0.10d, 0d, 0d, segmentIndex, 0d, 0d,
+                direction, EmBoundaryType.None, 0d, 0d),
+            new EmTrajectoryPoint(sign * 0.10d, 0d, 0d, sign * 0.10d, 0.10d, 0d, segmentIndex, 0.10d, 0.10d,
+                direction, EmBoundaryType.None, 0d, 0d),
+            new EmTrajectoryPoint(sign * 0.20d, 0d, 0d, sign * 0.10d, 0.20d, 0d, segmentIndex, 0.20d, 0.20d,
+                direction, EmBoundaryType.None, 0d, 0d),
+            new EmTrajectoryPoint(sign * 0.30d, 0d, 0d, sign * 0.10d, 0.30d, 0d, segmentIndex, 0.30d, 0.30d,
+                direction, EmBoundaryType.None, 0d, 0d),
+            new EmTrajectoryPoint(sign * 0.40d, 0d, 0d, sign * 0.10d, 0.40d, 0d, segmentIndex, 0.40d, 0.40d,
+                direction, EmBoundaryType.None, 0d, 0d),
+        });
+    }
+
     private static DirectionSegmentView CreateSegment(double length, EmBoundaryType endBoundaryType)
     {
         var points = new List<SmoothedPathPoint>