fix: stabilize stopped-start EM planning and add txt diagnostics

2026-08-07 15:51:31 +08:00
parent f2970d0aff
commit d378ad6257
13 changed files with 373 additions and 53 deletions
@@ -96,9 +96,9 @@ public sealed partial class EmPlannerConfiguration
            Validation = new ValidationConfiguration
            {
                SpatialToleranceMeters = 1e-8d,
-                KinematicTolerance = 1e-8d,
-                TerminalPositionToleranceMeters = 0.03d,
-                TerminalYawToleranceRadians = 5d * System.Math.PI / 180d,
+                KinematicTolerance = 1e-5d,
+                TerminalPositionToleranceMeters = 0d,
+                TerminalYawToleranceRadians = 0d,
            },
        };
    }
@@ -1,6 +1,7 @@
 using System;
 using System.Collections.Generic;
 using System.Diagnostics;
+using System.Globalization;
 using System.Threading;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;

@@ -38,6 +39,21 @@ public sealed class SequentialLongitudinalOptimizer
            return Failed(EmPlanningStatus.InvalidInput, "Longitudinal planning input is required.");
        if (cancellationToken.IsCancellationRequested)
            return Failed(EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled.");
+        if (input.PlanningScope == EmPlanningScope.FullDirectionSegment &&
+            input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
+        {
+            double staticStartSpeedTolerance = Math.Max(input.Configuration.Validation.SpatialToleranceMeters,
+                input.Configuration.Longitudinal.StopSpeedToleranceMetersPerSecond);
+            if (input.InitialProgressSpeedMetersPerSecond <= staticStartSpeedTolerance &&
+                Math.Abs(input.InitialAccelerationMetersPerSecondSquared) <=
+                    input.Configuration.Validation.KinematicTolerance)
+            {
+                input = new LongitudinalPlanningInput(input.Path, input.Direction, 0d,
+                    input.InitialAccelerationMetersPerSecondSquared, input.TerminalType, input.Mode,
+                    input.Configuration, input.PlanningScope, input.KnotSchedule,
+                    input.PreviousPathS, input.PreviousProgressSpeedMetersPerSecond);
+            }
+        }
        if (!TryCreateSettings(input, out QpSolverSettings settings, out TimeSpan totalBudget, out double convergenceTolerance,
                out int iterationLimit, out string configurationFailure))
        {
@@ -223,14 +239,27 @@ public sealed class SequentialLongitudinalOptimizer
        projectionSolveCount = 0;
        failureStatus = EmPlanningStatus.LongitudinalInfeasible;
        failureReason = string.Empty;
-        if (input.InitialProgressSpeedMetersPerSecond <= input.Configuration.Validation.SpatialToleranceMeters &&
+        double staticStartSpeedTolerance = Math.Max(input.Configuration.Validation.SpatialToleranceMeters,
+            input.Configuration.Longitudinal.StopSpeedToleranceMetersPerSecond);
+        bool staticStartEligible = input.InitialProgressSpeedMetersPerSecond <= staticStartSpeedTolerance &&
            Math.Abs(input.InitialAccelerationMetersPerSecondSquared) <=
-                input.Configuration.Validation.KinematicTolerance &&
-            TryCreateStaticStartSeed(input, speedLimit, out LongitudinalCandidate staticStartSeed))
+                input.Configuration.Validation.KinematicTolerance;
+        bool staticStartSeedUsed = false;
+        string staticStartSeedFailure = string.Empty;
+        if (staticStartEligible && TryCreateStaticStartSeed(input, speedLimit, out LongitudinalCandidate staticStartSeed,
+                out staticStartSeedFailure))
        {
+            staticStartSeedUsed = true;
            candidate = staticStartSeed;
            return true;
        }
+        string WithStaticSeedDiagnostic(string reason)
+        {
+            return staticStartEligible && !staticStartSeedUsed
+                ? "staticStartSeed=failed (" + staticStartSeedFailure + "); " + reason
+                : reason;
+        }
+
        LongitudinalCandidate linearizationIterate = CreateScheduleReferenceIterate(input);
        string lastRejection = string.Empty;
        for (int iteration = 0; iteration < iterationLimit; iteration++)
@@ -238,21 +267,21 @@ public sealed class SequentialLongitudinalOptimizer
            if (cancellationToken.IsCancellationRequested)
            {
                failureStatus = EmPlanningStatus.Cancelled;
-                failureReason = "Initial full-direction feasibility projection was cancelled.";
+                failureReason = WithStaticSeedDiagnostic("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.";
+                failureReason = WithStaticSeedDiagnostic("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;
+                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility constraints are infeasible: " + buildFailure);
                return false;
            }

@@ -267,45 +296,45 @@ public sealed class SequentialLongitudinalOptimizer
            if (solved == null)
            {
                failureStatus = EmPlanningStatus.Failed;
-                failureReason = "The initial full-direction feasibility solver returned no result.";
+                failureReason = WithStaticSeedDiagnostic("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 +
+                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection timed out (status=" + solved.NativeStatus +
                    ", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual + ", dual=" +
-                    solved.DualResidual + "): " + solved.Diagnostic;
+                    solved.DualResidual + "): " + solved.Diagnostic);
                return false;
            }
            if (solved.Status == QpSolveStatus.Cancelled)
            {
                failureStatus = EmPlanningStatus.Cancelled;
-                failureReason = "Initial full-direction feasibility projection was cancelled: " + solved.Diagnostic;
+                failureReason = WithStaticSeedDiagnostic("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;
+                failureReason = WithStaticSeedDiagnostic("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;
+                failureReason = WithStaticSeedDiagnostic("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;
+                failureReason = WithStaticSeedDiagnostic("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.";
+                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver primal does not match the ST layout.");
                return false;
            }
            if (solved.Status == QpSolveStatus.Solved || HasStrictResiduals(solved, convergenceTolerance))
@@ -319,7 +348,7 @@ public sealed class SequentialLongitudinalOptimizer
                if (validationStatus == EmPlanningStatus.NoProgress)
                {
                    failureStatus = validationStatus;
-                    failureReason = validationFailure;
+                    failureReason = WithStaticSeedDiagnostic(validationFailure);
                    return false;
                }
                lastRejection = validationFailure;
@@ -329,7 +358,7 @@ public sealed class SequentialLongitudinalOptimizer
                    out LongitudinalCandidate nextLinearization))
            {
                failureStatus = EmPlanningStatus.LongitudinalInfeasible;
-                failureReason = "Initial full-direction feasibility candidate could not be relinearized against the PathS envelope.";
+                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility candidate could not be relinearized against the PathS envelope.");
                return false;
            }
            linearizationIterate = nextLinearization;
@@ -339,8 +368,8 @@ public sealed class SequentialLongitudinalOptimizer
                lastRejection = "Initial feasibility projection violated the strict physical validator.";
        }
        failureStatus = EmPlanningStatus.LongitudinalInfeasible;
-        failureReason = "Initial full-direction feasibility projection exhausted the configured outer iterations. " +
-            lastRejection;
+        failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection exhausted the configured outer iterations. " +
+            lastRejection);
        return false;
    }

@@ -466,24 +495,31 @@ public sealed class SequentialLongitudinalOptimizer
    }

    private bool TryCreateStaticStartSeed(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
-        out LongitudinalCandidate candidate)
+        out LongitudinalCandidate candidate, out string failureReason)
    {
        candidate = null;
+        failureReason = "unknown";
        int stabilizationStart = input.KnotSchedule.TerminalHoldStartIndex;
        if (stabilizationStart < 5)
+        {
+            failureReason = "terminalHoldStartIndex=" + stabilizationStart.ToString(CultureInfo.InvariantCulture);
            return false;
+        }

        IReadOnlyList<double> times = input.KnotSchedule.KnotTimes;
        if (TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit, out candidate))
            return true;
+        failureReason = "exactJerkSeed=failed";
        double firstDuration = times[1] - times[0];
        double secondDuration = times[2] - times[1];
        LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
        double maximumFirstJerk = Math.Min(configuration.MaximumJerkMetersPerSecondCubed,
            Math.Min(configuration.MaximumAccelerationMetersPerSecondSquared / firstDuration,
                configuration.MaximumJerkMetersPerSecondCubed * secondDuration / firstDuration));
-        for (int sample = 1; sample <= 256; sample++)
+        for (int sample = -256; sample <= 256; sample++)
        {
+            if (sample == 0)
+                continue;
            double firstJerk = maximumFirstJerk * sample / 256d;
            var jerk = new double[stabilizationStart];
            jerk[0] = firstJerk;
@@ -497,9 +533,11 @@ public sealed class SequentialLongitudinalOptimizer
                    out LongitudinalCandidate strict, out _))
            {
                candidate = strict;
+                failureReason = string.Empty;
                return true;
            }
        }
+        failureReason = "exactJerkSeed=failed; sampledSeeds=failed";
        return false;
    }

