ParkingRobot/docs/superpowers/plans/2026-08-10-em-paths-monotonic-resampling-fix.md

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:

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:

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

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:

private readonly double pathSTolerance;

pathSTolerance = configuration.Validation.KinematicTolerance;
if (!IsFinite(pathSTolerance) || pathSTolerance < 0d)
    throw new ArgumentOutOfRangeException(nameof(configuration));

Construct the schedule with:

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:

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:

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

dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory

Expected: PASS trajectory.

• Step 4: Commit the minimal production fix

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

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

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.