ParkingRobot/docs/superpowers/plans/2026-08-03-em-planner-foundation-implementation.md

# EM Planner Foundation 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:** Build the immutable EM Planner contracts, validated configuration, direction-segment boundary model, reverse-safe Frenet transforms, and topology-preserving static lateral corridor.

**Architecture:** Keep the planner core request-driven and independent from hardware, UI, clocks, and solver details. Adapt one consumable `PathSmoothingResult` into an immutable current-direction reference window, then project only within that segment and build a collision-checked connected lateral interval around the selected seed.

**Tech Stack:** C# 10, .NET Standard 2.0 library, existing `PlanningGridMap`, `FootprintCollisionChecker`, and a `net10.0-windows` console verification host.

## Global Constraints

- Namespace for new production types: `MultiWheelC.TrajectoryPlanning.EMPlanner` plus responsibility-specific child namespaces.
- First release supports ordinary nonholonomic forward/reverse motion only; lateral translation, crab motion, and in-place rotation are rejected.
- World units are metres, seconds, radians, metres/second, radians/second, and inverse metres.
- Vehicle body X points forward, body Y points left, and positive yaw is counter-clockwise.
- `TravelYaw = VehicleYaw` forward and `TravelYaw = Normalize(VehicleYaw + π)` reverse.
- Frenet `s` always increases in actual travel direction; positive `l` is left of travel, including reverse.
- A planning window never crosses a gear-switch boundary and never drops an exact terminal anchor.
- Static-corridor selection preserves the topology chosen by Hybrid A*; it never jumps to another disconnected free interval.
- Initial corridor spacings are `0.10 m` longitudinal and `0.025 m` lateral; maximum lateral offset is `0.30 m`.
- Existing user changes in `ClumsyPilot.csproj`, Map, CoarsePath, and PathSmoothing must be preserved.
- Dynamic-obstacle trajectory prediction and dynamic space-time boundaries are outside this plan.

---

## Locked File Structure

```text
ClumsyPilot/ParkrobTrajplanner/EMPlanner/
├── Contracts/
│   ├── EmBoundaryType.cs
│   ├── EmMotionModel.cs
│   ├── EmPlanningRequest.cs
│   ├── EmPlanningResult.cs
│   ├── EmPlanningStatus.cs
│   ├── EmTerminalType.cs
│   ├── EmTrajectory.cs
│   ├── EmTrajectoryMetadata.cs
│   ├── EmTrajectoryPoint.cs
│   └── VehicleMotionState.cs
├── Configuration/
│   ├── CorridorConfiguration.cs
│   ├── EmPlannerConfiguration.cs
│   ├── FrenetConfiguration.cs
│   ├── LateralConfiguration.cs
│   ├── LateralWeights.cs
│   ├── LongitudinalConfiguration.cs
│   ├── LongitudinalWeights.cs
│   ├── SchedulingConfiguration.cs
│   ├── SolverConfiguration.cs
│   └── ValidationConfiguration.cs
├── Diagnostics/
│   ├── EmPlannerDebugOptions.cs
│   ├── EmPlanningDiagnostics.cs
│   └── IEmPlannerDebugSink.cs
├── Segmentation/
│   ├── DirectionSegmentView.cs
│   ├── ReferenceBoundary.cs
│   ├── ReferenceHorizonSlicer.cs
│   └── ReferencePathSegmenter.cs
├── Frenet/
│   ├── FrenetProjection.cs
│   ├── FrenetProjector.cs
│   ├── FrenetReferencePoint.cs
│   ├── FrenetTransform.cs
│   └── ReferencePathInterpolator.cs
├── Corridor/
│   ├── LateralInterval.cs
│   ├── StaticCorridor.cs
│   └── StaticCorridorBuilder.cs
└── Validation/
    └── EmPlanningRequestValidator.cs

ClumsyPilot/tests/EMPlannerVerificationHost/
├── EMPlannerVerificationHost.csproj
├── Program.cs
├── Verification.cs
├── EmFixtureFactory.cs
├── FoundationChecks.cs
├── SegmentationChecks.cs
├── FrenetChecks.cs
└── CorridorChecks.cs
```