@@ -94,8 +94,8 @@ LS 的变量是 `l`、`dl`、`ddl` 和区间 `dddl`；它对静态走廊、导
 | 静态走廊、LS、ST 或停车条件不可行 | `CorridorInfeasible`、`LateralInfeasible`、`LongitudinalInfeasible`、`StoppingDistanceInsufficient` | 空 | 不发布；由上层决定重试、停车或重新选路 |
 | 求解器不可用、超时、取消或发布复核失败 | `SolverUnavailable`、`SolverTimedOut`、`Cancelled`、`ValidationFailed`、`Failed` | 空 | 读取 `FailureReason`；不得使用中间轨迹 |
 | 已被更新周期替代 | `Superseded` | 空 | 由滚动协调器忽略旧结果 |
-| 完整方向段无有效进度 | `NoProgress` | 空 | 不发布；仅真实终点容差内或受监督停车保持观察可合法零进度 |
-| 完整方向段终端位姿超差 | `TerminalPoseMismatch` | 空 | 不发布；位置误差超过 `0.03 m` 或归一化 yaw 误差超过 `5°` |
+| 完整方向段无有效进度 | `NoProgress` | 空 | 不发布；仅真实终点精确匹配或受监督停车保持观察可合法零进度 |
+| 完整方向段终端位姿超差 | `TerminalPoseMismatch` | 空 | 不发布；位置误差超过 `0 m` 或归一化 yaw 误差超过 `0 rad` |
 | 自适应 knots 或发布样本超限 | `FullSegmentResourceLimitExceeded` | 空 | 不发布；不得截断轨迹或回退滚动模式 |

 `FailureReason` 始终以 `map=...;reference=...;state=...;previous=...;segment=...` 开头，供调用方追踪地图、参考路径、状态、上一轨迹和方向段绑定。
