docs: record EM full-direction phase 02 handoff
This commit is contained in:
梁薄云
@@ -1,153 +1,34 @@
|
|||||||
# EM 完整方向段规划与可视化修复：阶段 02 交接
|
# EM Complete Direction Visualization — Phase 02 Handoff
|
||||||
|
|
||||||
## 状态
|
## Status
|
||||||
|
|
||||||
阻塞。
|
Complete on 2026-08-06.
|
||||||
|
|
||||||
## 阶段目标与非目标
|
Implementation commit: `3269d55` — `feat: derive adaptive full-segment ST schedule`
|
||||||
|
|
||||||
- 目标：仅执行总实施计划 Task 3，从实际 Local G2 `PathS` 与速度/停车包络导出完整方向段的自适应 ST 优化结点和 `T_end`，并保持优化结点与发布采样分离。
|
## Delivered scope
|
||||||
- 非目标：不实现静止起步、`NoProgress`、终端世界位姿、MovementTest、Web 或 Painter 工作。
|
|
||||||
|
|
||||||
## 启动与前置核验
|
- Full-direction ST optimization knots and `T_end` are derived from the actual Local G2 `PathS` speed/stopping envelope and physical jerk/acceleration reachability.
|
||||||
|
- `LongitudinalKnotSchedule` is an optimization-only contract: knot times, `PathS`/speed references, and an explicit terminal-hold start index. It intentionally contains no reference candidate.
|
||||||
|
- Full exact stops first execute a bounded deterministic S/U/A/J feasibility QP through the injected `IQpSolver`. It uses nonuniform exact dynamics, actual-`PathS` speed-envelope relinearization, initial/terminal/hold equalities, hard acceleration/jerk bounds, and a shared solver budget.
|
||||||
|
- Only a strictly validated candidate can become a fallback. A timeout before one exists returns `SolverTimedOut`; a later timeout returns `SuccessWithFallback`.
|
||||||
|
- Full-scope publication resamples the optimized candidate and does not append the rolling zero-speed hold twice. Rolling-horizon publication behavior remains unchanged.
|
||||||
|
|
||||||
- 分支：`trajplanner`
|
## Tests
|
||||||
- 启动 HEAD：`f32560e2f1b2046f968e30def0919a7fe6bdeccd`（`docs: record EM full-direction phase 01 handoff`）
|
|
||||||
- 阶段 01 实现提交：`f4e89b4b4fe67f4924420ba8b3e06dea1d4ccc8b`、`048b4f618e2237cd0cb3d257bf6ee04a1d13670b`
|
|
||||||
- 阶段 01 交接提交：`f32560e2f1b2046f968e30def0919a7fe6bdeccd`
|
|
||||||
- `git merge-base --is-ancestor` 对提示词列出的七个提交均返回退出码 0：`f4e89b4…`、`048b4f6…`、`fcf7df1…`、`57ea36b…`、`c354f11…`、`dff223c…`、`46d5f97…`。
|
|
||||||
|
|
||||||
## 已进行的 TDD 与证据
|
The following commands were run sequentially with exit code 0:
|
||||||
|
|
||||||
1. 初始 RED：
|
|
||||||
|
|
||||||
```powershell
|
|
||||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
|
|
||||||
```
|
|
||||||
|
|
||||||
退出码 1；预期的 CS0246/CS1501，原因是缺少 `LongitudinalKnotSchedule`、`FullDirectionSegmentScheduleBuilder` 与 PathS 包络重载。没有静止起步、终端位姿或 MovementTest 失败。
|
|
||||||
|
|
||||||
2. 在允许的阶段 02 文件中完成了未提交的最小实现尝试，模型验证一度返回退出码 0：`PASS longitudinal-model`。初始集成验证也一度返回退出码 0：`PASS longitudinal-integration`。构建始终显示项目既有两条过时 API 警告（`Lidar2dDetect2LegTray.LegWidth` 与 `MultiWheelChassis.GetSteerWheels()`）。
|
|
||||||
|
|
||||||
3. 为满足 Task 3 的发布采样独立性要求，`LongitudinalIntegrationChecks` 加入真实 `EmTrajectoryAssembler` 断言：将 `OutputTimeStepSeconds` 从 `0.10` 改为 `0.05`，优化结点数保持不变，同时发布区间数应由 2 变为 4。最新复现：
|
|
||||||
|
|
||||||
```powershell
|
|
||||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-integration
|
|
||||||
```
|
|
||||||
|
|
||||||
退出码 1；首个有效失败为：`halving publication cadence doubles emitted trajectory intervals without changing optimization knots expected 4 but was 2`，位置为 `LongitudinalIntegrationChecks.cs:106`。
|
|
||||||
|
|
||||||
## 根因、影响与已排除原因
|
|
||||||
|
|
||||||
- `TrajectorySampleSchedule.cs` 的第 23–33 行以 `candidate.KnotTimes` 逐点加入发布样本；`outputTimeStepSeconds` 只在第 44–63 行计算终端稳定尾和零速 hold。`EmTrajectoryAssembler.cs:42` 仅构造该 schedule。因此 `OutputTimeStepSeconds` 不会重采样行驶区间。
|
|
||||||
- 这与 Task 3 “发布仍使用 `Scheduling.OutputTimeStepSeconds`”以及 Step 7 “0.10 改 0.05 使发布密度翻倍、但 `KnotSchedule.KnotTimes.Count` 不变”的验收要求冲突。
|
|
||||||
- 根因不在新 schedule 的结点派生：同一集成测试已确认两个 full schedule 的 `KnotTimes.Count` 相同。也不涉及静止起步、`NoProgress`、终端世界位姿、MovementTest、Web、Painter 或硬件写输出。
|
|
||||||
- 安全影响：不能将未按配置发布分辨率重采样的完整方向段轨迹宣称为阶段 02 成功；没有执行器、底盘、转向、制动、电机或档位写入被添加或调用。
|
|
||||||
|
|
||||||
## 阻塞原因与精确恢复范围
|
|
||||||
|
|
||||||
阶段 02 允许清单不包含以下必须修改的发布文件：
|
|
||||||
|
|
||||||
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs`
|
|
||||||
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs`
|
|
||||||
|
|
||||||
在没有用户明确授权将这两个文件加入阶段 02（或将发布重采样验收移动到另一批准阶段）的情况下，不得修改它们，也不得削弱失败断言。恢复窗口只处理此冲突：先取得书面范围决定，再在获准范围内使真实发布按 `OutputTimeStepSeconds` 重采样、保留精确终点且不改变优化结点，随后重跑模型和集成验证。不得推进到阶段 03。
|
|
||||||
|
|
||||||
## 未提交的阶段 02 工作区内容
|
|
||||||
|
|
||||||
以下都是本阶段尝试，未暂存、未提交；恢复窗口应在此基础上继续或按明确决定调整，不能误当作用户内容：
|
|
||||||
|
|
||||||
- 新建：`LongitudinalKnotSchedule.cs`、`FullDirectionSegmentScheduleBuilder.cs`
|
|
||||||
- 修改：`PathSpeedLimitBuilder.cs`、`LongitudinalPlanningInput.cs`、`LongitudinalConstraintBuilder.cs`、`LongitudinalSolutionValidator.cs`、`SequentialLongitudinalOptimizer.cs`、`LongitudinalPreviousTrajectorySeedBuilder.cs`、`EmPlanningService.cs`
|
|
||||||
- 修改测试：`LongitudinalModelChecks.cs`、`LongitudinalIntegrationChecks.cs`
|
|
||||||
|
|
||||||
尚未创建实现提交 `feat: derive adaptive full-segment ST schedule`；不得把这些未完成 hunk 暂存或提交为成功实现。
|
|
||||||
|
|
||||||
## 脏工作区与暂存保护
|
|
||||||
|
|
||||||
- 启动前和当前暂存区均为空；未使用 `git stash`、`git reset --hard`、`git checkout --`、`git add .` 或 `git add -A`。
|
|
||||||
- `EmPlannerConfiguration.cs` 的用户既有 `DistanceHorizonMeters = 500d` 与中文注释已在启动时保存 diff，未编辑、未暂存；该文件仍为脏。
|
|
||||||
- `ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs` 的既有修改未编辑、未暂存。
|
|
||||||
- 大量其他用户脏修改与未跟踪文件仍保留，未清理或覆盖。
|
|
||||||
|
|
||||||
## 下一步
|
|
||||||
|
|
||||||
本阶段为阻塞状态，不产生阶段 03 输入或提示词。恢复只允许处理上节的发布采样范围冲突；恢复成功后才补全阶段 02 GREEN、回归、实现提交与完成交接。
|
|
||||||
|
|
||||||
## 恢复续行记录
|
|
||||||
|
|
||||||
用户已在本交接之后明确授权将 `TrajectorySampleSchedule.cs` 和 `EmTrajectoryAssembler.cs` 纳入阶段 02。恢复窗口在前者中实现了 full scope 的按 `OutputTimeStepSeconds` 行驶段重采样，并在后者中只对 `FullDirectionSegment` 启用它；`RollingHorizon` 保留原有逐 ST-knot 发布。上述源文件仍未暂存、未提交。
|
|
||||||
|
|
||||||
发布采样 RED 已转为 GREEN：`longitudinal-integration` 退出码 0，输出 `PASS longitudinal-integration`。随后新鲜 `em-core-all` 通过 `longitudinal-model`、`longitudinal-integration` 和 `trajectory`，但在 `EmPlanningServiceChecks.VerifiesFullScopePublishesItsRequestScope` 失败。
|
|
||||||
|
|
||||||
当前首个有效失败是其 `ScriptedPipelineSolver` 用 `problem.VariableCount > 100` 识别纵向 QP。极短完整方向段的自适应 schedule 合理地产生少于该阈值的 knot，夹具于是将真实纵向 QP 当作横向并返回全零 primal，最终出现：`ST candidate does not satisfy the exact initial state`。该夹具位于 `ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs`，不在原始 Task 3 允许测试文件中，也未包含在本次两个发布源文件的授权中。
|
|
||||||
|
|
||||||
继续前必须取得用户对该一个测试文件的明确扩展授权；只允许把测试求解器的 QP 分类改为与 knot 数无关的真实结构判定，并新增或调整 full adaptive service regression。不得通过人为增加完整段 knot 数、改变资源上限或回退到滚动 schedule 来绕过夹具。取得授权后，从以下命令复现：
|
|
||||||
|
|
||||||
```powershell
|
```powershell
|
||||||
|
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
|
||||||
|
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-integration
|
||||||
|
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory
|
||||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-core-all
|
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-core-all
|
||||||
```
|
```
|
||||||
|
|
||||||
## 第二次恢复续行记录
|
`em-core-all` was rerun after the implementation commit and passed. The only warnings were existing obsolete API warnings for `Lidar2dDetect2LegTray.LegWidth` and `MultiWheelChassis.GetSteerWheels()`.
|
||||||
|
|
||||||
用户随后明确授权修改 `EmPlanningServiceChecks.cs`。该夹具已将纵向 QP 识别从 `VariableCount > 100` 改为布局结构判定（`4N-1` 变量及至少 `8N-2` 约束），从而不再把短完整段的纵向问题误判为横向问题。
|
## Worktree safeguards
|
||||||
|
|
||||||
在该修复后，真实问题暴露出来：
|
The existing user changes in `EmPlannerConfiguration.cs` and `MovementTest.TrajectoryObservationTest.cs`, together with unrelated dirty changes, were not edited or staged.
|
||||||
|
|
||||||
1. `FullDirectionSegmentScheduleBuilder` 最初在到达 `s_end/v=0` 时立即结束。为满足 Task 3 的零速保持与现有 exact-stop 稳定尾，已在该 schedule 后追加以 `MaximumOptimizationTimeStepSeconds` 切分的 `ZeroSpeedHoldSeconds` 零速 knot。
|
Phase 03+ work (static start, NoProgress, terminal world pose, MovementTest, Web, Painter) remains out of scope.
|
||||||
2. 该改动后，`longitudinal-model` 与 `longitudinal-integration` 都返回退出码 0；`em-core-all` 已通过 `trajectory`。
|
|
||||||
3. 但 full-scope 服务回归仍失败。脚本求解器返回的现有 `CreateExactStopSeed` warm start 在自适应时间 knot 上违反实际 PathS 速度包络（例如 `S=0.006326835275506704, U=0.0328891548874453, limit=0.030034851566717233`）。
|
|
||||||
4. 为排除脚本夹具，full-scope 测试临时改用真实 OSQP：追加 hold 前它报告 infeasible；追加 hold 后在默认服务夹具预算内达到 `maximum iterations reached`（4000 iterations，primal residual `0.00018704349099181022`）。因此不能将问题归因于夹具。
|
|
||||||
|
|
||||||
这说明阶段 02 的 schedule 消费路径尚未完成：`SequentialLongitudinalOptimizer.CreateExactStopSeed` 仍按旧的固定网格 seed 假设生成候选，未能生成与自适应 `input.KnotSchedule` 和实际 PathS 速度包络同时严格一致的 exact-stop seed。继续通过调整 fixture、增大 knot 数、放宽资源上限、回退滚动模式或改变阶段 03 静止起步逻辑都不安全且不符合计划。
|
|
||||||
|
|
||||||
恢复前需要对 seed 策略作出明确技术决定：允许阶段 02 在其已授权的 `SequentialLongitudinalOptimizer.cs`、`LongitudinalConstraintBuilder.cs`、`LongitudinalSolutionValidator.cs` 和允许测试文件内，设计并实现一个由 `input.KnotSchedule.ReferencePathS/ReferenceSpeedMetersPerSecond` 驱动、严格满足动态/速度包络/终端稳定尾的 full-scope exact-stop 初始候选；或由用户批准替代的等价最小策略。恢复窗口必须只处理该策略，不得进入阶段 03。
|
|
||||||
|
|
||||||
## 第三次恢复续行记录：可行动力学 reference 的架构阻塞
|
|
||||||
|
|
||||||
本恢复窗口重新执行了系统化根因调查并新增了 `LongitudinalModelChecks.VerifiesFullDirectionScheduleCarriesAFeasibleReferenceCandidate`。它要求完整方向段 schedule 显式携带与非均匀 knot 一致、满足真实 PathS 包络和 exact-stop hold 的 `ReferenceCandidate`，并验证改变发布周期不改变优化 knot 或 hold 边界。
|
|
||||||
|
|
||||||
首个有效 RED 是缺少 `ReferenceCandidate` 与 `TerminalHoldStartIndex` 的编译错误。实现这两个 schedule 契约后，最新有效失败可稳定复现为：
|
|
||||||
|
|
||||||
```powershell
|
|
||||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
|
|
||||||
```
|
|
||||||
|
|
||||||
失败断言：`adaptive schedule constructs a strict reference candidate`。诊断显示尝试的完整段 jerk reference 在真实 PathS 速度包络处被拒绝，例如 `S=0.4730000000000003, U=0.09, limit=0.08987114317838311`。这不是静止起步、终端世界位姿、MovementTest、Web 或 Painter 问题。
|
|
||||||
|
|
||||||
已排除的原因与尝试：
|
|
||||||
|
|
||||||
- 不是 scripted QP 分类夹具；此前已用真实 OSQP 复现无严格 warm start 时的不可行/超时。
|
|
||||||
- 不是发布周期耦合；此前 `longitudinal-integration` 已证明 0.10/0.05 发布密度独立于优化 knot。
|
|
||||||
- 不是单个 jerk 参数；已尝试最大 jerk 巡航后制动、全程三角 jerk、提前降速并接最大 jerk 停车，均在空间包络处失败。
|
|
||||||
- 不能把 `PathSpeedLimitBuilder` 的停车包络从“最大正加速度”改为零加速度基线：既有 `VerifiesFinitePathSIndexedSpeedEnvelope` 立即失败，证明那会破坏已定义的保守停车模型契约。
|
|
||||||
|
|
||||||
根因是当前 Phase 02 schedule 只持有 `S/U` 参考，却同时要求一个严格 `S/U/A/J` candidate；而 `PathSpeedLimitBuilder` 的保守空间停止包络与任一单相/手工拼接 jerk profile 不等价。旧 `TryCreateExactJerkSeed` 也只在优化器内事后修补非均匀 schedule，不能作为 schedule 的可行性证明。继续调节 profile、容差、knot 数、资源上限或测试夹具会掩盖这一契约缺口。
|
|
||||||
|
|
||||||
安全影响：没有执行器、底盘、转向、制动、电机或档位写入被添加或调用；但在没有严格 candidate 的情况下，完整方向段不得发布为成功。
|
|
||||||
|
|
||||||
精确恢复范围：仅在 Phase 02 已授权 Longitudinal 文件内定义一个由完整 `PathSpeedLimit` 与非均匀 `KnotTimes` 联合求解的 deterministic feasibility projection（其输出为严格 `S/U/A/J` reference candidate），再由 schedule/optimizer/validator 共用该结果。该 projection 必须保留 `PathSpeedLimitBuilder` 的既有保守包络契约，不能改为零加速度包络；必须显式携带停车 hold 起点；不得实现 Phase 03 的静止起步、NoProgress 或终端世界位姿门禁。恢复前应先为该 projection 写独立 RED，而不是继续调节现有单相 profile。
|
|
||||||
|
|
||||||
## 第四次恢复续行记录：非均匀 projection 仍无严格候选
|
|
||||||
|
|
||||||
用户确认恢复后，新增 RED 将微小完整段（`s_end=0.0075m`、`v0=0.05m/s`、`a0=0`、`a_max=1e-6`、`d_max=1`、`j_max=20`）直接送入 full adaptive schedule 和一个立即超时的 fake QP solver。验收要求是：QP 不返回 primal 时，优化器仍可发布严格验证过的 exact-stop fallback。
|
|
||||||
|
|
||||||
已完成并验证的最小改动：完整 schedule 现在显式记录 `TerminalHoldStartIndex`，constraint/validator/full exact-stop seed 不再从 `OutputTimeStepSeconds` 推断该边界；该 RED 随后推进到真正的 seed 可行性失败。
|
|
||||||
|
|
||||||
最新复现命令仍为：
|
|
||||||
|
|
||||||
```powershell
|
|
||||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
|
|
||||||
```
|
|
||||||
|
|
||||||
首个有效失败：`adaptive schedule creates a strict exact-stop fallback`，结果为 `SolverTimedOut` 而不是 `SuccessWithFallback`，因为没有严格 candidate 可作为 fallback。当前非均匀运动 knot 为：
|
|
||||||
|
|
||||||
`0, 0.020599672032065634, 0.06747741703657942, 0.10775263976184658, 0.14254459069441072, 0.1783398120069701, 0.22441002701746862, 0.30000000000000004`；hold 从 index 7 开始并延续到 `0.5`。
|
|
||||||
|
|
||||||
已排除：
|
|
||||||
|
|
||||||
- 仅将 schedule 终端时长提升到 `3 * JerkLimitedStoppingProfile.DurationSeconds = 0.3s`；仍无严格候选。
|
|
||||||
- 将 `TryCreateExactJerkSeed` 的基线由旧 `CreateApproachSeed` 改为 `KnotSchedule.ReferenceSpeedMetersPerSecond` 驱动的常 jerk积分；仍无严格候选。
|
|
||||||
- 旧 fixed-grid 对照 `0, 0.1, 0.2, 0.3, 0.4` 可行，说明问题是非均匀 grid 上的约束满足，而不是该车辆状态本身不可停车。
|
|
||||||
|
|
||||||
因此下一恢复窗口不得继续调节时长倍率、reference-speed 基线、jerk 搜索采样数或容差。需要明确实现一个真正的 deterministic constrained feasibility projection：直接以 `S/U/A/J` 决策变量、非均匀 exact dynamics、既有保守 PathS speed bounds、初始/终端/hold 等式和 jerk/acceleration bounds求解可行点；不能复用旧的 endpoint-nullspace coordinate-search 作为该 projection。该实现需要一个可审计的 deterministic active-set/投影算法，或用户明确批准将已注入的 QP solver 作为“初始可行性 QP”并相应调整测试 solver 语义。任何选择都必须保持在 Phase 02，不得进入静止起步、NoProgress、终端世界位姿或 UI。
|
|
||||||
|
|||||||
Reference in New Issue
Block a user