From f19034db1e8c1bdf0ce5c33b1477ecee3502d2ae Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Thu, 30 Jul 2026 15:44:37 +0800 Subject: [PATCH] docs: plan local G2 path presmoothing --- .../2026-07-30-local-g2-path-presmoothing.md | 1504 +++++++++++++++++ 1 file changed, 1504 insertions(+) create mode 100644 docs/superpowers/plans/2026-07-30-local-g2-path-presmoothing.md diff --git a/docs/superpowers/plans/2026-07-30-local-g2-path-presmoothing.md b/docs/superpowers/plans/2026-07-30-local-g2-path-presmoothing.md new file mode 100644 index 0000000..3e4fbd4 --- /dev/null +++ b/docs/superpowers/plans/2026-07-30-local-g2-path-presmoothing.md @@ -0,0 +1,1504 @@ +# Local G2 Path Presmoothing 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:** 在 Hybrid A* 与 SQP 之间新增局部 G2 五次 Hermite 预平滑主路线，安全地改善可处理的运动基元曲率跳变，并输出完整、可诊断的 SQP 初始路径。 + +**Architecture:** 新路线作为 `PathSmoothingService` 中的专用分支实现，不塞入旧的“强度重试”运行器。`LocalG2` 子目录负责曲率事件检测、窗口规划、五次 Hermite 几何、候选构造与评价；现有 `PathGeometryAnalyzer` 和 `SmoothedPathValidator` 继续作为统一几何和完整车体安全真源。 + +**Tech Stack:** C# 10、.NET Standard 2.0、现有规划栅格和完整车体碰撞检查器、PowerShell 反射验证脚本、Git。 + +**Design spec:** `docs/superpowers/specs/2026-07-30-local-g2-path-presmoothing-design.md` + +## Global Constraints + +- 不修改 Hybrid A* 的几何中心参考模型、搜索代价或运动基元生成策略。 +- 不实现 SQP、速度规划、轮速分解或每点速度上限。 +- 前进/倒车是硬分段；绝不跨换向点计算或平滑曲率。 +- 单事件窗口默认 `0.20 m`、`0.50 m`、`0.80 m`，允许左右不对称。 +- 最大局部路径偏移默认 `0.10 m`。 +- 曲率事件检测门槛为 `max(0.001 1/m, 0.05 × 最大车辆曲率)`。 +- 成功区域的峰值 `|dκ/ds|` 至少改善 `20%`。 +- 曲率变化代价只允许 `2%` 数值容差。 +- 每个区域最多评估 `12` 个确定性候选。 +- 输出采样间距和连续碰撞检查步长默认均为 `0.025 m`。 +- 扩大车体之外仍需保留默认 `0.02 m` 净空。 +- 旧 B 样条、局部 Bézier和旧五次算法不重写；其现有测试必须保持通过。 +- 所有新行为先写失败测试，再写最小实现；每个任务单独提交。 +- 工作区已有未提交修改。每次暂存必须显式列出本任务文件，禁止覆盖或顺带提交无关改动。 + +--- + +## File Structure + +### Public contracts + +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/LocalG2QuinticOptions.cs` + 新主路线的语义化配置和默认值。 +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionStatus.cs` + 单区域“已改善/保留原路径”状态。 +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionFailureReason.cs` + 稳定的区域失败原因枚举。 +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionReport.cs` + 不可变区域报告。 +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothingMethod.cs` + 追加 `LocalG2Quintic`，不改变旧成员顺序。 +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingStatus.cs` + 追加 `Complete`、`PartialImprovement`、`NotNeeded`、`Unchanged`。 +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPointSource.cs` + 追加 `LocalG2Transition`。 +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPoint.cs` + 新增 `VehicleCurvatureDerivative`，保留旧构造函数兼容性。 +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathQualityMetrics.cs` + 新增峰值曲率变化率和“曲率变化代价”别名。 +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingResult.cs` + 新增区域报告和本地主路线发布工厂。 +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingConfiguration.cs` + 暴露 `LocalG2Quintic` 配置。 +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRequest.cs` + 深复制新配置。 + +### Local G2 implementation + +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2OptionsSnapshot.cs` + 已校验的不可变配置快照。 +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransition.cs` + 单个同方向运动基元曲率事件。 +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransitionDetector.cs` + 从原始粗路径检测事件。 +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2SmoothingRegion.cs` + 一个独立或合并后的局部区域。 +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2WindowPlanner.cs` + 生成长度和左右分配均有界的区域候选。 +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/QuinticHermiteCurve2D.cs` + 纯二维五次 Hermite 数学。 +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateGeometry.cs` + 一个待验证的局部替换几何。 +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs` + 用方向、端点曲率和内部共享曲率生成有限候选。 +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PathSplicer.cs` + 将局部替换拼入完整 `PreparedPath`。 +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateEvaluator.cs` + 统一分析、偏移、质量、碰撞、净空和确定性排序。 +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PreSmoothingPipeline.cs` + 按区域运行、局部回退、全局复验并生成最终状态和报告。 + +### Shared infrastructure + +- Modify: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Output/CoarsePathAssembler.cs` + 保留真实起点车辆曲率。 +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalysis.cs` + 暴露峰值 `|dκ/ds|`。 +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs` + 在方向段内统一计算每点 `dκ/ds` 和峰值。 +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PreparedDirectionSegment.cs` + 可选保留方向段起点的真实车辆曲率边界条件。 +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathSmoothingPreprocessor.cs` + 将粗路径首点曲率传入方向段边界条件。 +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs` + 粗路径也使用统一几何分析器。 +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Validation/SmoothedPathValidator.cs` + 校验并保留每点 `dκ/ds`。 +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs` + 验证新配置并分派至专用流水线。 +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingComparisonService.cs` + 适配统一粗路径基线签名；默认比较列表仍只有旧三种算法。 +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/README.md` + 记录新主路线、状态和 SQP 边界。 + +### Verification + +- Modify: `ClumsyPilot/tests/verify_coarse_path_integration.ps1` +- Modify: `ClumsyPilot/tests/verify_path_smoothing_contracts.ps1` +- Modify: `ClumsyPilot/tests/verify_path_smoothing_geometry.ps1` +- Create: `ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1` +- Create: `ClumsyPilot/tests/verify_path_smoothing_local_g2_curve.ps1` +- Create: `ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1` +- Create: `ClumsyPilot/tests/verify_path_smoothing_local_g2_integration.ps1` +- Modify: `ClumsyPilot/tests/verify_path_smoothing_documentation.ps1` + +--- + +### Task 1: Add Local G2 public contracts and immutable configuration + +**Files:** + +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/LocalG2QuinticOptions.cs` +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionStatus.cs` +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionFailureReason.cs` +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionReport.cs` +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothingMethod.cs` +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingStatus.cs` +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPointSource.cs` +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPoint.cs` +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathQualityMetrics.cs` +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingResult.cs` +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingConfiguration.cs` +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRequest.cs` +- Test: `ClumsyPilot/tests/verify_path_smoothing_contracts.ps1` + +**Interfaces:** + +- Produces: `SmoothingMethod.LocalG2Quintic`. +- Produces: `PathSmoothingStatus.Complete`, `PartialImprovement`, `NotNeeded`, `Unchanged`. +- Produces: `LocalG2QuinticOptions` with the exact defaults from Global Constraints. +- Produces: `SmoothedPathPoint.VehicleCurvatureDerivative`. +- Produces: `PathSmoothingResult.PublishLocalG2(...)`. +- Produces: immutable `PathSmoothingResult.RegionReports`. + +- [ ] **Step 1: Extend the contract verification first** + +Append assertions equivalent to: + +```powershell +$localOptionsType = Get-RequiredType ($root + 'LocalG2QuinticOptions') +$regionStatusType = Get-RequiredType ($root + 'PathSmoothingRegionStatus') +$regionFailureType = Get-RequiredType ($root + 'PathSmoothingRegionFailureReason') +$regionReportType = Get-RequiredType ($root + 'PathSmoothingRegionReport') + +Assert-Equal 'CubicBSpline,LocalCubicBezier,PiecewiseQuintic,LocalG2Quintic' ` + ([string]::Join(',', [Enum]::GetNames($methodType))) ` + 'The Local G2 method must be appended without reordering legacy methods.' +Assert-True ([Enum]::GetNames($statusType) -contains 'Complete') 'Complete status is required.' +Assert-True ([Enum]::GetNames($statusType) -contains 'PartialImprovement') 'Partial status is required.' +Assert-True ([Enum]::GetNames($statusType) -contains 'NotNeeded') 'NotNeeded status is required.' +Assert-True ([Enum]::GetNames($statusType) -contains 'Unchanged') 'Unchanged status is required.' + +$local = [Activator]::CreateInstance($localOptionsType) +Assert-Near 0.20 $local.MinimumWindowLengthMeters 'Minimum Local G2 window must be 0.20 m.' +Assert-Near 0.50 $local.PreferredWindowLengthMeters 'Preferred Local G2 window must be 0.50 m.' +Assert-Near 0.80 $local.MaximumWindowLengthMeters 'Maximum Local G2 window must be 0.80 m.' +Assert-Near 0.10 $local.MaximumDeviationMeters 'Maximum Local G2 deviation must be 0.10 m.' +Assert-Near 0.001 $local.AbsoluteCurvatureJumpFloorPerMeter 'Absolute jump floor must be 0.001 1/m.' +Assert-Near 0.05 $local.CurvatureJumpRatioOfMaximum 'Relative jump threshold must be 5 percent.' +Assert-Near 0.20 $local.MinimumPeakGradientImprovementRatio 'Peak improvement must be 20 percent.' +Assert-Near 0.02 $local.MaximumVariationCostRegressionRatio 'Variation cost tolerance must be 2 percent.' +Assert-Equal 12 $local.MaximumCandidatesPerRegion 'At most twelve candidates are allowed.' + +Assert-True ($pointType.GetProperty('VehicleCurvatureDerivative') -ne $null) ` + 'Smoothed points must expose d-kappa/d-s.' +Assert-True ($resultType.GetProperty('RegionReports') -ne $null) ` + 'Local G2 results must expose immutable region reports.' +``` + +Also mutate every new option on the original configuration after building a `PathSmoothingRequest`, then assert the request snapshot still contains defaults. Repeat through the configuration getter to prove defensive copying. + +- [ ] **Step 2: Run the unchanged assembly against the new contract test** + +Run: + +```powershell +dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_contracts.ps1 +``` + +Expected: build passes; contract script fails because `LocalG2QuinticOptions` or the new enum members do not exist. + +- [ ] **Step 3: Implement the public options and enum additions** + +Create the options exactly as: + +```csharp +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing; + +public sealed class LocalG2QuinticOptions +{ + public double MinimumWindowLengthMeters { get; set; } = 0.20d; + public double PreferredWindowLengthMeters { get; set; } = 0.50d; + public double MaximumWindowLengthMeters { get; set; } = 0.80d; + public double MaximumDeviationMeters { get; set; } = 0.10d; + public double AbsoluteCurvatureJumpFloorPerMeter { get; set; } = 0.001d; + public double CurvatureJumpRatioOfMaximum { get; set; } = 0.05d; + public double MinimumPeakGradientImprovementRatio { get; set; } = 0.20d; + public double MaximumVariationCostRegressionRatio { get; set; } = 0.02d; + public int MaximumCandidatesPerRegion { get; set; } = 12; +} +``` + +Append, rather than reorder, enum members: + +```csharp +public enum PathSmoothingRegionStatus +{ + Improved, + RetainedOriginal, +} + +public enum PathSmoothingRegionFailureReason +{ + None, + WindowUnavailable, + CandidateGenerationFailed, + Collision, + InsufficientClearance, + CurvatureExceeded, + CurvatureOvershoot, + DeviationExceeded, + InsufficientImprovement, + VariationCostRegression, + GlobalValidationRollback, +} +``` + +Add `LocalG2Transition` at the end of `SmoothedPathPointSource`. Add a new `SmoothedPathPoint` constructor containing `vehicleCurvatureDerivativePerSquareMeter` immediately after vehicle curvature; retain the old constructor and delegate to the new one with `0d`. + +- [ ] **Step 4: Implement immutable report and result publication** + +Use this exact public report constructor: + +```csharp +public PathSmoothingRegionReport( + int segmentIndex, + double startArcLengthMeters, + double endArcLengthMeters, + IReadOnlyList curvatureJumpsPerMeter, + double plannedWindowLengthMeters, + double actualWindowLengthMeters, + double leftWindowLengthMeters, + double rightWindowLengthMeters, + int candidateCount, + int selectedCandidateIndex, + PathSmoothingRegionStatus status, + PathSmoothingRegionFailureReason failureReason, + double rawPeakCurvatureDerivativePerSquareMeter, + double resultPeakCurvatureDerivativePerSquareMeter, + double rawCurvatureVariationCost, + double resultCurvatureVariationCost, + double maximumDeviationMeters, + double minimumBodyClearanceMeters, + double maximumAbsoluteVehicleCurvaturePerMeter) +``` + +Use this factory boundary: + +```csharp +public static PathSmoothingResult PublishLocalG2( + PathSmoothingStatus status, + IReadOnlyList path, + IReadOnlyList segments, + PathSmoothingDiagnostics diagnostics, + IReadOnlyList regionReports) +``` + +Allow only `Complete`, `PartialImprovement`, `NotNeeded`, and `Unchanged`; require feasible diagnostics and a non-empty path. `Failure` continues to publish an empty path. Existing `Success` and `Fallback` factories remain source-compatible and publish an empty immutable report collection. + +`PathSmoothingRegionReport` must defensively copy its curvature-jump list. Use `SelectedCandidateIndex = -1` when no candidate was selected. + +Add `MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter` to `PathQualityMetrics`. Retain the old constructor and delegate to a new constructor that accepts the peak derivative immediately after maximum absolute curvature. Keep: + +```csharp +public double CurvatureVariationCost => CurvatureVariationEnergy; +``` + +as a compatibility-safe name for new code and reports. + +- [ ] **Step 5: Copy and validate the new configuration** + +Add: + +```csharp +public LocalG2QuinticOptions LocalG2Quintic { get; } = new LocalG2QuinticOptions(); +``` + +to `PathSmoothingConfiguration`. Copy every new property in `PathSmoothingRequest.CopyConfiguration`; do not reuse the caller-owned options object. + +- [ ] **Step 6: Build and run the contract test** + +Run: + +```powershell +dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_contracts.ps1 +``` + +Expected: build succeeds and output ends with `Path smoothing contract checks passed.` + +- [ ] **Step 7: Commit only Task 1 files** + +```powershell +git add -- ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/LocalG2QuinticOptions.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionStatus.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionFailureReason.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionReport.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothingMethod.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingStatus.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPointSource.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPoint.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathQualityMetrics.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingResult.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingConfiguration.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRequest.cs ` + ClumsyPilot/tests/verify_path_smoothing_contracts.ps1 +git diff --cached --check +git commit -m "feat: add local G2 smoothing contracts" +``` + +--- + +### Task 2: Preserve the true Hybrid A* start curvature + +**Files:** + +- Modify: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Output/CoarsePathAssembler.cs` +- Modify: `ClumsyPilot/tests/verify_coarse_path_integration.ps1` + +**Interfaces:** + +- Consumes: `PlanningRequest.StartVehicleCurvature`. +- Produces: `PlanningResult.Path[0].VehicleCurvature == StartVehicleCurvature`. +- Later tasks rely on the first point to expose a real start-to-first-primitive transition. + +- [ ] **Step 1: Add a failing end-to-end assertion** + +Before the first successful `Plan` call in `verify_coarse_path_integration.ps1`, set: + +```powershell +$request.StartVehicleCurvature = [double]0.20 +``` + +After success, add: + +```powershell +Assert-Near 0.20 $result.Path[0].VehicleCurvature ` + 'The assembled start point must retain the requested physical vehicle curvature.' +``` + +- [ ] **Step 2: Run the regression test and verify RED** + +Run: + +```powershell +dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_coarse_path_integration.ps1 +``` + +Expected: fail at the new start-curvature assertion because the assembler currently publishes the first primitive curvature at index zero. + +- [ ] **Step 3: Make the minimal assembler change** + +Replace: + +```csharp +double currentCurvature = backtrackedPath.Primitives.Count > 0 + ? backtrackedPath.Primitives[0].CurvaturePerMeter + : backtrackedPath.StartCurvaturePerMeter; +``` + +with: + +```csharp +double currentCurvature = backtrackedPath.StartCurvaturePerMeter; +``` + +Do not change `currentDirection`: direction still comes from the first primitive when one exists. + +- [ ] **Step 4: Run CoarsePath regression checks** + +Run: + +```powershell +dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_coarse_path_integration.ps1 +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_coarse_path_search.ps1 +``` + +Expected: both scripts end in their `... checks passed.` messages. + +- [ ] **Step 5: Commit** + +```powershell +git add -- ` + ClumsyPilot/ParkrobTrajplanner/CoarsePath/Output/CoarsePathAssembler.cs ` + ClumsyPilot/tests/verify_coarse_path_integration.ps1 +git diff --cached --check +git commit -m "fix: preserve coarse path start curvature" +``` + +--- + +### Task 3: Compute `dκ/ds` once and use one fair raw-path analyzer + +**Files:** + +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalysis.cs` +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs` +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PreparedDirectionSegment.cs` +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathSmoothingPreprocessor.cs` +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs` +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Validation/SmoothedPathValidator.cs` +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs` +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingComparisonService.cs` +- Modify: `ClumsyPilot/tests/verify_path_smoothing_geometry.ps1` +- Modify: `ClumsyPilot/tests/verify_path_smoothing_validation.ps1` + +**Interfaces:** + +- Produces: `PathGeometryAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter`. +- Produces: each analyzed `SmoothedPathPoint.VehicleCurvatureDerivative`. +- Produces: optional `PreparedDirectionSegment.StartVehicleCurvaturePerMeter`, used only when a real boundary state is available. +- Produces: `RawPathBaselineBuilder.TryCreate(..., PathGeometryAnalyzer analyzer, double outputSpacingMeters, ...)`. +- Guarantees: no derivative crosses a gear-switch duplicate. + +- [ ] **Step 1: Add failing geometry assertions** + +Extend `verify_path_smoothing_geometry.ps1`: + +```powershell +Assert-Near 0.0 $straightAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter 0.000000001 ` + 'A straight must have zero peak d-kappa/d-s.' +foreach ($point in $straightAnalysis.Path) { + Assert-Near 0.0 $point.VehicleCurvatureDerivative 0.000000001 ` + 'A straight point must carry zero d-kappa/d-s.' +} + +Assert-True (-not [double]::IsNaN($switchLeft.VehicleCurvatureDerivative)) ` + 'The forward side of a gear switch must have a finite one-sided derivative.' +Assert-True (-not [double]::IsNaN($switchRight.VehicleCurvatureDerivative)) ` + 'The reverse side of a gear switch must have a finite one-sided derivative.' +Assert-Near 0.0 $reverseStraightStart.VehicleCurvatureDerivative 0.000000001 ` + 'No curvature derivative may cross from the preceding forward arc into a reverse straight.' + +$physicalStart = New-DirectionSegmentWithStartCurvature 0 $forward 0.20 @( + (New-GeometryPoint 0.0 0.0 0.0 0.0 0.0), + (New-GeometryPoint 1.0 0.0 1.0 0.0 0.0)) +$physicalStartAnalysis = Invoke-Analysis @($physicalStart) +Assert-Near 0.20 $physicalStartAnalysis.Path[0].VehicleCurvature 0.000000001 ` + 'The unified analyzer must retain a real start steering-curvature boundary state.' +Assert-True ($physicalStartAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter -gt 0.0) ` + 'A real start-curvature mismatch must remain visible to the quality analyzer.' +``` + +Extend validation tests to assert that a nonzero derivative survives clearance recomputation unchanged. + +- [ ] **Step 2: Run tests and verify RED** + +```powershell +dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_geometry.ps1 +``` + +Expected: fail because analysis and points do not expose the derivative. + +- [ ] **Step 3: Preserve real direction-segment start curvature** + +Add a constructor overload: + +```csharp +public PreparedDirectionSegment( + int segmentIndex, + TravelDirection direction, + IReadOnlyList points, + bool startsAtGearSwitch, + bool endsAtGearSwitch, + double? startVehicleCurvaturePerMeter) +``` + +The existing five-argument constructor delegates with `null`, preserving legacy algorithms. Validate a supplied value as finite and expose it as `double? StartVehicleCurvaturePerMeter`. + +`PathSmoothingPreprocessor` must call the new overload with the original `CoarsePathPoint.VehicleCurvature` at that direction segment’s first raw index. Local G2 splicing in Task 6 must carry the value forward unchanged. + +- [ ] **Step 4: Compute curvature derivatives per direction segment** + +In `TryAnalyzeSegment`, first compute all `vehicleCurvatures`, then compute: + +```csharp +if (count == 1) +{ + curvatureDerivatives[index] = 0d; +} +else if (index == 0) +{ + curvatureDerivatives[index] = + (vehicleCurvatures[1] - vehicleCurvatures[0]) / + (localArcLengths[1] - localArcLengths[0]); +} +else if (index == count - 1) +{ + curvatureDerivatives[index] = + (vehicleCurvatures[index] - vehicleCurvatures[index - 1]) / + (localArcLengths[index] - localArcLengths[index - 1]); +} +else +{ + curvatureDerivatives[index] = + (vehicleCurvatures[index + 1] - vehicleCurvatures[index - 1]) / + (localArcLengths[index + 1] - localArcLengths[index - 1]); +} +``` + +Reject non-finite derivatives. Track the maximum absolute derivative. Construct output points only after both arrays are complete. Keep all calculations inside the current direction segment loop. + +After ordinary geometric curvature is computed, but before `dκ/ds`, apply a finite start-boundary override: + +```csharp +if (segment.StartVehicleCurvaturePerMeter.HasValue) +{ + double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d; + vehicleCurvatures[0] = segment.StartVehicleCurvaturePerMeter.Value; + geometricCurvatures[0] = directionSign * vehicleCurvatures[0]; +} +``` + +This is required because the vehicle’s physical start steering state is not recoverable from the first outgoing chord. Both raw and candidate paths pass through this same analyzer and the same boundary override. + +Rename only the user-facing description from “energy” to “cost”; retain the existing `CurvatureVariationEnergy` property as a compatibility alias: + +```csharp +public double CurvatureVariationCost => CurvatureVariationEnergy; +``` + +- [ ] **Step 5: Replace the unfair raw baseline** + +Change `RawPathBaselineBuilder.TryCreate` to analyze `preparedPath.Segments` through the same `PathGeometryAnalyzer` and `outputSpacingMeters`, then validate that analyzed path: + +```csharp +if (!analyzer.TryAnalyze( + preparedPath.Segments, + outputSpacingMeters, + out PathGeometryAnalysis analysis, + out reason)) +{ + return false; +} + +if (!validator.TryValidate( + analysis.Path, + analysis.Segments, + preparedPath, + request.Map, + request.Vehicle, + maximumCollisionCheckStepMeters, + out IReadOnlyList safePath, + out double minimumClearanceMeters, + out reason)) +{ + return false; +} +``` + +Build baseline metrics from `analysis`, including peak derivative, instead of recomputing from stored Hybrid A* curvature. Update all three callers with one shared analyzer and `configuration.OutputSpacingMeters`. + +- [ ] **Step 6: Preserve derivatives through validation** + +Update `SmoothedPathValidator.IsValidPoint` to require a finite derivative. When rebuilding points with checked clearance, copy `VehicleCurvatureDerivative` rather than falling back to the legacy constructor. + +- [ ] **Step 7: Run focused and legacy tests** + +```powershell +dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_geometry.ps1 +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_validation.ps1 +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_service.ps1 +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_comparison.ps1 +``` + +Expected: all four scripts pass. If old comparison percentages change because the baseline is now fair, update assertions to the newly unified calculations, not to hard-coded legacy stored-curvature values. + +- [ ] **Step 8: Commit** + +```powershell +git add -- ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalysis.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PreparedDirectionSegment.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathSmoothingPreprocessor.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Validation/SmoothedPathValidator.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingComparisonService.cs ` + ClumsyPilot/tests/verify_path_smoothing_geometry.ps1 ` + ClumsyPilot/tests/verify_path_smoothing_validation.ps1 +git diff --cached --check +git commit -m "fix: unify path smoothing geometry metrics" +``` + +--- + +### Task 4: Detect curvature events and plan bounded merged windows + +**Files:** + +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2OptionsSnapshot.cs` +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransition.cs` +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransitionDetector.cs` +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2SmoothingRegion.cs` +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2WindowPlanner.cs` +- Create: `ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1` + +**Interfaces:** + +- Produces: + +```csharp +internal bool CurvatureTransitionDetector.TryDetect( + PathSmoothingRequest request, + double maximumVehicleCurvaturePerMeter, + LocalG2OptionsSnapshot options, + out IReadOnlyList transitions, + out string reason); +``` + +- Produces: + +```csharp +internal bool LocalG2WindowPlanner.TryPlan( + PreparedPath originalPath, + IReadOnlyList transitions, + LocalG2OptionsSnapshot options, + out IReadOnlyList regions, + out string reason); +``` + +- `CurvatureTransition` exposes these immutable values: + +```csharp +int SegmentIndex +int LeftCoarsePathIndex +int RightCoarsePathIndex +double LocalArcLengthMeters +double X +double Y +double VehicleHeadingRadians +double LeftVehicleCurvaturePerMeter +double RightVehicleCurvaturePerMeter +double CurvatureJumpPerMeter +``` + +- Define `LocalG2WindowVariant` in the region file with `CandidateIndex`, `StartArcLengthMeters`, `EndArcLengthMeters`, `LeftWindowLengthMeters`, and `RightWindowLengthMeters`. `LocalG2SmoothingRegion` exposes `SegmentIndex`, ordered `Transitions`, the maximum merged range, and ordered `IReadOnlyList WindowVariants`. +- Guarantees: transition arc length is the left/ending point of the old primitive, not the first sampled point inside the new primitive. + +- [ ] **Step 1: Write detection and window tests** + +The new reflection script must cover these scenarios: + +```powershell +# Same direction: 0 -> 0.4167 is detected once. +Assert-Equal 1 $singleTransition.TransitionCount 'One primitive curvature jump must be detected.' +Assert-Near 0.4167 $singleTransition.MaximumJump 0.0001 'The jump magnitude must be retained.' + +# Same pose at a forward/reverse boundary: no event crosses the stop. +Assert-Equal 0 $gearSwitch.TransitionCount 'A stopped gear switch must not be a smoothing event.' + +# A 0.01 1/m numerical change is below max(0.001, 5% of 0.8333). +Assert-Equal 0 $noise.TransitionCount 'Sub-threshold curvature noise must be ignored.' + +# Two 0.50 m windows whose ranges overlap are merged. +Assert-Equal 1 $overlap.RegionCount 'Overlapping windows must form one joint region.' +Assert-Equal 2 $overlap.TransitionCountInFirstRegion 'The merged region must retain both events.' + +# Near a segment start, the window becomes asymmetric without crossing the hard boundary. +Assert-Near 0.0 $nearStart.StartArcLength 0.000000001 'A start window must be clamped to the segment.' +Assert-True ($nearStart.RightWindowLength -gt $nearStart.LeftWindowLength) ` + 'Unavailable left length must be shifted to the right.' +``` + +Expose a deterministic `public static TestHooks.Execute(string scenario)` snapshot on `CurvatureTransitionDetector` so the script does not need to instantiate internal event types directly. + +- [ ] **Step 2: Run and verify RED** + +```powershell +dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1 +``` + +Expected: fail because the Local G2 detector does not exist. + +- [ ] **Step 3: Implement strict option validation** + +`LocalG2OptionsSnapshot` must reject: + +```csharp +minimum <= 0 +preferred < minimum +maximum < preferred +maximumDeviation <= 0 +absoluteJumpFloor <= 0 +jumpRatio <= 0 || jumpRatio > 1 +minimumImprovement <= 0 || minimumImprovement >= 1 +costRegression < 0 +maximumCandidates < 1 +``` + +Copy scalar values into readonly properties. Do not retain `LocalG2QuinticOptions`. + +- [ ] **Step 4: Implement event detection** + +For each `PathSegment`, compare adjacent raw points only within its inclusive index range: + +```csharp +double threshold = Math.Max( + options.AbsoluteCurvatureJumpFloorPerMeter, + options.CurvatureJumpRatioOfMaximum * maximumVehicleCurvaturePerMeter); + +double delta = right.VehicleCurvature - left.VehicleCurvature; +if (Math.Abs(delta) >= threshold) +{ + double segmentStartArc = coarsePath[segment.StartIndex].ArcLength; + transitions.Add(new CurvatureTransition( + segment.SegmentIndex, + leftIndex, + rightIndex, + left.ArcLength - segmentStartArc, + left.X, + left.Y, + left.Heading, + left.VehicleCurvature, + right.VehicleCurvature)); +} +``` + +Never compare `segment.EndIndex` with the next segment start. Validate finite values and nondecreasing arc length. + +- [ ] **Step 5: Implement window variants and merging** + +Generate deterministic total-length targets in this order, removing duplicates after clamping: + +```text +preferred +0.75 × preferred +1.25 × preferred +minimum +maximum +``` + +For each target, start with a 50/50 split. If one side reaches a hard boundary, shift the missing length to the other side without exceeding the target or segment length. Also generate 40/60 and 60/40 variants when both are legal. Stop after `MaximumCandidatesPerRegion`. + +For region discovery, use each event's maximum legal window so that no later candidate can overlap a neighboring region that was treated independently. Sort by segment and start arc; merge ranges that overlap or touch within `1e-9 m`. A merged `LocalG2SmoothingRegion` retains all ordered events and all legal window variants. + +- [ ] **Step 6: Run tests** + +```powershell +dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1 +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_geometry.ps1 +``` + +Expected: both scripts pass. + +- [ ] **Step 7: Commit** + +```powershell +git add -- ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2OptionsSnapshot.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransition.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransitionDetector.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2SmoothingRegion.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2WindowPlanner.cs ` + ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1 +git diff --cached --check +git commit -m "feat: detect local curvature transition regions" +``` + +--- + +### Task 5: Implement the pure quintic Hermite geometry + +**Files:** + +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/QuinticHermiteCurve2D.cs` +- Create: `ClumsyPilot/tests/verify_path_smoothing_local_g2_curve.ps1` + +**Interfaces:** + +- Produces: + +```csharp +internal static bool QuinticHermiteCurve2D.TryCreate( + double x0, double y0, double dx0, double dy0, double ddx0, double ddy0, + double x1, double y1, double dx1, double dy1, double ddx1, double ddy1, + out QuinticHermiteCurve2D curve, + out string reason); + +internal void Evaluate( + double u, + out double x, out double y, + out double dx, out double dy, + out double ddx, out double ddy); +``` + +- Guarantees: exact endpoint position, first derivative and second derivative within `1e-10`. + +- [ ] **Step 1: Write the math tests** + +Add scenarios for a straight and a curved boundary: + +```powershell +$straight = Invoke-Curve 0 0 1 0 0 0 2 0 1 0 0 0 +Assert-Near 0.0 $straight.Start.X 1e-12 'Start X must match.' +Assert-Near 2.0 $straight.End.X 1e-12 'End X must match.' +Assert-Near 0.0 $straight.Mid.Y 1e-12 'A straight Hermite curve must remain on the axis.' + +$curved = Invoke-Curve 0 0 1 0 0 0.4 1 1 0 1 -0.4 0 +Assert-Near 0.4 $curved.Start.Curvature 1e-10 'Start curvature must match boundary derivatives.' +Assert-Near 0.4 $curved.End.Curvature 1e-10 'End curvature must match boundary derivatives.' +Assert-True $curved.AllFinite 'All sampled values must be finite.' + +Assert-Rejected (Invoke-InvalidCurve 'ZeroFirstDerivative') ` + 'A curve endpoint with zero first derivative must be rejected.' +``` + +Use curvature: + +```text +(dx * ddy - dy * ddx) / (dx² + dy²)^(3/2) +``` + +- [ ] **Step 2: Run and verify RED** + +```powershell +dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_curve.ps1 +``` + +Expected: fail because `QuinticHermiteCurve2D` is absent. + +- [ ] **Step 3: Implement the coefficient solver** + +For each coordinate, compute: + +```csharp +a0 = p0; +a1 = v0; +a2 = acceleration0 / 2d; +double c0 = p1 - (a0 + a1 + a2); +double c1 = v1 - (a1 + 2d * a2); +double c2 = acceleration1 - 2d * a2; +a3 = 10d * c0 - 4d * c1 + 0.5d * c2; +a4 = -15d * c0 + 7d * c1 - c2; +a5 = 6d * c0 - 3d * c1 + 0.5d * c2; +``` + +Evaluate with Horner form. Validate `u ∈ [0,1]`, finite inputs and nonzero endpoint derivative norms. The math class must not know about maps, vehicles, SQP or public path contracts. + +- [ ] **Step 4: Run tests** + +```powershell +dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_curve.ps1 +``` + +Expected: `Local G2 quintic curve checks passed.` + +- [ ] **Step 5: Commit** + +```powershell +git add -- ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/QuinticHermiteCurve2D.cs ` + ClumsyPilot/tests/verify_path_smoothing_local_g2_curve.ps1 +git diff --cached --check +git commit -m "feat: add quintic Hermite curve geometry" +``` + +--- + +### Task 6: Build and splice deterministic Local G2 candidates + +**Files:** + +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateGeometry.cs` +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs` +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PathSplicer.cs` +- Create: `ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1` + +**Interfaces:** + +- Produces: + +```csharp +internal IReadOnlyList Build( + PreparedDirectionSegment originalSegment, + LocalG2SmoothingRegion region, + double outputSpacingMeters, + LocalG2OptionsSnapshot options, + CancellationToken cancellationToken); +``` + +- Produces: + +```csharp +internal bool LocalG2PathSplicer.TryReplace( + PreparedPath currentPath, + LocalG2CandidateGeometry candidate, + out PreparedPath replacedPath, + out string reason); +``` + +- `LocalG2CandidateGeometry` exposes: + +```csharp +int CandidateIndex +int SegmentIndex +double StartArcLengthMeters +double EndArcLengthMeters +double LeftWindowLengthMeters +double RightWindowLengthMeters +IReadOnlyList RegionPoints +``` + +- Guarantees: candidate endpoints exactly match original window reference; no duplicate non-gear points are introduced. + +- [ ] **Step 1: Add failing candidate tests** + +The script must assert: + +```powershell +Assert-True ($isolated.CandidateCount -gt 0 -and $isolated.CandidateCount -le 12) ` + 'An isolated event must produce a bounded non-empty candidate set.' +Assert-Near 0.0 $isolated.StartPositionError 1e-9 'Window start position must match.' +Assert-Near 0.0 $isolated.EndPositionError 1e-9 'Window end position must match.' +Assert-Near 0.0 $isolated.StartCurvatureError 1e-8 'Window start curvature must match.' +Assert-Near 0.0 $isolated.EndCurvatureError 1e-8 'Window end curvature must match.' +Assert-True $isolated.ContainsLocalG2Source 'Generated samples must identify their source.' + +Assert-Equal 1 $cluster.OutputRegionCount 'Overlapping events must be built as one region.' +Assert-True $cluster.InternalConnectionsAreG2 'Internal anchors must share tangent and curvature.' + +Assert-Equal 'Reverse' $reverse.Direction 'Reverse candidates must preserve segment direction.' +Assert-True $reverse.VehicleAndGeometricCurvatureSignsAreOpposite ` + 'Reverse candidates must convert vehicle curvature to geometric curvature exactly once.' + +Assert-True $spliced.NoDuplicateNonGearPoints 'Splicing must remove duplicate boundary samples.' +Assert-True $spliced.EndpointsUnchanged 'Splicing must preserve the complete segment endpoints.' +``` + +- [ ] **Step 2: Run and verify RED** + +```powershell +dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 +``` + +Expected: fail because candidate construction is absent. + +- [ ] **Step 3: Construct Hermite boundary derivatives** + +For direction sign: + +```csharp +double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d; +double travelHeading = segment.Direction == TravelDirection.Forward + ? vehicleHeading + : vehicleHeading - Math.PI; +double geometricCurvature = directionSign * vehicleCurvature; +double tx = Math.Cos(travelHeading); +double ty = Math.Sin(travelHeading); +double nx = -ty; +double ny = tx; +double dx = derivativeScale * tx; +double dy = derivativeScale * ty; +double ddx = derivativeScale * derivativeScale * geometricCurvature * nx; +double ddy = derivativeScale * derivativeScale * geometricCurvature * ny; +``` + +Use one shared position, heading, derivative scale and shared curvature at every internal primitive boundary. The initial shared curvature is the distance-weighted value from the design spec. Sample each curve densely enough that adjacent raw candidate points are no farther than `outputSpacingMeters`. + +If the first event lies at the direction-segment start, do not create a zero-length left Hermite piece. Use `PreparedDirectionSegment.StartVehicleCurvaturePerMeter` as the outer start curvature and construct the one-sided transition directly toward the first stable primitive state. + +- [ ] **Step 4: Generate finite candidate combinations** + +Use window variants from Task 4. For each window, use derivative-scale multipliers in fixed order: + +```text +1.00 +0.85 +1.15 +``` + +The base scale for a curve piece is that piece’s original arc length. Stop at `MaximumCandidatesPerRegion`. Reject a candidate immediately if any curve evaluation is non-finite or its derivative norm falls below `1e-10`. + +- [ ] **Step 5: Implement safe splicing** + +Interpolate exact window endpoints using `PathReferenceInterpolator.TryInterpolateByArcLength`. Copy original points strictly before the start, append candidate region points, then copy original points strictly after the end. Recalculate local prepared arc lengths monotonically from geometry. Preserve `StartsAtGearSwitch` and `EndsAtGearSwitch`. + +For a multi-segment `PreparedPath`, replace only `candidate.SegmentIndex`; copy all other immutable segments. +When reconstructing the replaced `PreparedDirectionSegment`, preserve `StartVehicleCurvaturePerMeter` exactly. + +- [ ] **Step 6: Run tests** + +```powershell +dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_geometry.ps1 +``` + +Expected: both pass. + +- [ ] **Step 7: Commit** + +```powershell +git add -- ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateGeometry.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PathSplicer.cs ` + ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 +git diff --cached --check +git commit -m "feat: build local G2 smoothing candidates" +``` + +--- + +### Task 7: Evaluate candidates against safety and quality gates + +**Files:** + +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateEvaluator.cs` +- Modify: `ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1` + +**Interfaces:** + +- Produces: + +```csharp +internal LocalG2CandidateEvaluation Evaluate( + PreparedPath rawPath, + PreparedPath currentPath, + LocalG2SmoothingRegion region, + LocalG2CandidateGeometry candidate, + PathSmoothingRequest request, + LocalG2OptionsSnapshot options, + CancellationToken cancellationToken); +``` + +- Produces: accepted/rejected status, stable failure reason, safe full path, region metrics, maximum deviation and candidate index. +- Produces: + +```csharp +internal static LocalG2CandidateEvaluation SelectBest( + IReadOnlyList evaluations); +``` + +- Define `LocalG2CandidateEvaluation` in the evaluator file with these immutable properties: + +```csharp +bool Accepted +PathSmoothingRegionFailureReason FailureReason +int CandidateIndex +PreparedPath SplicedPreparedPath +IReadOnlyList SafePath +IReadOnlyList SafeSegments +double RawPeakCurvatureDerivativePerSquareMeter +double ResultPeakCurvatureDerivativePerSquareMeter +double RawCurvatureVariationCost +double ResultCurvatureVariationCost +double MaximumDeviationMeters +double MinimumBodyClearanceMeters +double MaximumAbsoluteVehicleCurvaturePerMeter +double AbsolutePathLengthChangeMeters +string Reason +``` + +Rejected evaluations use `SplicedPreparedPath = null` and empty immutable safe-path collections. + +- [ ] **Step 1: Add quality-gate scenarios** + +Extend the candidate script with deterministic scenarios: + +```powershell +Assert-Equal 'DeviationExceeded' $tooFar.FailureReason ` + 'A collision-free candidate more than 0.10 m from the raw window must be rejected.' +Assert-Equal 'CurvatureOvershoot' $overshoot.FailureReason ` + 'A candidate outside the raw regional curvature range must be rejected.' +Assert-Equal 'InsufficientImprovement' $noOp.FailureReason ` + 'A safe no-op must not be accepted.' +Assert-Equal 'VariationCostRegression' $oscillating.FailureReason ` + 'Repeated curvature oscillation must fail the variation-cost gate.' +Assert-Equal 'InsufficientClearance' $lowClearance.FailureReason ` + 'A path with less than 0.02 m checked clearance must be rejected.' +Assert-Equal 'Accepted' $improved.Status ` + 'A safe candidate with at least 20 percent peak improvement must be accepted.' +Assert-Equal $smallestDeviation.CandidateIndex $best.CandidateIndex ` + 'Among sufficient candidates, minimum deviation must win before extra smoothness.' +``` + +- [ ] **Step 2: Run and verify RED** + +```powershell +dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 +``` + +Expected: fail on missing evaluation results. + +- [ ] **Step 3: Implement same-analyzer regional comparison** + +Extract raw and candidate window point lists with exact interpolated endpoints. Analyze both through `PathGeometryAnalyzer` using `configuration.OutputSpacingMeters`. + +Accept quality only when: + +```csharp +candidatePeak <= rawPeak * (1d - options.MinimumPeakGradientImprovementRatio) +candidateCost <= rawCost * (1d + options.MaximumVariationCostRegressionRatio) +``` + +The first comparison is per region. The complete-path variation cost is checked again in Task 8. + +- [ ] **Step 4: Implement hard gates** + +In this order, reject: + +1. non-finite or failed geometry analysis; +2. curvature beyond vehicle maximum; +3. curvature outside the raw regional `[min, max]` plus `1e-6`; +4. maximum candidate-point-to-raw-polyline distance above `MaximumDeviationMeters`; +5. complete-path topology/collision validation failure; +6. checked minimum clearance below `MinimumClearanceReserveMeters`; +7. insufficient peak improvement; +8. variation cost regression. + +Compute point-to-segment distance with a clamped projection; do not compare equal normalized indices. + +- [ ] **Step 5: Implement lexicographic selection** + +Sort accepted evaluations by: + +```text +maximum deviation ascending +peak |dκ/ds| ascending +curvature variation cost ascending +absolute path-length change ascending +candidate index ascending +``` + +Do not combine these into an arbitrary weighted score. + +- [ ] **Step 6: Run focused tests** + +```powershell +dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_validation.ps1 +``` + +Expected: both pass. + +- [ ] **Step 7: Commit** + +```powershell +git add -- ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateEvaluator.cs ` + ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 +git diff --cached --check +git commit -m "feat: validate local G2 candidate quality" +``` + +--- + +### Task 8: Integrate the dedicated pipeline and publish partial results + +**Files:** + +- Create: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PreSmoothingPipeline.cs` +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs` +- Create: `ClumsyPilot/tests/verify_path_smoothing_local_g2_integration.ps1` +- Modify: `ClumsyPilot/tests/verify_path_smoothing_service.ps1` + +**Interfaces:** + +- Consumes: validated request, prepared path and fair raw baseline. +- Produces: + +```csharp +internal PathSmoothingResult Smooth( + PathSmoothingRequest request, + PreparedPath preparedPath, + RawPathBaseline rawBaseline, + CancellationToken cancellationToken); +``` + +- Guarantees: every valid input publishes one complete safe path; invalid raw input publishes no path. + +- [ ] **Step 1: Add end-to-end fixture assertions** + +Use `SmoothingScenarioFactory.CreateFixtureRequests(...)`, rebuild each public `PathSmoothingRequest` with: + +```powershell +$configuration.Method = [Enum]::Parse($methodType, 'LocalG2Quintic') +$configuration.AllowFallbackToCoarsePath = $true +``` + +Assert: + +```powershell +Straight -> NotNeeded +SingleTurn -> Complete +LargeHeadingChange -> Complete +SBend -> Complete or PartialImprovement +RectangleDetour -> Complete or PartialImprovement +MultiObstacleDetour -> Complete or PartialImprovement +ReverseGearSwitch -> Complete, PartialImprovement, or NotNeeded +``` + +For every published path: + +```powershell +Assert-True ($result.Path.Count -gt 0) 'A valid request must publish a complete path.' +Assert-Equal 0 $result.Segments[0].StartIndex 'Segments must start at zero.' +Assert-Equal ($result.Path.Count - 1) $result.Segments[-1].EndIndex 'Segments must cover the path.' +Assert-True $result.Diagnostics.Metrics.IsFeasible 'Published diagnostics must be feasible.' +Assert-True ($result.Diagnostics.Metrics.MinimumBodyClearanceMeters -ge 0.02) ` + 'Published paths must retain the configured clearance reserve.' +``` + +For `Complete` and `PartialImprovement`, require at least one region report with `Improved`. For `PartialImprovement`, require at least one `RetainedOriginal`. Run every fixture twice and compare status, point count, coordinates and region reports. + +Also create two explicit service cases: + +```powershell +# Force every real candidate below the acceptance bar without invalidating the request. +$strict.LocalG2Quintic.MinimumPeakGradientImprovementRatio = 0.99 +$unchanged = $service.Smooth($strictRequest, [Threading.CancellationToken]::None) +Assert-Equal 'Unchanged' $unchanged.Status.ToString() ` + 'Detected events with no accepted region must publish a verified raw path as Unchanged.' +Assert-True ($unchanged.Path.Count -gt 0) 'Unchanged must retain a complete SQP initial path.' + +$cancelSource = [Threading.CancellationTokenSource]::new() +$cancelSource.Cancel() +$cancelled = $service.Smooth($normalRequest, $cancelSource.Token) +Assert-Equal 'Cancelled' $cancelled.Status.ToString() 'Cancellation must propagate through Local G2.' +Assert-Equal 0 $cancelled.Path.Count 'A cancelled run must not publish a partial path.' +``` + +- [ ] **Step 2: Run and verify RED** + +```powershell +dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_integration.ps1 +``` + +Expected: fail because the service cannot resolve `LocalG2Quintic`. + +- [ ] **Step 3: Implement sequential regional processing** + +Pipeline pseudocode must be implemented directly: + +```csharp +PreparedPath current = preparedPath; +var reports = new List(); +int improvedCount = 0; + +foreach (LocalG2SmoothingRegion region in regions) +{ + cancellationToken.ThrowIfCancellationRequested(); + IReadOnlyList candidates = + _builder.Build(preparedPath.Segments[region.SegmentIndex], region, outputSpacing, options, cancellationToken); + var evaluations = new List(); + foreach (LocalG2CandidateGeometry candidate in candidates) + evaluations.Add(_evaluator.Evaluate(preparedPath, current, region, candidate, request, options, cancellationToken)); + + LocalG2CandidateEvaluation best = LocalG2CandidateEvaluator.SelectBest(evaluations); + if (best != null && best.Accepted) + { + current = best.SplicedPreparedPath; + improvedCount++; + reports.Add(CreateImprovedReport(region, best)); + } + else + { + reports.Add(CreateRetainedReport(region, evaluations)); + } +} +``` + +Regions are disjoint after merging, so successful earlier replacements remain when a later region fails. + +- [ ] **Step 4: Perform final full-path analysis and rollback** + +Analyze and validate `current`. Require: + +```csharp +finalVariationCost <= rawVariationCost * (1d + options.MaximumVariationCostRegressionRatio) +``` + +If final validation fails, roll back improved regions in reverse acceptance order, marking `GlobalValidationRollback`, until validation passes. If all are rolled back, publish the verified raw baseline as `Unchanged`. If raw baseline was not valid, publish `Failed` with an empty path. + +- [ ] **Step 5: Derive final status exactly** + +```csharp +if (transitions.Count == 0) status = PathSmoothingStatus.NotNeeded; +else if (improvedCount == regions.Count) status = PathSmoothingStatus.Complete; +else if (improvedCount > 0) status = PathSmoothingStatus.PartialImprovement; +else status = PathSmoothingStatus.Unchanged; +``` + +`Complete` and `PartialImprovement` cannot be emitted with zero improved reports. + +- [ ] **Step 6: Dispatch without disturbing legacy methods** + +In `PathSmoothingService`: + +1. validate common inputs; +2. validate only `LocalG2QuinticOptions` when method is `LocalG2Quintic`; +3. prepare and build the fair raw baseline once; +4. branch to `_localG2Pipeline.Smooth(...)`; +5. otherwise continue through the existing `IPathSmoother` runner. + +Do not add Local G2 to `PathSmoothingComparisonRequest.DefaultMethods`. + +- [ ] **Step 7: Run focused and legacy service tests** + +```powershell +dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_integration.ps1 +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_service.ps1 +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_runner.ps1 +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_integration.ps1 +``` + +Expected: all scripts pass. The old integration script may continue reporting zero successful legacy methods on difficult fixtures; that is outside this task. + +- [ ] **Step 8: Commit** + +```powershell +git add -- ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PreSmoothingPipeline.cs ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs ` + ClumsyPilot/tests/verify_path_smoothing_local_g2_integration.ps1 ` + ClumsyPilot/tests/verify_path_smoothing_service.ps1 +git diff --cached --check +git commit -m "feat: publish local G2 presmoothing results" +``` + +--- + +### Task 9: Document the main route and run the complete regression suite + +**Files:** + +- Modify: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/README.md` +- Modify: `ClumsyPilot/tests/verify_path_smoothing_documentation.ps1` + +**Interfaces:** + +- Documents: module role as SQP warm-start generator. +- Documents: statuses, defaults, output fields and non-goals. +- Does not claim old comparison algorithms are production choices. + +- [ ] **Step 1: Add failing documentation assertions** + +Require README sections or exact terms: + +```powershell +$required = @( + '局部 G2 预平滑', + 'SQP 初始路径', + 'Complete', + 'PartialImprovement', + 'NotNeeded', + 'Unchanged', + '0.20 m', + '0.50 m', + '0.80 m', + '0.10 m', + 'dκ/ds', + '不跨换向点', + '不输出速度' +) +foreach ($text in $required) { + if (-not $readme.Contains($text)) { throw "PathSmoothing README must contain: $text" } +} +``` + +- [ ] **Step 2: Run and verify RED** + +```powershell +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_documentation.ps1 +``` + +Expected: fail on the first missing Local G2 term. + +- [ ] **Step 3: Update README** + +Document: + +- Hybrid A* is G1 but can have primitive-boundary curvature jumps. +- Local G2 is the main pre-SQP route. +- hard direction segmentation and stopped gear-switch semantics; +- window defaults and maximum deviation; +- fair same-analyzer metrics; +- complete/partial/not-needed/unchanged/failed meanings; +- spatial output fields including `κ` and `dκ/ds`; +- speed planning owns `1 m/s`, `60°/s` and future speed configurations; +- old algorithms remain offline comparison only. + +- [ ] **Step 4: Run all PathSmoothing checks** + +Run: + +```powershell +dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore +$scripts = @( + 'verify_path_smoothing_contracts.ps1', + 'verify_path_smoothing_algorithm_input.ps1', + 'verify_path_smoothing_geometry.ps1', + 'verify_path_smoothing_validation.ps1', + 'verify_path_smoothing_runner.ps1', + 'verify_path_smoothing_service.ps1', + 'verify_path_smoothing_bspline.ps1', + 'verify_path_smoothing_bezier.ps1', + 'verify_path_smoothing_quintic.ps1', + 'verify_path_smoothing_comparison.ps1', + 'verify_path_smoothing_integration.ps1', + 'verify_path_smoothing_fixtures.ps1', + 'verify_path_smoothing_svg_csv.ps1', + 'verify_path_smoothing_png.ps1', + 'verify_path_smoothing_documentation.ps1', + 'verify_path_smoothing_local_g2_detection.ps1', + 'verify_path_smoothing_local_g2_curve.ps1', + 'verify_path_smoothing_local_g2_candidates.ps1', + 'verify_path_smoothing_local_g2_integration.ps1' +) +foreach ($script in $scripts) { + powershell -ExecutionPolicy Bypass -File (Join-Path 'ClumsyPilot/tests' $script) + if ($LASTEXITCODE -ne 0) { exit $LASTEXITCODE } +} +``` + +Expected: build exits `0`; every script prints its success message and exits `0`. + +- [ ] **Step 5: Run CoarsePath regression checks** + +```powershell +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_coarse_path_integration.ps1 +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_coarse_path_collision.ps1 +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_coarse_path_search.ps1 +``` + +Expected: all pass. + +- [ ] **Step 6: Inspect fixture outcomes** + +Run: + +```powershell +powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_integration.ps1 +``` + +Record the status, improved-region count, retained-region count, peak `|dκ/ds|`, variation cost, maximum deviation and minimum clearance for every fixture. Do not weaken assertions merely to make a fixture green; if `SingleTurn` or `LargeHeadingChange` is not `Complete`, return to Tasks 4–7 and correct the algorithm. + +- [ ] **Step 7: Commit documentation and final verification changes** + +```powershell +git add -- ` + ClumsyPilot/ParkrobTrajplanner/PathSmoothing/README.md ` + ClumsyPilot/tests/verify_path_smoothing_documentation.ps1 +git diff --cached --check +git commit -m "docs: describe local G2 presmoothing workflow" +``` + +- [ ] **Step 8: Final repository checks** + +```powershell +git diff --check +git status --short +git log --oneline -10 +``` + +Expected: + +- no whitespace errors; +- no staged files; +- only pre-existing unrelated working-tree changes remain; +- the task commits appear in order. + +Do not report implementation complete unless the full build and every listed verification script have fresh exit code `0`.