@@ -165,7 +165,7 @@ EmTrajectory trajectory = result.Trajectory;

 - 对完整活动方向段做一次优化；`s_end` 来自已选 Local G2 的实际 `PathS`，`T_end` 由可行加速度、巡航、jerk 限速停止和零速 hold 推导，不依赖旧的距离/时间截断窗口。
 - 前进期望/硬上限为 `1.0 m/s`，倒车期望/硬上限为 `0.5 m/s`。
- 成功终端必须停止，世界位置误差不超过 `0.03 m`，归一化 yaw 误差不超过 `5°`（约 `0.0873 rad`）。
+- 成功终端必须停止，世界位置误差和归一化 yaw 误差均为 `0`（精确到达目标）。
 - `NoProgress`、`TerminalPoseMismatch`、`FullSegmentResourceLimitExceeded` 失败均返回空轨迹，不降级为滚动或截断发布。
 - `DistanceHorizonMeters` 与 `TimeHorizonSeconds` 作为兼容字段保留，但不用于截断 full 模式。

@@ -150,8 +150,8 @@ public static class EmPlanningRequestValidator
            !Positive(scheduling.MaximumOptimizationTimeStepSeconds) ||
            !Positive(scheduling.MaximumOptimizationSpatialStepMeters) ||
            scheduling.MaximumOptimizationKnotCount < 3 || scheduling.MaximumPublishedSampleCount < 2 ||
-            !Positive(validation.TerminalPositionToleranceMeters) ||
-            !Positive(validation.TerminalYawToleranceRadians) || validation.TerminalYawToleranceRadians > Math.PI ||
+            validation.TerminalPositionToleranceMeters < 0d ||
+            validation.TerminalYawToleranceRadians < 0d || validation.TerminalYawToleranceRadians > Math.PI ||
            !WeightsAreValid(configuration.Lateral.Weights) || !WeightsAreValid(configuration.Longitudinal.Weights))
            return false;

@@ -204,6 +204,7 @@ internal static class TrajectoryObservationMovementTestRunner
    private static Task activeTask;
    private static TrajectoryObservationVisualizationPublisher activeWebPublisher;
    private static TrajectoryObservationPresentation activePresentation;
+    private static TrajectoryObservationReportWriter activeReportWriter;
    private static long nextSessionId;
    private static long activeSessionId;
    private static long stateSequence;
@@ -218,6 +219,7 @@ internal static class TrajectoryObservationMovementTestRunner
        var cancellation = new CancellationTokenSource();
        CancellationTokenSource previousCancellation;
        long sessionId;
+        TrajectoryObservationReportWriter reportWriter = null;
        lock (SessionSync)
        {
            previousCancellation = activeCancellation;
@@ -227,6 +229,8 @@ internal static class TrajectoryObservationMovementTestRunner
            activeSessionId = sessionId;
            StopActiveVisualizationNoLock();
            activePresentation = settings.EnableNativePainterVisualization ? new TrajectoryObservationPresentation() : null;
+            reportWriter = new TrajectoryObservationReportWriter(sessionId);
+            activeReportWriter = reportWriter;
            PrintStatus("Starting frozen observation session " +
                sessionId.ToString(CultureInfo.InvariantCulture) + ".");
        }
@@ -239,7 +243,7 @@ internal static class TrajectoryObservationMovementTestRunner
        {
            if (activeSessionId == sessionId) activeTask = task;
        }
-        _ = task.ContinueWith(completed => Finish(sessionId, cancellation, completed), TaskScheduler.Default);
+        _ = task.ContinueWith(completed => Finish(sessionId, cancellation, completed, reportWriter), TaskScheduler.Default);
    }

    internal static void Stop()
@@ -271,6 +275,13 @@ internal static class TrajectoryObservationMovementTestRunner
    {
        token.ThrowIfCancellationRequested();
        VehicleMotionState initialState = ReadVehicleState();
+        LogIfCurrent(sessionId, "initial state: pose=(" + Format(initialState.Pose.X) + ", " +
+            Format(initialState.Pose.Y) + ", " + Format(initialState.Pose.Heading) + ") m/rad, signed speed=" +
+            initialState.SignedLongitudinalSpeedMetersPerSecond.ToString("R", CultureInfo.InvariantCulture) + " m/s, acceleration=" +
+            (initialState.LongitudinalAccelerationMetersPerSecondSquared.HasValue
+                ? Format(initialState.LongitudinalAccelerationMetersPerSecondSquared.Value)
+                : "unavailable") + " m/s2, stateSequence=" +
+            initialState.SequenceId.ToString(CultureInfo.InvariantCulture));
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            initialState.Pose, goal, settings, obstacles, obstacleSnapshotVersion);

@@ -294,6 +305,15 @@ internal static class TrajectoryObservationMovementTestRunner
            new EmPlanningService(new OsqpNativeSolver()),
            "trajectory-observer-" + sessionId.ToString(CultureInfo.InvariantCulture));
        LogIfCurrent(sessionId, controller.CreateConfigurationDiagnostic().Text);