## Shared Public Interfaces

```csharp
public sealed class VehicleMotionState
{
    public VehicleMotionState(Pose2D pose, double signedLongitudinalSpeedMetersPerSecond,
        double? longitudinalAccelerationMetersPerSecondSquared,
        DateTimeOffset capturedAtUtc, long sequenceId);
    public Pose2D Pose { get; }
    public double SignedLongitudinalSpeedMetersPerSecond { get; }
    public double? LongitudinalAccelerationMetersPerSecondSquared { get; }
    public DateTimeOffset CapturedAtUtc { get; }
    public long SequenceId { get; }
}

public sealed class EmPlanningRequest
{
    public EmPlanningRequest(PathSmoothingResult referencePath, PlanningGridMap map,
        VehicleParameters vehicle, VehicleMotionState vehicleState,
        EmPlannerConfiguration configuration, int segmentIndex,
        EmTrajectory previousTrajectory, DateTimeOffset requestedAtUtc,
        DateTimeOffset effectiveAtUtc, string outputTrajectoryId,
        string referencePathId, string previousTrajectoryId,
        EmMotionModel motionModel);
}

public sealed class DirectionSegmentView
{
    public int SegmentIndex { get; }
    public TravelDirection Direction { get; }
    public IReadOnlyList<SmoothedPathPoint> Points { get; }
    public ReferenceBoundary StartBoundary { get; }
    public ReferenceBoundary EndBoundary { get; }
}

public sealed class FrenetProjector
{
    public bool TryProject(Pose2D worldPose, DirectionSegmentView segment,
        double minimumReferenceS, double maximumReferenceS,
        double maximumDistanceMeters, out FrenetProjection projection);
}

public sealed class StaticCorridorBuilder
{
    public bool TryBuild(DirectionSegmentView segment, double startReferenceS,
        double endReferenceS, IReadOnlyList<FrenetProjection> seed,
        PlanningGridMap map, VehicleParameters vehicle,
        CorridorConfiguration configuration, out StaticCorridor corridor,
        out string failureReason);
}
```

### Task 1: Verification Host and Immutable Contracts

**Files:**
- Modify: `ClumsyPilot/ClumsyPilot.csproj`
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj`
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/Verification.cs`
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs`
- Create: all files under `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/`

**Interfaces:**
- Consumes: existing `Pose2D`, `TravelDirection`, `PathSmoothingResult`, `PlanningGridMap`, and `VehicleParameters`.
- Produces: the contracts shown in “Shared Public Interfaces”, `EmPlanningStatus`, `EmTerminalType`, `EmBoundaryType`, and immutable trajectory containers.

- [ ] **Step 1: Add the isolated verification host and a failing contract check**

Add these exact MSBuild rules without rewriting surrounding user changes:

```xml
<Compile Remove="tests\EMPlannerVerificationHost\**\*.cs" />
<Compile Remove="ParkrobTrajplanner\auto_avoidance\**\*.cs"
         Condition="'$(ExcludeLegacyAutoAvoidance)' == 'true'" />
```

Create the host project with:

```xml
<Project Sdk="Microsoft.NET.Sdk">
  <PropertyGroup>
    <OutputType>Exe</OutputType>
    <TargetFramework>net10.0-windows</TargetFramework>
    <ImplicitUsings>disable</ImplicitUsings>
    <Nullable>enable</Nullable>
  </PropertyGroup>
  <ItemGroup>
    <ProjectReference Include="..\..\ClumsyPilot.csproj"
                      AdditionalProperties="ExcludeLegacyAutoAvoidance=true" />
  </ItemGroup>
</Project>
```

`FoundationChecks.Run()` must construct a reverse `VehicleMotionState`, assert that its signed speed remains negative, construct an `EmTrajectoryPoint`, and assert all constructor-supplied values are unchanged. `Program.Main` accepts `foundation`, runs the check, prints `PASS foundation`, and returns `1` after printing an exception when a check fails.

- [ ] **Step 2: Run the host and verify the contracts are absent**

Run:

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- foundation
```

