First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

docs: hand off EM rolling planning phase one

Browse Source
This commit is contained in:
梁薄云
2026-08-05 17:17:41 +08:00
parent 21b20d09d4
commit 105ec4bdad
2 changed files with 144 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
docs/superpowers/handoffs/2026-08-05-em-longitudinal-rolling-phase-1.md
+72
View File
@@ -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>, 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.
docs/superpowers/prompts/2026-08-05-em-longitudinal-rolling-phase-2.md
+72
View File
@@ -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.