+        LogIfCurrent(sessionId, CreateSettingsDiagnostic(settings));
+        EmPlannerConfiguration effectiveConfiguration = controller.CreateEffectiveConfigurationSnapshot();
+        double stopSpeedTolerance = effectiveConfiguration.Longitudinal.StopSpeedToleranceMetersPerSecond;
+        LogIfCurrent(sessionId, "static start check: initialSignedSpeed=" +
+            initialState.SignedLongitudinalSpeedMetersPerSecond.ToString("R", CultureInfo.InvariantCulture) +
+            ", stopSpeedTolerance=" + stopSpeedTolerance.ToString("R", CultureInfo.InvariantCulture) +
+            ", treatedAsStopped=" +
+            (Math.Abs(initialState.SignedLongitudinalSpeedMetersPerSecond) <= stopSpeedTolerance)
+                .ToString(CultureInfo.InvariantCulture));
        var observationLoop = new TrajectoryObservationLoop(controller);
        TrajectoryObservationVisualizationPublisher webPublisher = StartWebVisualizationIfEnabled(
            sessionId, bootstrap, controller, settings, obstacleSnapshotVersion);
@@ -332,7 +352,14 @@ internal static class TrajectoryObservationMovementTestRunner
            if (tick.PlanningStarted)
                LogIfCurrent(sessionId, "planning status=pending");
            if (tick.PlanningCompleted)
+            {
                LogIfCurrent(sessionId, diagnostic.Text);
+                if (observation.PublishedTrajectory != null && charts != null)
+                {
+                    AppendReportIfCurrent(sessionId,
+                        TrajectoryObservationDiagnostics.CreateCurveReport(charts, observation.PublishedTrajectory).Text);
+                }
+            }
        }
    }

@@ -490,14 +517,25 @@ internal static class TrajectoryObservationMovementTestRunner
        }
    }

+    private static void AppendReportIfCurrent(long sessionId, string text)
+    {
+        lock (SessionSync)
+        {
+            if (activeSessionId != sessionId) return;
+            activeReportWriter?.Append(text);
+        }
+    }
+
    private static void PrintStatus(string message)
    {
        string text = ObserveOnlyNotice + "\n" + message;
        Hedingben.ToastText(text, StatusChannel);
        Console.WriteLine("[TrajectoryObserver] " + text);
+        activeReportWriter?.Append(message);
    }