Expected: build failure naming `VehicleMotionState` or `EmTrajectoryPoint`.

- [ ] **Step 3: Implement immutable contracts with constructor validation**

Use `ReadOnlyCollection<T>` copies for every published list. `EmTrajectoryPoint` has exactly these public properties:

```csharp
public double X { get; }
public double Y { get; }
public double Yaw { get; }
public double SignedLongitudinalVelocity { get; }
public double Speed { get; }
public double VelocityX { get; }
public double VelocityY { get; }
public double YawRate { get; }
public double TimeFromStart { get; }
public double VehicleCurvature { get; }
public int SegmentIndex { get; }
public double SegmentLocalS { get; }
public double PathS { get; }
public TravelDirection Direction { get; }
public EmBoundaryType BoundaryType { get; }
```

Its constructor accepts the authoritative `signedLongitudinalVelocity` and derives, rather than accepts, the redundant fields:

```csharp
Speed = Math.Abs(signedLongitudinalVelocity);
VelocityX = signedLongitudinalVelocity * Math.Cos(yaw);
VelocityY = signedLongitudinalVelocity * Math.Sin(yaw);
YawRate = signedLongitudinalVelocity * vehicleCurvature;
```

Define these exact status values:

```text
Success
SuccessWithFallback
InvalidInput
UnsupportedMotionMode
StaleVehicleState
StateDirectionMismatch
InvalidReferencePath
ProjectionFailed
CorridorInfeasible
LateralInfeasible
LongitudinalInfeasible
StoppingDistanceInsufficient
SolverUnavailable
SolverTimedOut
Cancelled
ValidationFailed
Superseded
Failed
```

`EmMotionModel` has one supported value, `NonholonomicForwardReverse`, plus explicit `CrabTranslation` and `InPlaceRotation` values that validation maps to `UnsupportedMotionMode`. Only the two success statuses may construct a result with a non-empty trajectory. Define `EmBoundaryType` as `None`, `RollingSafetyStop`, `GearSwitchApproach`, `GearSwitchDeparture`, and `Goal`; define `EmTerminalType` as `RollingSafetyStop`, `GearSwitch`, and `Goal`.

`EmTrajectoryMetadata` contains `TrajectoryId`, `GeneratedAtUtc`, `EffectiveAtUtc`, `MapSnapshotId`, `ReferencePathId`, `VehicleStateSequenceId`, `PreviousTrajectoryId`, `SegmentIndex`, `Direction`, and `TerminalType`; `EmTrajectory` exposes that metadata plus immutable points. `GeneratedAtUtc` is copied from request `RequestedAtUtc`, and `EffectiveAtUtc` plus `OutputTrajectoryId` are caller-supplied so the pure planner never reads a clock or creates a random ID. `EmTrajectoryPoint` retains internal longitudinal acceleration and jerk values for validation but derives all public redundant velocity fields from signed longitudinal velocity.

- [ ] **Step 4: Run the foundation check**

Run the same command. Expected: `PASS foundation` and exit code `0`.

- [ ] **Step 5: Commit the host and contracts**

```powershell
git add ClumsyPilot/ClumsyPilot.csproj ClumsyPilot/tests/EMPlannerVerificationHost ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts
git commit -m "feat: add EM planner contracts"
```

### Task 2: Configuration, Diagnostics, and Request Validation

