6.2 KiB
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:
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
GoalandGearSwitchApproachsegment 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
ZeroSpeedHoldSecondsfrom 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:
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.