-    private static void Finish(long sessionId, CancellationTokenSource cancellation, Task completed)
+    private static void Finish(long sessionId, CancellationTokenSource cancellation, Task completed,
+        TrajectoryObservationReportWriter reportWriter)
    {
        try
        {
@@ -514,16 +552,38 @@ internal static class TrajectoryObservationMovementTestRunner
        {
            lock (SessionSync)
            {
+                if (ReferenceEquals(activeReportWriter, reportWriter))
+                    activeReportWriter = null;
                if (activeSessionId == sessionId)
                {
                    activeTask = null;
                    activeCancellation = null;
                }
            }
+            try
+            {
+                string reportPath = reportWriter.Save();
+                Console.WriteLine("[TrajectoryObserver] report=" + reportPath);
+            }
+            catch (Exception exception)
+            {
+                Console.WriteLine("[TrajectoryObserver] report write failed: " + exception.Message);
+            }
            cancellation.Dispose();
        }
    }

+    private static string CreateSettingsDiagnostic(TrajectoryObservationSettings settings)
+    {
+        return "observation settings: planningScope=" + settings.PlanningScope +
+            ", webVisualization=" + settings.EnableWebVisualization +
+            ", nativePainter=" + settings.EnableNativePainterVisualization +
+            ", outputTimeStep=" + Format(settings.OutputTimeStepSeconds) + "s" +
+            ", observerPeriod=" + Format(settings.ObserverPeriodSeconds) + "s" +
+            ", solverTimeout=" + Format(settings.SolverTimeoutSeconds) + "s" +
+            ", maximumOsqpIterations=" + settings.MaximumOsqpIterations.ToString(CultureInfo.InvariantCulture);
+    }
+
    private static void ClearAndLogRuntimeFaultIfCurrent(long sessionId, Exception exception)
    {
        lock (SessionSync)
@@ -10,6 +10,10 @@ diagnostic prediction only. Goal and rolling-safety-stop commands are logged, ne
 gear-switch trajectory, the observer remains on the current direction segment and waits for real direction confirmation;
 it does not create or dispatch a direction-change action.

+Every frozen observation session writes a text report to
+`TrajectoryObservationReports/trajectory-observation-session-<id>.txt` in the process working directory when the
+session stops; the report path is printed to the console.
+
 ## FullDirectionSegment observation (default)

 `UseFullDirectionSegmentPlanning=true` maps to `EmPlanningScope.FullDirectionSegment`. MovementTest performs one frozen
@@ -20,8 +24,7 @@ and zero-speed hold behavior instead of being forced by a legacy time horizon.

 `DistanceHorizonMeters` and `TimeHorizonSeconds` are rolling-compatible fields and do not truncate full mode. Forward
 desired/hard maximum speed is `1.0 m/s`; reverse desired/hard maximum speed is `0.5 m/s`. A successful real boundary
-requires a terminal stop plus world-position error no greater than `0.03 m` and normalized yaw error no greater than
-`5 deg` (`5 * PI / 180 rad`).
+requires a terminal stop plus exact world-position and normalized yaw matching (`0 m` / `0 rad`).

 ## Rolling trajectory observation

@@ -1,6 +1,7 @@
 using System;
 using System.Collections.Generic;
 using System.Globalization;
+using System.Text;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;
 using MultiWheelC.TrajectoryPlanning.EMPlanner;

@@ -18,10 +19,69 @@ public sealed class TrajectoryObservationDiagnostic

 public static class TrajectoryObservationDiagnostics
 {
+    public static TrajectoryObservationDiagnostic CreateCurveReport(TrajectoryObservationCharts charts,
+        EmTrajectory trajectory)
+    {
+        if (charts == null || trajectory == null)
+            return new TrajectoryObservationDiagnostic("curve report unavailable");
+
+        IReadOnlyList<EmTrajectoryPoint> points = trajectory.Points;
+        var text = new StringBuilder();
+        text.AppendLine("LS_CURVE");
+        text.AppendLine("index,referenceS(m),lateralOffset(m)");
+        for (int index = 0; index < charts.LsSamples.Count; index++)
+        {
+            TrajectoryObservationLsSample sample = charts.LsSamples[index];
+            text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," +
+                sample.PathS.ToString("R", CultureInfo.InvariantCulture) + "," +
+                sample.LateralOffset.ToString("R", CultureInfo.InvariantCulture));
+        }
+
+        text.AppendLine("ST_CURVE");
+        text.AppendLine("index,time(s),pathS(m)");
+        for (int index = 0; index < charts.StSamples.Count; index++)
+        {
+            TrajectoryObservationStSample sample = charts.StSamples[index];
+            text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," +
+                sample.TimeFromStart.ToString("R", CultureInfo.InvariantCulture) + "," +
+                sample.PathS.ToString("R", CultureInfo.InvariantCulture));
+        }
+
+        text.AppendLine("SPEED_CURVE");
+        text.AppendLine("index,time(s),signedSpeed(m/s)");
+        for (int index = 0; index < charts.SpeedSamples.Count; index++)
+        {
+            TrajectoryObservationSpeedSample sample = charts.SpeedSamples[index];
+            text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," +
+                sample.TimeFromStart.ToString("R", CultureInfo.InvariantCulture) + "," +
+                sample.SignedLongitudinalVelocity.ToString("R", CultureInfo.InvariantCulture));
+        }
+
+        text.AppendLine("ACCELERATION_CURVE");
+        text.AppendLine("index,time(s),acceleration(m/s2)");
+        for (int index = 0; index < points.Count; index++)
+        {
+            text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," +
+                points[index].TimeFromStart.ToString("R", CultureInfo.InvariantCulture) + "," +
+                points[index].LongitudinalAcceleration.ToString("R", CultureInfo.InvariantCulture));
+        }
+
+        text.AppendLine("JERK_CURVE");
+        text.AppendLine("index,time(s),jerk(m/s3)");
+        for (int index = 0; index + 1 < points.Count; index++)
+        {
+            text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," +
+                points[index].TimeFromStart.ToString("R", CultureInfo.InvariantCulture) + "," +
+                points[index].LongitudinalJerk.ToString("R", CultureInfo.InvariantCulture));
+        }
+
+        return new TrajectoryObservationDiagnostic(text.ToString());
+    }
+
    public static TrajectoryObservationDiagnostic CreateConfiguration(EmPlannerConfiguration configuration)
    {
        if (configuration == null || configuration.Scheduling == null || configuration.Solver == null ||
-            configuration.Longitudinal == null)
+            configuration.Longitudinal == null || configuration.Validation == null)
            throw new ArgumentNullException(nameof(configuration));

        double horizon = configuration.Scheduling.TimeHorizonSeconds;
@@ -57,6 +117,19 @@ public static class TrajectoryObservationDiagnostics
            "maximumOsqpIterations=" + configuration.Solver.MaximumOsqpIterations.ToString(CultureInfo.InvariantCulture) + "\n" +
            "solverTimeout=" + Format(configuration.Scheduling.SolverTimeoutSeconds, "F2") + "s\n" +
            "replanPeriod=" + Format(configuration.Scheduling.ReplanPeriodSeconds, "F2") + "s\n" +