**Files:**
- Create: all files under `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/`
- Create: all files under `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Diagnostics/`
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs`

**Interfaces:**
- Consumes: Task 1 contracts.
- Produces: `EmPlannerConfiguration.CreateDefault()`, `EmPlanningRequestValidator.Validate(EmPlanningRequest)`, debug options, and diagnostics.

- [ ] **Step 1: Write failing checks for exact defaults and rejection rules**

Add assertions for these exact defaults:

```text
ReplanPeriodSeconds=0.20
TimeHorizonSeconds=6.0
DistanceHorizonMeters=5.0
OutputTimeStepSeconds=0.05
SolverTimeoutSeconds=0.10
HandoffLookaheadSeconds=0.30
MaximumVehicleStateAgeSeconds=0.20
LongitudinalSampleSpacingMeters=0.10
LateralSampleSpacingMeters=0.025
MaximumLateralOffsetMeters=0.30
AdditionalClearanceReserveMeters=0.02
MaximumCollisionCheckStepMeters=0.025
MaximumProjectionDistanceMeters=0.50
MinimumFrenetDenominator=0.20
BoundaryAnchorToleranceMeters=1e-8
MaximumLateralStepPerIterationMeters=0.05
MaximumLateralSlope=0.50
MaximumLateralSecondDerivativePerMeter=1.00
MaximumLateralThirdDerivativePerSquareMeter=2.00
MaximumForwardSpeedMetersPerSecond=0.20
MaximumReverseSpeedMetersPerSecond=0.20
MaximumAccelerationMetersPerSecondSquared=0.20
MaximumDecelerationMetersPerSecondSquared=0.30
MaximumJerkMetersPerSecondCubed=0.50
MaximumLateralAccelerationMetersPerSecondSquared=0.20
MaximumCurvatureRatePerMeterPerSecond=0.50
StopSpeedToleranceMetersPerSecond=0.01
ZeroSpeedHoldSeconds=0.20
MaximumOuterIterations=5
MaximumOsqpIterations=4000
AbsoluteTolerance=1e-5
RelativeTolerance=1e-5
StrictResidualTolerance=1e-5
WarmStart=true
Polish=true
NativeVerbose=false
```

Assert LS weights `10,1,5,10,5,20,5,10` in reference/heading/second/third/curvature/curvature-variation/previous/rolling-terminal order, and ST weights `10,1,10,5,1` in speed/acceleration/jerk/previous/terminal-acceleration order.

Also assert validation rejects: null request members, a map with `PlanningReady=false`, a smoothing failure status, an out-of-range segment index, a non-finite speed, negative sequence ID, stale state, and vehicle geometry without a valid curvature limit.
Assert `CrabTranslation` and `InPlaceRotation` return `UnsupportedMotionMode` before projection.

- [ ] **Step 2: Run and verify the configuration types are absent**

Run the foundation group. Expected: build failure naming `EmPlannerConfiguration`.

- [ ] **Step 3: Implement configuration snapshots and validation**

Use settable configuration DTOs only at the request boundary, then make `EmPlanningRequestValidator` return a copied internal snapshot. Validate every double with `NumericGuard`; enforce positive sample spacings, `0 < MinimumFrenetDenominator < 1`, non-negative margins, and positive horizons. Treat only `Complete`, `PartialImprovement`, `NotNeeded`, and `Unchanged` smoothing statuses as consumable.

`LateralConfiguration` owns trust-region and derivative hard limits plus `LateralWeights`; `LongitudinalConfiguration` owns direction speed, acceleration, deceleration, jerk, lateral-acceleration, curvature-rate, stop tolerance, hold duration, and `LongitudinalWeights`; `ValidationConfiguration` owns absolute spatial/kinematic consistency tolerances. Configuration validation requires every weight to be finite and non-negative and every normalization scale to be finite and positive.

`EmPlannerDebugOptions` exposes only flags and a sink:

```csharp
EnableSummary
EnableProjectionTrace
EnableCorridorTrace
EnableLateralSolverTrace
EnableLongitudinalSolverTrace
EnableTrajectoryDump
EnableVisualization
Sink
```

Debug-sink exceptions are caught and recorded in diagnostics; they never escape the planner.

- [ ] **Step 4: Run foundation checks and confirm all rejection messages are deterministic**

Expected: `PASS foundation`; run twice and compare stdout exactly.

- [ ] **Step 5: Commit configuration and validation**

```powershell
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration ClumsyPilot/ParkrobTrajplanner/EMPlanner/Diagnostics ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs
git commit -m "feat: validate EM planner requests"
```

### Task 3: Direction Segmentation and Exact Boundary Anchors

**Files:**
- Create: all files under `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/`
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/SegmentationChecks.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/EmFixtureFactory.cs`

