✨ feat(fanuc): 打通飞拍轨迹完整执行链路

* 增加 J519 稠密发送采样校验与保姿回发逻辑
* 调整 saveTrajectory 导出与 sequence buffer 行为
* 补充 10010 解析脚本、ICSP 说明和回归测试

src/Flyshot.Core.Planning/ICSP-算法说明.md

@@ -0,0 +1,194 @@
+# ICSP 算法说明（`ICspPlanner`）
+
+本文档用于解释 `Flyshot.Core.Planning.ICspPlanner` 当前实现的 **ICSP 规划算法**在本仓库中的真实含义与计算逻辑，便于与逆向结论对照、以及指导后续改造（例如“按约束生成中间点位”）。
+
+> 适用范围：本文描述的是当前 C# 实现的 **“CubicSpline + 逐段时间缩放迭代（retiming）”** 版本。  
+> 重要澄清：`ICspPlanner` 的主要输出是 **时间轴**（每个示教点的时间戳），而不是直接输出固定周期的稠密点序列；稠密点在后续采样层生成。
+
+---
+
+## 1. 名词与数据形态
+
+### 1.1 输入
+
+- **示教点（路点）**：`request.Program.Waypoints`  
+  每个路点是关节空间向量 \(q_i \in \mathbb{R}^{dof}\)。
+- **关节约束**：`request.Robot.JointLimits[d]` 提供每轴上限：
+  - 速度上限 \(v_{lim}[d]\)
+  - 加速度上限 \(a_{lim}[d]\)
+  - 跃度（jerk）上限 \(j_{lim}[d]\)
+
+### 1.2 输出（`PlannedTrajectory`）
+
+`ICspPlanner` 的输出 **不是稠密轨迹点序列**，而是：
+
+- `PlannedWaypoints`：规划后路点（对于普通 `icsp`，与输入示教点相同；补点发生在 `SelfAdaptIcspPlanner`）
+- `WaypointTimes`：每个路点的绝对时间 \(t_i\)（秒）
+- `SegmentDurations`：每段时长 \(T_i = t_{i+1}-t_i\)（秒）
+- `SegmentScales`：每段缩放因子 `scale_i`
+- `Iterations` / `Threshold`：收敛信息
+
+后续模块会基于 `PlannedWaypoints + WaypointTimes` 重建样条并采样，生成稠密点：
+
+- 规划层稠密采样：`TrajectorySampler.SampleJointTrajectory(...)`
+- 运行时 J519 重采样（速度倍率映射 + rad->deg）：`J519SendTrajectorySampler.SampleDenseJointTrajectory(...)`
+
+---
+
+## 2. 算法总体目标（retiming）
+
+给定一组关节示教点 \(\{q_i\}_{i=0}^{N-1}\)，在不改变路点位置的前提下，为每段分配时长 \(\{T_i\}_{i=0}^{N-2}\)，使得用 **clamped-zero 三次样条**连接后的轨迹在每段上满足：
+
+- \(\max|\dot q_d(t)| \le v_{lim}[d]\)
+- \(\max|\ddot q_d(t)| \le a_{lim}[d]\)
+- \(\max|\dddot q_d(t)| \le j_{lim}[d]\)
+
+实现策略是“逐段缩放时长”的迭代法：每轮用当前 \(\{T_i\}\) 构造样条并解析求导峰值，再根据超限程度把相应段时长乘以缩放因子，使峰值回落。
+
+---
+
+## 3. 计算步骤（与代码一致）
+
+### 3.1 前置条件
+
+- 路点数 \(N \ge 4\)（否则抛异常）
+
+### 3.2 初始段时长
+
+段数 \(nseg = N-1\)。
+
+初始段时长取相邻路点关节空间欧氏距离：
+
+\[
+T_i^{(0)} = \|q_{i+1}-q_i\|_2
+\]
+
+### 3.3 由段时长构造绝对时间轴
+
+\[
+t_0 = 0,\quad t_{i+1} = t_i + T_i
+\]
+
+### 3.4 用 clamped-zero 边界构造三次样条
+
+以 \((t_i, q_i)\) 为节点构造分段三次多项式：
+
+\[
+S_i(t) = a_i t^3 + b_i t^2 + c_i t + d_i,\quad t \in [t_i, t_{i+1}]
+\]
+
+边界条件为 **clamped-zero**（起点/终点一阶导为 0），用于与逆向锁定的参考行为对齐。
+
+### 3.5 解析计算每段导数峰值
+
+对每段、每轴，解析求最大绝对值：
+
+- 一阶导（速度）是二次函数：端点与顶点候选取最大
+- 二阶导（加速度）是一次函数：端点取最大
+- 三阶导（跃度）是常数：直接取绝对值
+
+得到三张矩阵：
+
+- `maxDq[seg,d] = max_t |dq/dt|`
+- `maxDdq[seg,d] = max_t |d²q/dt²|`
+- `maxDddq[seg,d] = max_t |d³q/dt³|`
+
+### 3.6 计算每段缩放因子（核心公式）
+
+对段 `seg`，对每个关节 `d` 计算三类“超限比”：
+
+\[
+s_v = \left|\frac{maxDq[seg,d]}{v_{lim}[d]}\right|
+\]
+
+\[
+s_a = \sqrt{\left|\frac{maxDdq[seg,d]}{a_{lim}[d]}\right|}
+\]
+
+\[
+s_j = \sqrt[3]{\left|\frac{maxDddq[seg,d]}{j_{lim}[d]}\right|}
+\]
+
+段缩放因子取所有轴、三类约束的最大值：
+
+\[
+scale_{seg}=\max_d \max(s_v, s_a, s_j)
+\]
+
+> 指数来源：时间拉长 \(k\) 倍时，速度按 \(1/k\) 缩小、加速度按 \(1/k^2\) 缩小、跃度按 \(1/k^3\) 缩小，因此超限比需要分别取一次方/平方根/立方根来求“应当拉长多少倍”。
+
+### 3.7 收敛指标与最优解保存
+
+每轮用如下指标衡量“离约束满足还差多少”：
+
+\[
+threshold = \sum_{seg} |scale_{seg} - 1|
+\]
+
+若本轮 `threshold` 小于历史最佳，则保存当前解作为 `best`（包含 `bestDurations / bestScales / bestWaypointTimes` 等）。
+
+### 3.8 收敛判定与段时长更新
+
+- 若 `threshold < _threshold`（默认 `1e-3`），认为收敛并提前结束迭代
+- 否则更新每段时长：
+
+\[
+T_{seg} \leftarrow T_{seg} \cdot scale_{seg}
+\]
+
+并进入下一轮。
+
+---
+
+## 4. 最终判定（global_scale）
+
+迭代结束后取历史最优缩放因子的最大值：
+
+\[
+globalScale=\max_{seg}(scale_{seg})
+\]
+
+若启用强制判定（`enforceFinalScale=true`）且：
+
+\[
+globalScale > 1 + \text{finalScaleTolerance}
+\]
+
+则判定“未收敛/不可执行”并抛异常。默认容差 `finalScaleTolerance=1e-2`，用于容忍 C# spline 与参考实现间的小量数值差异。
+
+---
+
+## 5. 与“补点/中间点位”的关系（常见误解澄清）
+
+### 5.1 `ICspPlanner` 不负责生成固定周期的中间点位
+
+`ICspPlanner` 的核心工作是 **时间轴规划（retiming）**：在不改变示教点位置的情况下，通过缩放每段时长让样条导数峰值满足约束。
+
+固定周期（例如 8ms/16ms）的“中间点位序列”属于 **采样层**：
+
+- `TrajectorySampler`：按 `samplePeriod` 在样条上取样，得到 `[time, j1..jN]`（关节单位仍为 rad）
+- `J519SendTrajectorySampler`：按 `servoPeriod` 生成真实发送序列，用 `speedRatio` 把 `sendTime` 映射到 `trajectoryTime` 并线性插值，再做 `rad -> deg`
+
+### 5.2 `SelfAdaptIcspPlanner` 才包含“补点”逻辑，但它很粗
+
+`self-adapt-icsp` 的补点策略在 `SelfAdaptIcspPlanner` 中：当某些段 `scale > 1 + tolerance` 时，对这些段插入关节空间**中点**再重规划。该策略的目的主要是“救收敛”，不是生成最终稠密序列。
+
+---
+
+## 6. 后续改造建议（定位落点）
+
+如果需求是“根据示教点 + v/a/j 限制，直接生成可下发的稠密点位序列”，通常有两条路径：
+
+1. **保留 ICSP retiming**：继续用 `ICspPlanner` 求时间轴，再在采样层按固定周期生成中间点位（当前架构就是这条路）。此时需要讨论的是采样周期、速度倍率映射、以及是否要对采样序列再做约束校验或二次整形。
+2. **做真正的自适应插点/细分**：把“插点策略”升级为基于约束的细分（而不只是插中点），这更自然的落点是 `SelfAdaptIcspPlanner` 或新增一个“约束驱动细分器”，而不是把稠密点生成塞进 `ICspPlanner`。
+
+---
+
+## 7. 关联实现位置（便于跳转）
+
+- 算法入口：`src/Flyshot.Core.Planning/ICspPlanner.cs`
+- 自适应补点：`src/Flyshot.Core.Planning/SelfAdaptIcspPlanner.cs`
+- 三次样条实现（clamped-zero + 解析导峰值）：`src/Flyshot.Core.Planning/CubicSplineInterpolator.cs`
+- 规划层稠密采样：`src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs`
+- J519 实发重采样：`src/Flyshot.Core.Planning/Sampling/J519SendTrajectorySampler.cs`
+