diff --git a/docs/superpowers/handoffs/2026-08-05-em-longitudinal-rolling-phase-1.md b/docs/superpowers/handoffs/2026-08-05-em-longitudinal-rolling-phase-1.md new file mode 100644 index 0000000..40310e7 --- /dev/null +++ b/docs/superpowers/handoffs/2026-08-05-em-longitudinal-rolling-phase-1.md @@ -0,0 +1,72 @@ +# EM Longitudinal Rolling Planning — Phase 1 Handoff + +## Scope and commits + +- Phase start baseline: `6c2f406` (`docs: split EM rolling plan into four phases`) +- Task 1 commit: `e56220f` (`feat: add complete jerk-limited stopping math`) +- Task 2 commit / phase code end: `21b20d0` (`feat: separate rolling horizons from stop boundaries`) + +This phase implemented only original-plan Tasks 1 and 2. It did not implement the Task 3 speed-envelope behavior, Task 4 conditional QP constraints, Task 5 optimizer seeding, trajectory publication behavior, or any chassis command. `OBSERVE_ONLY` remains unchanged. + +## Delivered changes + +Task 1 added `JerkLimitedStoppingMath` and `JerkLimitedStoppingProfile` in `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/JerkLimitedStoppingMath.cs`. The public API is: + +```csharp +JerkLimitedStoppingMath.TryCalculate(double speed, double acceleration, + double maximumDeceleration, double maximumJerk, + out JerkLimitedStoppingProfile profile, out string failureReason) +JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(double availableDistance, + double conservativeInitialAcceleration, double maximumDeceleration, + double maximumJerk, double directionMaximumSpeed) +JerkLimitedStoppingMath.CalculateMaximumStoppedDistance(double initialSpeed, + double initialAcceleration, double maximumSpeed, double maximumAcceleration, + double maximumDeceleration, double maximumJerk, double timeHorizon) +``` + +The profile exposes `DistanceMeters`, `DurationSeconds`, `FinalSpeedMetersPerSecond`, and `FinalAccelerationMetersPerSecondSquared`. The model uses the ST constant-jerk integration equations and releases both speed and acceleration to zero. `PathSpeedLimitBuilder` now uses it for its existing stopping precheck; the old duplicate stopping classes were removed. + +Task 1 files were `JerkLimitedStoppingMath.cs`, `PathSpeedLimitBuilder.cs`, and `LongitudinalModelChecks.cs`. Two existing direct callers of the removed internal types also required minimal compilation adaptation: `PlanningHorizonSelector.cs` and `SequentialLongitudinalOptimizer.cs`. They now call the new math but otherwise preserve their pre-Task-2 behavior. + +Task 2 added `EmLongitudinalMode` (`RollingContinuation`, `ApproachStopBoundary`, `ExactStopAtBoundary`) and `LongitudinalTerminalSchedule.GetStabilizationStartIndex(IReadOnlyList, double)`. + +`PlanningHorizonSelection` now records `WindowEndReferenceS`, `WindowEndBoundaryType`, `TerminalType`, `LongitudinalMode`, `StopBoundaryReferenceS`, and `HasStopBoundary`. `TerminalReferenceS` remains as a read-only compatibility alias for the window end. + +`PlanningHorizonSelector` now: + +- rejects an already insufficient real segment stopping distance using the complete jerk model; +- sets the LS window with only `DistanceHorizonMeters`; +- uses only real `Goal` and `GearSwitchApproach` segment ends as stop boundaries; +- selects rolling when the boundary is outside the window, approach when visible but not time-reachable with the mandatory internal stabilization step, and exact stop otherwise; +- does not deduct `ZeroSpeedHoldSeconds` from the ST horizon. + +`LongitudinalPlanningInput` now accepts `EmLongitudinalMode` and exposes `PathUpperBoundS`, `HasStopBoundary`, `StopBoundaryPathS`, `StopBoundaryType`, and `Mode`. Its original constructor remains as a compatibility overload, mapping rolling terminals to `RollingContinuation` and Goal/GearSwitch terminals to `ExactStopAtBoundary`; new code must use the mode-explicit constructor. `TerminalPathS` remains a compatibility alias for now, because Tasks 3–5 must migrate its upper-bound and stop-boundary uses deliberately. + +`EmPlanningRequestValidator` checks the worst forward/reverse full jerk-limited stop at configured maximum positive acceleration plus one replan-period reserve against `DistanceHorizonMeters`, and checks that the ST knot horizon can reserve a full internal stabilization interval. `EmPlanningService` slices with `WindowEndReferenceS` and passes the selected longitudinal mode into the ST input. + +Task 2 files were `EmLongitudinalMode.cs`, `LongitudinalTerminalSchedule.cs`, `PlanningHorizonSelector.cs`, `LongitudinalPlanningInput.cs`, `EmPlanningRequestValidator.cs`, `EmPlanningService.cs`, `LongitudinalModelChecks.cs`, and `FoundationChecks.cs`. `LongitudinalIntegrationChecks.cs` was additionally updated only to pass `Mode` at every direct `LongitudinalPlanningInput` construction. + +## Verification evidence + +The phase-end commands were run from commit `21b20d0`; each exited `0`: + +```powershell +dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- foundation +# PASS foundation + +dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model +# PASS longitudinal-model + +git diff --check +# exit 0 +``` + +The .NET commands still print two pre-existing obsolete warnings from `MovementTests.TireFollowing.cs` and `TireFollowing.cs`; phase 1 added no warning source. The staged-file checks before each commit contained only the listed phase files and the explicitly noted direct-caller adaptations. + +## Remaining work and constraints for phase 2 + +Phase 2 is original-plan Tasks 3–5 only. It must make speed envelopes, QP terminal constraints, solution validation, and optimizer seeds conditional on `LongitudinalPlanningInput.Mode`. It must replace legacy `TerminalPathS` semantics with `PathUpperBoundS` versus `StopBoundaryPathS` at the relevant use sites, without changing trajectory publication (Task 6+) or adding any chassis command. + +Do not loosen jerk, acceleration, velocity, QP residual, or trajectory-validation tolerances. `DistanceHorizonMeters` remains the LS spatial look-ahead; `TimeHorizonSeconds` remains one ST solve duration. A rolling window end is not a stop boundary. Exact stops must reserve at least one full `OutputTimeStepSeconds` static interval inside the QP horizon. `ZeroSpeedHoldSeconds` stays outside that horizon and is not phase-2 work. + +The working tree still contains extensive unrelated user PathSmoothing, CoarsePath, Map, configuration, report, and untracked-file changes. Preserve them; do not reset, clean, broadly stage, or commit them. No target-machine OSQP probe was run in phase 1; it belongs to the later roadmap phase. diff --git a/docs/superpowers/prompts/2026-08-05-em-longitudinal-rolling-phase-2.md b/docs/superpowers/prompts/2026-08-05-em-longitudinal-rolling-phase-2.md new file mode 100644 index 0000000..1b14df3 --- /dev/null +++ b/docs/superpowers/prompts/2026-08-05-em-longitudinal-rolling-phase-2.md @@ -0,0 +1,72 @@ +# New Window Prompt: EM Longitudinal Rolling Planning — Phase 2 + +Work in `D:\Users\Desktop\项目\prakrobot\ParkingRobot`. This is an implementation task, not a design discussion. Read these files completely before editing: + +- `docs/superpowers/specs/2026-08-05-em-longitudinal-rolling-planning-design.md` +- `docs/superpowers/plans/2026-08-05-em-longitudinal-rolling-planning.md` +- `docs/superpowers/plans/2026-08-05-em-longitudinal-rolling-four-phase-roadmap.md` +- `docs/superpowers/handoffs/2026-08-05-em-longitudinal-rolling-phase-1.md` + +Phase 1 ended at `21b20d0` and consists of `e56220f` and `21b20d0`. Preserve their actual interfaces: `JerkLimitedStoppingMath`, `JerkLimitedStoppingProfile`, `EmLongitudinalMode`, `LongitudinalTerminalSchedule`, `PlanningHorizonSelection.WindowEndReferenceS`, `WindowEndBoundaryType`, `LongitudinalMode`, `StopBoundaryReferenceS`, `HasStopBoundary`, and the mode-explicit `LongitudinalPlanningInput` constructor/properties. `TerminalPathS` is only a temporary compatibility alias; use `PathUpperBoundS` for path bounds and `StopBoundaryPathS` for real stops. + +Execute only original-plan Tasks 3, 4, and 5. Do not begin Task 6 or later. Use `executing-plans`, test-driven development for every behavior change, systematic debugging for every unexpected test/build failure, and verification-before-completion before each commit and before phase handoff. No new brainstorming is needed. + +Start by recording: + +```powershell +git branch --show-current +git status --short +git log -8 --oneline +git show --stat --oneline e56220f +git show --stat --oneline 21b20d0 +``` + +The branch must be `trajplanner`; the worktree has substantial unrelated user changes. Never use reset, checkout, clean, `git add .`, or `git add -A`. Inspect any target file that is already dirty and preserve its user content. Explicitly stage only the current task files and run `git diff --cached --name-only` before every commit. + +## Task 3 — mode-conditioned PathS speed envelope + +Files: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimit.cs`, `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs`, and `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs`. + +Write and observe a failing `longitudinal-model` test first. Rolling must keep a nonzero speed allowance at `PathUpperBoundS` and report no stop boundary. For Approach and Exact modes, compute each stopping-speed limit by inverting `JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(remaining, maximumAcceleration, maximumDeceleration, maximumJerk, directionMaximum)` against `StopBoundaryPathS`; only the real stop-boundary sample is exactly zero. Replace the old square-root and discrete-tail approximations. Add `PathSpeedLimit.HasStopBoundary` and retain any necessary compatibility alias without introducing warnings. Then run: + +```powershell +dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model +``` + +Commit only these Task 3 files: + +```powershell +git add -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimit.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs +git diff --cached --name-only +git commit -m "feat: keep rolling speed envelopes open" +``` + +## Task 4 — conditional QP terminals and independent validation + +Files: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs`, `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs`, and `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs`. + +TDD requirements: Rolling and Approach have no terminal S/U/A equalities. Exact mode must use `LongitudinalTerminalSchedule.GetStabilizationStartIndex` and enforce `S=StopBoundaryPathS`, `U=0`, `A=0` at every knot in the internal stabilization tail. The exact mode row count is `8*K-2+3*M`; non-exact modes remain `8*K-2`. The validator always checks finite values, time layout, monotone PathS, bounds, jerk, and exact constant-jerk dynamics. For Approach and Exact, independently recompute every knot's complete jerk-limited stopping profile and reject if `S + stoppingDistance > StopBoundaryPathS + tolerance`; exact mode must also canonicalize its full stabilization tail. Run `longitudinal-model` red then green and commit only Task 4 files: + +```powershell +git add -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs +git diff --cached --name-only +git commit -m "feat: apply ST stop constraints only at real boundaries" +``` + +## Task 5 — mode-specific seeds and outer iteration + +Files: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs`, `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalObjectiveBuilder.cs`, and `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs`. + +Write failing `longitudinal-integration` tests first. A rolling two-second ST solve with a five-metre LS window must end before `PathUpperBoundS` at nonzero speed. Exact mode must have `S=StopBoundaryPathS`, `U=0`, `A=0` at every stabilization-tail knot. Dispatch seed creation by mode: rolling forward integration without a forced endpoint; approach as a cruise plus complete jerk-limited stop prefix limited to this ST horizon; exact stop reaches rest at the schedule anchor then fills the internal tail. In outer iterations, only Exact mode pins tail progress. Replace upper-bound uses with `PathUpperBoundS`; only real-stop endpoint uses may use `StopBoundaryPathS`. Keep terminal-acceleration soft objective only for non-exact modes. Verify both commands and commit only Task 5 files: + +```powershell +dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model +dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-integration +git add -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalObjectiveBuilder.cs ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs +git diff --cached --name-only +git commit -m "feat: seed rolling and exact-stop ST profiles" +``` + +Global semantics are non-negotiable: `DistanceHorizonMeters` is LS look-ahead only; `TimeHorizonSeconds` is one ST horizon only; only Goal/GearSwitchApproach can request an exact zero-speed terminal; `ZeroSpeedHoldSeconds` is outside the QP horizon; do not loosen jerk/acceleration/solver/validation tolerances; and retain `OBSERVE_ONLY` with no chassis command. + +At phase-2 end, rerun fresh `longitudinal-model` and `longitudinal-integration`, run `git diff --check`, inspect `git log -5 --oneline` and `git status --short`, then use `writing-plans` to create the phase-2 handoff and a phase-3 prompt. Commit only those two handoff documents and stop; do not implement phase 3.