335 lines
16 KiB
Markdown
335 lines
16 KiB
Markdown
# 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.
|