**Interfaces:**
- Consumes: `PathSmoothingResult.Path`, `PathSmoothingResult.Segments`, and Task 1 boundary enums.
- Produces: `DirectionSegmentView`, `ReferenceBoundary`, and exact horizon slices.

- [ ] **Step 1: Write failing gear-pair and horizon checks**

Build a fixture whose forward segment ends at `(2,0,0,s=2)`, whose reverse segment starts with a duplicated pose at the same source arc length, and whose direction changes only on the second member of the pair. Assert:

```text
segment 0 end boundary = GearSwitchApproach
segment 1 start boundary = GearSwitchDeparture
segment identity differs even when pose and source arc length match
horizon 1.95 injects an interpolated RollingSafetyStop at exactly 1.95
horizon 2.05 for segment 0 still ends at exactly 2.00 and never includes segment 1
```

- [ ] **Step 2: Run the segmentation group and verify failure**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- segmentation
```

Expected: build failure naming `ReferencePathSegmenter`.

- [ ] **Step 3: Implement segment views and exact slicing**

`ReferenceBoundary` identity is the tuple `(SegmentIndex, SegmentLocalS, BoundaryType)`. Rebase every selected segment to `SegmentLocalS=0` without changing the source point. `ReferenceHorizonSlicer.Slice` must interpolate X, Y, unwrapped vehicle yaw, geometric curvature, vehicle curvature, curvature derivative, and clearance at the exact terminal S, then label the anchor `RollingSafetyStop`, `GearSwitch`, or `Goal`.

Never implement horizon selection by filtering `point.ArcLength <= end`; always append or replace with the exact anchor after bracketing interpolation.

- [ ] **Step 4: Run segmentation checks**

Expected: `PASS segmentation`, with assertions covering exact equality within `1e-8 m`.

- [ ] **Step 5: Commit segmentation**

```powershell
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation ClumsyPilot/tests/EMPlannerVerificationHost
git commit -m "feat: preserve EM planner segment boundaries"
```

### Task 4: Reverse-Safe Frenet Projection and Reconstruction

**Files:**
- Create: all files under `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/`
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/FrenetChecks.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`

**Interfaces:**
- Consumes: `DirectionSegmentView` from Task 3.
- Produces: interpolation, bounded projection, world/Frenet conversion, and optimized-yaw reconstruction.

- [ ] **Step 1: Write failing forward, reverse, wraparound, and loop checks**

Cover these equations directly:

```text
travelYaw = forward ? vehicleYaw : Normalize(vehicleYaw + PI)
deltaS = dx*cos(travelYaw) + dy*sin(travelYaw)
l = -dx*sin(travelYaw) + dy*cos(travelYaw)
x = referenceX - l*sin(travelYaw)
y = referenceY + l*cos(travelYaw)
optimizedTravelYaw = referenceTravelYaw + atan2(dl, 1-referenceK*l)
optimizedVehicleYaw = reverse ? Normalize(optimizedTravelYaw + PI) : optimizedTravelYaw
```

Use a reverse reference whose vehicle yaw is near `-π`, points on both sides of the wrap, and a U-shaped segment with two spatially close branches. Assert the projector remains inside the supplied S interval and chooses the locally nearest branch, then reconstructs the original world point within `1e-8 m`.

- [ ] **Step 2: Run the Frenet group and verify failure**

Expected: build failure naming `FrenetProjector`.

- [ ] **Step 3: Implement deterministic bounded projection**

Use segment-line projection for the coarse candidate, clamp the interpolation fraction to `[0,1]`, then compare squared world distance. Resolve equal-distance ties by smaller absolute delta from the seed/reference S and then smaller S. Reject projections farther than `MaximumProjectionDistanceMeters`; never search another direction segment.

`FrenetTransform` rejects reconstruction when `1-referenceK*l < MinimumFrenetDenominator`. Angles used for interpolation are unwrapped; only public yaw is normalized with `AngleMath.NormalizeRadians`.

- [ ] **Step 4: Run Frenet checks**

Expected: `PASS frenet`, including reverse `l>0` being left of motion and therefore body-right.

