# EM Planner Lateral LS 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:** Optimize a collision-free, curvature-feasible lateral path inside the selected static corridor using sequential convex programming over OSQP QPs.

**Architecture:** Discretize `l, dl, ddl, dddl` over exact reference-S stations, build normalized quadratic costs and linear integration/corridor constraints, and linearize nonlinear vehicle curvature inside an outer trust-region loop. Reconstruct each accepted candidate in world coordinates, recompute true path arc length, and independently validate it before exposing it to ST.

**Tech Stack:** C# 10, .NET Standard 2.0, foundation Frenet/corridor types, solver-neutral `IQpSolver`, OSQP backend for integration checks.

## Global Constraints

- This plan depends on completion of the foundation and OSQP-backend plans.
- LS runs on exactly one current direction segment and uses `ReferenceS` as its independent variable.
- `l>0` is left of travel for both forward and reverse; do not reinterpret it as body-left in reverse.
- Corridor bounds, maximum lateral offset, trust region, start state, terminal event, Frenet denominator, and vehicle curvature are hard constraints.
- Initial derivative bounds: `|Δl|<=0.05 m` per SQP iteration, `|dl|<=0.50`, `|ddl|<=1.00 1/m`, `|dddl|<=2.00 1/m²`.
- Enforce `1-referenceK*l >= 0.20` at every knot.
- SQP outer-iteration limit is `5`; OSQP iteration limit is `4000`; absolute/relative tolerances are `1e-5`.
- Cost weights: reference `10`, heading `1`, second derivative `5`, third derivative `10`, curvature `5`, curvature variation `20`, previous trajectory `5`, rolling terminal `10`.
- Every cost term is divided by the square of its physical scale before its weight is applied.
- LS scales are maximum lateral offset for L, maximum slope for DL, maximum second derivative for DDL, maximum third derivative for DDDL, vehicle maximum curvature for curvature, and `max(1, reference max |dk/ds|)` for curvature variation.
- Gear-switch and goal terminals require `l=0` and `dl=0`; a rolling safety terminal uses a soft terminal penalty.
- Only the last independently validated feasible candidate may survive a later timeout or failed outer iteration.
- The output world path is re-parameterized by actual `PathS`; later ST code must not use `ReferenceS` as traveled distance.

---

## Locked File Structure

```text
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/
├── LateralCandidate.cs
├── LateralConstraintBuilder.cs
├── LateralGeometryEvaluator.cs
├── LateralObjectiveBuilder.cs
├── LateralPath.cs
├── LateralPathPoint.cs
├── LateralPlanner.cs
├── LateralPlanningInput.cs
├── LateralPlanningResult.cs
├── LateralSolutionValidator.cs
├── LateralVariableLayout.cs
└── SequentialConvexOptimizer.cs

ClumsyPilot/tests/EMPlannerVerificationHost/
├── FakeQpSolver.cs
├── LateralModelChecks.cs
└── LateralIntegrationChecks.cs
```

## Shared Interfaces

```csharp
public sealed class LateralPlanningInput
{
    public LateralPlanningInput(DirectionSegmentView referenceSegment,
        StaticCorridor corridor, FrenetProjection startProjection,
        EmTerminalType terminalType, VehicleParameters vehicle,
        EmPlannerConfiguration configuration,
        IReadOnlyList<FrenetProjection> previousTrajectorySeed);
}

public sealed class LateralPlanner
{
    public LateralPlanner(IQpSolver qpSolver);
    public LateralPlanningResult Plan(LateralPlanningInput input,
        CancellationToken cancellationToken);
}

public sealed class LateralPathPoint
{
    public double ReferenceS { get; }
    public double PathS { get; }
    public double L { get; }
    public double DL { get; }
    public double DDL { get; }
    public double DDDL { get; }
    public double X { get; }
    public double Y { get; }
    public double VehicleYaw { get; }
    public double GeometricCurvature { get; }
    public double VehicleCurvature { get; }
    public double VehicleCurvatureDerivative { get; }
}
```

### Task 1: Variable Layout and Exact Discrete Lateral Dynamics

**Files:**
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralVariableLayout.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanningInput.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralCandidate.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPathPoint.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPath.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanningResult.cs`
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`

**Interfaces:**
- Consumes: foundation corridor, reference, configuration, and terminal types.
- Produces: deterministic variable indices and immutable lateral inputs/results.

- [ ] **Step 1: Write failing layout and dynamics checks**

For `N=4`, assert disjoint contiguous ranges for `l[0..3]`, `dl[0..3]`, `ddl[0..3]`, and `dddl[0..2]`, with total variable count `4*N-1`. For unequal S gaps, verify the integration equations:

```text
ddl[i+1] = ddl[i] + ds*dddl[i]
dl[i+1]  = dl[i] + ds*ddl[i] + 0.5*ds^2*dddl[i]
l[i+1]   = l[i] + ds*dl[i] + 0.5*ds^2*ddl[i] + (ds^3/6)*dddl[i]
```

Reject fewer than two stations, non-increasing S, corridor/input station mismatch, and a start projection outside the first hard interval.

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

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

Expected: build failure naming `LateralVariableLayout`.

- [ ] **Step 3: Implement layouts and immutable model types**

Expose index methods `L(i)`, `DL(i)`, `DDL(i)`, and `DDDL(i)` that range-check every input. Copy all station and seed lists. A failed result has no candidate; success and fallback results require a non-empty independently validated `LateralPath`.

- [ ] **Step 4: Run the model checks**

Expected: `PASS lateral-model`.

- [ ] **Step 5: Commit lateral model types**

```powershell
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral ClumsyPilot/tests/EMPlannerVerificationHost
git commit -m "feat: add lateral optimization model"
```

### Task 2: Normalized Objective and Linear Hard Constraints

**Files:**
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralObjectiveBuilder.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralConstraintBuilder.cs`
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/FakeQpSolver.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs`

**Interfaces:**
- Consumes: Task 1 layout, `SparseTripletBuilder`, corridor bounds, a linearization candidate, and approved LS weights.
- Produces: a validated `QuadraticProgram` for one SQP iteration.

- [ ] **Step 1: Write failing coefficient-level QP checks**

For a three-station straight reference with unit scales, inspect P, q, A, lower, and upper arrays and assert:

```text
reference cost adds 2*w_l to P(l_i,l_i)
jerk cost adds 2*w_dddl to P(dddl_i,dddl_i)
previous-seed cost adds 2*w_previous and -2*w_previous*l_previous
every integration equality appears once with equal lower/upper bounds
corridor, derivative, trust-region, and Frenet-denominator rows use hard finite bounds
gear/goal terminal rows force l_N=0 and dl_N=0
rolling terminal adds objective terms but no zero terminal equalities
```

The test must also show every weight is applied after division by its named scale squared.

- [ ] **Step 2: Run and verify builders are absent**

Expected: build failure naming `LateralObjectiveBuilder`.

- [ ] **Step 3: Implement objective and hard-row assembly**

Build the OSQP objective convention `0.5*x'Px + q'x`, so a squared residual `w*((x-target)/scale)^2` contributes `2w/scale²` to P and `-2w*target/scale²` to q. Assemble integration rows exactly from Task 1. Intersect corridor bounds with maximum offset, trust region, and linearized denominator bounds before adding each L row; return infeasible before calling the solver when an intersection is empty.

Use `FakeQpSolver` only in the verification host. It records the last problem/settings/warm start and returns a caller-supplied `QpSolveResult`.

- [ ] **Step 4: Run coefficient-level checks**

Expected: `PASS lateral-model`; no coefficient comparison tolerance larger than `1e-10`.

- [ ] **Step 5: Commit QP assembly**

```powershell
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral ClumsyPilot/tests/EMPlannerVerificationHost
git commit -m "feat: assemble lateral LS quadratic programs"
```

### Task 3: Nonlinear Geometry Evaluation and Independent Validation