+            "kinematicTolerance=" + Format(configuration.Validation.KinematicTolerance, "G17") + "\n" +
+            "stopSpeedToleranceMetersPerSecond=" + Format(longitudinal.StopSpeedToleranceMetersPerSecond, "G17") + "\n" +
+            "terminalPositionToleranceMeters=" + Format(configuration.Validation.TerminalPositionToleranceMeters, "G17") + "\n" +
+            "terminalYawToleranceRadians=" + Format(configuration.Validation.TerminalYawToleranceRadians, "G17") + "\n" +
+            "maximumOptimizationTimeStepSeconds=" + Format(configuration.Scheduling.MaximumOptimizationTimeStepSeconds, "F3") + "\n" +
+            "maximumOptimizationSpatialStepMeters=" + Format(configuration.Scheduling.MaximumOptimizationSpatialStepMeters, "F3") + "\n" +
+            "maximumOptimizationKnotCount=" + configuration.Scheduling.MaximumOptimizationKnotCount.ToString(CultureInfo.InvariantCulture) + "\n" +
+            "maximumPublishedSampleCount=" + configuration.Scheduling.MaximumPublishedSampleCount.ToString(CultureInfo.InvariantCulture) + "\n" +
+            "maximumForwardSpeedMetersPerSecond=" + Format(longitudinal.MaximumForwardSpeedMetersPerSecond, "F3") + "\n" +
+            "maximumReverseSpeedMetersPerSecond=" + Format(longitudinal.MaximumReverseSpeedMetersPerSecond, "F3") + "\n" +
+            "maximumAccelerationMetersPerSecondSquared=" + Format(longitudinal.MaximumAccelerationMetersPerSecondSquared, "F3") + "\n" +
+            "maximumDecelerationMetersPerSecondSquared=" + Format(longitudinal.MaximumDecelerationMetersPerSecondSquared, "F3") + "\n" +
+            "maximumJerkMetersPerSecondCubed=" + Format(longitudinal.MaximumJerkMetersPerSecondCubed, "F3") + "\n" +
            "maximumJerkLimitedStopDistance=" + Format(worstStop.DistanceMeters, "F3") + "m\n" +
            "maximumJerkLimitedStopDuration=" + Format(worstStop.DurationSeconds, "F3") + "s\n" +
            "requiredDistanceHorizon=" + Format(requiredDistanceHorizon, "F3") + "m");
@@ -0,0 +1,67 @@
+using System;
+using System.Globalization;
+using System.IO;
+using System.Text;
+
+namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
+
+/// <summary>Buffers a readable text report for one trajectory observation session.</summary>
+public sealed class TrajectoryObservationReportWriter
+{
+    private readonly long _sessionId;
+    private readonly string _reportDirectory;
+    private readonly StringBuilder _content = new StringBuilder();
+    private readonly object _sync = new object();
+
+    public TrajectoryObservationReportWriter(long sessionId, string reportDirectory = null)
+    {
+        if (sessionId <= 0)
+            throw new ArgumentOutOfRangeException(nameof(sessionId));
+
+        _sessionId = sessionId;
+        _reportDirectory = reportDirectory ??
+            Path.Combine(Environment.CurrentDirectory, "TrajectoryObservationReports");
+        AppendHeader();
+    }
+
+    public string FilePath
+    {
+        get
+        {
+            return Path.Combine(_reportDirectory, "trajectory-observation-session-" +
+                _sessionId.ToString(CultureInfo.InvariantCulture) + ".txt");
+        }
+    }
+
+    public void Append(string text)
+    {
+        if (string.IsNullOrEmpty(text))
+            return;
+
+        lock (_sync)
+        {
+            _content.Append(DateTimeOffset.Now.ToString("yyyy-MM-dd HH:mm:ss.fff", CultureInfo.InvariantCulture));
+            _content.Append(" | ");
+            _content.AppendLine(text);
+        }
+    }
+
+    public string Save()
+    {
+        lock (_sync)
+        {
+            Directory.CreateDirectory(_reportDirectory);
+            File.WriteAllText(FilePath, _content.ToString(), new UTF8Encoding(false));
+            return FilePath;
+        }
+    }
+
+    private void AppendHeader()
+    {
+        _content.AppendLine("EM trajectory observation report");
+        _content.AppendLine("session=" + _sessionId.ToString(CultureInfo.InvariantCulture));
+        _content.AppendLine("OBSERVE_ONLY: no chassis command is sent.");
+        _content.AppendLine("reportGeneratedUtc=" + DateTimeOffset.UtcNow.ToString("O", CultureInfo.InvariantCulture));
+        _content.AppendLine();
+    }
+}
@@ -101,6 +101,8 @@ internal static class FoundationChecks
        EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.AbsoluteTolerance, "absolute tolerance");
        EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.RelativeTolerance, "relative tolerance");
        EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.StrictResidualTolerance, "strict residual tolerance");
+        EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Validation.KinematicTolerance,
+            "kinematic tolerance");
        EMPlannerVerificationHost.Verification.Equal(true, configuration.Solver.WarmStart, "warm start");
        EMPlannerVerificationHost.Verification.Equal(true, configuration.Solver.Polish, "polish");
        EMPlannerVerificationHost.Verification.Equal(false, configuration.Solver.NativeVerbose, "native verbose");
@@ -179,9 +181,9 @@ internal static class FoundationChecks
            "optimization knot cap");
        EMPlannerVerificationHost.Verification.Equal(5001, defaults.Scheduling.MaximumPublishedSampleCount,
            "publication sample cap");
