395 lines
19 KiB
Markdown
395 lines
19 KiB
Markdown
# EM Planner Longitudinal ST and Publication 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:** Add curvature-aware longitudinal optimization, assemble the complete immutable trajectory, independently validate it, and expose a pure one-shot `EmPlanningService.Plan` pipeline.
|
||
|
||
**Architecture:** Build a speed envelope over the LS result’s actual `PathS`, then optimize time-knot `s,u,a,j` variables with exact constant-jerk integration and hard stop boundaries. Convert the validated longitudinal/lateral pair into redundant but consistent public trajectory fields, run an independent world-space and kinematic publication validator, and only then return success.
|
||
|
||
**Tech Stack:** C# 10, .NET Standard 2.0, LS path types, solver-neutral QP layer, OSQP, existing map and swept-footprint collision checker.
|
||
|
||
## Global Constraints
|
||
|
||
- This plan depends on completion of foundation, OSQP backend, and lateral LS plans.
|
||
- ST uses actual optimized `PathS`; it never receives or publishes `ReferenceS` as traveled distance.
|
||
- Internal speed `u>=0`; public signed longitudinal velocity is `directionSign*u`.
|
||
- Initial limits: forward/reverse speed `0.20 m/s`, acceleration `0.20 m/s²`, deceleration `0.30 m/s²`, jerk `0.50 m/s³`, lateral acceleration `0.20 m/s²`, curvature-rate limit `0.50 1/(m*s)`.
|
||
- Default output time step is `0.05 s`, time horizon `6.0 s`, distance horizon `5.0 m`, stop tolerance `0.01 m/s`, and zero-speed hold `0.20 s`.
|
||
- Every terminal is hard bounded by `s_N=terminalS` and `u_N=0`; terminal acceleration is soft.
|
||
- Objective weights: speed reference `10`, acceleration `1`, jerk `10`, previous trajectory `5`, terminal acceleration `1`.
|
||
- ST scales are direction maximum speed for U, `max(MaximumAcceleration,MaximumDeceleration)` for A, maximum jerk for J, and terminal S for S-tracking; zero terminal S uses scale `1` only for the degenerate stopped result.
|
||
- There is no progress cost when terminal S is fixed.
|
||
- Speed limit is the minimum of direction maximum, lateral-acceleration limit, curvature-rate limit, and stopping envelope.
|
||
- Output contains `X,Y,Yaw,SignedLongitudinalVelocity,Speed,VelocityX,VelocityY,YawRate,TimeFromStart,VehicleCurvature`.
|
||
- `SignedLongitudinalVelocity` is authoritative; all redundant speed fields are constructor-derived and independently rechecked.
|
||
- Full rotated-body pose and swept-motion collision checks run after optimization.
|
||
- A non-success result never contains a non-empty trajectory.
|
||
- Dynamic obstacle prediction, time-varying obstacle boundaries, and dynamic behavior decisions remain outside this plan.
|
||
|
||
---
|
||
|
||
## Locked File Structure
|
||
|
||
```text
|
||
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/
|
||
├── LongitudinalCandidate.cs
|
||
├── LongitudinalConstraintBuilder.cs
|
||
├── LongitudinalObjectiveBuilder.cs
|
||
├── LongitudinalPlanner.cs
|
||
├── LongitudinalPlanningInput.cs
|
||
├── LongitudinalPlanningResult.cs
|
||
├── LongitudinalSolutionValidator.cs
|
||
├── LongitudinalVariableLayout.cs
|
||
├── PathSpeedLimit.cs
|
||
├── PathSpeedLimitBuilder.cs
|
||
└── SequentialLongitudinalOptimizer.cs
|
||
|
||
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/
|
||
└── PlanningHorizonSelector.cs
|
||
|
||
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/
|
||
├── EmTrajectoryAssembler.cs
|
||
├── LateralPathInterpolator.cs
|
||
└── TrajectorySampleSchedule.cs
|
||
|
||
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/
|
||
└── EmTrajectoryValidator.cs
|
||
|
||
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/
|
||
├── IEmPlanningService.cs
|
||
└── EmPlanningService.cs
|
||
|
||
ClumsyPilot/tests/EMPlannerVerificationHost/
|
||
├── LongitudinalModelChecks.cs
|
||
├── LongitudinalIntegrationChecks.cs
|
||
├── TrajectoryChecks.cs
|
||
└── EmPlanningServiceChecks.cs
|
||
```
|
||
|
||
## Shared Interfaces
|
||
|
||
```csharp
|
||
public sealed class LongitudinalPlanner
|
||
{
|
||
public LongitudinalPlanner(IQpSolver qpSolver);
|
||
public LongitudinalPlanningResult Plan(LongitudinalPlanningInput input,
|
||
CancellationToken cancellationToken);
|
||
}
|
||
|
||
public sealed class EmTrajectoryAssembler
|
||
{
|
||
public EmTrajectory Assemble(LateralPath path,
|
||
LongitudinalPlanningResult longitudinal,
|
||
EmTrajectoryMetadata metadata);
|
||
}
|
||
|
||
public interface IEmPlanningService
|
||
{
|
||
EmPlanningResult Plan(EmPlanningRequest request,
|
||
CancellationToken cancellationToken);
|
||
}
|
||
|
||
public sealed class EmPlanningService : IEmPlanningService
|
||
{
|
||
public EmPlanningService(IQpSolver qpSolver,
|
||
IEmPlannerDebugSink defaultDebugSink = null);
|
||
public EmPlanningResult Plan(EmPlanningRequest request,
|
||
CancellationToken cancellationToken);
|
||
}
|
||
```
|
||
|
||
### Task 1: Speed Envelope over Actual PathS
|
||
|
||
**Files:**
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimit.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningInput.cs`
|
||
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs`
|
||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
|
||
|
||
**Interfaces:**
|
||
- Consumes: validated `LateralPath`, direction, current speed/acceleration, terminal S/type, and longitudinal configuration.
|
||
- Produces: a finite piecewise-linear maximum-speed envelope indexed only by `PathS`.
|
||
|
||
- [ ] **Step 1: Write failing speed-limit checks**
|
||
|
||
For fixed path points, assert:
|
||
|
||
```text
|
||
direction limit = 0.20 m/s
|
||
if |k|=2 1/m, lateral limit = sqrt(0.20/2)
|
||
if |dk/ds|=4 1/m², curvature-rate limit = 0.50/4
|
||
zero curvature and zero derivative do not divide by zero
|
||
combined limit is the minimum finite non-negative value
|
||
stopping limit at path position s = sqrt(2*MaximumDeceleration*(terminalS-s))
|
||
speed envelope is interpolated by PathS, not ReferenceS
|
||
terminal speed is exactly zero
|
||
```
|
||
|
||
Add a start state whose jerk/deceleration-limited stopping distance exceeds the available terminal distance; expect `StoppingDistanceInsufficient` before a QP call.
|
||
|
||
- [ ] **Step 2: Run and verify the envelope types are absent**
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
|
||
```
|
||
|
||
Expected: build failure naming `PathSpeedLimitBuilder`.
|
||
|
||
- [ ] **Step 3: Implement finite limits and stopping precheck**
|
||
|
||
Use a curvature epsilon of `1e-10`. Clamp every computed limit to the direction maximum and to `sqrt(2*MaximumDeceleration*(terminalS-s))`; the terminal value is exactly zero. Compute a conservative jerk/deceleration stop by first ramping acceleration down at maximum negative jerk until maximum deceleration or zero speed, integrating distance exactly for constant jerk, then adding constant-deceleration distance. Reject when this distance plus `1e-8 m` exceeds terminal distance.
|
||
|
||
`PlanningHorizonSelector` computes `TerminalReferenceS` before LS as the minimum of remaining current-segment reference length, `DistanceHorizonMeters`, and the reference distance reachable within `TimeHorizonSeconds` while reserving the same jerk-limited stop tail and zero-speed hold. It returns `GearSwitch` or `Goal` when the exact segment boundary wins and `RollingSafetyStop` otherwise; it never returns a reference S past the current segment end. After LS reconstruction, ST discards that numeric reference distance and uses the last lateral point's actual `PathS` as `terminalS`; stopping feasibility is checked again against that actual distance.
|
||
|
||
- [ ] **Step 4: Run speed-envelope checks**
|
||
|
||
Expected: `PASS longitudinal-model`.
|
||
|
||
- [ ] **Step 5: Commit speed envelope**
|
||
|
||
```powershell
|
||
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal ClumsyPilot/tests/EMPlannerVerificationHost
|
||
git commit -m "feat: build EM path speed limits"
|
||
```
|
||
|
||
### Task 2: Time-Knot Layout, Dynamics, Objective, and Hard Constraints
|
||
|
||
**Files:**
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalVariableLayout.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalCandidate.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalObjectiveBuilder.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningResult.cs`
|
||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs`
|
||
|
||
**Interfaces:**
|
||
- Consumes: Task 1 envelope, a current S iterate, previous-trajectory samples, and QP sparse builders.
|
||
- Produces: one convex time-domain QP and immutable candidate/result types.
|
||
|
||
- [ ] **Step 1: Write failing variable and coefficient checks**
|
||
|
||
For `N=5`, assert ranges for `s[0..4]`, `u[0..4]`, `a[0..4]`, and `j[0..3]`, total `4*N-1`, and exact constant-jerk equations:
|
||
|
||
```text
|
||
a[i+1] = a[i] + dt*j[i]
|
||
u[i+1] = u[i] + dt*a[i] + 0.5*dt^2*j[i]
|
||
s[i+1] = s[i] + dt*u[i] + 0.5*dt^2*a[i] + (dt^3/6)*j[i]
|
||
```
|
||
|
||
Inspect the QP and prove: `u>=0`, monotonic S, acceleration/deceleration bounds, jerk bounds, speed-envelope bounds at the current S iterate, exact start state, `s_N=terminalS`, `u_N=0`, and no objective coefficient rewards fixed terminal progress.
|
||
|
||
- [ ] **Step 2: Run and verify ST builders are absent**
|
||
|
||
Expected: build failure naming `LongitudinalVariableLayout`.
|
||
|
||
- [ ] **Step 3: Implement normalized ST QP assembly**
|
||
|
||
Use the same `0.5*x'Px+q'x` convention. Apply squared-residual coefficients for reference speed, acceleration, jerk, previous S/U, and terminal acceleration after division by their physical scales squared. Determine knot count as `ceil(TimeHorizonSeconds/OutputTimeStepSeconds)+1`; include exact zero and exact horizon times.
|
||
|
||
At each outer iteration, read the piecewise speed envelope at the current candidate S. Bound S to `[0,terminalS]` and enforce `s[i+1]>=s[i]`. Add exact start S/U/A and terminal S/U equalities.
|
||
|
||
- [ ] **Step 4: Run coefficient-level ST checks**
|
||
|
||
Expected: `PASS longitudinal-model`, coefficient tolerance `1e-10`.
|
||
|
||
- [ ] **Step 5: Commit ST model assembly**
|
||
|
||
```powershell
|
||
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs
|
||
git commit -m "feat: assemble longitudinal ST quadratic programs"
|
||
```
|
||
|
||
### Task 3: Longitudinal Outer Loop and Strict Validation
|
||
|
||
**Files:**
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanner.cs`
|
||
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs`
|
||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
|
||
|
||
**Interfaces:**
|
||
- Consumes: Tasks 1–2 and `IQpSolver`.
|
||
- Produces: a validated `s,u,a,j` time profile with last-feasible fallback semantics.
|
||
|
||
- [ ] **Step 1: Write failing fake-solver state-machine checks**
|
||
|
||
Script solved, inaccurate, timeout, infeasible, and invalid-vector outcomes. Assert the same last-feasible rules as LS, maximum five envelope iterations, warm start, cancellation behavior, and that no candidate violating monotonic S, speed limit, acceleration, jerk, terminal S, or terminal zero speed can become fallback.
|
||
|
||
- [ ] **Step 2: Run and verify longitudinal optimizer is absent**
|
||
|
||
Expected: build failure naming `SequentialLongitudinalOptimizer`.
|
||
|
||
- [ ] **Step 3: Implement iteration and independent checks**
|
||
|
||
Seed S by simulating the requested speed while reserving a conservative stop tail; seed U/A/J consistently. Each iteration rebuilds the speed bounds at candidate S, solves with remaining budget, validates directly in physical units, and stores a deep copy only when strict. Converge on maximum S/U change and objective improvement; do not accept a vector merely because OSQP reports solved.
|
||
|
||
- [ ] **Step 4: Run scripted and real-OSQP longitudinal checks**
|
||
|
||
Cover forward, reverse, curvature-limited, jerk-limited stop, short segment, and zero-start-speed cases. Expected: `PASS longitudinal-integration`.
|
||
|
||
- [ ] **Step 5: Commit longitudinal optimizer**
|
||
|
||
```powershell
|
||
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal ClumsyPilot/tests/EMPlannerVerificationHost
|
||
git commit -m "feat: optimize longitudinal ST profiles"
|
||
```
|
||
|
||
### Task 4: Complete Trajectory Assembly and Redundant-Field Consistency
|
||
|
||
**Files:**
|
||
- Create: all files under `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/`
|
||
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs`
|
||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
|
||
|
||
**Interfaces:**
|
||
- Consumes: validated `LateralPath` and longitudinal profile.
|
||
- Produces: immutable public `EmTrajectory` with exact terminal event and zero-speed hold.
|
||
|
||
- [ ] **Step 1: Write failing forward/reverse trajectory-field checks**
|
||
|
||
At every time knot assert:
|
||
|
||
```text
|
||
signedV = directionSign*u
|
||
speed = abs(signedV)
|
||
vx = signedV*cos(yaw)
|
||
vy = signedV*sin(yaw)
|
||
yawRate = signedV*VehicleCurvature
|
||
TimeFromStart strictly increases
|
||
PathS never decreases
|
||
```
|
||
|
||
For reverse, prove the world velocity points along travel rather than vehicle yaw. Assert the exact gear/goal/rolling terminal point is present, has zero signed speed and zero yaw rate, and is followed by `0.20 s` of identical-pose zero-speed hold samples at `0.05 s` spacing.
|
||
|
||
- [ ] **Step 2: Run and verify assembler is absent**
|
||
|
||
Expected: build failure naming `EmTrajectoryAssembler`.
|
||
|
||
- [ ] **Step 3: Implement interpolation and immutable assembly**
|
||
|
||
Interpolate lateral geometry by `PathS` with unwrapped yaw, then normalize public yaw. Construct `EmTrajectoryPoint` only from authoritative signed speed and vehicle curvature so redundant fields cannot diverge. Inject the exact terminal time/position before the hold tail if it is not already a regular knot; never drop it through list filtering.
|
||
|
||
- [ ] **Step 4: Run trajectory checks**
|
||
|
||
Expected: `PASS trajectory`, including reverse and exact-boundary fixtures.
|
||
|
||
- [ ] **Step 5: Commit trajectory assembly**
|
||
|
||
```powershell
|
||
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory ClumsyPilot/tests/EMPlannerVerificationHost
|
||
git commit -m "feat: assemble complete EM trajectories"
|
||
```
|
||
|
||
### Task 5: Independent World-Space Publication Validator
|
||
|
||
**Files:**
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs`
|
||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs`
|
||
|
||
**Interfaces:**
|
||
- Consumes: assembled trajectory, map, vehicle, configuration, current segment, and terminal boundary.
|
||
- Produces: a strict validation report used as the only publication gate.
|
||
|
||
- [ ] **Step 1: Write failing mutation-style validation checks**
|
||
|
||
Start from a valid trajectory, create altered copies, and assert rejection for: NaN, non-increasing time, decreasing PathS, wrong direction sign, inconsistent speed components, inconsistent yaw rate, excessive speed/acceleration/jerk/curvature/curvature-rate, skipped terminal anchor, nonzero terminal speed, pose collision, swept collision, and a point beyond segment end.
|
||
|
||
- [ ] **Step 2: Run and verify validator is absent**
|
||
|
||
Expected: build failure naming `EmTrajectoryValidator`.
|
||
|
||
- [ ] **Step 3: Implement independent publication validation**
|
||
|
||
Recompute finite differences in time for acceleration and jerk, recompute yaw-rate identity, check every point through `FootprintCollisionChecker.IsPoseCollisionFree`, and check each adjacent pair through `IsSweptMotionCollisionFree` with maximum step `0.025 m`. Use absolute/relative numeric tolerances from configuration; do not reuse solver status as evidence.
|
||
|
||
- [ ] **Step 4: Run all mutation checks**
|
||
|
||
Expected: `PASS trajectory`; every altered trajectory has a deterministic first failure code and point index.
|
||
|
||
- [ ] **Step 5: Commit publication validator**
|
||
|
||
```powershell
|
||
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs
|
||
git commit -m "feat: validate published EM trajectories"
|
||
```
|
||
|
||
### Task 6: Pure One-Shot EmPlanningService
|
||
|
||
**Files:**
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/IEmPlanningService.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs`
|
||
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs`
|
||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md`
|
||
|
||
**Interfaces:**
|
||
- Consumes: every completed core component.
|
||
- Produces: the approved pure `Plan(request, cancellationToken)` API.
|
||
|
||
- [ ] **Step 1: Write failing end-to-end status and dataflow checks**
|
||
|
||
Cover success forward, success reverse, invalid smoothing status, stale state, direction mismatch, projection failure, corridor infeasible, lateral infeasible, stopping-distance insufficient, longitudinal infeasible, solver unavailable, timeout with and without fallback, cancellation, validation failure, gear switch, goal, and rolling stop. Assert input map ID, reference path ID, state sequence ID, prior trajectory ID, and segment index appear unchanged in result diagnostics.
|
||
|
||
- [ ] **Step 2: Run and verify facade is absent**
|
||
|
||
Expected: build failure naming `EmPlanningService`.
|
||
|
||
- [ ] **Step 3: Implement deterministic pipeline orchestration**
|
||
|
||
Call stages in this exact order:
|
||
|
||
```text
|
||
request/config validation
|
||
direction-segment selection
|
||
bounded ego projection
|
||
exact horizon and terminal selection through PlanningHorizonSelector
|
||
previous-trajectory seed projection
|
||
static connected corridor
|
||
LS optimization and validation
|
||
PathS speed envelope
|
||
ST optimization and validation
|
||
trajectory assembly
|
||
world-space publication validation
|
||
immutable result publication
|
||
```
|
||
|
||
Use the request timestamp rather than reading the system clock. Invoke debug sinks only behind flags and catch sink exceptions. Never expose a partial solver vector or mutate request-owned lists.
|
||
|
||
- [ ] **Step 4: Run the complete core gate twice**
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-core-all
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-core-all
|
||
git diff --check
|
||
```
|
||
|
||
Expected output both times:
|
||
|
||
```text
|
||
PASS longitudinal-model
|
||
PASS longitudinal-integration
|
||
PASS trajectory
|
||
PASS em-planning-service
|
||
```
|
||
|
||
- [ ] **Step 5: Document and commit the one-shot API**
|
||
|
||
Document request construction, all output fields/units/signs, statuses, terminal types, and a minimal forward/reverse usage example, then commit:
|
||
|
||
```powershell
|
||
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md ClumsyPilot/tests/EMPlannerVerificationHost
|
||
git commit -m "feat: publish one-shot EM trajectories"
|
||
```
|
||
|
||
## Completion Gate
|
||
|
||
- ST coefficient tests prove exact time dynamics, bounds, normalized costs, and fixed-terminal semantics.
|
||
- Every successful trajectory ends at an exact zero-speed terminal and contains a safe hold tail.
|
||
- Redundant output fields satisfy their authoritative formulas for every point.
|
||
- Full pose and swept collision checks pass in world coordinates.
|
||
- Every failure status publishes an empty trajectory and deterministic diagnostics.
|
||
- Two identical one-shot requests produce identical statuses and numeric trajectories.
|