**Files:**
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralGeometryEvaluator.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralSolutionValidator.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs`

**Interfaces:**
- Consumes: solved `l/dl/ddl/dddl`, Frenet interpolation, direction, and vehicle curvature limit.
- Produces: world-space `LateralPath` with recomputed `PathS`, curvature, and validation residuals.

- [ ] **Step 1: Write failing reconstruction and curvature checks**

Cover straight and constant-curvature references in both directions. Assert:

```text
world X/Y use x_ref-l*sin(travelYaw), y_ref+l*cos(travelYaw)
vehicle yaw adds PI only for reverse
PathS[0]=0 and increments by actual reconstructed chord/geometry length
PathS is strictly increasing even when ReferenceS gaps vary
VehicleCurvature = directionSign*GeometricCurvature
yawRate identity remains valid for a signed test speed
denominator below 0.20 is rejected
curvature beyond vehicle limit is rejected
non-finite values are rejected
```

- [ ] **Step 2: Run and verify geometry evaluator is absent**

Expected: build failure naming `LateralGeometryEvaluator`.

- [ ] **Step 3: Implement evaluation and strict validation**

Evaluate geometry from the full Frenet derivative formulas used by the design, not a small-angle replacement. Compute unwrapped travel yaw first, derive geometric curvature with respect to actual path direction, convert to vehicle curvature using direction sign, and compute curvature derivative over actual `PathS`. Use centred differences internally and one-sided endpoints.

The validator independently recomputes corridor membership, start/terminal residuals, derivative limits, denominator, curvature limit, finite values, and strictly increasing S. It does not trust solver residuals or reuse the QP constraint matrix as its only proof.

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

Expected: `PASS lateral-model`, including forward/reverse mirrored cases.

- [ ] **Step 5: Commit nonlinear evaluation**

```powershell
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs
git commit -m "feat: validate lateral path geometry"
```

### Task 4: Sequential Convex Outer Loop and Feasible-Candidate Fallback

**Files:**
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/SequentialConvexOptimizer.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanner.cs`
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/LateralIntegrationChecks.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`

**Interfaces:**
- Consumes: Tasks 1–3, `IQpSolver`, warm starts, cancellation, and timeout settings.
- Produces: `LateralPlanningResult` with the last strict feasible path or an explicit failure.

- [ ] **Step 1: Write failing SQP state-machine checks with `FakeQpSolver`**

Script solver outcomes and assert:

```text
first solved candidate is validated before becoming fallback
second timeout returns first candidate as SuccessWithFallback
an invalid solved vector never replaces the fallback
SolvedInaccurate requires QP residual <=1e-5 and full lateral validation
trust region is centred on the previous iterate and never exceeds 0.05 m
outer loop stops after at most 5 calls
cancellation before a call returns Cancelled
no feasible candidate plus timeout returns SolverTimedOut with no path
```

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

Expected: build failure naming `SequentialConvexOptimizer`.

- [ ] **Step 3: Implement the outer loop**

Initialize from the previous trajectory seed when it covers all stations; otherwise use the corridor-clamped zero-offset seed. Per iteration: linearize geometry, assemble the QP, solve with the remaining time budget, evaluate world geometry, validate independently, store a deep copy if feasible, and test convergence using max absolute L change plus objective improvement. Warm-start the next QP with the complete previous primal vector.

Return the most specific status. A timeout/cancellation after a validated candidate maps to fallback success; infeasible corridor/QP with no candidate maps to lateral infeasible.

- [ ] **Step 4: Run scripted SQP checks**

Expected: `PASS lateral-integration`.

- [ ] **Step 5: Commit SQP orchestration**

```powershell
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral ClumsyPilot/tests/EMPlannerVerificationHost
git commit -m "feat: optimize lateral paths with SQP"
```

### Task 5: Real-OSQP Lateral Scenarios and Gate

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

**Interfaces:**
- Consumes: `OsqpNativeSolver`, foundation fixtures, and complete LS pipeline.
- Produces: a verified lateral path contract ready for ST.

- [ ] **Step 1: Add fixed real-solver scenarios**

Run: straight empty map forward, straight empty map reverse, gentle curve, static obstacle narrowing the existing corridor, gear-switch terminal, and rolling terminal. Assert every result is solved or documented fallback, stays in corridor, respects curvature, and ends at the exact ReferenceS anchor.

- [ ] **Step 2: Add determinism and topology assertions**

Run each scenario twice with identical inputs. Compare status, point count, and every numeric output within `1e-10`; assert the obstacle case remains in the seed-connected interval and does not cross to the disconnected side.

- [ ] **Step 3: Run the complete lateral gate**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- lateral-all
git diff --check
```

Expected output:

```text
PASS lateral-model
PASS lateral-integration
PASS lateral-real-osqp
```

- [ ] **Step 4: Document LS variables, hard constraints, costs, and fallback**

Add the exact equations, normalization scales, terminal differences, `ReferenceS`/`PathS` boundary, and last-feasible publication rule to the README.

- [ ] **Step 5: Commit lateral integration evidence**

```powershell
git add ClumsyPilot/tests/EMPlannerVerificationHost/LateralIntegrationChecks.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md
git commit -m "test: verify lateral LS scenarios"
```

## Completion Gate

- Coefficient-level tests prove the intended normalized QP, not merely a plausible output path.
- Forward and reverse reconstructed geometry obey the same world-coordinate convention.
- Exact gear/goal terminal L conditions and rolling soft terminal behavior are distinct.
- No candidate outside hard corridor, denominator, derivative, curvature, or boundary constraints is published.
- The published lateral path has actual strictly increasing `PathS` ready for longitudinal optimization.