-        EMPlannerVerificationHost.Verification.NearlyEqual(0.03d,
+        EMPlannerVerificationHost.Verification.NearlyEqual(0d,
            defaults.Validation.TerminalPositionToleranceMeters, "terminal position tolerance");
-        EMPlannerVerificationHost.Verification.NearlyEqual(5d * Math.PI / 180d,
+        EMPlannerVerificationHost.Verification.NearlyEqual(0d,
            defaults.Validation.TerminalYawToleranceRadians, "terminal yaw tolerance");
        EMPlannerVerificationHost.Verification.Equal(EmPlanningStatus.NoProgress, EmPlanningStatus.NoProgress,
            "no-progress status exists");
@@ -200,7 +202,7 @@ internal static class FoundationChecks
            copied.Longitudinal.DesiredForwardSpeedMetersPerSecond, "copy keeps desired forward speed");
        EMPlannerVerificationHost.Verification.Equal(401, copied.Scheduling.MaximumOptimizationKnotCount,
            "copy keeps knot cap");
-        EMPlannerVerificationHost.Verification.NearlyEqual(0.03d,
+        EMPlannerVerificationHost.Verification.NearlyEqual(0d,
            copied.Validation.TerminalPositionToleranceMeters, "copy keeps terminal tolerance");

        EmPlannerConfiguration invalidKnotCap = EmPlannerConfiguration.CreateDefault();
@@ -15,6 +15,7 @@ internal static class LongitudinalIntegrationChecks
        VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed();
        VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence();
        VerifiesFullDirectionStaticStartMakesProgress();
+        VerifiesFullDirectionNearZeroSpeedUsesStaticStartSeed();
        VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold();
        VerifiesExactStopIncludesAStabilizationTail();
        VerifiesLastStrictCandidateSurvivesLaterTimeout();
@@ -150,6 +151,42 @@ internal static class LongitudinalIntegrationChecks
            "full segment stops at the terminal boundary");
    }

+    private static void VerifiesFullDirectionNearZeroSpeedUsesStaticStartSeed()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Scheduling.SolverTimeoutSeconds = 5d;
+        configuration.Solver.MaximumOsqpIterations = 100000;
+        LateralPath path = new LateralPath(new[]
+        {
+            Point(0d, 0d, 0d),
+            Point(2.5d, 2.5d, 0d),
+            Point(5d, 5d, 0d),
+        }, true);
+
+        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.01d,
+            EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "near-zero envelope: " + failureReason);
+        status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.01d, 0d,
+            configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
+            out LongitudinalKnotSchedule schedule, out failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "near-zero schedule: " + failureReason);
+
+        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.01d, 0d,
+            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
+            EmPlanningScope.FullDirectionSegment, schedule, Array.Empty<double>(), Array.Empty<double>());
+        LongitudinalPlanningResult result = new LongitudinalPlanner(new OsqpNativeSolver()).Plan(input,
+            CancellationToken.None);
+
+        Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
+            "near-zero full-direction start succeeds: " + result.FailureReason);
+        LongitudinalCandidate candidate = result.Candidate ??
+            throw new InvalidOperationException("Near-zero full-direction candidate was missing.");
+        Verification.NearlyEqual(path.Points[path.Points.Count - 1].PathS, candidate.S[candidate.S.Count - 1],
+            "near-zero start reaches the terminal boundary");
+        Verification.NearlyEqual(0d, candidate.U[candidate.U.Count - 1],
+            "near-zero start stops at the terminal boundary");
+    }
+
    private static void VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
@@ -769,9 +769,9 @@ internal static class LongitudinalModelChecks
            out speedFailure);
        Verification.Equal(EmPlanningStatus.Success, speedStatus, "near-terminal no-progress envelope: " + speedFailure);
        Verification.True(!validator.TryValidate(nearTerminalInput, nearTerminalLimit, stationary, out _,
-            out failureStatus, out _), "near-terminal fixture remains subject to its stop-tail contract");
-        Verification.True(failureStatus != EmPlanningStatus.NoProgress,
-            "near-terminal gear-switch stop is not classified as no progress");
+            out failureStatus, out _), "near-terminal stationary full segment is rejected");
+        Verification.Equal(EmPlanningStatus.NoProgress, failureStatus,
+            "near-terminal gear-switch stop is classified as no progress");
    }

    private static void VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically()
@@ -194,31 +194,27 @@ internal static class TrajectoryChecks
        EmTrajectoryPoint terminal = valid.Points[terminalIndex];
        var validator = new EmTrajectoryValidator();

-        EmTrajectoryValidationResult insidePosition = validator.Validate(valid, context.EmptyMap, context.Vehicle,
-            context.Configuration, 2, 0.0055d, new Pose2D(terminal.X + 0.029d, terminal.Y, terminal.Yaw),
+        EmTrajectoryValidationResult exactPosition = validator.Validate(valid, context.EmptyMap, context.Vehicle,
+            context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, terminal.Yaw),
            EmBoundaryType.Goal);
-        Verification.True(insidePosition.IsValid, "2.9-centimetre terminal position error is accepted");
+        Verification.True(exactPosition.IsValid, "exact terminal position is accepted");
        EmTrajectoryValidationResult outsidePosition = validator.Validate(valid, context.EmptyMap, context.Vehicle,
-            context.Configuration, 2, 0.0055d, new Pose2D(terminal.X + 0.031d, terminal.Y, terminal.Yaw),
+            context.Configuration, 2, 0.0055d, new Pose2D(terminal.X + 0.001d, terminal.Y, terminal.Yaw),
            EmBoundaryType.Goal);
        Verification.Equal(EmTrajectoryValidationFailure.TerminalPoseMismatch, outsidePosition.Failure,
-            "3.1-centimetre terminal position error is rejected");
+            "1-millimetre terminal position error is rejected");

        double degrees = Math.PI / 180d;
