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

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/EmPlannerConfiguration.cs

@@ -96,9 +96,9 @@ public sealed partial class EmPlannerConfiguration
             Validation = new ValidationConfiguration
             {
                 SpatialToleranceMeters = 1e-8d,
-                KinematicTolerance = 1e-8d,
+                KinematicTolerance = 1e-5d,
-                TerminalPositionToleranceMeters = 0.03d,
+                TerminalPositionToleranceMeters = 0d,
-                TerminalYawToleranceRadians = 5d * System.Math.PI / 180d,
+                TerminalYawToleranceRadians = 0d,
             },
         };
     }

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs

@@ -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 &&
+                input.Configuration.Validation.KinematicTolerance;
-            TryCreateStaticStartSeed(input, speedLimit, out LongitudinalCandidate staticStartSeed))
+        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. " +
+        failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection exhausted the configured outer iterations. " +
-            lastRejection;
+            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;
     }
 

ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md

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

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs

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

ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs

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

ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md

@@ -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
+requires a terminal stop plus exact world-position and normalized yaw matching (`0 m` / `0 rad`).
-`5 deg` (`5 * PI / 180 rad`).
 
 ## Rolling trajectory observation
 

ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs

@@ -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");

ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationReportWriter.cs

@@ -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();
+    }
+}

ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs

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

ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs

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

ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs

@@ -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");
+            out failureStatus, out _), "near-terminal stationary full segment is rejected");
-        Verification.True(failureStatus != EmPlanningStatus.NoProgress,
+        Verification.Equal(EmPlanningStatus.NoProgress, failureStatus,
-            "near-terminal gear-switch stop is not classified as no progress");
+            "near-terminal gear-switch stop is classified as no progress");
     }
 
     private static void VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically()

ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs

@@ -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,
+        EmTrajectoryValidationResult exactPosition = validator.Validate(valid, context.EmptyMap, context.Vehicle,
-            context.Configuration, 2, 0.0055d, new Pose2D(terminal.X + 0.029d, terminal.Y, terminal.Yaw),
+            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");
+        Verification.True(wrapped.IsValid, "PI and -PI 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");
         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(),

ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs

@@ -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",