- [ ] **Step 5: Commit Frenet support**

```powershell
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet ClumsyPilot/tests/EMPlannerVerificationHost
git commit -m "feat: add reverse-safe Frenet transforms"
```

### Task 5: Topology-Preserving Static Corridor

**Files:**
- Create: all files under `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor/`
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/CorridorChecks.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`

**Interfaces:**
- Consumes: Task 4 interpolation/transforms, `PlanningGridMap`, `VehicleParameters`, and `FootprintCollisionChecker`.
- Produces: immutable `StaticCorridor` stations containing `ReferenceS`, `MinimumL`, `MaximumL`, and seed L.

- [ ] **Step 1: Write failing corridor connectivity checks**

Create maps through `PlanningMapFactory`: an empty map, a static rectangle narrowing the left side of a straight path, and an obstacle splitting lateral samples into disconnected left/right intervals. Assert:

```text
empty map corridor = [-0.30, +0.30] at interior stations
every accepted lateral sample passes exact rotated-footprint collision checking
prior-trajectory seed is used before l=0
the chosen interval contains the seed at every station
a disappearing seed-connected interval returns false instead of switching sides
first and last stations match exact requested ReferenceS anchors
```

- [ ] **Step 2: Run the corridor group and verify failure**

Expected: build failure naming `StaticCorridorBuilder`.

- [ ] **Step 3: Implement broad-phase plus exact-footprint sampling**

At each `0.10 m` S station, sample L in `0.025 m` increments from `-0.30` to `+0.30`, always injecting the exact seed L and exact offset limits. Use the distance field only to accept obviously clear samples; all remaining samples go through `FootprintCollisionChecker.IsPoseCollisionFree` with `AdditionalClearanceReserveMeters=0.02`. Group adjacent free samples, choose only the group containing the seed, and propagate overlap with the previously chosen interval.

When propagation loses overlap, return failure with the first failed S. Do not select another free group, invert L for reverse, or infer a new obstacle side.

- [ ] **Step 4: Run all foundation-plan checks**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- all-foundation
```

Expected output:

```text
PASS foundation
PASS segmentation
PASS frenet
PASS corridor
```

- [ ] **Step 5: Commit corridor support**

```powershell
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor ClumsyPilot/tests/EMPlannerVerificationHost
git commit -m "feat: build static EM lateral corridors"
```

### Task 6: Foundation Documentation and Gate

**Files:**
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`

**Interfaces:**
- Consumes: all preceding tasks.
- Produces: a documented, repeatable foundation verification command for later plans.

- [ ] **Step 1: Add README contract examples**

Document the pipeline, coordinate formulas, reverse sign example, exact gear-pair behavior, configuration units, and the command used to run `all-foundation`. State explicitly that the module does not yet solve LS or ST at this gate.

- [ ] **Step 2: Run format and placeholder checks**

```powershell
git diff --check
rg -n "NotImplementedException|throw new Exception\(\)" ClumsyPilot/ParkrobTrajplanner/EMPlanner ClumsyPilot/tests/EMPlannerVerificationHost
```

Expected: `git diff --check` returns no diagnostics and the incomplete-implementation scan returns no matches.

- [ ] **Step 3: Run the complete foundation gate twice**

Run `all-foundation` twice. Expected: identical four PASS lines and exit code `0` both times.

- [ ] **Step 4: Confirm the normal project baseline separately**

```powershell
dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
```

Expected at this repository baseline: only the already-documented legacy `auto_avoidance/MultiWheelAutoAvoidance.cs` missing-reference errors may remain. Any new error under `EMPlanner` fails this gate.

- [ ] **Step 5: Commit documentation**

```powershell
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs
git commit -m "docs: describe EM planner foundation"
```

## Completion Gate

- `all-foundation` passes twice with deterministic output.
- Forward and reverse projection/reconstruction agree within `1e-8 m`.
- No segment, projection, horizon, or corridor crosses a gear boundary.
- Corridor samples pass exact body collision checks and never change disconnected topology.
- No solver, scheduler, hardware access, dynamic prediction, or trajectory publication logic has leaked into this phase.