-        EmTrajectory wrappedYaw = Replace(valid, terminalIndex, Clone(terminal, yaw: 179d * degrees));
+        EmTrajectory wrappedYaw = Replace(valid, terminalIndex, Clone(terminal, yaw: Math.PI));
        EmTrajectoryValidationResult wrapped = validator.Validate(wrappedYaw, context.EmptyMap, context.Vehicle,
-            context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -179d * degrees),
+            context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -Math.PI),
            EmBoundaryType.Goal);
-        Verification.True(wrapped.IsValid, "179 and -179 degrees use normalized yaw error");
-        EmTrajectoryValidationResult insideYaw = validator.Validate(valid, context.EmptyMap, context.Vehicle,
-            context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -4.9d * degrees),
-            EmBoundaryType.Goal);
-        Verification.True(insideYaw.IsValid, "4.9-degree terminal yaw error is accepted");
+        Verification.True(wrapped.IsValid, "PI and -PI use normalized yaw error");
        EmTrajectoryValidationResult outsideYaw = validator.Validate(valid, context.EmptyMap, context.Vehicle,
-            context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -5.1d * degrees),
+            context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -0.1d * degrees),
            EmBoundaryType.Goal);
        Verification.Equal(EmTrajectoryValidationFailure.TerminalPoseMismatch, outsideYaw.Failure,
-            "5.1-degree terminal yaw error is rejected");
+            "0.1-degree terminal yaw error is rejected");

        EmTrajectory rolling = new EmTrajectoryAssembler().Assemble(
            CreatePath(TravelDirection.Forward, 0d, 0d), CreateRollingLongitudinalResult(),
@@ -23,6 +23,7 @@ internal static class TrajectoryObservationChecks
    {
        TrajectoryObservationSettingsChecks.Run();
        TrajectoryObservationSegmentChecks.Run();
+        VerifiesTrajectoryObservationReportWriterCreatesTxt();
        VerifiesPainterPoseGeometryReplacesLargeArrow();
        TrajectoryObservationVisualizationChecks.Run();
        VerifiesObservationSourceHasNoActuatorCalls();
@@ -47,6 +48,7 @@ internal static class TrajectoryObservationChecks
        VerifiesPresentationTextDescribesObservationWithoutSendingCommand();
        VerifiesPlanningDiagnosticsKeepRawFailureReason();
        VerifiesPublishedPlanningDiagnosticsIncludeTrajectorySummary();
+        VerifiesCurveReportIncludesLsStSpeedAccelerationAndJerk();
        VerifiesEmptyChartsReceivePersistentPlanningDiagnostic();
        VerifiesLsPresentationUsesPathSOnHorizontalAxis();
        VerifiesPainterStAxesUsePathSAndTimeUnits();
@@ -81,6 +83,29 @@ internal static class TrajectoryObservationChecks
        }
    }

+    private static void VerifiesTrajectoryObservationReportWriterCreatesTxt()
+    {
+        string reportDirectory = Path.Combine(Path.GetTempPath(),
+            "em-trajectory-report-" + Guid.NewGuid().ToString("N"));
+        try
+        {
+            var writer = new TrajectoryObservationReportWriter(17L, reportDirectory);
+            writer.Append("cycle=1 status=SolverTimedOut primal=140.918");
+            string reportPath = writer.Save();
+
+            Verification.True(File.Exists(reportPath), "trajectory observation txt report exists");
+            string text = File.ReadAllText(reportPath);
+            Verification.True(text.Contains("session=17"), "report includes session identity");
+            Verification.True(text.Contains("OBSERVE_ONLY"), "report records observation-only boundary");
+            Verification.True(text.Contains("SolverTimedOut"), "report includes cycle failure");
+        }
+        finally
+        {
+            if (Directory.Exists(reportDirectory))
+                Directory.Delete(reportDirectory, true);
+        }
+    }
+
    private static void VerifiesObservationSourceUsesRequiredOperatorText()
    {
        string movementTestSource = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
@@ -960,6 +985,25 @@ internal static class TrajectoryObservationChecks
        }
    }

+    private static void VerifiesCurveReportIncludesLsStSpeedAccelerationAndJerk()
+    {
+        DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 5, 0, 0, 0, TimeSpan.Zero);
+        EmTrajectory trajectory = CreatePublishedTrajectory(effectiveAt);
+        DirectionSegmentView segment = CreateStraightSegment();
+        TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(trajectory, segment, 0.5d);
+
+        string text = TrajectoryObservationDiagnostics.CreateCurveReport(charts, trajectory).Text;
+
+        foreach (string section in new[]
+        {
+            "LS_CURVE", "ST_CURVE", "SPEED_CURVE", "ACCELERATION_CURVE", "JERK_CURVE",
+            "referenceS(m)", "time(s)", "signedSpeed(m/s)", "acceleration(m/s2)", "jerk(m/s3)",
+        })
+        {
+            Verification.True(text.Contains(section), "curve report includes " + section);
+        }
+    }
+
    private static void VerifiesEmptyChartsReceivePersistentPlanningDiagnostic()
    {
        string presentationPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",