docs: plan EM planner implementation
This commit is contained in:
梁薄云
@@ -0,0 +1,557 @@
|
||||
# EM Planner Foundation 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:** Build the immutable EM Planner contracts, validated configuration, direction-segment boundary model, reverse-safe Frenet transforms, and topology-preserving static lateral corridor.
|
||||
|
||||
**Architecture:** Keep the planner core request-driven and independent from hardware, UI, clocks, and solver details. Adapt one consumable `PathSmoothingResult` into an immutable current-direction reference window, then project only within that segment and build a collision-checked connected lateral interval around the selected seed.
|
||||
|
||||
**Tech Stack:** C# 10, .NET Standard 2.0 library, existing `PlanningGridMap`, `FootprintCollisionChecker`, and a `net10.0-windows` console verification host.
|
||||
|
||||
## Global Constraints
|
||||
|
||||
- Namespace for new production types: `MultiWheelC.TrajectoryPlanning.EMPlanner` plus responsibility-specific child namespaces.
|
||||
- First release supports ordinary nonholonomic forward/reverse motion only; lateral translation, crab motion, and in-place rotation are rejected.
|
||||
- World units are metres, seconds, radians, metres/second, radians/second, and inverse metres.
|
||||
- Vehicle body X points forward, body Y points left, and positive yaw is counter-clockwise.
|
||||
- `TravelYaw = VehicleYaw` forward and `TravelYaw = Normalize(VehicleYaw + π)` reverse.
|
||||
- Frenet `s` always increases in actual travel direction; positive `l` is left of travel, including reverse.
|
||||
- A planning window never crosses a gear-switch boundary and never drops an exact terminal anchor.
|
||||
- Static-corridor selection preserves the topology chosen by Hybrid A*; it never jumps to another disconnected free interval.
|
||||
- Initial corridor spacings are `0.10 m` longitudinal and `0.025 m` lateral; maximum lateral offset is `0.30 m`.
|
||||
- Existing user changes in `ClumsyPilot.csproj`, Map, CoarsePath, and PathSmoothing must be preserved.
|
||||
- Dynamic-obstacle trajectory prediction and dynamic space-time boundaries are outside this plan.
|
||||
|
||||
---
|
||||
|
||||
## Locked File Structure
|
||||
|
||||
```text
|
||||
ClumsyPilot/ParkrobTrajplanner/EMPlanner/
|
||||
├── Contracts/
|
||||
│ ├── EmBoundaryType.cs
|
||||
│ ├── EmMotionModel.cs
|
||||
│ ├── EmPlanningRequest.cs
|
||||
│ ├── EmPlanningResult.cs
|
||||
│ ├── EmPlanningStatus.cs
|
||||
│ ├── EmTerminalType.cs
|
||||
│ ├── EmTrajectory.cs
|
||||
│ ├── EmTrajectoryMetadata.cs
|
||||
│ ├── EmTrajectoryPoint.cs
|
||||
│ └── VehicleMotionState.cs
|
||||
├── Configuration/
|
||||
│ ├── CorridorConfiguration.cs
|
||||
│ ├── EmPlannerConfiguration.cs
|
||||
│ ├── FrenetConfiguration.cs
|
||||
│ ├── LateralConfiguration.cs
|
||||
│ ├── LateralWeights.cs
|
||||
│ ├── LongitudinalConfiguration.cs
|
||||
│ ├── LongitudinalWeights.cs
|
||||
│ ├── SchedulingConfiguration.cs
|
||||
│ ├── SolverConfiguration.cs
|
||||
│ └── ValidationConfiguration.cs
|
||||
├── Diagnostics/
|
||||
│ ├── EmPlannerDebugOptions.cs
|
||||
│ ├── EmPlanningDiagnostics.cs
|
||||
│ └── IEmPlannerDebugSink.cs
|
||||
├── Segmentation/
|
||||
│ ├── DirectionSegmentView.cs
|
||||
│ ├── ReferenceBoundary.cs
|
||||
│ ├── ReferenceHorizonSlicer.cs
|
||||
│ └── ReferencePathSegmenter.cs
|
||||
├── Frenet/
|
||||
│ ├── FrenetProjection.cs
|
||||
│ ├── FrenetProjector.cs
|
||||
│ ├── FrenetReferencePoint.cs
|
||||
│ ├── FrenetTransform.cs
|
||||
│ └── ReferencePathInterpolator.cs
|
||||
├── Corridor/
|
||||
│ ├── LateralInterval.cs
|
||||
│ ├── StaticCorridor.cs
|
||||
│ └── StaticCorridorBuilder.cs
|
||||
└── Validation/
|
||||
└── EmPlanningRequestValidator.cs
|
||||
|
||||
ClumsyPilot/tests/EMPlannerVerificationHost/
|
||||
├── EMPlannerVerificationHost.csproj
|
||||
├── Program.cs
|
||||
├── Verification.cs
|
||||
├── EmFixtureFactory.cs
|
||||
├── FoundationChecks.cs
|
||||
├── SegmentationChecks.cs
|
||||
├── FrenetChecks.cs
|
||||
└── CorridorChecks.cs
|
||||
```
|
||||
|
||||
## Shared Public Interfaces
|
||||
|
||||
```csharp
|
||||
public sealed class VehicleMotionState
|
||||
{
|
||||
public VehicleMotionState(Pose2D pose, double signedLongitudinalSpeedMetersPerSecond,
|
||||
double? longitudinalAccelerationMetersPerSecondSquared,
|
||||
DateTimeOffset capturedAtUtc, long sequenceId);
|
||||
public Pose2D Pose { get; }
|
||||
public double SignedLongitudinalSpeedMetersPerSecond { get; }
|
||||
public double? LongitudinalAccelerationMetersPerSecondSquared { get; }
|
||||
public DateTimeOffset CapturedAtUtc { get; }
|
||||
public long SequenceId { get; }
|
||||
}
|
||||
|
||||
public sealed class EmPlanningRequest
|
||||
{
|
||||
public EmPlanningRequest(PathSmoothingResult referencePath, PlanningGridMap map,
|
||||
VehicleParameters vehicle, VehicleMotionState vehicleState,
|
||||
EmPlannerConfiguration configuration, int segmentIndex,
|
||||
EmTrajectory previousTrajectory, DateTimeOffset requestedAtUtc,
|
||||
DateTimeOffset effectiveAtUtc, string outputTrajectoryId,
|
||||
string referencePathId, string previousTrajectoryId,
|
||||
EmMotionModel motionModel);
|
||||
}
|
||||
|
||||
public sealed class DirectionSegmentView
|
||||
{
|
||||
public int SegmentIndex { get; }
|
||||
public TravelDirection Direction { get; }
|
||||
public IReadOnlyList<SmoothedPathPoint> Points { get; }
|
||||
public ReferenceBoundary StartBoundary { get; }
|
||||
public ReferenceBoundary EndBoundary { get; }
|
||||
}
|
||||
|
||||
public sealed class FrenetProjector
|
||||
{
|
||||
public bool TryProject(Pose2D worldPose, DirectionSegmentView segment,
|
||||
double minimumReferenceS, double maximumReferenceS,
|
||||
double maximumDistanceMeters, out FrenetProjection projection);
|
||||
}
|
||||
|
||||
public sealed class StaticCorridorBuilder
|
||||
{
|
||||
public bool TryBuild(DirectionSegmentView segment, double startReferenceS,
|
||||
double endReferenceS, IReadOnlyList<FrenetProjection> seed,
|
||||
PlanningGridMap map, VehicleParameters vehicle,
|
||||
CorridorConfiguration configuration, out StaticCorridor corridor,
|
||||
out string failureReason);
|
||||
}
|
||||
```
|
||||
|
||||
### Task 1: Verification Host and Immutable Contracts
|
||||
|
||||
**Files:**
|
||||
- Modify: `ClumsyPilot/ClumsyPilot.csproj`
|
||||
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj`
|
||||
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
|
||||
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/Verification.cs`
|
||||
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs`
|
||||
- Create: all files under `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: existing `Pose2D`, `TravelDirection`, `PathSmoothingResult`, `PlanningGridMap`, and `VehicleParameters`.
|
||||
- Produces: the contracts shown in “Shared Public Interfaces”, `EmPlanningStatus`, `EmTerminalType`, `EmBoundaryType`, and immutable trajectory containers.
|
||||
|
||||
- [ ] **Step 1: Add the isolated verification host and a failing contract check**
|
||||
|
||||
Add these exact MSBuild rules without rewriting surrounding user changes:
|
||||
|
||||
```xml
|
||||
<Compile Remove="tests\EMPlannerVerificationHost\**\*.cs" />
|
||||
<Compile Remove="ParkrobTrajplanner\auto_avoidance\**\*.cs"
|
||||
Condition="'$(ExcludeLegacyAutoAvoidance)' == 'true'" />
|
||||
```
|
||||
|
||||
Create the host project with:
|
||||
|
||||
```xml
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
<PropertyGroup>
|
||||
<OutputType>Exe</OutputType>
|
||||
<TargetFramework>net10.0-windows</TargetFramework>
|
||||
<ImplicitUsings>disable</ImplicitUsings>
|
||||
<Nullable>enable</Nullable>
|
||||
</PropertyGroup>
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\..\ClumsyPilot.csproj"
|
||||
AdditionalProperties="ExcludeLegacyAutoAvoidance=true" />
|
||||
</ItemGroup>
|
||||
</Project>
|
||||
```
|
||||
|
||||
`FoundationChecks.Run()` must construct a reverse `VehicleMotionState`, assert that its signed speed remains negative, construct an `EmTrajectoryPoint`, and assert all constructor-supplied values are unchanged. `Program.Main` accepts `foundation`, runs the check, prints `PASS foundation`, and returns `1` after printing an exception when a check fails.
|
||||
|
||||
- [ ] **Step 2: Run the host and verify the contracts are absent**
|
||||
|
||||
Run:
|
||||
|
||||
```powershell
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- foundation
|
||||
```
|
||||
|
||||
Expected: build failure naming `VehicleMotionState` or `EmTrajectoryPoint`.
|
||||
|
||||
- [ ] **Step 3: Implement immutable contracts with constructor validation**
|
||||
|
||||
Use `ReadOnlyCollection<T>` copies for every published list. `EmTrajectoryPoint` has exactly these public properties:
|
||||
|
||||
```csharp
|
||||
public double X { get; }
|
||||
public double Y { get; }
|
||||
public double Yaw { get; }
|
||||
public double SignedLongitudinalVelocity { get; }
|
||||
public double Speed { get; }
|
||||
public double VelocityX { get; }
|
||||
public double VelocityY { get; }
|
||||
public double YawRate { get; }
|
||||
public double TimeFromStart { get; }
|
||||
public double VehicleCurvature { get; }
|
||||
public int SegmentIndex { get; }
|
||||
public double SegmentLocalS { get; }
|
||||
public double PathS { get; }
|
||||
public TravelDirection Direction { get; }
|
||||
public EmBoundaryType BoundaryType { get; }
|
||||
```
|
||||
|
||||
Its constructor accepts the authoritative `signedLongitudinalVelocity` and derives, rather than accepts, the redundant fields:
|
||||
|
||||
```csharp
|
||||
Speed = Math.Abs(signedLongitudinalVelocity);
|
||||
VelocityX = signedLongitudinalVelocity * Math.Cos(yaw);
|
||||
VelocityY = signedLongitudinalVelocity * Math.Sin(yaw);
|
||||
YawRate = signedLongitudinalVelocity * vehicleCurvature;
|
||||
```
|
||||
|
||||
Define these exact status values:
|
||||
|
||||
```text
|
||||
Success
|
||||
SuccessWithFallback
|
||||
InvalidInput
|
||||
UnsupportedMotionMode
|
||||
StaleVehicleState
|
||||
StateDirectionMismatch
|
||||
InvalidReferencePath
|
||||
ProjectionFailed
|
||||
CorridorInfeasible
|
||||
LateralInfeasible
|
||||
LongitudinalInfeasible
|
||||
StoppingDistanceInsufficient
|
||||
SolverUnavailable
|
||||
SolverTimedOut
|
||||
Cancelled
|
||||
ValidationFailed
|
||||
Superseded
|
||||
Failed
|
||||
```
|
||||
|
||||
`EmMotionModel` has one supported value, `NonholonomicForwardReverse`, plus explicit `CrabTranslation` and `InPlaceRotation` values that validation maps to `UnsupportedMotionMode`. Only the two success statuses may construct a result with a non-empty trajectory. Define `EmBoundaryType` as `None`, `RollingSafetyStop`, `GearSwitchApproach`, `GearSwitchDeparture`, and `Goal`; define `EmTerminalType` as `RollingSafetyStop`, `GearSwitch`, and `Goal`.
|
||||
|
||||
`EmTrajectoryMetadata` contains `TrajectoryId`, `GeneratedAtUtc`, `EffectiveAtUtc`, `MapSnapshotId`, `ReferencePathId`, `VehicleStateSequenceId`, `PreviousTrajectoryId`, `SegmentIndex`, `Direction`, and `TerminalType`; `EmTrajectory` exposes that metadata plus immutable points. `GeneratedAtUtc` is copied from request `RequestedAtUtc`, and `EffectiveAtUtc` plus `OutputTrajectoryId` are caller-supplied so the pure planner never reads a clock or creates a random ID. `EmTrajectoryPoint` retains internal longitudinal acceleration and jerk values for validation but derives all public redundant velocity fields from signed longitudinal velocity.
|
||||
|
||||
- [ ] **Step 4: Run the foundation check**
|
||||
|
||||
Run the same command. Expected: `PASS foundation` and exit code `0`.
|
||||
|
||||
- [ ] **Step 5: Commit the host and contracts**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/ClumsyPilot.csproj ClumsyPilot/tests/EMPlannerVerificationHost ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts
|
||||
git commit -m "feat: add EM planner contracts"
|
||||
```
|
||||
|
||||
### Task 2: Configuration, Diagnostics, and Request Validation
|
||||
|
||||
**Files:**
|
||||
- Create: all files under `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration/`
|
||||
- Create: all files under `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Diagnostics/`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs`
|
||||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: Task 1 contracts.
|
||||
- Produces: `EmPlannerConfiguration.CreateDefault()`, `EmPlanningRequestValidator.Validate(EmPlanningRequest)`, debug options, and diagnostics.
|
||||
|
||||
- [ ] **Step 1: Write failing checks for exact defaults and rejection rules**
|
||||
|
||||
Add assertions for these exact defaults:
|
||||
|
||||
```text
|
||||
ReplanPeriodSeconds=0.20
|
||||
TimeHorizonSeconds=6.0
|
||||
DistanceHorizonMeters=5.0
|
||||
OutputTimeStepSeconds=0.05
|
||||
SolverTimeoutSeconds=0.10
|
||||
HandoffLookaheadSeconds=0.30
|
||||
MaximumVehicleStateAgeSeconds=0.20
|
||||
LongitudinalSampleSpacingMeters=0.10
|
||||
LateralSampleSpacingMeters=0.025
|
||||
MaximumLateralOffsetMeters=0.30
|
||||
AdditionalClearanceReserveMeters=0.02
|
||||
MaximumCollisionCheckStepMeters=0.025
|
||||
MaximumProjectionDistanceMeters=0.50
|
||||
MinimumFrenetDenominator=0.20
|
||||
BoundaryAnchorToleranceMeters=1e-8
|
||||
MaximumLateralStepPerIterationMeters=0.05
|
||||
MaximumLateralSlope=0.50
|
||||
MaximumLateralSecondDerivativePerMeter=1.00
|
||||
MaximumLateralThirdDerivativePerSquareMeter=2.00
|
||||
MaximumForwardSpeedMetersPerSecond=0.20
|
||||
MaximumReverseSpeedMetersPerSecond=0.20
|
||||
MaximumAccelerationMetersPerSecondSquared=0.20
|
||||
MaximumDecelerationMetersPerSecondSquared=0.30
|
||||
MaximumJerkMetersPerSecondCubed=0.50
|
||||
MaximumLateralAccelerationMetersPerSecondSquared=0.20
|
||||
MaximumCurvatureRatePerMeterPerSecond=0.50
|
||||
StopSpeedToleranceMetersPerSecond=0.01
|
||||
ZeroSpeedHoldSeconds=0.20
|
||||
MaximumOuterIterations=5
|
||||
MaximumOsqpIterations=4000
|
||||
AbsoluteTolerance=1e-5
|
||||
RelativeTolerance=1e-5
|
||||
StrictResidualTolerance=1e-5
|
||||
WarmStart=true
|
||||
Polish=true
|
||||
NativeVerbose=false
|
||||
```
|
||||
|
||||
Assert LS weights `10,1,5,10,5,20,5,10` in reference/heading/second/third/curvature/curvature-variation/previous/rolling-terminal order, and ST weights `10,1,10,5,1` in speed/acceleration/jerk/previous/terminal-acceleration order.
|
||||
|
||||
Also assert validation rejects: null request members, a map with `PlanningReady=false`, a smoothing failure status, an out-of-range segment index, a non-finite speed, negative sequence ID, stale state, and vehicle geometry without a valid curvature limit.
|
||||
Assert `CrabTranslation` and `InPlaceRotation` return `UnsupportedMotionMode` before projection.
|
||||
|
||||
- [ ] **Step 2: Run and verify the configuration types are absent**
|
||||
|
||||
Run the foundation group. Expected: build failure naming `EmPlannerConfiguration`.
|
||||
|
||||
- [ ] **Step 3: Implement configuration snapshots and validation**
|
||||
|
||||
Use settable configuration DTOs only at the request boundary, then make `EmPlanningRequestValidator` return a copied internal snapshot. Validate every double with `NumericGuard`; enforce positive sample spacings, `0 < MinimumFrenetDenominator < 1`, non-negative margins, and positive horizons. Treat only `Complete`, `PartialImprovement`, `NotNeeded`, and `Unchanged` smoothing statuses as consumable.
|
||||
|
||||
`LateralConfiguration` owns trust-region and derivative hard limits plus `LateralWeights`; `LongitudinalConfiguration` owns direction speed, acceleration, deceleration, jerk, lateral-acceleration, curvature-rate, stop tolerance, hold duration, and `LongitudinalWeights`; `ValidationConfiguration` owns absolute spatial/kinematic consistency tolerances. Configuration validation requires every weight to be finite and non-negative and every normalization scale to be finite and positive.
|
||||
|
||||
`EmPlannerDebugOptions` exposes only flags and a sink:
|
||||
|
||||
```csharp
|
||||
EnableSummary
|
||||
EnableProjectionTrace
|
||||
EnableCorridorTrace
|
||||
EnableLateralSolverTrace
|
||||
EnableLongitudinalSolverTrace
|
||||
EnableTrajectoryDump
|
||||
EnableVisualization
|
||||
Sink
|
||||
```
|
||||
|
||||
Debug-sink exceptions are caught and recorded in diagnostics; they never escape the planner.
|
||||
|
||||
- [ ] **Step 4: Run foundation checks and confirm all rejection messages are deterministic**
|
||||
|
||||
Expected: `PASS foundation`; run twice and compare stdout exactly.
|
||||
|
||||
- [ ] **Step 5: Commit configuration and validation**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Configuration ClumsyPilot/ParkrobTrajplanner/EMPlanner/Diagnostics ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs
|
||||
git commit -m "feat: validate EM planner requests"
|
||||
```
|
||||
|
||||
### Task 3: Direction Segmentation and Exact Boundary Anchors
|
||||
|
||||
**Files:**
|
||||
- Create: all files under `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/`
|
||||
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/SegmentationChecks.cs`
|
||||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
|
||||
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/EmFixtureFactory.cs`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: `PathSmoothingResult.Path`, `PathSmoothingResult.Segments`, and Task 1 boundary enums.
|
||||
- Produces: `DirectionSegmentView`, `ReferenceBoundary`, and exact horizon slices.
|
||||
|
||||
- [ ] **Step 1: Write failing gear-pair and horizon checks**
|
||||
|
||||
Build a fixture whose forward segment ends at `(2,0,0,s=2)`, whose reverse segment starts with a duplicated pose at the same source arc length, and whose direction changes only on the second member of the pair. Assert:
|
||||
|
||||
```text
|
||||
segment 0 end boundary = GearSwitchApproach
|
||||
segment 1 start boundary = GearSwitchDeparture
|
||||
segment identity differs even when pose and source arc length match
|
||||
horizon 1.95 injects an interpolated RollingSafetyStop at exactly 1.95
|
||||
horizon 2.05 for segment 0 still ends at exactly 2.00 and never includes segment 1
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Run the segmentation group and verify failure**
|
||||
|
||||
```powershell
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- segmentation
|
||||
```
|
||||
|
||||
Expected: build failure naming `ReferencePathSegmenter`.
|
||||
|
||||
- [ ] **Step 3: Implement segment views and exact slicing**
|
||||
|
||||
`ReferenceBoundary` identity is the tuple `(SegmentIndex, SegmentLocalS, BoundaryType)`. Rebase every selected segment to `SegmentLocalS=0` without changing the source point. `ReferenceHorizonSlicer.Slice` must interpolate X, Y, unwrapped vehicle yaw, geometric curvature, vehicle curvature, curvature derivative, and clearance at the exact terminal S, then label the anchor `RollingSafetyStop`, `GearSwitch`, or `Goal`.
|
||||
|
||||
Never implement horizon selection by filtering `point.ArcLength <= end`; always append or replace with the exact anchor after bracketing interpolation.
|
||||
|
||||
- [ ] **Step 4: Run segmentation checks**
|
||||
|
||||
Expected: `PASS segmentation`, with assertions covering exact equality within `1e-8 m`.
|
||||
|
||||
- [ ] **Step 5: Commit segmentation**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation ClumsyPilot/tests/EMPlannerVerificationHost
|
||||
git commit -m "feat: preserve EM planner segment boundaries"
|
||||
```
|
||||
|
||||
### Task 4: Reverse-Safe Frenet Projection and Reconstruction
|
||||
|
||||
**Files:**
|
||||
- Create: all files under `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/`
|
||||
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/FrenetChecks.cs`
|
||||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: `DirectionSegmentView` from Task 3.
|
||||
- Produces: interpolation, bounded projection, world/Frenet conversion, and optimized-yaw reconstruction.
|
||||
|
||||
- [ ] **Step 1: Write failing forward, reverse, wraparound, and loop checks**
|
||||
|
||||
Cover these equations directly:
|
||||
|
||||
```text
|
||||
travelYaw = forward ? vehicleYaw : Normalize(vehicleYaw + PI)
|
||||
deltaS = dx*cos(travelYaw) + dy*sin(travelYaw)
|
||||
l = -dx*sin(travelYaw) + dy*cos(travelYaw)
|
||||
x = referenceX - l*sin(travelYaw)
|
||||
y = referenceY + l*cos(travelYaw)
|
||||
optimizedTravelYaw = referenceTravelYaw + atan2(dl, 1-referenceK*l)
|
||||
optimizedVehicleYaw = reverse ? Normalize(optimizedTravelYaw + PI) : optimizedTravelYaw
|
||||
```
|
||||
|
||||
Use a reverse reference whose vehicle yaw is near `-π`, points on both sides of the wrap, and a U-shaped segment with two spatially close branches. Assert the projector remains inside the supplied S interval and chooses the locally nearest branch, then reconstructs the original world point within `1e-8 m`.
|
||||
|
||||
- [ ] **Step 2: Run the Frenet group and verify failure**
|
||||
|
||||
Expected: build failure naming `FrenetProjector`.
|
||||
|
||||
- [ ] **Step 3: Implement deterministic bounded projection**
|
||||
|
||||
Use segment-line projection for the coarse candidate, clamp the interpolation fraction to `[0,1]`, then compare squared world distance. Resolve equal-distance ties by smaller absolute delta from the seed/reference S and then smaller S. Reject projections farther than `MaximumProjectionDistanceMeters`; never search another direction segment.
|
||||
|
||||
`FrenetTransform` rejects reconstruction when `1-referenceK*l < MinimumFrenetDenominator`. Angles used for interpolation are unwrapped; only public yaw is normalized with `AngleMath.NormalizeRadians`.
|
||||
|
||||
- [ ] **Step 4: Run Frenet checks**
|
||||
|
||||
Expected: `PASS frenet`, including reverse `l>0` being left of motion and therefore body-right.
|
||||
|
||||
- [ ] **Step 5: Commit Frenet support**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet ClumsyPilot/tests/EMPlannerVerificationHost
|
||||
git commit -m "feat: add reverse-safe Frenet transforms"
|
||||
```
|
||||
|
||||
### Task 5: Topology-Preserving Static Corridor
|
||||
|
||||
**Files:**
|
||||
- Create: all files under `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor/`
|
||||
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/CorridorChecks.cs`
|
||||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: Task 4 interpolation/transforms, `PlanningGridMap`, `VehicleParameters`, and `FootprintCollisionChecker`.
|
||||
- Produces: immutable `StaticCorridor` stations containing `ReferenceS`, `MinimumL`, `MaximumL`, and seed L.
|
||||
|
||||
- [ ] **Step 1: Write failing corridor connectivity checks**
|
||||
|
||||
Create maps through `PlanningMapFactory`: an empty map, a static rectangle narrowing the left side of a straight path, and an obstacle splitting lateral samples into disconnected left/right intervals. Assert:
|
||||
|
||||
```text
|
||||
empty map corridor = [-0.30, +0.30] at interior stations
|
||||
every accepted lateral sample passes exact rotated-footprint collision checking
|
||||
prior-trajectory seed is used before l=0
|
||||
the chosen interval contains the seed at every station
|
||||
a disappearing seed-connected interval returns false instead of switching sides
|
||||
first and last stations match exact requested ReferenceS anchors
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Run the corridor group and verify failure**
|
||||
|
||||
Expected: build failure naming `StaticCorridorBuilder`.
|
||||
|
||||
- [ ] **Step 3: Implement broad-phase plus exact-footprint sampling**
|
||||
|
||||
At each `0.10 m` S station, sample L in `0.025 m` increments from `-0.30` to `+0.30`, always injecting the exact seed L and exact offset limits. Use the distance field only to accept obviously clear samples; all remaining samples go through `FootprintCollisionChecker.IsPoseCollisionFree` with `AdditionalClearanceReserveMeters=0.02`. Group adjacent free samples, choose only the group containing the seed, and propagate overlap with the previously chosen interval.
|
||||
|
||||
When propagation loses overlap, return failure with the first failed S. Do not select another free group, invert L for reverse, or infer a new obstacle side.
|
||||
|
||||
- [ ] **Step 4: Run all foundation-plan checks**
|
||||
|
||||
```powershell
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- all-foundation
|
||||
```
|
||||
|
||||
Expected output:
|
||||
|
||||
```text
|
||||
PASS foundation
|
||||
PASS segmentation
|
||||
PASS frenet
|
||||
PASS corridor
|
||||
```
|
||||
|
||||
- [ ] **Step 5: Commit corridor support**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor ClumsyPilot/tests/EMPlannerVerificationHost
|
||||
git commit -m "feat: build static EM lateral corridors"
|
||||
```
|
||||
|
||||
### Task 6: Foundation Documentation and Gate
|
||||
|
||||
**Files:**
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md`
|
||||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: all preceding tasks.
|
||||
- Produces: a documented, repeatable foundation verification command for later plans.
|
||||
|
||||
- [ ] **Step 1: Add README contract examples**
|
||||
|
||||
Document the pipeline, coordinate formulas, reverse sign example, exact gear-pair behavior, configuration units, and the command used to run `all-foundation`. State explicitly that the module does not yet solve LS or ST at this gate.
|
||||
|
||||
- [ ] **Step 2: Run format and placeholder checks**
|
||||
|
||||
```powershell
|
||||
git diff --check
|
||||
rg -n "NotImplementedException|throw new Exception\(\)" ClumsyPilot/ParkrobTrajplanner/EMPlanner ClumsyPilot/tests/EMPlannerVerificationHost
|
||||
```
|
||||
|
||||
Expected: `git diff --check` returns no diagnostics and the incomplete-implementation scan returns no matches.
|
||||
|
||||
- [ ] **Step 3: Run the complete foundation gate twice**
|
||||
|
||||
Run `all-foundation` twice. Expected: identical four PASS lines and exit code `0` both times.
|
||||
|
||||
- [ ] **Step 4: Confirm the normal project baseline separately**
|
||||
|
||||
```powershell
|
||||
dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
|
||||
```
|
||||
|
||||
Expected at this repository baseline: only the already-documented legacy `auto_avoidance/MultiWheelAutoAvoidance.cs` missing-reference errors may remain. Any new error under `EMPlanner` fails this gate.
|
||||
|
||||
- [ ] **Step 5: Commit documentation**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs
|
||||
git commit -m "docs: describe EM planner foundation"
|
||||
```
|
||||
|
||||
## Completion Gate
|
||||
|
||||
- `all-foundation` passes twice with deterministic output.
|
||||
- Forward and reverse projection/reconstruction agree within `1e-8 m`.
|
||||
- No segment, projection, horizon, or corridor crosses a gear boundary.
|
||||
- Corridor samples pass exact body collision checks and never change disconnected topology.
|
||||
- No solver, scheduler, hardware access, dynamic prediction, or trajectory publication logic has leaked into this phase.
|
||||
@@ -0,0 +1,334 @@
|
||||
# EM Planner Lateral LS 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:** Optimize a collision-free, curvature-feasible lateral path inside the selected static corridor using sequential convex programming over OSQP QPs.
|
||||
|
||||
**Architecture:** Discretize `l, dl, ddl, dddl` over exact reference-S stations, build normalized quadratic costs and linear integration/corridor constraints, and linearize nonlinear vehicle curvature inside an outer trust-region loop. Reconstruct each accepted candidate in world coordinates, recompute true path arc length, and independently validate it before exposing it to ST.
|
||||
|
||||
**Tech Stack:** C# 10, .NET Standard 2.0, foundation Frenet/corridor types, solver-neutral `IQpSolver`, OSQP backend for integration checks.
|
||||
|
||||
## Global Constraints
|
||||
|
||||
- This plan depends on completion of the foundation and OSQP-backend plans.
|
||||
- LS runs on exactly one current direction segment and uses `ReferenceS` as its independent variable.
|
||||
- `l>0` is left of travel for both forward and reverse; do not reinterpret it as body-left in reverse.
|
||||
- Corridor bounds, maximum lateral offset, trust region, start state, terminal event, Frenet denominator, and vehicle curvature are hard constraints.
|
||||
- Initial derivative bounds: `|Δl|<=0.05 m` per SQP iteration, `|dl|<=0.50`, `|ddl|<=1.00 1/m`, `|dddl|<=2.00 1/m²`.
|
||||
- Enforce `1-referenceK*l >= 0.20` at every knot.
|
||||
- SQP outer-iteration limit is `5`; OSQP iteration limit is `4000`; absolute/relative tolerances are `1e-5`.
|
||||
- Cost weights: reference `10`, heading `1`, second derivative `5`, third derivative `10`, curvature `5`, curvature variation `20`, previous trajectory `5`, rolling terminal `10`.
|
||||
- Every cost term is divided by the square of its physical scale before its weight is applied.
|
||||
- LS scales are maximum lateral offset for L, maximum slope for DL, maximum second derivative for DDL, maximum third derivative for DDDL, vehicle maximum curvature for curvature, and `max(1, reference max |dk/ds|)` for curvature variation.
|
||||
- Gear-switch and goal terminals require `l=0` and `dl=0`; a rolling safety terminal uses a soft terminal penalty.
|
||||
- Only the last independently validated feasible candidate may survive a later timeout or failed outer iteration.
|
||||
- The output world path is re-parameterized by actual `PathS`; later ST code must not use `ReferenceS` as traveled distance.
|
||||
|
||||
---
|
||||
|
||||
## Locked File Structure
|
||||
|
||||
```text
|
||||
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/
|
||||
├── LateralCandidate.cs
|
||||
├── LateralConstraintBuilder.cs
|
||||
├── LateralGeometryEvaluator.cs
|
||||
├── LateralObjectiveBuilder.cs
|
||||
├── LateralPath.cs
|
||||
├── LateralPathPoint.cs
|
||||
├── LateralPlanner.cs
|
||||
├── LateralPlanningInput.cs
|
||||
├── LateralPlanningResult.cs
|
||||
├── LateralSolutionValidator.cs
|
||||
├── LateralVariableLayout.cs
|
||||
└── SequentialConvexOptimizer.cs
|
||||
|
||||
ClumsyPilot/tests/EMPlannerVerificationHost/
|
||||
├── FakeQpSolver.cs
|
||||
├── LateralModelChecks.cs
|
||||
└── LateralIntegrationChecks.cs
|
||||
```
|
||||
|
||||
## Shared Interfaces
|
||||
|
||||
```csharp
|
||||
public sealed class LateralPlanningInput
|
||||
{
|
||||
public LateralPlanningInput(DirectionSegmentView referenceSegment,
|
||||
StaticCorridor corridor, FrenetProjection startProjection,
|
||||
EmTerminalType terminalType, VehicleParameters vehicle,
|
||||
EmPlannerConfiguration configuration,
|
||||
IReadOnlyList<FrenetProjection> previousTrajectorySeed);
|
||||
}
|
||||
|
||||
public sealed class LateralPlanner
|
||||
{
|
||||
public LateralPlanner(IQpSolver qpSolver);
|
||||
public LateralPlanningResult Plan(LateralPlanningInput input,
|
||||
CancellationToken cancellationToken);
|
||||
}
|
||||
|
||||
public sealed class LateralPathPoint
|
||||
{
|
||||
public double ReferenceS { get; }
|
||||
public double PathS { get; }
|
||||
public double L { get; }
|
||||
public double DL { get; }
|
||||
public double DDL { get; }
|
||||
public double DDDL { get; }
|
||||
public double X { get; }
|
||||
public double Y { get; }
|
||||
public double VehicleYaw { get; }
|
||||
public double GeometricCurvature { get; }
|
||||
public double VehicleCurvature { get; }
|
||||
public double VehicleCurvatureDerivative { get; }
|
||||
}
|
||||
```
|
||||
|
||||
### Task 1: Variable Layout and Exact Discrete Lateral Dynamics
|
||||
|
||||
**Files:**
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralVariableLayout.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanningInput.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralCandidate.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPathPoint.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPath.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanningResult.cs`
|
||||
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs`
|
||||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: foundation corridor, reference, configuration, and terminal types.
|
||||
- Produces: deterministic variable indices and immutable lateral inputs/results.
|
||||
|
||||
- [ ] **Step 1: Write failing layout and dynamics checks**
|
||||
|
||||
For `N=4`, assert disjoint contiguous ranges for `l[0..3]`, `dl[0..3]`, `ddl[0..3]`, and `dddl[0..2]`, with total variable count `4*N-1`. For unequal S gaps, verify the integration equations:
|
||||
|
||||
```text
|
||||
ddl[i+1] = ddl[i] + ds*dddl[i]
|
||||
dl[i+1] = dl[i] + ds*ddl[i] + 0.5*ds^2*dddl[i]
|
||||
l[i+1] = l[i] + ds*dl[i] + 0.5*ds^2*ddl[i] + (ds^3/6)*dddl[i]
|
||||
```
|
||||
|
||||
Reject fewer than two stations, non-increasing S, corridor/input station mismatch, and a start projection outside the first hard interval.
|
||||
|
||||
- [ ] **Step 2: Run the lateral-model group and verify failure**
|
||||
|
||||
```powershell
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- lateral-model
|
||||
```
|
||||
|
||||
Expected: build failure naming `LateralVariableLayout`.
|
||||
|
||||
- [ ] **Step 3: Implement layouts and immutable model types**
|
||||
|
||||
Expose index methods `L(i)`, `DL(i)`, `DDL(i)`, and `DDDL(i)` that range-check every input. Copy all station and seed lists. A failed result has no candidate; success and fallback results require a non-empty independently validated `LateralPath`.
|
||||
|
||||
- [ ] **Step 4: Run the model checks**
|
||||
|
||||
Expected: `PASS lateral-model`.
|
||||
|
||||
- [ ] **Step 5: Commit lateral model types**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral ClumsyPilot/tests/EMPlannerVerificationHost
|
||||
git commit -m "feat: add lateral optimization model"
|
||||
```
|
||||
|
||||
### Task 2: Normalized Objective and Linear Hard Constraints
|
||||
|
||||
**Files:**
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralObjectiveBuilder.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralConstraintBuilder.cs`
|
||||
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/FakeQpSolver.cs`
|
||||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: Task 1 layout, `SparseTripletBuilder`, corridor bounds, a linearization candidate, and approved LS weights.
|
||||
- Produces: a validated `QuadraticProgram` for one SQP iteration.
|
||||
|
||||
- [ ] **Step 1: Write failing coefficient-level QP checks**
|
||||
|
||||
For a three-station straight reference with unit scales, inspect P, q, A, lower, and upper arrays and assert:
|
||||
|
||||
```text
|
||||
reference cost adds 2*w_l to P(l_i,l_i)
|
||||
jerk cost adds 2*w_dddl to P(dddl_i,dddl_i)
|
||||
previous-seed cost adds 2*w_previous and -2*w_previous*l_previous
|
||||
every integration equality appears once with equal lower/upper bounds
|
||||
corridor, derivative, trust-region, and Frenet-denominator rows use hard finite bounds
|
||||
gear/goal terminal rows force l_N=0 and dl_N=0
|
||||
rolling terminal adds objective terms but no zero terminal equalities
|
||||
```
|
||||
|
||||
The test must also show every weight is applied after division by its named scale squared.
|
||||
|
||||
- [ ] **Step 2: Run and verify builders are absent**
|
||||
|
||||
Expected: build failure naming `LateralObjectiveBuilder`.
|
||||
|
||||
- [ ] **Step 3: Implement objective and hard-row assembly**
|
||||
|
||||
Build the OSQP objective convention `0.5*x'Px + q'x`, so a squared residual `w*((x-target)/scale)^2` contributes `2w/scale²` to P and `-2w*target/scale²` to q. Assemble integration rows exactly from Task 1. Intersect corridor bounds with maximum offset, trust region, and linearized denominator bounds before adding each L row; return infeasible before calling the solver when an intersection is empty.
|
||||
|
||||
Use `FakeQpSolver` only in the verification host. It records the last problem/settings/warm start and returns a caller-supplied `QpSolveResult`.
|
||||
|
||||
- [ ] **Step 4: Run coefficient-level checks**
|
||||
|
||||
Expected: `PASS lateral-model`; no coefficient comparison tolerance larger than `1e-10`.
|
||||
|
||||
- [ ] **Step 5: Commit QP assembly**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral ClumsyPilot/tests/EMPlannerVerificationHost
|
||||
git commit -m "feat: assemble lateral LS quadratic programs"
|
||||
```
|
||||
|
||||
### Task 3: Nonlinear Geometry Evaluation and Independent Validation
|
||||
|
||||
**Files:**
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralGeometryEvaluator.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralSolutionValidator.cs`
|
||||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: solved `l/dl/ddl/dddl`, Frenet interpolation, direction, and vehicle curvature limit.
|
||||
- Produces: world-space `LateralPath` with recomputed `PathS`, curvature, and validation residuals.
|
||||
|
||||
- [ ] **Step 1: Write failing reconstruction and curvature checks**
|
||||
|
||||
Cover straight and constant-curvature references in both directions. Assert:
|
||||
|
||||
```text
|
||||
world X/Y use x_ref-l*sin(travelYaw), y_ref+l*cos(travelYaw)
|
||||
vehicle yaw adds PI only for reverse
|
||||
PathS[0]=0 and increments by actual reconstructed chord/geometry length
|
||||
PathS is strictly increasing even when ReferenceS gaps vary
|
||||
VehicleCurvature = directionSign*GeometricCurvature
|
||||
yawRate identity remains valid for a signed test speed
|
||||
denominator below 0.20 is rejected
|
||||
curvature beyond vehicle limit is rejected
|
||||
non-finite values are rejected
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Run and verify geometry evaluator is absent**
|
||||
|
||||
Expected: build failure naming `LateralGeometryEvaluator`.
|
||||
|
||||
- [ ] **Step 3: Implement evaluation and strict validation**
|
||||
|
||||
Evaluate geometry from the full Frenet derivative formulas used by the design, not a small-angle replacement. Compute unwrapped travel yaw first, derive geometric curvature with respect to actual path direction, convert to vehicle curvature using direction sign, and compute curvature derivative over actual `PathS`. Use centred differences internally and one-sided endpoints.
|
||||
|
||||
The validator independently recomputes corridor membership, start/terminal residuals, derivative limits, denominator, curvature limit, finite values, and strictly increasing S. It does not trust solver residuals or reuse the QP constraint matrix as its only proof.
|
||||
|
||||
- [ ] **Step 4: Run geometry checks**
|
||||
|
||||
Expected: `PASS lateral-model`, including forward/reverse mirrored cases.
|
||||
|
||||
- [ ] **Step 5: Commit nonlinear evaluation**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs
|
||||
git commit -m "feat: validate lateral path geometry"
|
||||
```
|
||||
|
||||
### Task 4: Sequential Convex Outer Loop and Feasible-Candidate Fallback
|
||||
|
||||
**Files:**
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/SequentialConvexOptimizer.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanner.cs`
|
||||
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/LateralIntegrationChecks.cs`
|
||||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: Tasks 1–3, `IQpSolver`, warm starts, cancellation, and timeout settings.
|
||||
- Produces: `LateralPlanningResult` with the last strict feasible path or an explicit failure.
|
||||
|
||||
- [ ] **Step 1: Write failing SQP state-machine checks with `FakeQpSolver`**
|
||||
|
||||
Script solver outcomes and assert:
|
||||
|
||||
```text
|
||||
first solved candidate is validated before becoming fallback
|
||||
second timeout returns first candidate as SuccessWithFallback
|
||||
an invalid solved vector never replaces the fallback
|
||||
SolvedInaccurate requires QP residual <=1e-5 and full lateral validation
|
||||
trust region is centred on the previous iterate and never exceeds 0.05 m
|
||||
outer loop stops after at most 5 calls
|
||||
cancellation before a call returns Cancelled
|
||||
no feasible candidate plus timeout returns SolverTimedOut with no path
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Run the lateral-integration group and verify failure**
|
||||
|
||||
Expected: build failure naming `SequentialConvexOptimizer`.
|
||||
|
||||
- [ ] **Step 3: Implement the outer loop**
|
||||
|
||||
Initialize from the previous trajectory seed when it covers all stations; otherwise use the corridor-clamped zero-offset seed. Per iteration: linearize geometry, assemble the QP, solve with the remaining time budget, evaluate world geometry, validate independently, store a deep copy if feasible, and test convergence using max absolute L change plus objective improvement. Warm-start the next QP with the complete previous primal vector.
|
||||
|
||||
Return the most specific status. A timeout/cancellation after a validated candidate maps to fallback success; infeasible corridor/QP with no candidate maps to lateral infeasible.
|
||||
|
||||
- [ ] **Step 4: Run scripted SQP checks**
|
||||
|
||||
Expected: `PASS lateral-integration`.
|
||||
|
||||
- [ ] **Step 5: Commit SQP orchestration**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral ClumsyPilot/tests/EMPlannerVerificationHost
|
||||
git commit -m "feat: optimize lateral paths with SQP"
|
||||
```
|
||||
|
||||
### Task 5: Real-OSQP Lateral Scenarios and Gate
|
||||
|
||||
**Files:**
|
||||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/LateralIntegrationChecks.cs`
|
||||
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: `OsqpNativeSolver`, foundation fixtures, and complete LS pipeline.
|
||||
- Produces: a verified lateral path contract ready for ST.
|
||||
|
||||
- [ ] **Step 1: Add fixed real-solver scenarios**
|
||||
|
||||
Run: straight empty map forward, straight empty map reverse, gentle curve, static obstacle narrowing the existing corridor, gear-switch terminal, and rolling terminal. Assert every result is solved or documented fallback, stays in corridor, respects curvature, and ends at the exact ReferenceS anchor.
|
||||
|
||||
- [ ] **Step 2: Add determinism and topology assertions**
|
||||
|
||||
Run each scenario twice with identical inputs. Compare status, point count, and every numeric output within `1e-10`; assert the obstacle case remains in the seed-connected interval and does not cross to the disconnected side.
|
||||
|
||||
- [ ] **Step 3: Run the complete lateral gate**
|
||||
|
||||
```powershell
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- lateral-all
|
||||
git diff --check
|
||||
```
|
||||
|
||||
Expected output:
|
||||
|
||||
```text
|
||||
PASS lateral-model
|
||||
PASS lateral-integration
|
||||
PASS lateral-real-osqp
|
||||
```
|
||||
|
||||
- [ ] **Step 4: Document LS variables, hard constraints, costs, and fallback**
|
||||
|
||||
Add the exact equations, normalization scales, terminal differences, `ReferenceS`/`PathS` boundary, and last-feasible publication rule to the README.
|
||||
|
||||
- [ ] **Step 5: Commit lateral integration evidence**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/tests/EMPlannerVerificationHost/LateralIntegrationChecks.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md
|
||||
git commit -m "test: verify lateral LS scenarios"
|
||||
```
|
||||
|
||||
## Completion Gate
|
||||
|
||||
- Coefficient-level tests prove the intended normalized QP, not merely a plausible output path.
|
||||
- Forward and reverse reconstructed geometry obey the same world-coordinate convention.
|
||||
- Exact gear/goal terminal L conditions and rolling soft terminal behavior are distinct.
|
||||
- No candidate outside hard corridor, denominator, derivative, curvature, or boundary constraints is published.
|
||||
- The published lateral path has actual strictly increasing `PathS` ready for longitudinal optimization.
|
||||
@@ -0,0 +1,394 @@
|
||||
# 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
|
||||
|
||||
```text
|
||||
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
|
||||
|
||||
```csharp
|
||||
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:
|
||||
|
||||
```text
|
||||
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**
|
||||
|
||||
```powershell
|
||||
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**
|
||||
|
||||
```powershell
|
||||
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:
|
||||
|
||||
```text
|
||||
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**
|
||||
|
||||
```powershell
|
||||
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**
|
||||
|
||||
```powershell
|
||||
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:
|
||||
|
||||
```text
|
||||
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**
|
||||
|
||||
```powershell
|
||||
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**
|
||||
|
||||
```powershell
|
||||
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:
|
||||
|
||||
```text
|
||||
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**
|
||||
|
||||
```powershell
|
||||
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:
|
||||
|
||||
```text
|
||||
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:
|
||||
|
||||
```powershell
|
||||
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.
|
||||
@@ -0,0 +1,382 @@
|
||||
# EM Planner OSQP Backend 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:** Provide a solver-independent convex-QP contract and a pinned Windows x64 OSQP 1.0.0 backend that loads `osqp.dll` safely from the plugin directory.
|
||||
|
||||
**Architecture:** Mathematical planners build validated immutable CSC problems against `IQpSolver`; the OSQP adapter owns all native memory and maps native outcomes into planner-neutral statuses. The upstream shared library is built with a fixed ABI configuration, preloaded by absolute path, version-checked, and never allowed to crash the host when absent or incompatible.
|
||||
|
||||
**Tech Stack:** C# 10, .NET Standard 2.0, P/Invoke with Cdecl, OSQP 1.0.0 C API, builtin QDLDL algebra, CMake 3.18+, Visual Studio x64 compiler.
|
||||
|
||||
## Global Constraints
|
||||
|
||||
- This plan depends on `2026-08-03-em-planner-foundation-implementation.md` Task 1 and its verification host.
|
||||
- Pin upstream source tag `v1.0.0`; do not bind the incompatible 0.6 API.
|
||||
- Build Windows x64, double precision, 32-bit indices, unpacked settings, builtin algebra, shared library, no MKL or CUDA.
|
||||
- Required build switches: `OSQP_USE_FLOAT=OFF`, `OSQP_USE_LONG=OFF`, `OSQP_PACK_SETTINGS=OFF`, `OSQP_ALGEBRA_BACKEND=builtin`, `OSQP_BUILD_SHARED_LIB=ON`.
|
||||
- Native library filename in source and deployed plugin is exactly `osqp.dll`.
|
||||
- `ClumsyPilot.dll` locates and preloads the sibling DLL from `Assembly.Location`; current directory and system `PATH` are not inputs.
|
||||
- Every native entry point uses `CallingConvention.Cdecl`.
|
||||
- All pinned arrays, CSC wrappers, settings, and solver handles are released in reverse acquisition order.
|
||||
- `SolvedInaccurate` is publishable only after independent strict residual and domain validation.
|
||||
- Missing DLL, wrong architecture, version mismatch, invalid exports, and setup failure return structured solver outcomes.
|
||||
- Native verbose output is disabled.
|
||||
|
||||
---
|
||||
|
||||
## Locked File Structure
|
||||
|
||||
```text
|
||||
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/
|
||||
├── IQpSolver.cs
|
||||
├── QpSolveResult.cs
|
||||
├── QpSolveStatus.cs
|
||||
├── QpSolverSettings.cs
|
||||
├── QuadraticProgram.cs
|
||||
├── SparseCscMatrix.cs
|
||||
├── SparseTripletBuilder.cs
|
||||
└── Osqp/
|
||||
├── OsqpNativeLoader.cs
|
||||
├── OsqpNativeMethods.cs
|
||||
├── OsqpNativeSolver.cs
|
||||
├── OsqpNativeStructures.cs
|
||||
└── OsqpStatusMapper.cs
|
||||
|
||||
ClumsyPilot/ThirdParty/OSQP/
|
||||
├── build-win-x64.ps1
|
||||
├── LICENSE
|
||||
├── NOTICE
|
||||
├── VERSION
|
||||
├── SHA256SUMS
|
||||
└── win-x64/osqp.dll
|
||||
|
||||
ClumsyPilot/tests/EMPlannerVerificationHost/
|
||||
├── OptimizationChecks.cs
|
||||
└── OsqpChecks.cs
|
||||
```
|
||||
|
||||
## Shared Interfaces
|
||||
|
||||
```csharp
|
||||
public interface IQpSolver
|
||||
{
|
||||
QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings,
|
||||
IReadOnlyList<double> warmStart, CancellationToken cancellationToken);
|
||||
}
|
||||
|
||||
public sealed class QuadraticProgram
|
||||
{
|
||||
public QuadraticProgram(SparseCscMatrix upperTriangularP, IReadOnlyList<double> q,
|
||||
SparseCscMatrix a, IReadOnlyList<double> lowerBounds,
|
||||
IReadOnlyList<double> upperBounds);
|
||||
public int VariableCount { get; }
|
||||
public int ConstraintCount { get; }
|
||||
}
|
||||
|
||||
public sealed class QpSolveResult
|
||||
{
|
||||
public QpSolveStatus Status { get; }
|
||||
public IReadOnlyList<double> Primal { get; }
|
||||
public double Objective { get; }
|
||||
public double PrimalResidual { get; }
|
||||
public double DualResidual { get; }
|
||||
public int Iterations { get; }
|
||||
public TimeSpan SolveTime { get; }
|
||||
public string NativeStatus { get; }
|
||||
public string Diagnostic { get; }
|
||||
}
|
||||
```
|
||||
|
||||
Official references used to lock this ABI:
|
||||
|
||||
- `https://osqp.org/docs/interfaces/C.html`
|
||||
- `https://osqp.org/docs/get_started/migration_guide.html`
|
||||
- `https://github.com/osqp/osqp/tree/v1.0.0`
|
||||
|
||||
### Task 1: Solver-Neutral Sparse QP Contracts
|
||||
|
||||
**Files:**
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/IQpSolver.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/QpSolveResult.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/QpSolveStatus.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/QpSolverSettings.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/QuadraticProgram.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/SparseCscMatrix.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/SparseTripletBuilder.cs`
|
||||
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/OptimizationChecks.cs`
|
||||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: `NumericGuard` and cancellation tokens.
|
||||
- Produces: the shared interfaces above and deterministic sparse-matrix assembly used by LS and ST.
|
||||
|
||||
- [ ] **Step 1: Write failing CSC canonicalization checks**
|
||||
|
||||
Build triplets in shuffled order with duplicate coordinates and assert the resulting CSC matrix:
|
||||
|
||||
```text
|
||||
has ColumnPointers length ColumnCount+1
|
||||
sorts row indices ascending inside each column
|
||||
sums duplicate coordinates
|
||||
drops exact zero sums
|
||||
rejects NaN, infinity, negative indices, and out-of-range indices
|
||||
stores only the upper triangle for P
|
||||
```
|
||||
|
||||
Also construct the micro problem `min 0.5*x^2 - 2*x` subject to `0 <= x <= 1` and assert its immutable arrays cannot be changed through the source lists.
|
||||
|
||||
- [ ] **Step 2: Run and verify solver contracts are absent**
|
||||
|
||||
```powershell
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- optimization
|
||||
```
|
||||
|
||||
Expected: build failure naming `SparseTripletBuilder`.
|
||||
|
||||
- [ ] **Step 3: Implement canonical CSC and QP validation**
|
||||
|
||||
`SparseCscMatrix` stores copied arrays `Values`, `RowIndices`, and `ColumnPointers`. Validate monotonic pointers, `ColumnPointers[0]==0`, final pointer equals nonzero count, and all rows are in range. `QuadraticProgram` enforces square P, matching variable dimensions, matching constraint dimensions, `lower<=upper`, finite coefficients, and bounds limited to `±1e30` rather than CLR infinity.
|
||||
|
||||
Use these exact statuses:
|
||||
|
||||
```csharp
|
||||
public enum QpSolveStatus
|
||||
{
|
||||
Solved,
|
||||
SolvedInaccurate,
|
||||
PrimalInfeasible,
|
||||
DualInfeasible,
|
||||
MaximumIterations,
|
||||
TimeLimit,
|
||||
Cancelled,
|
||||
SolverUnavailable,
|
||||
InvalidProblem,
|
||||
NativeError
|
||||
}
|
||||
```
|
||||
|
||||
- [ ] **Step 4: Run optimization checks**
|
||||
|
||||
Expected: `PASS optimization`.
|
||||
|
||||
- [ ] **Step 5: Commit QP contracts**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization ClumsyPilot/tests/EMPlannerVerificationHost
|
||||
git commit -m "feat: add solver-neutral QP contracts"
|
||||
```
|
||||
|
||||
### Task 2: Reproducible OSQP 1.0.0 Native Package
|
||||
|
||||
**Files:**
|
||||
- Create: all files under `ClumsyPilot/ThirdParty/OSQP/`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: Git, CMake 3.18+, and a Visual Studio x64 compiler.
|
||||
- Produces: a versioned `win-x64/osqp.dll` with a recorded SHA-256 and matching license files.
|
||||
|
||||
- [ ] **Step 1: Write the native build script**
|
||||
|
||||
`build-win-x64.ps1` must create a unique temporary directory, clone only tag `v1.0.0`, configure with this exact command shape, and remove the temporary directory in `finally`:
|
||||
|
||||
```powershell
|
||||
cmake -S $sourceRoot -B $buildRoot -A x64 `
|
||||
-DOSQP_ALGEBRA_BACKEND=builtin `
|
||||
-DOSQP_BUILD_SHARED_LIB=ON `
|
||||
-DOSQP_BUILD_STATIC_LIB=OFF `
|
||||
-DOSQP_BUILD_DEMO_EXE=OFF `
|
||||
-DOSQP_BUILD_UNITTESTS=OFF `
|
||||
-DOSQP_USE_FLOAT=OFF `
|
||||
-DOSQP_USE_LONG=OFF `
|
||||
-DOSQP_PACK_SETTINGS=OFF `
|
||||
-DOSQP_ENABLE_PRINTING=OFF `
|
||||
-DOSQP_CODEGEN=OFF `
|
||||
-DOSQP_ENABLE_DERIVATIVES=OFF
|
||||
cmake --build $buildRoot --config Release --target osqp
|
||||
```
|
||||
|
||||
The script resolves the generated DLL explicitly, verifies exactly one match, copies upstream `LICENSE` and `NOTICE`, writes `VERSION` with tag and build flags, computes `Get-FileHash -Algorithm SHA256`, and writes `SHA256SUMS` using a lowercase hexadecimal digest.
|
||||
|
||||
- [ ] **Step 2: Execute the build script**
|
||||
|
||||
```powershell
|
||||
powershell -ExecutionPolicy Bypass -File ClumsyPilot/ThirdParty/OSQP/build-win-x64.ps1
|
||||
```
|
||||
|
||||
Expected: `win-x64/osqp.dll`, `LICENSE`, `NOTICE`, `VERSION`, and `SHA256SUMS` exist; the script prints `OSQP v1.0.0 win-x64 package ready`.
|
||||
|
||||
- [ ] **Step 3: Verify architecture, exports, and hash**
|
||||
|
||||
Use `dumpbin /headers` to assert machine `x64`, `dumpbin /exports` to assert `osqp_version`, `osqp_setup`, `osqp_solve`, and `osqp_cleanup`, then recompute SHA-256 and compare with `SHA256SUMS`. A missing tool is a failed packaging gate, not a skipped check.
|
||||
|
||||
- [ ] **Step 4: Verify license contents came from the pinned tag**
|
||||
|
||||
Compare bytes against the tag checkout before the temporary checkout is removed. Expected: exact equality for both files.
|
||||
|
||||
- [ ] **Step 5: Commit the reproducible native package**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/ThirdParty/OSQP
|
||||
git commit -m "build: pin OSQP 1.0.0 win-x64"
|
||||
```
|
||||
|
||||
### Task 3: Absolute-Path Native Loader and ABI Structures
|
||||
|
||||
**Files:**
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpNativeLoader.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpNativeMethods.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpNativeStructures.cs`
|
||||
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/OsqpChecks.cs`
|
||||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: the pinned DLL and Task 1 QP contracts.
|
||||
- Produces: a process-lifetime loader plus exact OSQP 1.0.0 double/int32 structures.
|
||||
|
||||
- [ ] **Step 1: Write failing loader checks**
|
||||
|
||||
Copy the verification host output to a temporary `plugins` directory with `ClumsyPilot.dll` and, in separate cases, no native DLL, a text file named `osqp.dll`, and the real DLL. Assert the first two return `SolverUnavailable` diagnostics without `BadImageFormatException` escaping; the real DLL reports version `1.0.0`. Start 16 parallel first-use calls and assert a single stable module handle.
|
||||
|
||||
- [ ] **Step 2: Run the OSQP group without loader implementation**
|
||||
|
||||
Expected: build failure naming `OsqpNativeLoader`.
|
||||
|
||||
- [ ] **Step 3: Implement loader and ABI definitions**
|
||||
|
||||
Use Windows `LoadLibraryW`, `GetProcAddress`, and `FreeLibrary` from `kernel32`; resolve the plugin directory from `typeof(OsqpNativeLoader).Assembly.Location`. Reject `IntPtr.Size != 8`. Preload the absolute sibling path and retain the handle for process lifetime.
|
||||
|
||||
Define `OSQPInt` as C# `int` and `OSQPFloat` as C# `double`, matching the pinned build. Define sequential layouts for `OSQPCscMatrix`, `OSQPSettings`, `OSQPInfo`, `OSQPSolution`, and the four-pointer prefix of `OSQPSolver` exactly as the v1.0.0 public headers specify. Add an internal layout check for expected offsets and total sizes before the first solve.
|
||||
|
||||
Declare only these native functions initially:
|
||||
|
||||
```text
|
||||
osqp_version
|
||||
osqp_set_default_settings
|
||||
osqp_setup
|
||||
osqp_warm_start
|
||||
osqp_solve
|
||||
osqp_cleanup
|
||||
```
|
||||
|
||||
Do not depend on `OSQPCscMatrix_new`, `OSQPCscMatrix_free`, `OSQPSettings_new`, or `OSQPSettings_free`: those helpers are not marked with the public export macro in the pinned header. Allocate the two CSC structures and settings block with `Marshal.AllocHGlobal`, initialize settings through `osqp_set_default_settings`, and release those managed-owned blocks with `Marshal.FreeHGlobal`.
|
||||
|
||||
- [ ] **Step 4: Run missing, corrupt, real, and concurrent loader checks**
|
||||
|
||||
Expected: `PASS osqp-loader` and no process crash.
|
||||
|
||||
- [ ] **Step 5: Commit loader and structures**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp ClumsyPilot/tests/EMPlannerVerificationHost
|
||||
git commit -m "feat: load pinned OSQP native library"
|
||||
```
|
||||
|
||||
### Task 4: OSQP Solve Lifecycle and Status Mapping
|
||||
|
||||
**Files:**
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpNativeSolver.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp/OsqpStatusMapper.cs`
|
||||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/OsqpChecks.cs`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: `IQpSolver`, Task 3 native functions, and `QpSolverSettings`.
|
||||
- Produces: `OsqpNativeSolver : IQpSolver`.
|
||||
|
||||
- [ ] **Step 1: Write failing solve/status checks**
|
||||
|
||||
Test three fixed QPs:
|
||||
|
||||
```text
|
||||
bounded optimum: min 0.5*x^2 - 2*x, 0<=x<=1, expected x=1
|
||||
equality optimum: min x^2+y^2, x+y=1, expected x=y=0.5
|
||||
infeasible: x>=1 and x<=0, expected PrimalInfeasible
|
||||
```
|
||||
|
||||
Assert residuals, iteration count, objective, native status, and solve time are populated. Add a `1e-9 second` time-limit case that maps only to `TimeLimit` or a valid solved status; no native status may be silently treated as solved.
|
||||
|
||||
- [ ] **Step 2: Run and verify `OsqpNativeSolver` is absent**
|
||||
|
||||
Expected: build failure naming `OsqpNativeSolver`.
|
||||
|
||||
- [ ] **Step 3: Implement one-shot native ownership**
|
||||
|
||||
Pin P/Q/A/L/U and optional warm-start arrays; allocate and populate P/A `OSQPCscMatrix` blocks; allocate settings and initialize it through `osqp_set_default_settings`; overwrite `verbose=0`, `warm_starting`, `polishing`, `max_iter`, `eps_abs`, `eps_rel`, and `time_limit`; call setup, optional warm start, solve, then marshal solution and info. Copy all result values before cleanup. Release the solver through `osqp_cleanup`, then settings/matrix blocks through `Marshal.FreeHGlobal`, then array pins in reverse order inside `finally`.
|
||||
|
||||
Map native status values exactly:
|
||||
|
||||
```text
|
||||
1 Solved
|
||||
2 SolvedInaccurate
|
||||
3/4 PrimalInfeasible
|
||||
5/6 DualInfeasible
|
||||
7 MaximumIterations
|
||||
8 TimeLimit
|
||||
9/10/11 NativeError
|
||||
```
|
||||
|
||||
Cancellation is checked before native setup and after solve. OSQP's configured time limit is the bound for a solve already inside native code.
|
||||
|
||||
- [ ] **Step 4: Run all OSQP checks repeatedly**
|
||||
|
||||
```powershell
|
||||
1..20 | ForEach-Object {
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- osqp
|
||||
if ($LASTEXITCODE -ne 0) { throw "OSQP verification failed on iteration $_" }
|
||||
}
|
||||
```
|
||||
|
||||
Expected: every iteration prints `PASS osqp-loader` and `PASS osqp-solve`.
|
||||
|
||||
- [ ] **Step 5: Commit solver lifecycle**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/Optimization/Osqp ClumsyPilot/tests/EMPlannerVerificationHost/OsqpChecks.cs
|
||||
git commit -m "feat: solve QPs through OSQP"
|
||||
```
|
||||
|
||||
### Task 5: Backend Completion Gate
|
||||
|
||||
**Files:**
|
||||
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md`
|
||||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: all tasks in this plan.
|
||||
- Produces: the stable `IQpSolver` boundary required by LS and ST plans.
|
||||
|
||||
- [ ] **Step 1: Document native deployment and diagnostics**
|
||||
|
||||
Add the exact source/deployment layouts, pinned version, build flags, license placement, absolute loading rule, and solver status mapping to the README.
|
||||
|
||||
- [ ] **Step 2: Run optimization and OSQP gates**
|
||||
|
||||
```powershell
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- optimization
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- osqp
|
||||
git diff --check
|
||||
```
|
||||
|
||||
Expected: both groups pass and Git reports no whitespace errors.
|
||||
|
||||
- [ ] **Step 3: Verify the DLL is self-contained**
|
||||
|
||||
Run a dependency inspection on `win-x64/osqp.dll`. Expected: only Windows system/runtime DLLs; no MKL, CUDA, or separately deployed QDLDL DLL.
|
||||
|
||||
- [ ] **Step 4: Verify clean plugin-directory loading**
|
||||
|
||||
Copy only `ClumsyPilot.dll` and `osqp.dll` to a fresh directory, copy the verification host executable beside them, and run the micro QP with the working directory set elsewhere. Expected: solved result, proving loading does not depend on current directory.
|
||||
|
||||
- [ ] **Step 5: Commit backend documentation**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs
|
||||
git commit -m "docs: describe OSQP plugin deployment"
|
||||
```
|
||||
|
||||
## Completion Gate
|
||||
|
||||
- `IQpSolver` contains no OSQP-specific type.
|
||||
- Pinned native metadata, license, notice, hash, and DLL agree with OSQP v1.0.0.
|
||||
- Loader failures are structured and never terminate the host.
|
||||
- Fixed feasible and infeasible QPs map to the correct statuses with finite diagnostics.
|
||||
- Twenty repeated solve/cleanup cycles pass without handle growth or access violations.
|
||||
@@ -0,0 +1,375 @@
|
||||
# EM Planner Rolling Execution and Plugin Deployment 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 version-safe rolling replanning, trajectory handoff, gear-switch execution states, controller adaptation, and deterministic packaging of `ClumsyPilot.dll` with `osqp.dll` and licenses.
|
||||
|
||||
**Architecture:** Keep `EmPlanningService` pure and place scheduling, cancellation, stale-result suppression, previous-trajectory reuse, and command generation in a sibling `TrajectoryExecution` module. The coordinator publishes only a fully validated current-version trajectory; the executor samples that immutable trajectory, manages zero-speed gear transitions, and converts its fields into a generic controller command without coupling EM optimization to the hardware API.
|
||||
|
||||
**Tech Stack:** C# 10, .NET Standard 2.0, `Task`/`CancellationToken`, immutable EM trajectories, PowerShell packaging, Windows x64 plugin layout.
|
||||
|
||||
## Global Constraints
|
||||
|
||||
- This plan depends on all four preceding implementation plans.
|
||||
- Default replan period is `0.20 s`; time horizon `6.0 s`; distance horizon `5.0 m`; handoff lookahead `0.30 s`.
|
||||
- The coordinator accepts captured `VehicleMotionState`; it does not read localization, wheel speed, UI, or hardware directly.
|
||||
- Each cycle binds `MapSnapshotId`, `ReferencePathId`, `VehicleState.SequenceId`, `PreviousTrajectoryId`, and `SegmentIndex`.
|
||||
- A result is publishable only if all bound identities still match the latest cycle and the cycle version is current.
|
||||
- Normal replans hand off from the previous trajectory only when tracking error and age are within configuration limits and no gear boundary is crossed.
|
||||
- If a replan fails, the previously published trajectory remains executable and ends in its own zero-speed safety tail.
|
||||
- Gear change occurs only after measured speed remains below `0.01 m/s` for at least `0.20 s`.
|
||||
- The executor never commands lateral body velocity or in-place rotation.
|
||||
- The generic control command uses signed longitudinal velocity and yaw rate; world `vx/vy`, speed, curvature, and pose remain available for monitoring.
|
||||
- No dynamic-obstacle prediction or dynamic behavior state is introduced.
|
||||
- Plugin runtime layout is `plugins/ClumsyPilot.dll`, `plugins/osqp.dll`, and `plugins/licenses/*`.
|
||||
- Deployment refuses a non-x64 host/runtime package or an OSQP hash mismatch.
|
||||
|
||||
---
|
||||
|
||||
## Locked File Structure
|
||||
|
||||
```text
|
||||
ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/
|
||||
├── EmPlanningCoordinator.cs
|
||||
├── GearSwitchStateMachine.cs
|
||||
├── GearSwitchState.cs
|
||||
├── IEmPlanningCycleSink.cs
|
||||
├── IVehicleStateProvider.cs
|
||||
├── PlanningCycleIdentity.cs
|
||||
├── PlanningCycleInput.cs
|
||||
├── PlanningCycleResult.cs
|
||||
├── TrajectoryControlAdapter.cs
|
||||
├── TrajectoryControlCommand.cs
|
||||
├── TrajectoryExecutionState.cs
|
||||
├── TrajectoryExecutor.cs
|
||||
├── TrajectoryHandoffSelector.cs
|
||||
└── TrajectorySampler.cs
|
||||
|
||||
ClumsyPilot/scripts/
|
||||
└── Publish-ClumsyPilotPlugin.ps1
|
||||
|
||||
ClumsyPilot/tests/EMPlannerVerificationHost/
|
||||
├── CoordinatorChecks.cs
|
||||
├── ExecutorChecks.cs
|
||||
└── PluginPackagingChecks.cs
|
||||
```
|
||||
|
||||
## Shared Interfaces
|
||||
|
||||
```csharp
|
||||
public interface IVehicleStateProvider
|
||||
{
|
||||
VehicleMotionState Capture();
|
||||
}
|
||||
|
||||
public sealed class EmPlanningCoordinator
|
||||
{
|
||||
public EmPlanningCoordinator(IEmPlanningService planningService,
|
||||
IEmPlanningCycleSink sink = null);
|
||||
public Task<PlanningCycleResult> PlanLatestAsync(PlanningCycleInput input,
|
||||
CancellationToken cancellationToken);
|
||||
public EmTrajectory PublishedTrajectory { get; }
|
||||
}
|
||||
|
||||
public sealed class TrajectoryExecutor
|
||||
{
|
||||
public TrajectoryExecutionState State { get; }
|
||||
public TrajectoryControlCommand Update(DateTimeOffset now,
|
||||
VehicleMotionState measuredState, EmTrajectory trajectory);
|
||||
}
|
||||
|
||||
public sealed class TrajectoryControlCommand
|
||||
{
|
||||
public double SignedLongitudinalVelocity { get; }
|
||||
public double YawRate { get; }
|
||||
public TravelDirection Direction { get; }
|
||||
public bool RequestDirectionChange { get; }
|
||||
public bool HoldBrake { get; }
|
||||
public bool IsTrajectoryComplete { get; }
|
||||
}
|
||||
```
|
||||
|
||||
### Task 1: Cycle Identity, Scheduling Decision, and Stale-Result Suppression
|
||||
|
||||
**Files:**
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleIdentity.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleInput.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleResult.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/IEmPlanningCycleSink.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/EmPlanningCoordinator.cs`
|
||||
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs`
|
||||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: pure `IEmPlanningService`, captured request inputs, and configuration scheduling values.
|
||||
- Produces: latest-wins asynchronous planning and a read-only published trajectory.
|
||||
|
||||
- [ ] **Step 1: Write failing coordinator concurrency checks**
|
||||
|
||||
Use a controllable fake planning service to start cycle A, then cycle B before A completes. Complete B first with success and A later with success. Assert B alone is published and A returns `Superseded`. Add checks for map ID, reference ID, state sequence, segment, and prior trajectory ID changes invalidating an otherwise successful result.
|
||||
|
||||
Also assert `ShouldStartCycle(now)` is false before `0.20 s` and true at exactly `0.20 s`; this decision uses caller-supplied time.
|
||||
|
||||
- [ ] **Step 2: Run and verify coordinator types are absent**
|
||||
|
||||
```powershell
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- coordinator
|
||||
```
|
||||
|
||||
Expected: build failure naming `EmPlanningCoordinator`.
|
||||
|
||||
- [ ] **Step 3: Implement latest-wins coordination**
|
||||
|
||||
Use an incrementing `long` cycle version, a private lock only around publication state, and a per-cycle linked cancellation source. Starting a newer cycle cancels the prior source. After planning, compare the complete identity and version again under the publication lock; map stale success to `Superseded` without exposing its trajectory.
|
||||
|
||||
Sink exceptions are caught and reported in the cycle diagnostic. Never hold the publication lock while running the planner or invoking a sink.
|
||||
|
||||
- [ ] **Step 4: Run concurrency checks 100 times**
|
||||
|
||||
```powershell
|
||||
1..100 | ForEach-Object {
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- coordinator
|
||||
if ($LASTEXITCODE -ne 0) { throw "Coordinator verification failed on iteration $_" }
|
||||
}
|
||||
```
|
||||
|
||||
Expected: every iteration prints `PASS coordinator`.
|
||||
|
||||
- [ ] **Step 5: Commit coordinator identity logic**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution ClumsyPilot/tests/EMPlannerVerificationHost
|
||||
git commit -m "feat: coordinate rolling EM replans"
|
||||
```
|
||||
|
||||
### Task 2: Safe Previous-Trajectory Handoff
|
||||
|
||||
**Files:**
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectorySampler.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryHandoffSelector.cs`
|
||||
- Modify: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/EmPlanningCoordinator.cs`
|
||||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: current published trajectory, measured state, latest segment identity, and `0.30 s` handoff lookahead.
|
||||
- Produces: either a future trajectory-derived start state/seed or a measured-state start with no seed.
|
||||
|
||||
- [ ] **Step 1: Write failing handoff acceptance/rejection checks**
|
||||
|
||||
Assert acceptance only when trajectory age is valid, position/yaw/speed tracking errors are inside configured tolerances, the future sample remains on the same segment/direction, and the interval contains no gear boundary. Assert rejection for stale trajectory, large error, terminal proximity, segment mismatch, direction mismatch, and a handoff time beyond the trajectory.
|
||||
|
||||
- [ ] **Step 2: Run and verify selector is absent**
|
||||
|
||||
Expected: build failure naming `TrajectoryHandoffSelector`.
|
||||
|
||||
- [ ] **Step 3: Implement time interpolation and selection**
|
||||
|
||||
Binary-search `TimeFromStart`, interpolate X/Y, unwrapped yaw, signed speed, curvature, and PathS, then derive redundant fields through the trajectory-point constructor. Never interpolate across different boundary types, segment indices, or directions. Return a result object that states `PreviousTrajectory` or `MeasuredState` and includes a deterministic rejection reason.
|
||||
|
||||
- [ ] **Step 4: Run handoff checks**
|
||||
|
||||
Expected: `PASS coordinator`, including a reverse same-segment handoff.
|
||||
|
||||
- [ ] **Step 5: Commit handoff logic**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs
|
||||
git commit -m "feat: select safe EM trajectory handoffs"
|
||||
```
|
||||
|
||||
### Task 3: Gear-Switch State Machine and Trajectory Executor
|
||||
|
||||
**Files:**
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchState.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchStateMachine.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutionState.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutor.cs`
|
||||
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs`
|
||||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: measured state, current time, sampled trajectory, and exact boundary types.
|
||||
- Produces: zero-speed holding, one-shot direction-change request, confirmed departure, and completion states.
|
||||
|
||||
- [ ] **Step 1: Write failing state-transition checks**
|
||||
|
||||
Cover this exact sequence:
|
||||
|
||||
```text
|
||||
Following -> ApproachingGearSwitch
|
||||
ApproachingGearSwitch -> HoldingZero when command reaches boundary
|
||||
HoldingZero remains while |measured speed| >= 0.01 m/s
|
||||
HoldingZero timer resets if speed rises above tolerance
|
||||
HoldingZero -> RequestingDirectionChange after continuous 0.20 s below tolerance
|
||||
RequestingDirectionChange emits exactly one request
|
||||
AwaitingDirectionConfirmation holds zero
|
||||
confirmed direction -> Following next segment
|
||||
goal/rolling terminal -> Completed while holding zero
|
||||
```
|
||||
|
||||
Assert no transition can output nonzero signed speed during holding or direction confirmation.
|
||||
|
||||
- [ ] **Step 2: Run and verify executor types are absent**
|
||||
|
||||
Expected: build failure naming `GearSwitchStateMachine`.
|
||||
|
||||
- [ ] **Step 3: Implement explicit state and event inputs**
|
||||
|
||||
The state machine receives caller-supplied `now`, measured signed speed, desired/current directions, and a boolean direction-confirmation input. It does not call hardware. `TrajectoryExecutor` samples the trajectory, delegates boundary behavior to the state machine, and returns an immutable execution state containing the selected point and reason.
|
||||
|
||||
- [ ] **Step 4: Run executor state checks**
|
||||
|
||||
Expected: `PASS executor`, including forward-to-reverse and reverse-to-forward sequences.
|
||||
|
||||
- [ ] **Step 5: Commit executor state machine**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs
|
||||
git commit -m "feat: execute EM gear-switch boundaries"
|
||||
```
|
||||
|
||||
### Task 4: Generic Control Adapter
|
||||
|
||||
**Files:**
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryControlCommand.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryControlAdapter.cs`
|
||||
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/IVehicleStateProvider.cs`
|
||||
- Modify: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutor.cs`
|
||||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: an `EmTrajectoryPoint` and gear-switch execution state.
|
||||
- Produces: signed linear velocity, yaw rate, direction-change request, brake hold, and completion flags.
|
||||
|
||||
- [ ] **Step 1: Write failing command-mapping checks**
|
||||
|
||||
For forward and reverse points assert command signed velocity and yaw rate equal the point fields exactly. Assert `Speed`, world `vx/vy`, pose, and curvature remain available in execution telemetry but are not reinterpreted as body lateral velocity. Assert holding states always override both command velocities to zero.
|
||||
|
||||
- [ ] **Step 2: Run and verify adapter is absent**
|
||||
|
||||
Expected: build failure naming `TrajectoryControlAdapter`.
|
||||
|
||||
- [ ] **Step 3: Implement adapter without hardware coupling**
|
||||
|
||||
`TrajectoryControlAdapter.CreateCommand` copies signed longitudinal velocity and yaw rate in normal following. For hold, switch, invalid, or completed states it sets both to zero and sets the corresponding flags. Do not reference `MultiVehicleScriptVx`, `MultiVehicleScriptVy`, or `MultiVehicleScriptVth`; a later hardware-specific adapter may map this generic command after the existing controller field semantics are confirmed.
|
||||
|
||||
- [ ] **Step 4: Run adapter and executor checks**
|
||||
|
||||
Expected: `PASS executor`; add an invariant that ordinary following never requests nonzero body lateral velocity or `v=0, yawRate!=0`.
|
||||
|
||||
- [ ] **Step 5: Commit control adaptation**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs
|
||||
git commit -m "feat: adapt EM trajectories to control commands"
|
||||
```
|
||||
|
||||
### Task 5: Plugin Output and License Packaging
|
||||
|
||||
**Files:**
|
||||
- Modify: `ClumsyPilot/ClumsyPilot.csproj`
|
||||
- Create: `ClumsyPilot/scripts/Publish-ClumsyPilotPlugin.ps1`
|
||||
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/PluginPackagingChecks.cs`
|
||||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: a successfully built `ClumsyPilot.dll` plus the pinned OSQP package.
|
||||
- Produces: the exact deployable `plugins` tree.
|
||||
|
||||
- [ ] **Step 1: Write failing packaging checks**
|
||||
|
||||
Create a temporary output directory, invoke the future script, and assert exactly:
|
||||
|
||||
```text
|
||||
plugins/ClumsyPilot.dll
|
||||
plugins/osqp.dll
|
||||
plugins/licenses/OSQP-LICENSE.txt
|
||||
plugins/licenses/OSQP-NOTICE.txt
|
||||
plugins/licenses/OSQP-VERSION.txt
|
||||
```
|
||||
|
||||
Assert the deployed native hash equals `SHA256SUMS`, `ClumsyPilot.dll` is a managed assembly, OSQP is x64, and rerunning packaging replaces files without leaving stale temporary files.
|
||||
|
||||
- [ ] **Step 2: Run and verify packaging script is absent**
|
||||
|
||||
Expected: `PluginPackagingChecks` fails because `Publish-ClumsyPilotPlugin.ps1` does not exist.
|
||||
|
||||
- [ ] **Step 3: Add build-output metadata and transactional publish script**
|
||||
|
||||
Add these items to `ClumsyPilot.csproj` without rewriting existing targets:
|
||||
|
||||
```xml
|
||||
<None Include="ThirdParty\OSQP\win-x64\osqp.dll"
|
||||
Link="osqp.dll" CopyToOutputDirectory="PreserveNewest" />
|
||||
<None Include="ThirdParty\OSQP\LICENSE"
|
||||
Link="licenses\OSQP-LICENSE.txt" CopyToOutputDirectory="PreserveNewest" />
|
||||
<None Include="ThirdParty\OSQP\NOTICE"
|
||||
Link="licenses\OSQP-NOTICE.txt" CopyToOutputDirectory="PreserveNewest" />
|
||||
<None Include="ThirdParty\OSQP\VERSION"
|
||||
Link="licenses\OSQP-VERSION.txt" CopyToOutputDirectory="PreserveNewest" />
|
||||
```
|
||||
|
||||
The publish script takes mandatory `-ManagedDll` and `-OutputDirectory`, resolves both absolute paths, validates inputs and hash, stages the five files in a unique sibling temporary directory, then renames the completed `plugins` directory into place. It refuses to operate when the resolved output is a drive root or workspace root.
|
||||
|
||||
- [ ] **Step 4: Run packaging checks against a fresh directory**
|
||||
|
||||
```powershell
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- plugin-package
|
||||
```
|
||||
|
||||
Expected: `PASS plugin-package`; no files exist outside the temporary test root.
|
||||
|
||||
- [ ] **Step 5: Commit packaging**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/ClumsyPilot.csproj ClumsyPilot/scripts/Publish-ClumsyPilotPlugin.ps1 ClumsyPilot/tests/EMPlannerVerificationHost
|
||||
git commit -m "build: package ClumsyPilot with OSQP"
|
||||
```
|
||||
|
||||
### Task 6: Rolling End-to-End and Final Gate
|
||||
|
||||
**Files:**
|
||||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs`
|
||||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs`
|
||||
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: all core, coordinator, executor, and packaging components.
|
||||
- Produces: a complete first-version EM Planner workflow with safe rolling fallback.
|
||||
|
||||
- [ ] **Step 1: Add deterministic rolling scenarios**
|
||||
|
||||
Simulate caller-supplied time and measured states for: normal repeated forward replans, reverse replans, a solver failure with old-trajectory continuation, a superseded slow cycle, tracking-error reset to measured state, forward/reverse gear switch, and final goal stop. Assert every executed command comes from a currently published validated trajectory or is a zero hold.
|
||||
|
||||
- [ ] **Step 2: Add failure-tail assertions**
|
||||
|
||||
Force all new plans to fail after a successful publication. Advance time through the old trajectory and assert it reaches its exact zero-speed terminal and stays zero; no extrapolated nonzero command is allowed after its final point.
|
||||
|
||||
- [ ] **Step 3: Run the full first-version gate**
|
||||
|
||||
```powershell
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-all
|
||||
git diff --check
|
||||
```
|
||||
|
||||
Expected groups include foundation, OSQP, lateral, longitudinal, trajectory, facade, coordinator, executor, and plugin packaging, all with PASS output.
|
||||
|
||||
- [ ] **Step 4: Update README with ownership and deployment**
|
||||
|
||||
Document the pure-planner/coordinator/executor boundary, caller responsibilities, update cadence, handoff rules, gear state sequence, all trajectory fields, generic controller command, OSQP files, packaging command, and explicitly deferred dynamic-obstacle scope.
|
||||
|
||||
- [ ] **Step 5: Commit rolling integration evidence**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/tests/EMPlannerVerificationHost ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md
|
||||
git commit -m "test: verify rolling EM execution"
|
||||
```
|
||||
|
||||
## Completion Gate
|
||||
|
||||
- One slow cycle can never overwrite a newer published trajectory.
|
||||
- All handoffs remain within one segment/direction and use measured state when tracking is unsafe.
|
||||
- Failed replans leave a complete prior trajectory that terminates safely at zero.
|
||||
- Gear changes require measured zero-speed dwell and emit one explicit request.
|
||||
- Generic control output never invents crab or in-place-rotation behavior.
|
||||
- Packaging produces the exact DLL/license tree and validates the pinned native hash.
|
||||
Reference in New Issue
Block a user