diff --git a/docs/superpowers/plans/2026-08-10-em-paths-monotonic-resampling-fix.md b/docs/superpowers/plans/2026-08-10-em-paths-monotonic-resampling-fix.md new file mode 100644 index 0000000..3149cea --- /dev/null +++ b/docs/superpowers/plans/2026-08-10-em-paths-monotonic-resampling-fix.md @@ -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.