ParkingRobot/docs/superpowers/plans/2026-08-09-em-full-direction-correctness-fixes.md

EM FullDirection Correctness Fixes Implementation Plan

For agentic workers: REQUIRED SUB-SKILL: Use test-driven-development for every task. Execute tasks serially because they share the EM solver pipeline.

Goal: Fix confirmed FullDirection projection, cancellation, curvature-constraint, timeout-budget, and trajectory-coordinate defects without replacing the existing LS/ST planner.

Architecture: Keep IEmPlanningService, EmPlanningRequest, LateralPlanner, LongitudinalPlanner, and trajectory contracts compatible. Apply local fixes where one component owns the invariant; introduce only a shared internal lateral-curvature affine model and internal explicit-budget overloads where the same invariant necessarily crosses components.

Tech Stack: C# 10, .NET Standard 2.0 production assembly, .NET 8 verification host, solver-neutral IQpSolver tests.

Global Constraints

• FullDirectionSegment plans exactly one complete direction segment; RollingHorizon behavior is not redesigned in this plan.
• FullDirection ego admission is restricted to the segment-start prefix [0, min(L, MaximumProjectionDistanceMeters)]; it must not select a later U-shape/self-overlap branch.
• A start heading error with magnitude greater than or equal to π/2 is rejected before tan(headingError) is evaluated.
• Cancelled always carries a null trajectory/path/candidate, even if a strict fallback candidate exists.
• SolverTimeoutSeconds is one combined LS+ST solve budget for a service call, not a fresh budget for each optimizer.
• Every LS QP has a finite linearized vehicle-curvature hard-bound row at every station; nonlinear validation remains authoritative.
• SegmentLocalS stores interpolated direction-segment reference S; PathS stores optimized lateral-path arc length.
• Do not edit or revert the user's existing EmPlannerConfiguration.cs change, PathSmoothing work, Map work, or ClumsyPilot.csproj changes.
• Do not add actuator calls, change public EM request/result signatures, or commit/stage files from the dirty shared worktree.

---

Task 1: Anchor FullDirection start projection and reject folded headings

Files:

• Modify: ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs
• Modify: ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs

Interfaces:

• Consumes: existing FrenetProjector.TryProject bounded-window overload.

• Produces: service-local FullDirection start-prefix admission; Rolling continues using [0,L].

• Step 1: Write failing service tests. Add a FullDirection U-shaped all-forward reference whose later arm is closer to the measured pose, and assert ProjectionFailed rather than accepting a later ReferenceS. Add a same-position start pose with yaw π, and assert ProjectionFailed with a null trajectory.

EmPlanningResult wrongBranch = service.Plan(fullURequest, CancellationToken.None);
Verification.Equal(EmPlanningStatus.ProjectionFailed, wrongBranch.Status,
    "FullDirection cannot enter through a later U branch");

EmPlanningResult reversedHeading = service.Plan(oppositeHeadingRequest, CancellationToken.None);
Verification.Equal(EmPlanningStatus.ProjectionFailed, reversedHeading.Status,
    "opposite start heading is rejected before slope conversion");

• Step 2: Run the focused test and verify RED.

dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-planning-service

Expected: the later U branch and/or opposite-heading assertion fails under the current global [0,L], seed-zero projection.

• Step 3: Implement the local admission rule. In EmPlanningService.Plan, choose the projection upper bound from scope and validate heading before constructing lateral input.

double startProjectionUpperS = request.PlanningScope == EmPlanningScope.FullDirectionSegment
    ? Math.Min(segment.LengthMeters, configuration.Frenet.MaximumProjectionDistanceMeters)
    : segment.LengthMeters;
if (!projector.TryProject(request.VehicleState.Pose, segment, 0d, startProjectionUpperS,
        configuration.Frenet.MaximumProjectionDistanceMeters, 0d, out FrenetProjection startProjection) ||
    Math.Abs(startProjection.HeadingError) >= Math.PI / 2d)
{
    return Failure(EmPlanningStatus.ProjectionFailed, request,
        "Vehicle pose is not an admissible start state for the selected direction segment.");
}

• Step 4: Re-run em-planning-service and verify GREEN. Existing Rolling projection behavior must remain green.

Task 2: Make cancellation terminal and non-publishable

Files:

• Modify: ClumsyPilot/tests/EMPlannerVerificationHost/LateralIntegrationChecks.cs
• Modify: ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs
• Modify: ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs
• Modify: ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/SequentialConvexOptimizer.cs
• Modify: ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs
• Modify: ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs

Interfaces:

• Produces: Cancelled results with null candidate/path/trajectory at every layer.

• Step 1: Write failing optimizer tests. Use a solver that returns one valid candidate and cancels the supplied source before the next iteration. Assert both optimizers return Cancelled, not SuccessWithFallback, and expose no candidate.

Verification.Equal(EmPlanningStatus.Cancelled, result.Status,
    "cancellation is never converted to fallback success");
Verification.True(result.Path == null, "cancelled lateral result has no path");

• Step 2: Verify RED with the focused groups.

dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- lateral-integration
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-integration

Expected: at least one optimizer currently returns SuccessWithFallback.

• Step 3: Implement minimal cancellation precedence. Special-case cancellation in each FallbackOrFailure, and check the token after LS, after ST, after assembly, and immediately before service success publication.

if (failureStatus == EmPlanningStatus.Cancelled)
    return Failed(EmPlanningStatus.Cancelled, failureReason);

• Step 4: Re-run both optimizer groups and em-planning-service; verify GREEN.

Task 3: Add shared linearized curvature hard constraints

Files:

• Create: ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralCurvatureLinearization.cs
• Modify: ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralObjectiveBuilder.cs
• Modify: ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralConstraintBuilder.cs
• Modify: ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs
• Modify: ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs

Interfaces:

• Produces: LateralCurvatureLinearization.Create(input, layout, iterate) returning immutable station affines with indices, gradient, and constant.

• Consumers: objective terms and hard constraints use the exact same affine coefficients.

• Step 1: Write a failing QP-shape test. For a straight reference and zero iterate, set vehicle maximum curvature to 0.25 1/m; assert every station has a row equivalent to -0.25 <= DDL(i) <= 0.25. Also assert the total constraint count increases by the station count.

Verification.Equal(expectedOldRows + layout.StationCount, problem.ConstraintCount,
    "one curvature hard-bound row is emitted per station");
Verification.True(HasBound(problem,
    new Dictionary<int, double> { { layout.DDL(station), 1d } }, -0.25d, 0.25d),
    "straight-path curvature affine is hard bounded");

• Step 2: Run lateral-model and verify RED.

• Step 3: Extract the existing affine calculation without changing its formula. Move CreateCurvatureAffines and its value type from LateralObjectiveBuilder to the new internal file. Keep the nonlinear formula in LateralGeometryEvaluator/independent validator unchanged.

• Step 4: Add one curvature row per station in LateralConstraintBuilder. Bounds are [-maximumVehicleCurvature, +maximumVehicleCurvature] after subtracting the affine constant.

AddRow(constraints, lower, upper, ref row,
    -maximumCurvature - affine.Constant,
     maximumCurvature - affine.Constant,
    affine.Indices, affine.Gradient);

• Step 5: Update test solver problem classification so the added lateral rows are not mistaken for ST rows, then run lateral-model, lateral-integration, and em-planning-service GREEN.

Task 4: Relinearize rejected solved vectors and report iteration exhaustion honestly

Files:

• Modify: ClumsyPilot/tests/EMPlannerVerificationHost/LateralIntegrationChecks.cs
• Modify: ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/SequentialConvexOptimizer.cs

Interfaces:

• Produces: rejected, parseable solver candidates may advance only the SQP iterate/warm start; they never replace lastValidatedPath.

• Step 1: Write failing tests. Configure a low curvature limit so the first candidate is strict and the second parseable candidate fails nonlinear validation. Assert the third QP/warm start is based on the second candidate, while a later timeout still returns the first strict path. Change the outer-limit assertion from ordinary Success to SuccessWithFallback with a non-empty reason.

• Step 2: Run lateral-integration and verify RED.

• Step 3: Move iterate/warm-start advancement to immediately after a parseable solved candidate, while updating lastValidatedPath only after independent validation. Return SuccessWithFallback when the outer loop ends with a strict candidate but without satisfying convergence.

• Step 4: Run lateral-integration and lateral-real-osqp GREEN.

Task 5: Share one LS/ST solver timeout budget

Files:

• Modify: ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs
• Modify: ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanner.cs
• Modify: ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/SequentialConvexOptimizer.cs
• Modify: ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanner.cs
• Modify: ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs
• Modify: ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs

Interfaces:

• Keeps: existing public Plan(input, token) and Optimize(input, token) entry points.

• Adds: internal overloads accepting a finite positive TimeSpan solveBudget.

• Step 1: Write a failing service test. Delay a lateral fake solve by at least 100 ms under a 2 s configured timeout, record all QpSolverSettings.MaximumSolveDuration values, and assert the first ST call receives less than 1.95 s rather than a fresh 2 s.

• Step 2: Run em-planning-service and verify RED.

• Step 3: Add internal explicit-budget overloads. Default public overloads continue deriving budget from configuration; service starts one monotonic Stopwatch immediately before LS and passes configuredBudget - elapsed to LS and then ST. Non-positive remaining time returns SolverTimedOut with no trajectory.

• Step 4: Add a final elapsed/cancellation check before publication and run lateral-integration, longitudinal-integration, and em-planning-service GREEN.

Task 6: Preserve reference S separately from optimized PathS

Files:

• Modify: ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs
• Modify: ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/LateralPathInterpolator.cs
• Modify: ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs
• Modify: ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs

Interfaces:

• Extends internal InterpolatedLateralPathPoint with ReferenceS.

• Keeps public EmTrajectoryPoint shape unchanged.

• Step 1: Write a failing curved/offset-path assembly test. Construct a validated lateral path where reference-S and chord PathS differ; assert each output point's SegmentLocalS is the interpolated reference-S and PathS remains the ST progress value.

• Step 2: Run trajectory and verify RED. Current assembly writes sample.PathS into both fields.

• Step 3: Interpolate ReferenceS using the same PathS bracket and pass geometry.ReferenceS as SegmentLocalS. Update publication bounds so segment-local S is checked against direction-segment length/reference bound, while PathS is checked against the optimized path upper bound.

• Step 4: Run trajectory and em-core-all GREEN.

Task 7: Core regression and diff hygiene

Files:

• Verify only; no broad formatting.

• Step 1: Run the complete EM verification set.

dotnet build ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj --no-restore
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj --no-build -- em-all

• Step 2: Check only scoped diffs.

git diff --check -- ClumsyPilot/ParkrobTrajplanner/EMPlanner ClumsyPilot/tests/EMPlannerVerificationHost
git status --short -- ClumsyPilot/ParkrobTrajplanner/EMPlanner ClumsyPilot/tests/EMPlannerVerificationHost

Expected: all groups pass; no whitespace errors; EmPlannerConfiguration.cs remains exactly the user's pre-existing modification.