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

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

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

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:

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

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

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:

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

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:

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

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:

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

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

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

Expected output:

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

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.