docs: plan EM PathS resampling fix

2026-08-10 14:19:26 +08:00
parent 7f33a6c255
commit feae644659
1 changed files with 166 additions and 0 deletions
@@ -0,0 +1,166 @@
+# EM PathS Monotonic Resampling Fix Implementation Plan
+
+> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
+
+**Goal:** Normalize solver-sized `PathS` residuals during full-direction EM trajectory publication while rejecting material regressions.
+
+**Architecture:** `EmTrajectoryAssembler` passes `Validation.KinematicTolerance` into `TrajectorySampleSchedule`. Each output sample is normalized into `[previousPathS, terminalPathS]` only when the required correction is within that tolerance; larger violations retain fail-closed behavior with numeric diagnostics.
+
+**Tech Stack:** C#, .NET 10 verification host, netstandard2.0 plugin, `EMPlannerVerificationHost`.
+
+## Global Constraints
+
+- Do not modify `ClumsyPilot/Control`, `Shared`, `ClumsyPilot/StateEstimation`, `ClumsyPilot/Trajectory`, or controller Movement sources.
+- Reuse `Validation.KinematicTolerance`; add no independent tolerance setting.
+- Published `PathS` must be nondecreasing and no greater than terminal `PathS`.
+- Violations greater than tolerance must still be rejected.
+
+---
+
+### Task 1: Add PathS resampling regression coverage
+
+**Files:**
+- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs`
+
+**Interfaces:**
+- Consumes: `EmTrajectoryAssembler.Assemble(...)` with `EmPlanningScope.FullDirectionSegment`.
+- Produces: regression checks invoked by `TrajectoryChecks.Run()`.
+
+- [ ] **Step 1: Write the failing tolerance-sized regression test**
+
+Add `VerifiesFullScopeNormalizesToleranceSizedPathSResiduals()` to `Run()`. Construct four knots at `0.0, 0.1, 0.2, 0.3` seconds with `S = 0.0, 0.05, 0.05 - 0.5 * KinematicTolerance, 0.10`, use `RollingContinuation` plus `FullDirectionSegment`, assemble it, and assert every published `PathS` is at least its predecessor and no greater than `candidate.S[^1]`.
+
+Use this metadata helper signature so existing calls remain valid:
+
+```csharp
+private static EmTrajectoryMetadata CreateMetadata(TravelDirection direction, EmTerminalType terminalType,
+    EmLongitudinalMode longitudinalMode = EmLongitudinalMode.ExactStopAtBoundary,
+    EmPlanningScope planningScope = EmPlanningScope.RollingHorizon)
+{
+    return new EmTrajectoryMetadata("trajectory", DateTimeOffset.UnixEpoch, DateTimeOffset.UnixEpoch, 3L,
+        "reference", 4L, string.Empty, 2, direction, terminalType, longitudinalMode, planningScope);
+}
+```
+
+- [ ] **Step 2: Write the material-regression protection test**
+
+Add `VerifiesFullScopeRejectsMaterialPathSRegressionWithDiagnostics()`. Use the same time knots with `S = 0.0, 0.05, 0.04, 0.10`; assert assembly throws `ArgumentException` and its message contains `sampleIndex=2`, `previousPathS=`, `candidatePathS=`, and `difference=`.
+
+- [ ] **Step 3: Run the focused test and verify RED**
+
+Run:
+
+```powershell
+dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory
+```
+
+Expected: FAIL with `Trajectory PathS cannot decrease. (Parameter 'candidate')` in the tolerance-sized regression test.
+
+- [ ] **Step 4: Commit the red tests**
+
+```powershell
+git add -- ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs
+git commit -m "test: reproduce EM PathS resampling regression"
+```
+
+### Task 2: Implement bounded monotonic normalization
+
+**Files:**
+- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs`
+- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs`
+
+**Interfaces:**
+- Consumes: `EmPlannerConfiguration.Validation.KinematicTolerance`.
+- Produces: `TrajectorySampleSchedule(..., bool resampleMotion, double pathSTolerance)`.
+
+- [ ] **Step 1: Pass the configured tolerance into the schedule**
+
+In `EmTrajectoryAssembler`, store and validate:
+
+```csharp
+private readonly double pathSTolerance;
+
+pathSTolerance = configuration.Validation.KinematicTolerance;
+if (!IsFinite(pathSTolerance) || pathSTolerance < 0d)
+    throw new ArgumentOutOfRangeException(nameof(configuration));
+```
+
+Construct the schedule with:
+
+```csharp
+var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds,
+    holdDurationSeconds, metadata.LongitudinalMode, isFullDirectionSegment, pathSTolerance);
+```
+
+- [ ] **Step 2: Normalize only tolerance-sized deviations**
+
+Add `using System.Globalization;`. Validate `pathSTolerance`, set `terminalPathS = candidate.S[candidate.S.Count - 1]`, and pass `samples.Count` into each `AddSample` call. Implement the core behavior:
+
+```csharp
+double lowerDifference = previousPathS - sample.PathS;
+double upperDifference = sample.PathS - terminalPathS;
+if (lowerDifference > pathSTolerance || upperDifference > pathSTolerance)
+    throw PathSFailure(sampleIndex, previousPathS, sample.PathS,
+        Math.Max(lowerDifference, upperDifference), candidate);
+
+double normalizedPathS = Math.Min(terminalPathS, Math.Max(previousPathS, sample.PathS));
+if (normalizedPathS != sample.PathS)
+    sample = new TrajectorySample(sample.TimeFromStart, normalizedPathS, sample.ProgressSpeed,
+        sample.Acceleration, sample.Jerk, sample.IsHoldSample);
+```
+
+Retain the existing negative progress-speed check. Create diagnostics with invariant round-trip formatting:
+
+```csharp
+private static ArgumentException PathSFailure(int sampleIndex, double previousPathS,
+    double candidatePathS, double difference, LongitudinalCandidate candidate)
+{
+    return new ArgumentException("Trajectory PathS violates monotonic publication bounds: sampleIndex=" +
+        sampleIndex + ";previousPathS=" + previousPathS.ToString("R", CultureInfo.InvariantCulture) +
+        ";candidatePathS=" + candidatePathS.ToString("R", CultureInfo.InvariantCulture) +
+        ";difference=" + difference.ToString("R", CultureInfo.InvariantCulture) + ".", nameof(candidate));
+}
+```
+
+- [ ] **Step 3: Run focused tests and verify GREEN**
+
+```powershell
+dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory
+```
+
+Expected: `PASS trajectory`.
+
+- [ ] **Step 4: Commit the minimal production fix**
+
+```powershell
+git add -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs
+git commit -m "fix: normalize EM PathS resampling residuals"
+```
+
+### Task 3: Run integration regression and scoped verification
+
+**Files:**
+- Verify only; no production edits expected.
+
+**Interfaces:**
+- Consumes: the normalized `EmTrajectory` publication behavior.
+- Produces: evidence that planning, observation, adapter, and plugin compilation remain valid.
+
+- [ ] **Step 1: Run EM integration checks**
+
+```powershell
+dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-planning-service
+dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
+dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-control-adapter
+```
+
+Expected: all three commands print `PASS`.
+
+- [ ] **Step 2: Build and inspect only the scoped files**
+
+```powershell
+dotnet build ClumsyPilot/ClumsyPilot.csproj -c Release --no-restore
+git diff --check -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs
+```
+
+Expected: zero build errors; only the two known obsolete-API warnings may remain; diff check emits no errors.