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

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

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

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:

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

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

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:

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

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

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:

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

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

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:

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

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:

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:

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.