Compare commits
16
Commits
main
...
d9137f4ebc
| Author | SHA1 | Date | |
|---|---|---|---|
 Hark
|
d9137f4ebc | ||
|
Hark
|
0c5f01c921 | ||
|
Hark
|
c13366b830 | ||
|
Hark
|
ea83de8bb0 | ||
|
Hark
|
39259ea55a | ||
|
Hark
|
ec62d31f6b | ||
|
Hark
|
34c98e0371 | ||
|
Hark
|
2462c66a6b | ||
|
Hark
|
5895deb071 | ||
|
Hark
|
fecc59b0af | ||
|
Hark
|
c51f8f9d1f | ||
|
Hark
|
40af3b4dd6 | ||
 cbb
|
3c7c814d40 | ||
|
Hark
|
c78ec0b48e | ||
|
Hark
|
919e704771 | ||
|
Hark
|
8ad5b3b4fa |
+33
-38
@@ -1,46 +1,41 @@
|
||||
|
||||
# Python
|
||||
__pycache__/
|
||||
*.pyc
|
||||
|
||||
# 构建产物（可用 python -m arm.* 重新生成）
|
||||
out/
|
||||
|
||||
# 临时调试脚本与截图
|
||||
scratch/
|
||||
|
||||
# 单文件查看器优化前的完整备份（带版本标识，勿随手删）
|
||||
backups/
|
||||
|
||||
# 重构前的源码快照与脚本自动备份（git 历史里已有旧版本，不重复入库）
|
||||
.bak/
|
||||
*.bak_*
|
||||
|
||||
# 编辑器 / 系统文件
|
||||
Thumbs.db
|
||||
Desktop.ini
|
||||
|
||||
# 构建产物：由 python -m arm.* 重新生成，体积大（mesh 14MB + 单文件查看器 16MB×3），
|
||||
# 过去被跟踪导致仓库背着 47MB 生成物；取消跟踪后重建不再脏 git status。
|
||||
# 旧 web/ 单文件产物目录（已删，源码在 webapp/）
|
||||
web/
|
||||
/机器人手臂3D查看器.html
|
||||
webapp/data/
|
||||
webapp/generated/
|
||||
dist/
|
||||
data/cache/
|
||||
|
||||
# 关卡截图落点 = 纯生成物：每次 `python -m tests.verify --shots` 由 shot.py 重写
|
||||
# （其中有 13 张在同一份构建下也不逐字节复现，抗锯齿/量化噪声）。
|
||||
# 比对的权威物在 tests/baselines/ —— shots_stable.md5 是 92 张的 md5 名单，
|
||||
# flaky/ 是那 13 张的参考像素 + thresholds.json 阈值；PNG 本身不需要入库。
|
||||
docs/shots/
|
||||
|
||||
# 临时调试脚本与截图
|
||||
# 关卡截图落点 = 纯生成物：每次 `python -m tests.verify --shots` 由 shot.py 重写
|
||||
# 关卡比对的权威物是 tests/baselines/（md5 名单 + flaky 参考像素），不在这里。
|
||||
# 其它编辑器的本地配置/会话残留（不是工程产物，也不该跟着版本走）
|
||||
# 单文件查看器优化前的完整备份（带版本标识，勿随手删）
|
||||
# 定位图/ 只是"给用户看的核验图落点"，一次性、会反复清空；
|
||||
# 旧 web/ 单文件产物目录（已删，源码在 webapp/）
|
||||
# 构建产物（可用 python -m arm.* 重新生成）
|
||||
# 构建产物：由 python -m arm.* 重新生成，体积大（mesh 14MB + 单文件查看器 16MB×3），
|
||||
# 比对的权威物在 tests/baselines/ —— shots_stable.md5 是 92 张的 md5 名单，
|
||||
# 编辑器 / 系统文件
|
||||
# 过去被跟踪导致仓库背着 47MB 生成物；取消跟踪后重建不再脏 git status。
|
||||
# 重构前的源码快照与脚本自动备份（git 历史里已有旧版本，不重复入库）
|
||||
# （其中有 13 张在同一份构建下也不逐字节复现，抗锯齿/量化噪声）。
|
||||
*.bak_*
|
||||
*.pyc
|
||||
.bak/
|
||||
.trae/
|
||||
.workbuddy/
|
||||
.zcode/
|
||||
.zcodeignore
|
||||
|
||||
# 定位图/ 只是"给用户看的核验图落点"，一次性、会反复清空；
|
||||
# 关卡比对的权威物是 tests/baselines/（md5 名单 + flaky 参考像素），不在这里。
|
||||
/机器人手臂3D查看器.html
|
||||
Desktop.ini
|
||||
Thumbs.db
|
||||
__pycache__/
|
||||
backups/
|
||||
data/cache/
|
||||
dist/
|
||||
docs/shots/
|
||||
ml/data/
|
||||
ml/node_modules/
|
||||
ml/runs/
|
||||
out/
|
||||
scratch/
|
||||
web/
|
||||
webapp/data/
|
||||
webapp/generated/
|
||||
定位图/
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,528 @@
|
||||
# Transformer 避障运动策略：最终方案
|
||||
|
||||
> 基于 `main@3c7c814`（贴立柱修复后的规划器）。本文是方案，代码尚未编写。
|
||||
> 训练机：RTX 3090 24 GB。部署：浏览器（后续真机）。
|
||||
|
||||
---
|
||||
|
||||
## 0. 定位与边界
|
||||
|
||||
| 项目 | 决定 |
|
||||
|---|---|
|
||||
| 部署形态 | **无运行时老师**：运行时只有「神经策略 + 碰撞安全层」，不加载 IK 大表、PRM 路网、A*/RRT，也不调用 `solveDyn` / `routeRT` |
|
||||
| 运行方式 | **闭环滚动执行**：执行频率 50 Hz（每步 20 ms）；每 60 ms 读一次当前关节角和目标，推理一段 16 步动作块，安全层认证后提交前 3 步 |
|
||||
| 碰撞安全层 | 不做搜索，只对模型给出的候选做**距离下界认证**（§8.3），不安全就换候选或沿已认证刹停段停下 |
|
||||
| 老师的角色 | 只在**离线**出现：生成预训练数据；后训练以可微碰撞代价为主（§7） |
|
||||
| 不做的事 | 不学动力学（速度/加速度上限由时间律与插补器保证）；不用相机/点云（环境固定：台架 + P01 立柱，几何已知） |
|
||||
| 收益目标 | 冷启动（新目标到第一个动作）< 10 ms；每次推理 + 安全认证 < 20 ms；消除现有拖拽重规划 P99 约 321 ms 的长尾 |
|
||||
| 底线 | 实时拖拽**碰撞帧 = 0**（由安全层距离认证保证）；到达率、行程在 §9 指标上不低于现有规划器 |
|
||||
|
||||
**关于"风险"：** 论文证据只能说明"别人在相近条件下做成过"，没有一篇论文的条件（低维状态、单根固定立柱、约 4M 参数、浏览器推理、30/30）与本项目完全一致。因此本方案中只有安全层是**硬保证**；每个设计点都已定为唯一选择（§1.1），只有 MeanFlow 头带一个预先写定触发条件的回退（碰撞与否不依赖模型好坏）；其余每一项都配有证据等级、本仓库可测的**门禁**（不过不进下一步）和**回退**（§10）。
|
||||
|
||||
---
|
||||
|
||||
## 1. 方案总览
|
||||
|
||||
| 模块 | 最终选择 | 证据等级 | 关键依据 |
|
||||
|---|---|---|---|
|
||||
| 模型规模 | 约 4.9M 参数编码器-解码器 Transformer，不用 VLA 大模型 | A | IMPACT 4.67M、300 Hz；π0 3B 单次预填充 46 ms（4090） |
|
||||
| 数据 | 规划器离线造 5 万条任务 → 约 200 万"状态→动作块"样本 | B | MπNets、DRP、ELMP 均用规划器老师 |
|
||||
| 时间律 | 匀速巡航 + 梯形加减速，只依赖"当前速度 + 剩余路程"；当前速度作输入 | B | MπNets 将老师路径重定时为匀速 |
|
||||
| 动作表示 | 16 步 × 6 轴关节增量 Δq | A | DRP；Diffusion Policy（位置式优于速度式） |
|
||||
| 动作头 | **MeanFlow 一步生成头**，每次并行采 8 个噪声 → 8 条候选 | B | 沙箱双模态实验（§2.4）；MP1 约 6.8 ms |
|
||||
| 块间连续 | 训练时前缀条件化：输入已提交未执行的步 | B | 2512.05964：推理零开销，d≥2 时优于推理时 RTC |
|
||||
| 碰撞修正 | 输出后 1 步解析胶囊距离梯度投影（每轴 ≤ 0.5°，仅 d < 余量时） | B | DiffusionSeeder：少步生成下在环引导收益很小；投影在已知环境下够用 |
|
||||
| 候选排序 | 与已提交块重叠步的一致性打分 + 价值头 | B | BID：一致性打分 + 多样本相对提升 32% |
|
||||
| 预训练 | 行为克隆 + FK 胶囊端点损失 | B | ELMP 式 FK 损失 |
|
||||
| 后训练（主） | APG：可微运动学展开 + 胶囊距离代价反传 | A | ELMP：碰撞率 31.3%→8.9%；1 万示范 + APG 胜 60 万纯 BC |
|
||||
| 后训练（辅） | 无老师挖错循环 + 门禁 | B | DRP 迭代微调 +45% |
|
||||
| 安全层 | 距离下界认证 + 可刹停不变式 | A | Schwarzer 2005 连续认证；Thumm & Althoff 2022 failsafe |
|
||||
| 30/30 手段 | 子目标头、价值头、冷启动/卡死时短前瞻、卡死换向 | B / C | AWE、SHAC、BID |
|
||||
| 老师 | PRM 路网 A* + 捷径平滑（确定性）；A* 失败才用 `solveDyn` | B | MπNets：老师不一致碰撞率 13.78%，一致老师 0.94% |
|
||||
| 推理后端 | onnxruntime-web wasm-SIMD 多线程 | C | 小模型下 WebGPU 调度开销占主导 |
|
||||
|
||||
### 1.1 取舍决定（每项只有一个答案）
|
||||
|
||||
| 选项 | 利 | 弊 | 决定 |
|
||||
|---|---|---|---|
|
||||
| 动作头：MeanFlow 一步头 vs WTA K=4 | MeanFlow：连续多模态，能同时表达左/右绕行与位姿目标的多个 IK 分支；沙箱 1 步达到 10 步流匹配质量。WTA：训练稳、实现简单 | MeanFlow：训练需 JVP（训练慢约 1.5–2 倍）、原生版本在部分任务失败（K-MF 表 1）。WTA：只有 4 个固定模式，左右 × 肘/腕分支可能超过 4 个；沙箱分不出两者高下 | **上 MeanFlow**。WTA 不实现；只有 M5 门禁不过（near_column 穿柱率高于 2% 或训练发散）才换成 WTA K=8，二者共用骨干，切换约 1 天 |
|
||||
| Shortcut + 条件 OT 配对 | 无需 JVP | 本项目无任何实测，组合无论文先例 | **不上** |
|
||||
| B 样条动作表示 | 平滑由表示保证；输出维度小 | 贴柱时拐角被抹圆、局部精度下降；只有间接证据；平滑已由梯形时间律 + 平滑损失保证 | **不上**，固定 16 步 Δq |
|
||||
| 输出后梯度投影 | 修掉 1–2 mm 的轻微侵入，成本约 0.1 ms，减少刹停 | 推离数据分布、改变块间一致性；修不了"绕错侧" | **上**，限 1 步、每轴修正量 ≤ 0.5°，仅 d < 余量时触发 |
|
||||
| 短前瞻（多推一块打分） | 避开"走两步就撞墙"的分支 | 推理次数翻倍，加剧最大不确定项（浏览器延迟） | **只在冷启动和卡死换向时上**，平时不做 |
|
||||
| 拒绝采样自蒸馏 | 进一步缩短行程 | 偏向贴柱、余量变小、多样性塌缩；APG 行程项已覆盖同一目标 | **不上** |
|
||||
| ReinFlow 强化学习微调 | 能直接优化到达/卡死/刹停等离散目标 | RL 会重塑左右模式比例；奖励调参与训练多出数天；这些离散目标已由卡死换向规则 + APG 覆盖 | **不上** |
|
||||
| 挖错循环中 E2 错题请老师补路径 | 解决可微代价绕不出的局部极小 | 量小、离线 | **上** |
|
||||
| 老师：PRM 路网 A* vs `solveDyn` | A*：确定性，同类任务左右选择一致，且是路网上的最短；`solveDyn` 覆盖更全 | A*：受路网分辨率限制，个别任务失败 | **A* 为主老师**，失败的任务用 `solveDyn`（固定种子）补 |
|
||||
| 推理后端：WebGPU vs wasm | WebGPU 大模型快 | 本模型约 4M 参数，WebGPU 每次调度 33–67 μs × 150–300 次，调度开销反而大 | **wasm-SIMD 多线程**；M4 实测若 WebGPU P99 更低再换 |
|
||||
| 训练余量 | 越大越不易被认证拒绝 | 越大行程越长、贴柱越难 | **5 mm** |
|
||||
| 关节噪声增广 σ | 覆盖偏离状态 | 过大产生无意义状态 | **1°** |
|
||||
| 局部牛顿精修 | 末端精度到 < 1 mm | 无 | **上**，同样过安全层 |
|
||||
|
||||
证据等级：**A** 有直接先例与数字 · **B** 有相近先例，需本仓库验证 · **C** 仅推断。
|
||||
|
||||
---
|
||||
|
||||
## 2. 依据
|
||||
|
||||
### 2.1 业界神经运动规划
|
||||
|
||||
| 工作 | 年份 / 会议 | 输入 | 输出 | 老师与数据量 | 关键做法 | 运行方式 |
|
||||
|---|---|---|---|---|---|---|
|
||||
| **MπNets**（NVIDIA） | 2022 CoRL | 点云 + 关节角 + 目标位姿 | 下一步关节位置 | 规划器 300 万+ 问题 | 端到端行为克隆；老师路径重定时为匀速 | 闭环 9 Hz，1 kHz PD 跟踪 |
|
||||
| **Neural MP**（CMU） | 2024 / IROS 2025 | 点云 + 关节角 + 目标 | GMM（LSTM） | 100 万–300 万条 | 测试时采样多条，选穿透最少的（去掉后成功率 81%→31%） | 开环为主 |
|
||||
| **DRP / IMPACT**（CMU） | 2025 CoRL | 点云 + 关节角 + 目标关节角 | 10 步关节增量 | cuRobo 1000 万条 | 大规模预训练 + 迭代师生微调 + "目标被挡停在最近安全处" | 闭环；4.67M 参数，300 Hz |
|
||||
| **ELMP**（ETH） | 2026 arXiv | 点云 + 关节角 + 目标位姿 | 关节增量 | AIT* 约 60 万条 | 行为克隆后 **APG 微调**，碰撞/到达/平滑损失直接反传 | 闭环 30 Hz |
|
||||
|
||||
- ELMP：1 万条老师轨迹 + APG（成功率 72.0%）超过 60 万条纯行为克隆（57.9%）；APG 把碰撞率降到 1/3 以下。
|
||||
- 最好的方法在静态场景仍有约 9–10% 碰撞（ELMP-APG 8.9%，DRP 10.16%），实际部署都配检查或反应层。
|
||||
- 共识：闭环、短动作块滚动执行、老师只离线用、必须后训练。
|
||||
|
||||
### 2.2 本项目的条件
|
||||
|
||||
| 条件 | 主流 | 本项目 | 意味着 |
|
||||
|---|---|---|---|
|
||||
| 环境 | 未知、多变，靠点云 | 固定且精确已知（台架 + 立柱，1936 个胶囊） | 不需要点云编码器；模型可以小；碰撞检查精确 |
|
||||
| 机器人 | Franka 7 轴 | 自研 6 轴，J1/J3/J6 连续转 | 需处理连续轴展开 |
|
||||
| 碰撞模型 | SDF / 球近似 | 胶囊解析距离，可微 | 直接做 APG；安全层可认证 |
|
||||
| 平台 | 台式 GPU | 浏览器（后续真机） | 模型小，ONNX + WebGPU/wasm |
|
||||
|
||||
资源应投在**闭环稳定性、左右绕行选择和安全层**上，而不是感知和泛化。
|
||||
|
||||
### 2.3 前沿生成式技术取舍
|
||||
|
||||
| 技术 | 结论 | 理由 |
|
||||
|---|---|---|
|
||||
| VLA 大模型（π0/π0.5、GR00T、OpenVLA） | 不采用 | 无图像、无语言指令；3B 参数更慢不更准 |
|
||||
| 一步生成：MeanFlow（MP1） | **采用** | 1 次前向；沙箱双模态实验通过 |
|
||||
| Consistency Policy、OneDP、朴素 1 步流匹配 | 不采用 | CP 原文承认偏向一侧；OneDP 有意取单模态；朴素 1 步 FM 等于条件均值，会穿柱 |
|
||||
| Shortcut / 条件 OT 配对 | 不采用 | 见 §1.1 |
|
||||
| OFP 自蒸馏 | 不采用 | 未找到公开代码；自引导 λ=0.5 时出现模态塌缩 |
|
||||
| 生成过程中代价引导（MPD/EDMP/GPD） | 不采用 | 无论文在 1–2 步下验证；改用输出后投影 |
|
||||
| 推理时 RTC | 不采用 | 原文只验证 5 步去噪且需反向传播；改用训练时前缀条件化 |
|
||||
| 时间集成 / 交叉淡入 | 不采用 | 执行的是未认证的混合块；左右两块平均可能穿柱；π0 弃用 |
|
||||
| FAST 动作分词 | 不采用 | 服务于语言骨干，自回归推理慢 |
|
||||
| B 样条动作表示 | 不采用 | 见 §1.1 |
|
||||
| ReinFlow / DPPO | 不采用 | 见 §1.1 |
|
||||
|
||||
### 2.4 沙箱双模态对照实验
|
||||
|
||||
2D 点从柱左侧到右侧，示教一半从上绕一半从下绕，16 步动作块、每 3 步重推理，与本方案同构。代码 `ml/toy/toy_bimodal.py`，150 个"起终点都贴近中轴"的难例：
|
||||
|
||||
| 动作头 | 单样本、无安全层：到达 / 穿柱回合 | 8 候选 + 安全层：到达 / 穿柱 / 平均刹停 | 推理（CPU，8 候选） |
|
||||
|---|---|---|---|
|
||||
| MSE 回归 | 100% / 0% | 100% / 0% / 0 | 0.13 ms |
|
||||
| WTA K=4 | 100% / 0% | 100% / 0% / 0 | 0.19 ms |
|
||||
| 流匹配 10 步 | 100% / 4.0% | 99.3% / 0% / 3.7 | 2.13 ms |
|
||||
| 朴素流匹配 1 步 | 87.3% / **23.3%** | 81.3% / 0% / 14.0 | 0.25 ms |
|
||||
| **MeanFlow 1 步** | 100% / 2.7% | 100% / 0% / 0.3 | 0.25 ms |
|
||||
|
||||
- 朴素 1 步流匹配会把模态平均进柱子；MeanFlow 1 步达到 10 步流匹配的质量，推理快约 8 倍。
|
||||
- 安全层在所有方法上把穿柱降为 0，代价是刹停。
|
||||
- 该任务过于简单（闭环中状态一偏离中轴即打破对称），**分不出一步头与 WTA 谁更好**；因此 WTA 只作为 M5 门禁不过时的预定回退（§1.1）。
|
||||
- 训练仅 300 步的 MSE 在同一难例上 100% 穿柱、加安全层后 0% 到达，说明欠拟合时平均模态的风险真实存在。
|
||||
|
||||
---
|
||||
|
||||
## 3. 训练环境
|
||||
|
||||
| 阶段 | 在哪里跑 | 说明 |
|
||||
|---|---|---|
|
||||
| 数据生成 | Hark 沙箱（2 vCPU / 7 GB，Node）或用户机器 | 纯 JS，无需 GPU |
|
||||
| 预训练 + 后训练 | 用户 RTX 3090 24 GB | batch 1024–2048 |
|
||||
| 部署推理 | 浏览器 onnxruntime-web wasm-SIMD 多线程；后续真机控制器 | 与现有查看器同页 |
|
||||
|
||||
```bash
|
||||
conda create -n armtf python=3.11 -y && conda activate armtf
|
||||
pip install torch --index-url https://download.pytorch.org/whl/cu121 # 按本机 CUDA 版本选
|
||||
pip install numpy pyarrow tqdm tensorboard onnx onnxruntime
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 4. 离线数据（老师只在这里出现）
|
||||
|
||||
### 4.1 文件
|
||||
|
||||
| 文件 | 作用 |
|
||||
|---|---|
|
||||
| `ml/gen/gen_dataset.cjs` | 采样任务 → 调老师 → 校验 → 时间参数化 → 写分片 |
|
||||
| `ml/gen/sampler.cjs` | 起点/目标采样 |
|
||||
| `ml/gen/run_parallel.sh` | 多进程，不同种子 |
|
||||
| `ml/data/shard_XXXX.jsonl` | 原始样本（不入库） |
|
||||
|
||||
复用 `tests/route_bench_setup.cjs` 装配求解器（关节表、`docs/travel.json`、`docs/collide.json`）。
|
||||
|
||||
### 4.2 原始样本
|
||||
|
||||
```json
|
||||
{
|
||||
"id": "s0003_000127", "mode": "pos" | "pose",
|
||||
"q0": [6], "T": [x,y,z], "R": [[3x3]] | null, "tool": 0,
|
||||
"path": [[6], ...],
|
||||
"reached": true,
|
||||
"travel_deg": 123.4, "plan_ms": 812.0,
|
||||
"fp": "collide.json 指纹"
|
||||
}
|
||||
```
|
||||
|
||||
`reached=false` 表示目标不可达，路径终点为离目标最近的安全位形（"停靠"）。
|
||||
|
||||
### 4.3 采样分布（5 万条任务）
|
||||
|
||||
关节限位：J1/J3/J6 连续 ±180°；J2 [−92, 9]，J4 [−111, 16]，J5 [−93, 34]。
|
||||
|
||||
| 类别 | 占比 | 采样方式 |
|
||||
|---|---:|---|
|
||||
| A 均匀 | 45% | q0、目标位形 b 均匀采样且无干涉；目标 = `fk(b)`（pose 模式加 `orient(b)`） |
|
||||
| B 直线被挡 | 30% | q0→b 关节直线有干涉 |
|
||||
| C 贴立柱 | 15% | b 满足 `nearColumn`，q0 在立柱另一侧 |
|
||||
| D 目标不可达 | 10% | 目标在立柱内/所有分支都撞/工作空间外；标签为停靠 |
|
||||
|
||||
- pos / pose 各半；老师为 PRM 路网 A* + 捷径平滑（确定性；复用 `routeRT` 的路网图搜索，放宽时间预算）；A* 失败的任务用 `solveDyn`（固定种子）补。
|
||||
- 另造三个互不重叠的集合：**门禁集** 2000 条（挖错循环每轮用）、**held-out 验收集** 2000 条、**near_column 扩展验收集** ≥1000 条；两个验收集只在最终验收时用一次。现有三个基准不进训练集。
|
||||
|
||||
### 4.4 过滤
|
||||
|
||||
1. 整条路径用距离下界认证（与 §8.3 同口径）复核，任一段不过即丢弃。仓库 `tests/route_bench.cjs` 上现有规划器有 1/40 例路径细验不过，这类样本必须丢掉。
|
||||
2. `poseRelaxed` 样本只当 pos 模式用。
|
||||
3. 行程 > 同任务 `routeRT` 1.5 倍的先统计，再定是否丢弃；按类别统计丢弃率（C 类 > 20% 说明老师在贴柱场景不可靠）。
|
||||
|
||||
### 4.5 时间参数化 → 闭环训练样本
|
||||
|
||||
1. 时间律：按轴速比 `VREL = [0.6, 0.6, 0.9, 0.9, 1.2, 1.8]` 匀速巡航，梯形加减速，速度只取决于"当前速度 + 剩余路程"（每轴加速度上限 a_max 为待定参数，真机型号确定后设定），dt = 20 ms。
|
||||
2. 得到状态序列 q_0 … q_N，每隔 3 步取一个样本：
|
||||
- 输入：§5.1 全部字段（含当前速度、已提交前缀）
|
||||
- 标签：未来 16 步 Δq_{t+k} = q_{t+k} − q_t（到终点后补零）
|
||||
3. 每条路径约 40–50 个样本 → 约 200 万样本（以 `stats.json` 为准）。
|
||||
4. 协变量偏移对策：关节噪声增广（30% 样本，σ = 1°；噪声态须无干涉且间隙 > 1 mm，标签为 8 步内线性收回老师轨迹的增量）；偏离状态的进一步覆盖靠 §7。
|
||||
|
||||
### 4.6 耗时与存储
|
||||
|
||||
- 路网 A* 每条数十毫秒，耗时主要在过滤认证；按 `solveDyn` 0.7 s/条上限估算，沙箱 2 进程不超过 5 小时。
|
||||
- 200 万样本以 float32 `.npy` 存放约 1.5 GB（训练时整体载入内存）。
|
||||
|
||||
### 4.7 检查点
|
||||
|
||||
- [ ] `ml/data/stats.json`：类别、到达率、行程、样本数、补零样本占比
|
||||
- [ ] 老师一致性：起终点扰动 ±5 mm 的邻居任务同伦类一致率 ≥ 90%
|
||||
- [ ] 抽 20 条在查看器回放（新增 `?replay=` 参数）
|
||||
- [ ] 各集合与训练集无重叠
|
||||
|
||||
---
|
||||
|
||||
## 5. 表示与预处理
|
||||
|
||||
文件 `ml/prep/build_dataset.cjs`（Node，特征直接调用部署运行时 `webapp/js/ik-policy.js` 的同一份实现）→ `ml/data/samples/*.{X,Y,V,SG,TID}.npy` + `*.tasks.jsonl`；按任务 5% 划为验证集。PyTorch 侧特征 `ml/common/features.py` 与 JS 逐项对拍（闭环仿真与 APG 使用）。
|
||||
|
||||
### 5.1 输入
|
||||
|
||||
| 量 | 维度 | 编码 |
|
||||
|---|---:|---|
|
||||
| 当前 q_t | 6 | 每轴 (sin, cos) → 12；限位轴另加归一化值 → +3 |
|
||||
| 当前关节速度 q_t − q_{t−1} | 6 | 按每轴速度上限归一化 |
|
||||
| 当前 TCP 位置与姿态 | 3 + 6 | `fk(q_t)`、`orient(q_t)` 的 6D 表示 |
|
||||
| 目标位置 T | 3 | /600 mm |
|
||||
| 目标姿态 R | 6 | 6D 表示；pos 模式填 0 |
|
||||
| 目标 − 当前 TCP | 3 | /600 mm |
|
||||
| 模式 | 2 | one-hot [pos, pose] |
|
||||
| 已提交未执行前缀 | 3×6 + 3 | 前缀 Δq + 有效长度掩码；训练时随机遮盖 0–3 步 |
|
||||
| 当前子目标 | 6 | 子目标头输出的关节途经点（归一化）；无子目标时填当前 q |
|
||||
| 关键胶囊对最小距离 | 8 | 臂各段到立柱的胶囊距离 |
|
||||
|
||||
### 5.2 输出
|
||||
|
||||
- MeanFlow 一步头：每条候选 16 步 × 6 轴 Δq（按每轴最大步长归一化）。
|
||||
- 附加头：价值头（剩余时间/代价）、子目标头（2 个关节途经点 + 是否需要）。
|
||||
|
||||
### 5.3 连续轴
|
||||
|
||||
J1/J3/J6 统一用 `unwrapTo` 展开后再做增量；预处理与部署共用一份实现，对拍时一并验证。
|
||||
|
||||
---
|
||||
|
||||
## 6. 模型与预训练
|
||||
|
||||
### 6.1 结构
|
||||
|
||||
```
|
||||
条件特征（约 90 维）──MLP──▶ 8 个条件 token (d=192)
|
||||
│
|
||||
4 层 Transformer encoder
|
||||
│ (memory)
|
||||
16 个动作 token（注入噪声 z + 时间嵌入 (r, t)）
|
||||
▼
|
||||
4 层 Transformer decoder（非自回归，一次前向）
|
||||
├─ 动作输出：每 token 6 维 Δq
|
||||
├─ 价值头：1
|
||||
└─ 子目标头：2 个关节途经点 + 是否需要
|
||||
```
|
||||
|
||||
| 超参 | 值 |
|
||||
|---|---|
|
||||
| d_model / 头数 / FFN | 192 / 8 / 768 |
|
||||
| encoder / decoder 层数 | 4 / 4 |
|
||||
| 参数量 | 约 4.9M |
|
||||
|
||||
子目标触发：新目标或目标跳变 > 50 mm 时输出；到达当前子目标 < 5° 后切下一个。
|
||||
|
||||
### 6.2 动作头：MeanFlow 一步生成
|
||||
|
||||
| 项 | 做法 |
|
||||
|---|---|
|
||||
| 多模态 | 连续分布；每次推理并行采 8 个噪声 → 8 条候选 |
|
||||
| 训练 | MeanFlow 平均速度场损失（MP1 配方：25% 样本 r≠t，自适应损失加权），JVP 用 `torch.func.jvp` |
|
||||
| 推理 | 1 次批量前向（r=0, t=1） |
|
||||
| 后处理 | 1 步解析胶囊距离梯度投影，每轴修正 ≤ 0.5°，仅 d < 余量时（JS 实现，不走 ONNX） |
|
||||
| APG 兼容 | 固定噪声后为确定性可微映射 |
|
||||
| 预定回退 | M5 门禁不过（near_column 穿柱率 > 2% 或训练发散）→ 同骨干换 WTA K=8 头（WTA-L1 + 模式 CE），约 1 天 |
|
||||
|
||||
### 6.3 损失
|
||||
|
||||
```
|
||||
L = MeanFlow 损失 1.0
|
||||
+ FK 损失：一步输出（固定噪声）的各连杆胶囊端点位置差 0.5
|
||||
+ 平滑：二阶差分 0.05
|
||||
+ 价值头 L1 + 子目标头 L1 0.1 / 0.1
|
||||
```
|
||||
|
||||
### 6.4 训练配置
|
||||
|
||||
| 项 | 值 |
|
||||
|---|---|
|
||||
| 优化器 | AdamW，β=(0.9, 0.95)，wd 0.01 |
|
||||
| 学习率 | 1e-3，预热 1000 步，余弦退火 |
|
||||
| batch | 1024 |
|
||||
| 精度 | bf16 混合精度 |
|
||||
| 步数 | 先跑 2000 步测吞吐再定；参考 200 万样本 × 30 epoch ÷ 1024 ≈ 6 万步；验证集闭环到达率不再上升即停 |
|
||||
| 预计耗时 | RTX 3090 约 1.5–5.6 小时（按 2000 步吞吐实测重算） |
|
||||
|
||||
### 6.5 对拍与验证
|
||||
|
||||
- `ml/eval/collide_torch.py`：可微胶囊距离 PyTorch 批量版，与 JS `collideFree` 对拍 10 万位形：布尔判定 0 差异，距离误差 < 1e-4 mm，梯度与有限差分相对误差 < 1e-3。
|
||||
- 每 epoch 闭环验证：从 1000 个验证任务出发，按部署节奏（每 60 ms 推理、提交 3 步）运行到到达或超时（老师路径时长 × 2，最少 3 s），统计到达率、用时 ≤ 老师 × 1.3 的达标率、碰撞步数（关安全层）、行程比、左/右/穿柱三类比例。
|
||||
|
||||
---
|
||||
|
||||
## 7. 后训练
|
||||
|
||||
### 7.1 主力：APG 可微微调
|
||||
|
||||
```
|
||||
for 每个 batch（随机采样 q0 和目标，不需要老师）:
|
||||
q = q0
|
||||
for t in 0..H-1 (H = 60 次推理 × 3 步 = 180 步 ≈ 3.6 s；长任务分段截断反传):
|
||||
chunk = π(obs(q, 目标, 前缀), z) # 每条展开固定一个噪声 z
|
||||
Δq = chunk[0:3] # 与部署一致
|
||||
q = clamp(q + Δq)
|
||||
累计 L_col(q), L_smooth(Δq)
|
||||
L = w_tgt·L_target(q_H) + w_col·ΣL_col + w_smooth·ΣL_smooth (+ w_len·行程) + w_bc·L_BC
|
||||
反传，梯度裁剪 ≤ 1.0，lr 1e-4
|
||||
```
|
||||
|
||||
- 碰撞项 L_col = Σ ReLU(margin − 胶囊距离)，margin = `margin_mm` + 训练余量 5 mm（覆盖安全层认证的保守量）；含臂×立柱、臂×台架、臂×臂（非相邻段）。
|
||||
- 到达项：位置误差² + λ_rot·‖R − R*‖_F²。
|
||||
- 平滑项：速度² + 加速度²。
|
||||
- BC 锚定项：保留预训练损失，防止多模态被压缩；每轮监控左右比例。
|
||||
- 采样分布同 §4.3，B/C/D 类提到 60%，其中贴柱类 40%。
|
||||
|
||||
### 7.2 辅助：无老师挖错循环
|
||||
|
||||
每轮：
|
||||
1. 出题：新种子 5 万任务 + 2000 段模型在环拖拽仿真。
|
||||
2. 闭环作答：完全按部署方式运行，**关闭安全层**以暴露真实错误。
|
||||
3. 验算：GPU 批量胶囊检查。
|
||||
4. 错题分类：E1 撞 / E2 没到（卡在局部极小）/ E3 绕远（> 1.2× 参考）/ E4 末端精度差 / E5 摇摆（≥ 3 次切换）/ E6 拖拽失稳。
|
||||
5. 错题及其 ±5 mm / ±3° 的 10 个邻居加入 APG 采样池（E1、E6 ×2）；仅 E2 可离线请老师补少量路径。
|
||||
6. 门禁（用门禁集，不用验收集）：到达率上升、A 类每轮降幅 ≤ 0.5 个百分点、上一轮修好的错题 95% 仍对；否则回滚。
|
||||
7. 停止：连续 2 轮提升 < 0.5 个百分点，或满 6 轮。
|
||||
|
||||
---
|
||||
|
||||
## 8. 部署
|
||||
|
||||
### 8.1 执行循环
|
||||
|
||||
```
|
||||
执行：50 Hz，每步 20 ms，按已提交步走
|
||||
每 60 ms：
|
||||
1. 读当前 q、当前速度、已提交未执行前缀、最新目标
|
||||
2. 推理：8 个噪声 → 8 条 16 步动作块（冷启动或卡死换向时，再各推一块做短前瞻）
|
||||
3. 每条候选做 1 步胶囊距离梯度投影（每轴 ≤ 0.5°，仅 d < 余量时）
|
||||
4. 排序（§8.2）
|
||||
5. 安全层依次认证候选前 3 步 + 刹停段（§8.3），第一个通过的提交给执行器
|
||||
6. 全部不通过 → 沿上一次已认证的刹停段停下，下次继续推理
|
||||
7. 到达：TCP 误差 < 1 mm（pose 另需 < 0.5°）且速度≈0；
|
||||
接近目标（< 5 mm）时做 1–3 次局部牛顿精修（solveTo 单步），精修段同样过安全层认证
|
||||
```
|
||||
|
||||
执行的始终是安全层认证过的那一块，不做新旧块混合。执行时钟放在 Web Worker、按实际 dt 推进，避免 `requestAnimationFrame`（60 Hz）与 20 ms 不整除及后台节流造成的抖动。
|
||||
|
||||
### 8.2 候选排序（防左右摇摆）
|
||||
|
||||
分数 = −Σ w_k‖候选第 k 步 − 已提交块第 k 步‖²（重叠步加权）− λ·价值头剩余代价。冷启动时无已提交块，只用价值头 + 短前瞻打分。
|
||||
|
||||
### 8.3 安全层（唯一的硬保证）
|
||||
|
||||
- **距离下界认证**：对一段运动 q_a → q_b，若两端到障碍的最小距离之和 d(q_a) + d(q_b) > Σ_j r_j·|Δθ_j|，则整段安全；否则二分，直到全部子段通过或判不安全。按组对分别判定：组对 (g_A, g_B) 的间隙只受关节 g_A…g_B−1 带动组 g_B 影响，右端取 Σ_{k=g_A}^{g_B−1} T[k][g_B]·|Δθ_k|，T 为分组杠杆表（组 g_B 胶囊端点到轴 k 的最大距离，随机 4000 位形采样再放大 5%；末端组对 J1…J6 分别为 9.56 / 7.37 / 4.47 / 4.48 / 2.47 / 0.69 mm/°）。间隙取全部 |Δg| ≥ 2 胶囊对的 距离 − (r_i + r_j + margin)。外加关节限位。先用离散 `collideFree` 剔除明显干涉的候选；运行时每条候选最多 40 次距离查询、每次推理安全层总预算 30 ms，超出按刹停处理。
|
||||
- 离散采样检查（现有 `pathFine`，1.5° + 半格）只作快速预筛，不作保证依据。
|
||||
- **可刹停不变式**：只有当"候选前 3 步 + 从第 3 步末按每轴 a_max 减速到停"整段都通过认证时才提交。任何时候系统都持有一条已认证的停车路线，下一次推理即使完全失效也只会停下。
|
||||
- 耗时：每段一次距离查询覆盖整段，不通过才二分（通常 2–6 次）；按排序逐个认证，第一个通过即停；包围球预筛先行。目标每次 < 5 ms，否则放 Web Worker 与推理流水线并行。
|
||||
- `ik-glue.js` 的每帧碰撞守卫保留为第二道保险。
|
||||
|
||||
### 8.4 部署包
|
||||
|
||||
| 有 | 没有 |
|
||||
|---|---|
|
||||
| `traj_policy.onnx`（fp16，约 9.8 MB）+ `traj_policy.meta.json`（指纹 / 特征版本 / TCP） | PRM 路网、IK 候选大表 |
|
||||
| 胶囊距离与认证（`collideFree`、包围球预筛、`pair2d`） | `solveDyn` / `routeRT` / RRT / A* |
|
||||
| `fk` / `orient`、局部牛顿精修 `solveTo` 单步 | 全量 IK 枚举 |
|
||||
|
||||
`collide.json` 指纹写进模型；不一致时前端拒绝运行并提示重训。
|
||||
|
||||
### 8.5 near_column 30/30 的保障手段
|
||||
|
||||
"超时"指机械臂在规定时间内没走到目标，是决策问题而非计算速度问题。典型失效：局部极小/活锁（每次都想直穿、每次被安全层挡住）、左右摇摆、绕远。对策（运行时均不需要老师）：
|
||||
|
||||
1. **子目标头**：给出 1–2 个关节途经点作为绕柱方向锚点，标签取老师路径拐点（离线）。
|
||||
2. **价值头**：预测剩余时间/代价，用于候选打分与进展判断；APG 中用 TD 自举监督。
|
||||
3. **短前瞻**（仅冷启动和卡死换向时）：对每条候选再推一块，用胶囊认证 + 价值头给 32 步后终态打分。
|
||||
4. **卡死检测 + 强制换向**：0.3 s 内价值不降或连续 2 次刹停 → 切到另一绕行方向并锁定 1 s。
|
||||
5. **训练侧加强**：贴柱类占 APG 采样 40%；展开覆盖整段绕行。
|
||||
|
||||
### 8.6 控制周期
|
||||
|
||||
伺服层（参考）：Franka FCI 1 ms；UR e-Series 2 ms、CB3 8 ms；ABB EGM 4 ms（滞后 10–20 ms）；KUKA RSI 4/12 ms；FANUC DPM 约 8 ms；MoveIt Servo 默认 10 ms；RC 舵机 20 ms。
|
||||
|
||||
策略层（参考）：MπNets 9 Hz；Diffusion Policy 10 Hz、预测 16 执行 8；ELMP 30 Hz；π0 50 Hz、约每 0.5 s 推理一次；DRP 300 Hz。业界均为"策略层 10–50 Hz + 伺服层 125 Hz–1 kHz"两层结构。
|
||||
|
||||
本方案：执行 50 Hz、每 60 ms 推理一次、提交 3 步。真机上提交步数 n ≥ ⌈(通信滞后 + 一次推理最坏耗时) / 20 ms⌉ + 1，刹停段另行认证；策略输出 50 Hz 关节目标，由插补器升采样到电机控制器频率。`dt`、n、动作块长度均可配置。
|
||||
|
||||
---
|
||||
|
||||
## 9. 评估与验收
|
||||
|
||||
### 9.1 测试集合
|
||||
|
||||
| 集合 | 来源 | 用途 |
|
||||
|---|---|---|
|
||||
| route_bench | `tests/route_bench.cjs`（40 例）+ 60 例随机基准 | 验收 |
|
||||
| near_column_bench | `tests/near_column_bench.cjs`，30 例（pos / pose / rt） | 验收 |
|
||||
| near_column 扩展 | §4.3 新造 ≥1000 例 | 一次性验收 |
|
||||
| rt_drag_bench | `tests/rt_drag_bench.cjs`，12 例（模型在环） | 验收 |
|
||||
| held-out | §4.3 2000 条 | 一次性验收 |
|
||||
| 门禁集 | §4.3 2000 条 | 训练中门禁，不参与验收 |
|
||||
|
||||
新增 `tests/policy_bench.cjs`：①现有规划器 ②策略（关安全层）③策略 + 安全层，输出对比表。
|
||||
|
||||
### 9.2 验收标准（部署形态 = 策略 + 安全层）
|
||||
|
||||
| 指标 | 要求 | 现有规划器 |
|
||||
|---|---|---|
|
||||
| 实际执行的碰撞 | **= 0**（距离认证或 0.01° 审计测量） | rt_drag_bench 0 |
|
||||
| 到达率 | route 全部可达例到达；near pos **30/30**、near rt **30/30**；near_column 扩展集 **0 失败**；held-out 可达任务 ≥ 98%，D 类停靠距离 ≤ 老师 + 5 mm | `route_bench` 40 例：到达 40/40、细验有效 39/40；60 例随机基准有效 60/60；near 30/30、30/30 |
|
||||
| 到达用时 | ≤ 老师路径时长 × 1.3 | — |
|
||||
| 关节行程 | ≤ 现有规划器 × 1.10（按类别分别报告） | 60 例平均 467° |
|
||||
| 冷启动 | < 10 ms | 首次全量 IK 最差约 2 s |
|
||||
| 每次推理 + 安全认证 P99 | < 20 ms | 拖拽重规划 P99 约 321 ms |
|
||||
| 刹停发生率 | < 2% 的推理周期 | — |
|
||||
| 模式摇摆（≥3 次切换） | < 1% 的任务 | — |
|
||||
| 策略单独（关安全层）碰撞率 | < 3% | — |
|
||||
|
||||
说明：30 例全对时，失败率的 95% 置信上界约 9.5%；扩展集 1000 例 0 失败时约 0.3%，因此以扩展集作为 30/30 的统计支撑。
|
||||
|
||||
---
|
||||
|
||||
## 10. 当前存在的问题与风险
|
||||
|
||||
### 10.1 只能靠实测消除的风险（按里程碑顺序）
|
||||
|
||||
| # | 风险 | 门禁（不过不进下一步） | 回退 | 何时测 |
|
||||
|---|---|---|---|---|
|
||||
| R1 | 老师在相邻任务上左右绕行跳变，标签自相矛盾 | 起终点扰动 ±5 mm 的邻居任务同伦类一致率 ≥ 90% | 对 A* 边代价加左右偏好项，强制同区域选同侧 | M1 |
|
||||
| R2 | 时间律仍有不可观测成分，闭环起不了步 | 近邻样本标签方差 < 10% 步长；起步失败率 = 0 | 进一步把剩余路程估计加入输入 | M1 / M5 |
|
||||
| R3 | 距离认证过于保守，贴柱处拒绝率高，影响 30/30 | near_column 上认证拒绝率统计；离线 10⁵ 段 0.01° 审计漏检 = 0 | 训练余量提高到 3–6 mm，接受行程变长；收紧杠杆界（按位形分区） | M2 / M7 |
|
||||
| R4 | 浏览器推理延迟 | M4 后立即导出 ONNX，wasm-SIMD 多线程测 batch=8 P99 < 15 ms | 减小模型（d=128，约 1.8M 参数）；同时测 WebGPU，更快则换 | **M4（最大不确定项）** |
|
||||
| R5 | MeanFlow 头在 6 轴、位姿目标下穿柱或训练不稳 | near_column 穿柱率（关安全层）≤ 2%；损失不发散 | 同骨干换 WTA K=8 头 | M5 |
|
||||
| R6 | 位姿目标的多 IK 分支带来额外多峰 | 不同肘/腕分支子集到达率 ≥ 主集 − 2 个百分点 | 加目标分支标志 | M5 |
|
||||
| R7 | APG 压缩多模态或拉长路径（ELMP 中路径 +23%） | 每轮左右比例与行程门禁 | 加大 BC 锚定权重；行程项 | M6 |
|
||||
| R8 | 卡死换向活锁 | 50 个"两侧都要绕远"对抗用例 3 s 内恢复 ≥ 95%，活锁 = 0 | 子目标改为离散绕行锚点分类 | M6 / M7 |
|
||||
| R9 | 安全层贴柱时耗时超标 | near_column 全程 P99 < 5 ms | Web Worker 流水线；提交步数降到 2 | M7 |
|
||||
| R10 | 训练耗时估计偏差 | 2000 步吞吐 ≥ 2 万样本/s | 降 epoch 或增大 batch | M5 |
|
||||
|
||||
### 10.2 已知缺陷
|
||||
|
||||
1. 现有规划器（老师）在 `tests/route_bench.cjs` 上有 1/40 例到达但路径细验不过；§4.4 的认证过滤会丢弃这类样本。
|
||||
2. `ci27` 姿态根枚举漏分支（姿态偏差约 97.5°）会影响 pose 数据；APG 姿态项可部分纠正，必要时先修老师。
|
||||
3. 胶囊模型或立柱变动即需重训：数据（用户机器 16 核约 2 h）+ 预训练 1.5–6 h + 后训练数小时。
|
||||
4. 老师一致性沙箱首测 78%（100 任务、373 对邻居，门禁 90%）：不一致主要来自 J6 冗余与时间预算造成的非确定性。已改为确定性老师（预算按碰撞判定次数计、每条前清空跨调用缓存）并在 pos 任务冻结 J6，待在用户机器复测；仍不达标则生成时启用 4 邻居共识过滤。
|
||||
5. 安全层耗时：未训练模型（候选为噪声）在 Node 单线程实测每次推理安全层约 20–60 ms，高于 5 ms 目标；训练后候选平滑会下降，以 `policy_bench` 与浏览器 `__POLICY.latency()` 实测为准，超标则把安全层移入 Web Worker 与推理流水线并行。
|
||||
|
||||
### 10.3 需要用户确认
|
||||
|
||||
1. **真机电机/驱动型号与通信方式**：决定每轴加速度上限 a_max、插补频率与提交步数。
|
||||
2. **运行时多候选打分是否算"运行时老师"**：本方案的理解是不算（只对模型自己的候选做距离认证和打分，不做搜索）。
|
||||
|
||||
---
|
||||
|
||||
## 11. 目录规划
|
||||
|
||||
运行步骤、命令与耗时见 `ml/README.md`。
|
||||
|
||||
```
|
||||
ml/
|
||||
pipeline.sh 阶段脚本：setup / kin / gate1 / gen / prep / train / m5 / apg / mine / export / bench
|
||||
gen/ sampler / teacher（确定性 PRM A*）/ timelaw / gen_dataset / consistency / stats / run_parallel.sh
|
||||
prep/ export_kin.cjs → assets/kin.json；build_dataset.cjs → data/samples/*.npy；run_prep.sh
|
||||
data/ （不入库）shard_*.jsonl stats.json consistency.json samples/
|
||||
common/ kin_torch.py（可微运动学 + 胶囊间隙） features.py（83 维特征）
|
||||
model/ policy.py（MeanFlow 头；WTA K=8 回退头） meanflow.py
|
||||
data.py train.py 样本加载、预训练
|
||||
sim/closed_loop.py 闭环仿真（验证、挖错、门禁共用）
|
||||
apg/apg_finetune.py APG 可微微调
|
||||
mining/ tasks.py mine_round.py gate.py loop.sh
|
||||
eval/ dump_parity.cjs check_parity.py（JS ↔ PyTorch 对拍）
|
||||
configs/ base.json wta.json apg.json
|
||||
export_onnx.py serve.py README.md
|
||||
webapp/js/ik-policy.js 运行时：特征、距离下界认证、刹停、投影、精修、Runner（浏览器与 Node 共用）
|
||||
webapp/js/ik-policy-glue.js 查看器接入（?policy=1）
|
||||
webapp/models/ traj_policy.onnx traj_policy.meta.json
|
||||
tests/policy_bench.cjs 部署形态评测
|
||||
```
|
||||
|
||||
## 12. 里程碑
|
||||
|
||||
| # | 内容 | 产出 | 在哪跑 |
|
||||
|---|---|---|---|
|
||||
| M1 | 数据生成器 + 时间律 + 1000 条试跑；老师一致性门禁 | `ml/gen/*`、stats.json | 沙箱 |
|
||||
| M2 | 可微胶囊距离（PyTorch）与 JS 对拍；距离认证实现与 10⁵ 段审计 | `collide_torch.py`、认证模块 | 沙箱 |
|
||||
| M3 | 5 万条老师数据 → 约 180 万样本 | `samples/*.npy` | 用户机器 |
|
||||
| M4 | 模型 + 训练工程 + 闭环仿真；小数据过拟合；**导出 ONNX 测浏览器延迟** | `train.py`、`closed_loop.py`、延迟报告 | 沙箱 + 浏览器 |
|
||||
| M5 | 预训练 + MeanFlow 头门禁 | checkpoint、门禁报告 | 用户 GPU |
|
||||
| M6 | APG 微调 + 挖错循环 3–6 轮 | 每轮 report.md | 用户 GPU |
|
||||
| M7 | `ik-policy.js` 闭环部署 + 安全层实测 + `policy_bench` + 一次性验收 | 对比表 | 沙箱 + 浏览器 |
|
||||
| M8 | 移除查看器对 `solveDyn`/`routeRT`/路网的运行时依赖（保留在 `ml/gen` 离线使用） | 部署包瘦身 | 沙箱 |
|
||||
|
||||
## 13. 参考文献
|
||||
|
||||
- MπNets：https://arxiv.org/abs/2210.12209
|
||||
- Neural MP：https://arxiv.org/abs/2409.05864
|
||||
- Deep Reactive Policy / IMPACT：https://arxiv.org/abs/2509.06953
|
||||
- ELMP：https://arxiv.org/abs/2607.00215
|
||||
- Diffusion Policy：https://arxiv.org/abs/2303.04137
|
||||
- π0：https://arxiv.org/abs/2410.24164 ；π0.5：https://arxiv.org/abs/2504.16054
|
||||
- FAST：https://arxiv.org/abs/2501.09747
|
||||
- MP1 / MeanFlow：https://arxiv.org/abs/2507.10543
|
||||
- One-Step Flow Policy：https://arxiv.org/abs/2603.12480
|
||||
- 条件 OT 流策略：https://proceedings.mlr.press/v305/sochopoulos25a.html
|
||||
- Kinematic MeanFlow：https://arxiv.org/abs/2610.00864
|
||||
- Consistency Policy：https://arxiv.org/abs/2405.07503 ；OneDP：https://arxiv.org/abs/2410.21257
|
||||
- MPD：https://arxiv.org/abs/2308.01557 ；EDMP：https://arxiv.org/abs/2309.11414 ；GPD：https://arxiv.org/abs/2501.18229 ；DiffusionSeeder：https://arxiv.org/abs/2410.16727
|
||||
- Real-Time Chunking：https://arxiv.org/abs/2506.07339 ；训练时动作条件化：https://arxiv.org/abs/2512.05964
|
||||
- Bidirectional Decoding（BID）：https://arxiv.org/abs/2408.17355
|
||||
- DPPO：https://arxiv.org/abs/2409.00588 ；ReinFlow：https://arxiv.org/abs/2505.22094
|
||||
- Schwarzer, Saha, Latombe, *Adaptive Dynamic Collision Checking for Single and Multiple Articulated Robots in Complex Environments*, IEEE T-RO 2005
|
||||
- Thumm & Althoff, *Provably Safe Deep Reinforcement Learning for Robotic Manipulation in Human Environments*, ICRA 2022
|
||||
- Zhou et al., *On the Continuity of Rotation Representations in Neural Networks*, CVPR 2019
|
||||
- AWE（航点提取）：https://arxiv.org/abs/2307.14326 ；SHAC：https://arxiv.org/abs/2204.07137
|
||||
- 控制周期：Franka FCI https://frankarobotics.github.io/docs/overview.html ；UR RTDE https://www.universal-robots.com/articles/ur/interface-communication/remote-control-via-tcpip ；ABB EGM 应用手册 3HAC073318；KUKA RSI https://docs.quanser.com/quarc/documentation/kuka_rsi_block.html
|
||||
+5
-2
File diff suppressed because one or more lines are too long
@@ -0,0 +1,102 @@
|
||||
# 避障路径规划优化报告（2026-10-15，基于 1dfdd48）
|
||||
|
||||
## 现状问题（旧 routeOrVia 阶梯）
|
||||
1. **穿模**：`routeAroundPlan` 每段只查 ≤8 点（长段采样间隔 15°+），`coordinated` 二分松弛只查 6 点。
|
||||
60 个"直线被挡"随机场景里 18 条路径在 1° 细验下实际穿模（13 条为 coordinated）。
|
||||
2. **怪动作**：`CRUISE` 巡航位形把 J3/J5/J6 强制拉到固定值（0/-60/0），与障碍无关的腕部和大臂自转也被
|
||||
"归零再转回"；回退绕行（retract_pass）先收臂再伸展，路径绕远。
|
||||
3. **单目标**：绕行只认一个目标解 qT，换个肘/腕构型就能直走的情况被忽略。
|
||||
4. **动画顿挫**：`animTo` 每个中间位形各占固定 ms 并各自 smoothstep —— 每过一个 via 都减速到 0；
|
||||
5° 小段与 150° 大段用同样时间。
|
||||
|
||||
## 改动
|
||||
- `ik-solver.js` 新增 6c 节：
|
||||
- `gatherGoals`：候选表里端点安全的 IK 解按加权关节距离取前 6 个（同盆地去重）作为目标集；
|
||||
- 先试多目标直线（1.5° 细验）→ 再试弦中点 J2/J4 微调单中间点 → 再上 **Lazy RRT-Connect**
|
||||
（起点树 ↔ 多根目标树；生长时只查节点 + 边中点，连通后整条路径细验，坏边判死子树）；
|
||||
- `shortcut`：随机捷径 + 贪心最远可达 + 逐轴拉直（去掉与障碍无关轴的来回摆）；
|
||||
- 加权度量 `PW=[1.6,1.4,1,1,0.7,0.5]`（近端轴代价高），定种子 LCG，结果可复现；
|
||||
- 连续轴段内变化 <179°，保证与查看器短弧插值走的是同一条线；
|
||||
- 规划失败时仍回落旧阶梯与 routeVia，最坏行为不劣于旧版。
|
||||
- 碰撞窄相位前加**胶囊级包围球预筛**（保守、零漏判）：`collideFree` 2.9 → 1.8 ms/次，
|
||||
4000 个随机位形与旧实现判定 0 差异。
|
||||
- `ik-glue.js animTo`：各段时长按"最慢轴所需时间"（轴速比 VREL）分配，整条折线一次全局缓入缓出，
|
||||
中间位形不停顿；总时长按路径长度伸缩。
|
||||
|
||||
## 基准（`node tests/route_bench.cjs [solver] 60`，同一批 60 个直线被挡场景）
|
||||
| 指标 | 旧 | 新 |
|
||||
|---|---|---|
|
||||
| 1° 细验无穿模的路径 | 42/60 | 58/60（剩 2 条最大侵入 0.28/0.08 mm，在 0.3 mm 安全裕度内） |
|
||||
| 到达且无穿模 | 37 | 53 |
|
||||
| 两版都成功的 37 例：平均关节总行程 | 417° | 388°（−7%） |
|
||||
| 同上：估算执行时间 | 2.56 s | 2.46 s |
|
||||
| 平均规划耗时（Node 单线程） | 0.72 s | 1.23 s（全集）/ 0.94 s（37 例） |
|
||||
|
||||
到不了的 5 例两版相同，属于目标超出安全可达区（IK 残差 4~235 mm），不是规划问题。
|
||||
|
||||
## 已知取舍 / 后续
|
||||
- 规划耗时上升约 0.3~0.5 s（Worker 里跑，不卡界面）；拖拽实时模式预算 150 ms，超时回落旧阶梯。
|
||||
- 执行层仍是关节空间折线；若要上真机，下一步应做带加速度约束的时间最优参数化（TOPP）+ 拐角抛物线过渡，
|
||||
过渡段需重新做碰撞校验。
|
||||
|
||||
## 二期：规划提速（2026-10-15）
|
||||
|
||||
逐段计时（20 个被挡场景）：时间主要花在"单中间点微调"（~350 ms/例）、RRT（~280 ms）和平滑（~250 ms 截止）；
|
||||
每次碰撞判定约 1.5 ms，其中 80% 的线段距离计算来自 (2,4) 类零件对（P05/M3 × P07/M5，长期相距 ~5 mm）。
|
||||
|
||||
改动：
|
||||
1. **两关节零件对类间隙表**：(2,4) 类的相对位姿只取决于 J3、J4，离线按 1° 网格生成最小间隙表
|
||||
（`webapp/tools/gen_pair2d.cjs` → `docs/collide.json` 的 `pair2d`）。运行时取最近网格点，按杠杆界
|
||||
（J3 3.24 mm/°、J4 2.54 mm/°）给出上下界，能定论就跳过/判撞，否则回退精算。2 万随机姿态与精算判定 0 差异；
|
||||
单次判定 1.60 → 1.25 ms。指纹校验：重新拟合胶囊后未重生成表会自动弃表（只变慢不出错）。
|
||||
2. **前移启发**：撞上的零件对/簇对移到表头，规划中反复撞同一处时首查即短路。
|
||||
3. **单次求解内判定记忆**：routeFrom / gatherGoals / 规划器对同一批 IK 候选不再重复判定。
|
||||
4. **routeFrom 惰性回退**：被挡时先不算"贴边停"位形（findSafeTarget + 二分），规划失败才补算。
|
||||
5. 微调只试前 2 个目标；平滑 50 次随机捷径 / 160 ms 截止，逐轴拉直 1 轮。
|
||||
|
||||
同一批 60 例（`node tests/route_bench.cjs <solver> 60`）：
|
||||
|
||||
| 指标 | 一期 | 二期 |
|
||||
|---|---:|---:|
|
||||
| 平均规划耗时 | 1239 ms | 837 ms（−32%） |
|
||||
| 到达目标 | 55 | 58 |
|
||||
| 1° 细验不穿模 | 58 | 58 |
|
||||
| 关节总行程均值 | 438° | 436° |
|
||||
| 往返次数均值 | 2.08 | 1.98 |
|
||||
|
||||
尝试过但未采用：基于间隙 + 杠杆界的保守连续检测。(2,4) 类常年仅 ~5 mm 间隙把步长压到 <1°，
|
||||
单次间隙查询又比布尔判定贵，实测比 1.5° 离散检测慢 25%。
|
||||
|
||||
## 第三轮：实时版（2026-10-16）
|
||||
|
||||
### 思路
|
||||
立柱和自身零件都是**静态障碍**，关节空间的自由区永远不变，所以把"安全路网"离线算好、随碰撞模型发布；运行时只做"接入 + 查图"。
|
||||
|
||||
1. **离线路网（PRM）**：`webapp/tools/gen_roadmap.cjs` 均匀采样 2500 个无干涉位形，连 10 近邻，每条边按 1.5° 细验（双线程约 9 分钟）。结果：15103 条有效边、**单一连通分量**。写入 `docs/collide.json` 的 `roadmap` 字段（约 130 KB），按碰撞模型 + 限位指纹校验，不符自动弃用、退回在线 RRT。
|
||||
- 重新拟合胶囊或改限位后，按顺序重跑：`node webapp/tools/gen_pair2d.cjs`，再跑 `node webapp/tools/gen_roadmap.cjs`（pair2d 生成会截掉其后的 roadmap 字段）。
|
||||
2. **在线 Lazy A\***：起点和各 IK 目标接到 12 近邻，只细验路径上的"接入边"，失败删边重搜；路网内部边零成本。
|
||||
3. **会话内终身增广**：每条细验通过的接入边连同起点/目标并入内存路网；**超时也保留已验进度**，下一帧接着用（跨帧随时可停）。
|
||||
4. **拖拽热启动**：重规划起点就在上一条已验路径上，"起点 → 段终点"免检；新目标只需验 1~2 条短边。
|
||||
5. **实时管线 `routeRT`**（拖拽 quick 模式自动启用）：直连 → 热启动 → 路网 A\*（40 ms）→ 沿旧路径继续走 → 都不行就停在直线上最后一个安全采样点。**不再进入旧的启发式阶梯**（仿真里它的段只抽查少量点，会穿模）；松手口径的旧阶梯 / 抬高 via 结果执行前也补了整条 1.5° 细验。
|
||||
6. 热跟随解撞了时补一次 `solveDyn` 全量解（同一目标缓存；拖动中 40 mm 内复用附近目标的安全解，目标停下再精解）。
|
||||
7. 碰撞判定：立柱组（组 0）的胶囊/簇/零件中心变换只算一次永久缓存（4000 个随机位形与上一版判定差异 0）。
|
||||
8. 前端 `ik-glue.js`：动画每帧对显示位形做一次碰撞判定（实时碰撞守护，约 1.2 ms/帧，命中即停并亮红标，`window.__RT_GUARD()` 看统计）；被新规划打断时从运动中接续（只缓出），拖拽跟随不再一顿一顿。
|
||||
|
||||
### 实时拖拽仿真（`tests/rt_drag_bench.cjs`，12 个横穿立柱场景 × 480 帧 @60 fps）
|
||||
|
||||
| 指标 | 原版 1dfdd48 | 上一版 919e704 | 实时版 |
|
||||
|---|---:|---:|---:|
|
||||
| 重规划耗时 P50 | 14.9 ms | 2.6 ms | **5.3 ms** |
|
||||
| 重规划耗时 P90 | 43.7 ms | 14.5 ms | **18.2 ms** |
|
||||
| 重规划耗时 P99 | 464 ms | 171 ms | **169 ms** |
|
||||
| 穿模帧（1° 细验） | 15 | 430 | **0** |
|
||||
| 拖完 6 s 内到达 | 9/12 | 10/12 | 10/12 |
|
||||
|
||||
上一版 P50 更低是因为它在拖拽中几乎不绕行（直接停/直走），代价是 430 帧穿模。
|
||||
|
||||
### 松手全量规划（`tests/route_bench.cjs`，60 场景）
|
||||
有效路径 **60/60**（上一版 58），到达 58，平均关节行程 458°（上一版 436°），往返 1.86 次，执行 2.60 s。平均规划耗时 662 ms（与拖拽仿真并行跑测得，偏高）。
|
||||
|
||||
### 剩余问题
|
||||
- 尾部尖峰（最大约 2 s）来自目标刚进入"热跟随解会撞"区域时的首次全量 IK，以及偶发长接入边细验；下一步可把全量 IK 改为分帧增量。
|
||||
- 未做真实浏览器交互与实体电机测试。
|
||||
+190
@@ -0,0 +1,190 @@
|
||||
# 机械臂避障策略：从数据到部署的运行手册
|
||||
|
||||
目标：在你的机器上从零跑出 Transformer 避障策略，并在查看器里直接用上。
|
||||
方案依据见 `docs/Transformer轨迹拟合训练方案.md`。所有命令在**仓库根目录**执行。
|
||||
|
||||
最省事的跑法是按阶段执行 `bash ml/pipeline.sh <阶段>`，每一阶段完成后看一眼本手册对应小节的「检查」再进下一步。
|
||||
|
||||
| 阶段 | 命令 | 预计耗时（16 核 CPU + RTX 3090） | 产出 |
|
||||
|---|---|---|---|
|
||||
| 1 环境 | `bash ml/pipeline.sh setup` | 10 分钟 | 依赖 |
|
||||
| 2 对拍 | `bash ml/pipeline.sh kin` | 2 分钟 | `ml/assets/kin.json`、对拍 PASS |
|
||||
| 3 老师门禁 | `bash ml/pipeline.sh gate1` | 25–30 分钟（单核） | `ml/data/consistency.json` |
|
||||
| 4 生成数据 | `bash ml/pipeline.sh gen` | 1–2 小时；加共识过滤 5–9 小时 | `ml/data/shard_*.jsonl`、`stats.json` |
|
||||
| 5 打包样本 | `bash ml/pipeline.sh prep` | 15–30 分钟 | `ml/data/samples/` |
|
||||
| 6 预训练 | `bash ml/pipeline.sh train` | 先 2000 步测速，正式约 1.5–6 小时 | `ml/runs/base/best.pt` |
|
||||
| 7 M5 门禁 | `bash ml/pipeline.sh m5` | 10 分钟 | `ml/runs/m5_gate.json` |
|
||||
| 8 APG 微调 | `bash ml/pipeline.sh apg` | 以前 100 步测速为准（预计 2–4 小时） | `ml/runs/apg/best.pt` |
|
||||
| 9 挖错循环 | `bash ml/pipeline.sh mine` | 每轮约 1–2 小时，3–6 轮 | `ml/runs/current.pt` |
|
||||
| 10 导出 | `bash ml/pipeline.sh export` | 1 分钟 | `webapp/models/traj_policy.onnx` |
|
||||
| 11 评测 | `bash ml/pipeline.sh bench` | 约 1 小时（Node CPU，1070 个任务） | `ml/runs/policy_bench.json` |
|
||||
| 12 使用 | `python ml/serve.py` → 浏览器 `?policy=1` | — | — |
|
||||
|
||||
耗时除第 4 步外均为估算，以每步首次输出的速度为准。
|
||||
|
||||
---
|
||||
|
||||
## 1. 环境
|
||||
|
||||
- Linux 或 WSL2；NVIDIA 驱动 + CUDA（`nvidia-smi` 能看到 3090）
|
||||
- Node ≥ 18，Python 3.10–3.12
|
||||
- 先按 CUDA 版本装 PyTorch（https://pytorch.org ，例：`pip install torch --index-url https://download.pytorch.org/whl/cu124`），再：
|
||||
|
||||
```bash
|
||||
git pull
|
||||
bash ml/pipeline.sh setup # pip install -r ml/requirements.txt；cd ml && npm install（onnxruntime-node）
|
||||
```
|
||||
|
||||
## 2. 碰撞模型导出与对拍（必须 PASS）
|
||||
|
||||
```bash
|
||||
bash ml/pipeline.sh kin
|
||||
```
|
||||
|
||||
检查：最后打印的 JSON 里 `"PASS": true`（fk、姿态、组对间隙、83 维特征、间隙梯度与 JS 一致）。
|
||||
`docs/collide.json` 以后只要重新拟合或重生成 `pair2d`/`roadmap`，这一步、数据、模型都要重来（指纹不符时各脚本会拒绝运行）。
|
||||
|
||||
## 3. 老师一致性门禁
|
||||
|
||||
```bash
|
||||
bash ml/pipeline.sh gate1
|
||||
```
|
||||
|
||||
看 `consistentByGateRule`：≥ 0.90 直接生成；< 0.90 时第 4 步会自动加 `--consensus 4`（过滤与邻居绕法不一致的样本，生成成本约 5 倍）。也可手动用 `CONSENSUS=0/1` 覆盖。
|
||||
|
||||
## 4. 生成老师数据
|
||||
|
||||
```bash
|
||||
bash ml/pipeline.sh gen # 默认 5 万条、进程数 = CPU 核数
|
||||
# 自定义：N_TASKS=50000 NPROC=16 CONSENSUS=1 bash ml/pipeline.sh gen
|
||||
```
|
||||
|
||||
每条约 2 秒/核（不共识）。检查 `ml/data/stats.json`：`keepRate` 应在 0.95 左右，`byCatMode` 里 C 类（贴立柱）有足够样本，`projectedSamplesFor50k` 约 180 万。
|
||||
|
||||
## 5. 打包训练样本
|
||||
|
||||
```bash
|
||||
bash ml/pipeline.sh prep
|
||||
```
|
||||
|
||||
每条轨迹按 50 Hz 时间律重放，每 3 步取一个样本（特征与部署同一份 JS 代码计算）。检查 `ml/data/samples/index.json`：`train_samples` 约 170 万、`val_samples` 约 9 万。
|
||||
|
||||
## 6. 预训练
|
||||
|
||||
```bash
|
||||
bash ml/pipeline.sh train
|
||||
```
|
||||
|
||||
先跑 2000 步测吞吐（`ml/runs/bench/log.jsonl` 的 `samples_per_s`），再正式训练到 `configs/base.json` 的 60000 步或闭环到达率连续 5 次不升为止。
|
||||
训练中每 2000 步打印一行 `EVAL {...}`：闭环（部署节奏）`success` 到达率、`collision_rate`、`time_ok_1p3`。
|
||||
显存不够就把 `configs/base.json` 的 `batch` 改成 512。中断后加 `--resume ml/runs/base/last.pt` 续训：
|
||||
|
||||
```bash
|
||||
python -m ml.train --samples ml/data/samples --out ml/runs/base --resume ml/runs/base/last.pt
|
||||
```
|
||||
|
||||
## 7. M5 门禁：MeanFlow 头是否可用
|
||||
|
||||
```bash
|
||||
bash ml/pipeline.sh m5
|
||||
```
|
||||
|
||||
在 1000 个贴立柱任务上关安全层闭环运行。穿柱率 ≤ 2% 通过；不通过（或训练日志出现「训练发散」）则换预定回退的 WTA K=8 头重训：
|
||||
|
||||
```bash
|
||||
CFG=ml/configs/wta.json bash ml/pipeline.sh train && bash ml/pipeline.sh m5
|
||||
```
|
||||
|
||||
## 8. APG 可微微调
|
||||
|
||||
```bash
|
||||
bash ml/pipeline.sh apg
|
||||
```
|
||||
|
||||
按部署节奏可微展开策略，直接优化到达、碰撞余量（5 mm）、平滑与行程，并保留 BC 锚定。每 500 步闭环验证，按到达率存 `best.pt`，完成后复制为 `ml/runs/current.pt`。
|
||||
显存不够把 `configs/apg.json` 的 `batch` 降到 64。
|
||||
|
||||
## 9. 挖错循环
|
||||
|
||||
```bash
|
||||
bash ml/pipeline.sh mine # 默认 6 轮；ROUNDS=3 APG_STEPS=1000 MINE_N=50000 可调
|
||||
```
|
||||
|
||||
每轮：5 万道新题关安全层作答 → 错题分类 E1–E5 → 错题与邻居进 APG 采样池 → E2（局部极小）请老师补少量路径 → APG 微调 → 门禁集比较，不达标自动回滚。连续 2 轮提升 < 0.5 个百分点自动停止。每轮报告在 `ml/runs/mine/r*/report.md` 与 `gate.json`。
|
||||
|
||||
## 10. 导出
|
||||
|
||||
```bash
|
||||
bash ml/pipeline.sh export # fp16 ONNX + meta.json → webapp/models/
|
||||
```
|
||||
|
||||
fp16 转换报错时去掉 `--fp16` 手动导出 fp32（约 20 MB，同样可用）：
|
||||
|
||||
```bash
|
||||
python -m ml.export_onnx --ckpt ml/runs/current.pt --out webapp/models/traj_policy.onnx
|
||||
```
|
||||
|
||||
## 11. 部署形态评测（策略 + 安全层）
|
||||
|
||||
```bash
|
||||
bash ml/pipeline.sh bench
|
||||
```
|
||||
|
||||
跑与浏览器同一份运行时（`webapp/js/ik-policy.js`）：贴立柱 30 例（pos、pose 各一遍）、直线被挡 40 例、一次性贴立柱扩展集 1000 例。
|
||||
验收看 `ml/runs/policy_bench.json`：各集合 `collisionFrames` 必须为 0；`near_column_*` 的 `ok` 30/30；`near_column_1000` 0 失败为目标。
|
||||
Node 里的推理/安全层耗时只作参考，浏览器延迟用第 12 步实测。
|
||||
|
||||
## 12. 在查看器里使用
|
||||
|
||||
```bash
|
||||
python -m arm.viewer.build_viewer # 若 webapp/generated/00_data.js 还没生成（查看器原有构建步骤）
|
||||
python ml/serve.py # 带跨源隔离头的本地服务，wasm 可用多线程
|
||||
```
|
||||
|
||||
浏览器打开 `http://localhost:8000/?policy=1`：右上角徽标显示「策略模式就绪」后，拖动黄球或松手，手臂由策略闭环执行（不调用任何规划器）。
|
||||
- `&ep=webgpu` 改用 WebGPU 后端对比延迟
|
||||
- 控制台 `__POLICY.latency()` 查看推理与安全层的 P50/P99；`__POLICY.stats()` 查看刹停、换向、精修次数
|
||||
- 工具偏移 ≠ 0 时自动交回原规划器
|
||||
- 离线环境：把 onnxruntime-web 的 `ort.min.js` 与 wasm 文件放到 `webapp/vendor/ort/`，地址加 `&ortLocal=1`
|
||||
|
||||
## 13. 回传给我
|
||||
|
||||
每完成一步把对应文件发我即可，我据此判断下一步或改代码：
|
||||
|
||||
| 步骤 | 文件 |
|
||||
|---|---|
|
||||
| 2 | 对拍输出（终端 JSON） |
|
||||
| 3–5 | `ml/data/consistency.json`、`ml/data/stats.json`、`ml/data/samples/index.json` |
|
||||
| 6 | `ml/runs/bench/log.jsonl`（测速）、`ml/runs/base/log.jsonl` |
|
||||
| 7 | `ml/runs/m5_gate.json` |
|
||||
| 8–9 | `ml/runs/apg/log.jsonl`、`ml/runs/mine/r*/report.json` 与 `gate.json` |
|
||||
| 11–12 | `ml/runs/policy_bench.json`、浏览器 `__POLICY.latency()` 输出 |
|
||||
|
||||
## 常见问题
|
||||
|
||||
- **指纹不符**：`collide.json` 变了。依次重跑第 2、4、5 步并重训。
|
||||
- **训练发散**（loss 出现 nan/inf，自动停在 `diverged.pt`）：先把 `lr` 降到 5e-4 重试一次，仍发散就走 WTA 回退（第 7 步）。
|
||||
- **显存不足**：降 `batch`；APG 还可把 `seg_plans` 从 10 降到 5。
|
||||
- **浏览器只有 1 线程**：没走 `ml/serve.py`（缺 COOP/COEP 头）。
|
||||
- **查看器徽标「策略模式不可用」**：看控制台报错，多为模型文件缺失、指纹或 TCP 不符。
|
||||
|
||||
## 文件地图
|
||||
|
||||
```
|
||||
ml/
|
||||
pipeline.sh 一条龙阶段脚本
|
||||
gen/ 老师数据：sampler / teacher（确定性 PRM A*）/ timelaw / gen_dataset / consistency / stats / run_parallel.sh
|
||||
prep/ export_kin.cjs（→ assets/kin.json）、build_dataset.cjs（→ samples/*.npy）、run_prep.sh
|
||||
common/ kin_torch.py（可微运动学 + 胶囊间隙）、features.py（83 维特征，与 JS 逐项一致）
|
||||
model/ policy.py（Transformer + MeanFlow / WTA 头）、meanflow.py
|
||||
data.py train.py 样本加载、预训练
|
||||
sim/closed_loop.py 部署节奏闭环仿真（验证 / 挖错 / 门禁）
|
||||
apg/apg_finetune.py APG 可微微调
|
||||
mining/ tasks.py（无老师出题）、mine_round.py、gate.py、loop.sh
|
||||
eval/ dump_parity.cjs + check_parity.py（JS↔PyTorch 对拍）
|
||||
export_onnx.py serve.py
|
||||
configs/ base.json（MeanFlow）、wta.json（回退）、apg.json
|
||||
webapp/js/ik-policy.js 运行时：特征、距离下界认证、刹停、投影、精修、Runner（浏览器与 Node 共用）
|
||||
webapp/js/ik-policy-glue.js 查看器接入（?policy=1）
|
||||
tests/policy_bench.cjs 部署形态评测
|
||||
```
|
||||
@@ -0,0 +1,141 @@
|
||||
"""APG 可微微调（方案 §7.1）：按部署节奏可微展开策略，直接对"走得到、不碰、平滑"求梯度；不需要老师。
|
||||
python -m ml.apg.apg_finetune --init ml/runs/base/best.pt --samples ml/data/samples --out ml/runs/apg [--pool ml/runs/mine/pool.jsonl]
|
||||
每条展开固定一个噪声 z（确定性可微映射）；每次推理提交 3 步（已提交前缀 3 步，与部署一致）；
|
||||
截断反传：每 seg_plans 次推理（=3·seg_plans 步）反传一次并截断状态梯度。
|
||||
损失：段末到达项 + Σ碰撞铰链 ReLU(margin_train − 间隙)（逐胶囊对）+ 平滑（速度²+加速度²）+ 行程 + BC 锚定（MeanFlow 损失）。"""
|
||||
import argparse, json, math, os, random, time
|
||||
import torch
|
||||
|
||||
from ml.common.kin_torch import Kin
|
||||
from ml.common.features import features, STEP, H, wrap180
|
||||
from ml.data import Samples, load_tasks
|
||||
from ml.model.policy import build, wta_loss
|
||||
from ml.model.meanflow import meanflow_loss
|
||||
from ml.sim.closed_loop import evaluate, tasks_to_tensors, goal_err
|
||||
|
||||
|
||||
def load_pool(paths):
|
||||
out = []
|
||||
for p in paths or []:
|
||||
if os.path.exists(p):
|
||||
out += [json.loads(l) for l in open(p, encoding='utf-8') if l.strip()]
|
||||
return out
|
||||
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument('--init', required=True); ap.add_argument('--samples', nargs='+', default=['ml/data/samples'])
|
||||
ap.add_argument('--pool', nargs='*', default=[]); ap.add_argument('--out', default='ml/runs/apg')
|
||||
ap.add_argument('--config', default='ml/configs/apg.json'); ap.add_argument('--steps', type=int, default=None)
|
||||
ap.add_argument('--device', default='cuda' if torch.cuda.is_available() else 'cpu'); ap.add_argument('--seed', type=int, default=0)
|
||||
a = ap.parse_args()
|
||||
cfg = json.load(open(a.config)); C = cfg['apg']
|
||||
if a.steps: C['steps'] = a.steps
|
||||
dev = a.device; torch.manual_seed(a.seed); random.seed(a.seed)
|
||||
os.makedirs(a.out, exist_ok=True)
|
||||
ck = torch.load(a.init, map_location=dev)
|
||||
mcfg = ck['cfg']
|
||||
model = build(mcfg).to(dev); model.load_state_dict(ck['model'])
|
||||
kin = Kin(device=dev)
|
||||
data = Samples(a.samples, device=dev, split='train')
|
||||
all_tasks = load_tasks(a.samples)
|
||||
train_tasks = [t for t in all_tasks if t['split'] == 'train'] + load_pool(a.pool)
|
||||
val_tasks = [t for t in all_tasks if t['split'] == 'val'][:C['eval_tasks']]
|
||||
w = [C['cat_weights'].get(t.get('cat', 'A'), 1.0) * (2.0 if t.get('hard') else 1.0) for t in train_tasks]
|
||||
opt = torch.optim.AdamW(model.parameters(), lr=C['lr'], betas=(0.9, 0.95), weight_decay=0.0)
|
||||
amp = dev.startswith('cuda') and C.get('amp') == 'bf16'
|
||||
step = torch.tensor(STEP, device=dev)
|
||||
log = open(os.path.join(a.out, 'log.jsonl'), 'a')
|
||||
base, _ = evaluate(model.eval(), kin, val_tasks, dev)
|
||||
print('EVAL0', json.dumps(base), flush=True); log.write(json.dumps({'eval': base, 'step': 0}) + '\n')
|
||||
best = base['success']
|
||||
torch.save({'model': model.state_dict(), 'cfg': mcfg, 'step': 0, 'fp': kin.fp, 'eval': base}, os.path.join(a.out, 'best.pt'))
|
||||
M = C['margin_train_mm']; t0 = time.time()
|
||||
for it in range(C['steps']):
|
||||
model.train()
|
||||
batch = random.choices(train_tasks, weights=w, k=C['batch'])
|
||||
q0, T, R, ex, tr = tasks_to_tensors(batch, dev)
|
||||
B = q0.shape[0]
|
||||
z = torch.randn(B, H, 6, device=dev)
|
||||
q, qp = q0.clone(), q0.clone()
|
||||
queue = torch.zeros(B, 0, 6, device=dev)
|
||||
subs = torch.zeros(B, 2, 6, device=dev); nsub = torch.zeros(B, dtype=torch.long, device=dev); sub_t = torch.zeros(B, device=dev)
|
||||
opt.zero_grad(set_to_none=True)
|
||||
stats = {'col': 0.0, 'tgt': 0.0, 'smooth': 0.0}
|
||||
nseg = math.ceil(C['plans'] / C['seg_plans'])
|
||||
seg_loss = torch.zeros((), device=dev); prev_d = torch.zeros(B, 6, device=dev)
|
||||
for p in range(C['plans']):
|
||||
tnow = p * 3 * 0.02
|
||||
r = queue.shape[1]
|
||||
pre = torch.zeros(B, 3, 6, device=dev); pre[:, :r] = queue[:, :r]
|
||||
qe = q + queue.sum(1)
|
||||
with torch.no_grad():
|
||||
if (nsub > 0).any():
|
||||
cur = subs[:, 0]; dd = torch.where(kin.cont, wrap180(cur - qe), cur - qe).abs().max(-1).values
|
||||
adv = (nsub > 0) & ((dd < 5) | (tnow - sub_t > 1.5))
|
||||
subs = torch.where(adv[:, None, None], torch.cat([subs[:, 1:], subs[:, 1:]], 1), subs)
|
||||
nsub = torch.where(adv, nsub - 1, nsub); sub_t = torch.where(adv, torch.full_like(sub_t, tnow), sub_t)
|
||||
feat = features(kin, q, qp, T, R, pre, torch.full((B,), r, device=dev), subs[:, 0], (nsub > 0).float())
|
||||
with torch.autocast('cuda', dtype=torch.bfloat16, enabled=amp):
|
||||
act, val, sub = model.sample_k(feat, 1, z)
|
||||
act = act[:, 0].float()
|
||||
if p == 0:
|
||||
with torch.no_grad():
|
||||
need = sub[:, 12].float() > 0.5
|
||||
subs = q.detach()[:, None] + sub[:, :12].float().view(B, 2, 6) * 180
|
||||
nsub = torch.where(need, torch.full_like(nsub, 2), torch.zeros_like(nsub))
|
||||
inc = act.clamp(-1, 1) * step # [B,16,6]
|
||||
commit = inc[:, r:r + 3]
|
||||
if r == 0: # 冷启动：先提交一块，下一轮才开始执行
|
||||
queue = commit; continue
|
||||
# 执行队列中的 3 步（上一轮提交的）
|
||||
for k in range(3):
|
||||
d = queue[:, k]
|
||||
qp = q; q = kin.clamp_box(q + d)
|
||||
G = kin.gaps(q, M)
|
||||
hinge = torch.relu(M - G['gap'])
|
||||
col = torch.zeros(B, device=dev).index_add(0, G['b'], hinge)
|
||||
sm = ((d / step) ** 2).sum(-1) + (((d - prev_d) / step) ** 2).sum(-1)
|
||||
prev_d = d
|
||||
seg_loss = seg_loss + C['w_col'] * col.mean() + C['w_smooth'] * sm.mean() + C['w_len'] * d.abs().sum(-1).mean()
|
||||
stats['col'] += col.mean().item(); stats['smooth'] += sm.mean().item()
|
||||
queue = commit
|
||||
if (p + 1) % C['seg_plans'] == 0 or p == C['plans'] - 1:
|
||||
xf = kin.cum_xf(q)
|
||||
pe = kin.fk(q, xf); Rq = kin.orient(q, xf).reshape(B, 9)
|
||||
pose = (R.abs().sum(-1) > 0).float()
|
||||
lt = ((T - pe) / 100).pow(2).sum(-1) + C['w_rot'] * pose * (Rq - R).pow(2).sum(-1)
|
||||
seg_loss = seg_loss + C['w_tgt'] * lt.mean()
|
||||
stats['tgt'] += lt.mean().item()
|
||||
(seg_loss / nseg).backward()
|
||||
seg_loss = torch.zeros((), device=dev)
|
||||
q, qp, queue, prev_d = q.detach(), qp.detach(), queue.detach(), prev_d.detach()
|
||||
# BC 锚定
|
||||
X, Y, V, SG = data.batch(C['batch'] * 4)
|
||||
with torch.autocast('cuda', dtype=torch.bfloat16, enabled=amp):
|
||||
mem = model.encode(X)
|
||||
l_bc = wta_loss(model, mem, Y)[0] if model.head == 'wta' else meanflow_loss(model, mem, Y)[0]
|
||||
v, s = model.heads(mem)
|
||||
l_bc = l_bc + 0.1 * (v.float() - V).abs().mean() + 0.1 * (s.float() - SG).abs().mean()
|
||||
(C['w_bc'] * l_bc).backward()
|
||||
gn = torch.nn.utils.clip_grad_norm_(model.parameters(), C['grad_clip'])
|
||||
opt.step()
|
||||
with torch.no_grad():
|
||||
e, oe = goal_err(kin, q, T, R)
|
||||
rec = {'it': it, 'err_mm_mean': e.mean().item(), 'reach_frac': ((e < 1) & (oe < 0.5)).float().mean().item(), 'bc': l_bc.item(),
|
||||
'gnorm': gn.item(), **stats, 'min': (time.time() - t0) / 60}
|
||||
if it % 10 == 0: print(json.dumps(rec), flush=True)
|
||||
log.write(json.dumps(rec) + '\n'); log.flush()
|
||||
if (it + 1) % C['eval_every'] == 0 or it == C['steps'] - 1:
|
||||
res, _ = evaluate(model.eval(), kin, val_tasks, dev)
|
||||
res['it'] = it + 1
|
||||
print('EVAL', json.dumps(res), flush=True); log.write(json.dumps({'eval': res}) + '\n'); log.flush()
|
||||
sd = {'model': model.state_dict(), 'cfg': mcfg, 'step': it + 1, 'fp': kin.fp, 'eval': res}
|
||||
torch.save(sd, os.path.join(a.out, 'last.pt'))
|
||||
if res['success'] > best + 1e-4:
|
||||
best = res['success']; torch.save(sd, os.path.join(a.out, 'best.pt'))
|
||||
print(f'done, best success {best:.4f}', flush=True)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
File diff suppressed because one or more lines are too long
@@ -0,0 +1,59 @@
|
||||
"""策略输入特征（83 维）—— 与 webapp/js/ik-policy.js features() 逐项一致。改动须两边同步并升 FEAT_VER。"""
|
||||
import math
|
||||
import torch
|
||||
|
||||
FEAT_VER = 1
|
||||
VMAX = [60., 60., 90., 90., 120., 180.]
|
||||
DT = 0.02
|
||||
STEP = [v * DT for v in VMAX]
|
||||
ACC = 5.0
|
||||
H, NJ, PREFIX = 16, 6, 3
|
||||
GP = [(0, 2), (0, 3), (0, 4), (0, 5), (0, 6), (2, 4), (2, 5), (3, 5)]
|
||||
GAP_CAP = 40.0
|
||||
F_DIM = 83
|
||||
D2R = math.pi / 180.0
|
||||
ROT6 = [0, 3, 6, 1, 4, 7] # 行主序 3×3 的前两列
|
||||
|
||||
|
||||
def wrap180(x):
|
||||
return torch.remainder(x + 180.0, 360.0) - 180.0
|
||||
|
||||
|
||||
def features(kin, q, q_prev, T, R, prefix, prefix_len, sub, sub_flag, gm=None, xf=None):
|
||||
"""全部为 batch 张量：
|
||||
q, q_prev [B,6] 度；T [B,3] mm；R [B,9]（pos 模式全 0）；pose 标志由 R 是否全 0 判定
|
||||
prefix [B,3,6] 增量（度），prefix_len [B]；sub [B,6]，sub_flag [B]（0/1）
|
||||
gm：kin.gaps(q, 40)['gm'] 可传入复用。"""
|
||||
B = q.shape[0]
|
||||
dt = q.dtype
|
||||
step = torch.tensor(STEP, dtype=dt, device=q.device)
|
||||
xf = xf if xf is not None else kin.cum_xf(q)
|
||||
out = []
|
||||
rad = q * D2R
|
||||
out.append(torch.stack([torch.sin(rad), torch.cos(rad)], -1).reshape(B, 12))
|
||||
lo, hi = kin.lo.to(dt), kin.hi.to(dt)
|
||||
idx = [1, 3, 4]
|
||||
out.append(2 * (q[:, idx] - lo[idx]) / (hi[idx] - lo[idx]) - 1)
|
||||
d = torch.where(kin.cont, wrap180(q - q_prev), q - q_prev)
|
||||
out.append(d / step)
|
||||
p = kin.fk(q, xf); Rq = kin.orient(q, xf).reshape(B, 9)
|
||||
out.append(p / 600)
|
||||
out.append(Rq[:, ROT6])
|
||||
out.append(T / 600)
|
||||
pose = (R.abs().sum(-1) > 0).to(dt)
|
||||
out.append(R[:, ROT6] * pose[:, None])
|
||||
out.append((T - p) / 600)
|
||||
out.append(((T - p) / 50).clamp(-1, 1))
|
||||
out.append(torch.stack([1 - pose, pose], -1))
|
||||
m = (torch.arange(PREFIX, device=q.device)[None, :] < prefix_len[:, None]).to(dt) # [B,3]
|
||||
out.append((prefix / step * m[:, :, None]).reshape(B, 18))
|
||||
out.append(m)
|
||||
sd = torch.where(kin.cont, wrap180(sub - q), sub - q) / 180 * sub_flag[:, None].to(dt)
|
||||
out.append(sd)
|
||||
out.append(sub_flag[:, None].to(dt))
|
||||
if gm is None:
|
||||
gm = kin.gaps(q, GAP_CAP, xf)['gm']
|
||||
out.append(torch.stack([gm[:, a * 7 + b].clamp(max=GAP_CAP) / GAP_CAP for a, b in GP], -1))
|
||||
f = torch.cat(out, -1)
|
||||
assert f.shape[1] == F_DIM, f.shape
|
||||
return f
|
||||
@@ -0,0 +1,160 @@
|
||||
"""PyTorch 批量运动学 + 可微胶囊间隙（与 webapp/js/ik-solver.js 同口径）。
|
||||
|
||||
数据来自 ml/assets/kin.json（node ml/prep/export_kin.cjs 生成）：
|
||||
- cum_xf / fk / orient：与 JS cumXf / fk(q,0) / orient(q) 一致（角度制输入）
|
||||
- gaps(q, cap)：所有 |Δg|≥2 胶囊对的 间隙 = 线段距离 − (r_i + r_j + margin)。
|
||||
宽相位复刻 JS 的簇球筛选（被筛掉的对间隙必 > cap），故 min(gap, cap) 与 JS gapMatrix 逐位同口径；
|
||||
筛选在 no_grad 下做，幸存对的精确距离保留梯度（APG / FK 损失用）。
|
||||
"""
|
||||
import json, math, os
|
||||
import torch
|
||||
|
||||
HERE = os.path.dirname(os.path.abspath(__file__))
|
||||
DEFAULT_KIN = os.path.join(HERE, '..', 'assets', 'kin.json')
|
||||
D2R = math.pi / 180.0
|
||||
|
||||
|
||||
def rot_axis(d, a):
|
||||
"""d: [3] 单位轴；a: [...] 弧度 → [..., 3, 3]（Rodrigues，右手）"""
|
||||
c, s = torch.cos(a), torch.sin(a)
|
||||
x, y, z = float(d[0]), float(d[1]), float(d[2])
|
||||
C = 1 - c
|
||||
row0 = torch.stack([c + x * x * C, x * y * C - z * s, x * z * C + y * s], -1)
|
||||
row1 = torch.stack([y * x * C + z * s, c + y * y * C, y * z * C - x * s], -1)
|
||||
row2 = torch.stack([z * x * C - y * s, z * y * C + x * s, c + z * z * C], -1)
|
||||
return torch.stack([row0, row1, row2], -2)
|
||||
|
||||
|
||||
def seg_seg_dist(p1, q1, p2, q2, eps=1e-12):
|
||||
"""两线段最近距离（Ericson, Real-Time Collision Detection §5.1.9），[..., 3] → [...]"""
|
||||
d1, d2, r = q1 - p1, q2 - p2, p1 - p2
|
||||
a = (d1 * d1).sum(-1); e = (d2 * d2).sum(-1); f = (d2 * r).sum(-1)
|
||||
c = (d1 * r).sum(-1); b = (d1 * d2).sum(-1)
|
||||
denom = a * e - b * b
|
||||
s = torch.where(denom > eps, ((b * f - c * e) / denom.clamp_min(eps)).clamp(0, 1), torch.zeros_like(a))
|
||||
t = (b * s + f) / e.clamp_min(eps)
|
||||
# t 越界：夹回 [0,1] 并重算 s
|
||||
t_lo, t_hi = t < 0, t > 1
|
||||
s = torch.where(t_lo, (-c / a.clamp_min(eps)).clamp(0, 1), s)
|
||||
s = torch.where(t_hi, ((b - c) / a.clamp_min(eps)).clamp(0, 1), s)
|
||||
t = t.clamp(0, 1)
|
||||
# 退化（线段长度≈0）
|
||||
s = torch.where(a <= eps, torch.zeros_like(s), s)
|
||||
t = torch.where(e <= eps, torch.zeros_like(t), t)
|
||||
c1 = p1 + d1 * s.unsqueeze(-1)
|
||||
c2 = p2 + d2 * t.unsqueeze(-1)
|
||||
return ((c1 - c2).pow(2).sum(-1) + 1e-18).sqrt()
|
||||
|
||||
|
||||
class Kin:
|
||||
def __init__(self, path=DEFAULT_KIN, device='cpu', dtype=torch.float32):
|
||||
with open(path, 'r', encoding='utf-8') as f:
|
||||
K = json.load(f)
|
||||
self.fp = K['fp']
|
||||
self.device, self.dtype = device, dtype
|
||||
T = lambda x, dt=dtype: torch.tensor(x, dtype=dt, device=device)
|
||||
self.P = T(K['P']); self.D = [list(map(float, d)) for d in K['D']]
|
||||
lim = K['lim']
|
||||
self.lo = T([l[0] for l in lim]); self.hi = T([l[1] for l in lim]); self.cont = T([l[2] for l in lim]).bool()
|
||||
self.tcp0 = T(K['tcp0']); self.capm = float(K['capm'])
|
||||
caps = K['caps']
|
||||
self.cap_g = torch.tensor([c['g'] for c in caps], device=device)
|
||||
self.cap_a = T([c['a'] for c in caps]); self.cap_b = T([c['b'] for c in caps]); self.cap_r = T([c['r'] for c in caps])
|
||||
cl = K['clusters']
|
||||
self.cl_g = torch.tensor([c['g'] for c in cl], device=device)
|
||||
self.cl_c = T([c['c'] for c in cl])
|
||||
cp = K['cpairs']
|
||||
self.cp_a = torch.tensor([p[0] for p in cp], device=device)
|
||||
self.cp_b = torch.tensor([p[1] for p in cp], device=device)
|
||||
self.cp_lim = T([p[2] + p[3] + self.capm for p in cp])
|
||||
# 每个簇对展开成胶囊对（全组合），按簇对连续存放
|
||||
ci, cj, start, cnt = [], [], [], []
|
||||
for p in cp:
|
||||
A, B = cl[p[0]]['caps'], cl[p[1]]['caps']
|
||||
start.append(len(ci)); cnt.append(len(A) * len(B))
|
||||
for i in A:
|
||||
for j in B:
|
||||
ci.append(i); cj.append(j)
|
||||
self.pi = torch.tensor(ci, device=device, dtype=torch.long)
|
||||
self.pj = torch.tensor(cj, device=device, dtype=torch.long)
|
||||
self.cp_start = torch.tensor(start, device=device, dtype=torch.long)
|
||||
self.cp_cnt = torch.tensor(cnt, device=device, dtype=torch.long)
|
||||
hl = 0.5 * (self.cap_b - self.cap_a).norm(dim=-1)
|
||||
self.pair_need = self.cap_r[self.pi] + self.cap_r[self.pj] + self.capm
|
||||
self.pair_bl = hl[self.pi] + hl[self.pj] + self.cap_r[self.pi] + self.cap_r[self.pj] + self.capm
|
||||
gi, gj = self.cap_g[self.pi], self.cap_g[self.pj]
|
||||
self.pair_gp = torch.minimum(gi, gj) * 7 + torch.maximum(gi, gj)
|
||||
self.lever = T(K['lever'])
|
||||
|
||||
# ---------------- 运动学 ----------------
|
||||
def cum_xf(self, q):
|
||||
"""q: [B,6] 度 → Rs [B,7,3,3], Ts [B,7,3]"""
|
||||
B = q.shape[0]
|
||||
R = torch.eye(3, dtype=q.dtype, device=q.device).expand(B, 3, 3)
|
||||
Tt = torch.zeros(B, 3, dtype=q.dtype, device=q.device)
|
||||
Rs, Ts = [R], [Tt]
|
||||
for k in range(6):
|
||||
Rk = rot_axis(self.D[k], q[:, k] * D2R)
|
||||
Pk = self.P[k].to(q.dtype)
|
||||
tk = Pk - torch.einsum('bij,j->bi', Rk, Pk)
|
||||
Tt = torch.einsum('bij,bj->bi', R, tk) + Tt
|
||||
R = R @ Rk
|
||||
Rs.append(R); Ts.append(Tt)
|
||||
return torch.stack(Rs, 1), torch.stack(Ts, 1)
|
||||
|
||||
def fk(self, q, xf=None):
|
||||
Rs, Ts = xf if xf is not None else self.cum_xf(q)
|
||||
return torch.einsum('bij,j->bi', Rs[:, 6], self.tcp0.to(q.dtype)) + Ts[:, 6]
|
||||
|
||||
def orient(self, q, xf=None):
|
||||
Rs, _ = xf if xf is not None else self.cum_xf(q)
|
||||
return Rs[:, 6]
|
||||
|
||||
def caps_world(self, q, xf=None):
|
||||
Rs, Ts = xf if xf is not None else self.cum_xf(q)
|
||||
Rg, Tg = Rs[:, self.cap_g], Ts[:, self.cap_g] # [B,C,3,3], [B,C,3]
|
||||
a = torch.einsum('bcij,cj->bci', Rg, self.cap_a.to(q.dtype)) + Tg
|
||||
b = torch.einsum('bcij,cj->bci', Rg, self.cap_b.to(q.dtype)) + Tg
|
||||
return a, b
|
||||
|
||||
# ---------------- 间隙 ----------------
|
||||
def gaps(self, q, cap=40.0, xf=None, max_pairs=None):
|
||||
"""返回 dict：
|
||||
gm [B,49]：组对最小间隙 min(gap, cap)（gA*7+gB，gA<gB；其余为 cap）
|
||||
b, i, j, gap：幸存胶囊对（gap 可微）
|
||||
"""
|
||||
xf = xf if xf is not None else self.cum_xf(q)
|
||||
Rs, Ts = xf
|
||||
B = q.shape[0]
|
||||
with torch.no_grad():
|
||||
cc = torch.einsum('bkij,kj->bki', Rs[:, self.cl_g], self.cl_c.to(q.dtype)) + Ts[:, self.cl_g]
|
||||
d2 = (cc[:, self.cp_a] - cc[:, self.cp_b]).pow(2).sum(-1) # [B,M]
|
||||
keep = d2 <= (self.cp_lim + cap).pow(2)
|
||||
bb, mm = keep.nonzero(as_tuple=True)
|
||||
cnt = self.cp_cnt[mm]
|
||||
rep_b = torch.repeat_interleave(bb, cnt)
|
||||
off = torch.repeat_interleave(self.cp_start[mm] - torch.cumsum(cnt, 0) + cnt, cnt) + torch.arange(int(cnt.sum()), device=q.device)
|
||||
pidx = off
|
||||
a, b = self.caps_world(q, xf)
|
||||
with torch.no_grad():
|
||||
pi, pj = self.pi[pidx], self.pj[pidx]
|
||||
mid = 0.5 * (a[rep_b, pi] + b[rep_b, pi] - a[rep_b, pj] - b[rep_b, pj])
|
||||
k2 = mid.pow(2).sum(-1) <= (self.pair_bl[pidx] + cap).pow(2)
|
||||
rep_b, pidx, pi, pj = rep_b[k2], pidx[k2], pi[k2], pj[k2]
|
||||
d = seg_seg_dist(a[rep_b, pi], b[rep_b, pi], a[rep_b, pj], b[rep_b, pj])
|
||||
gap = d - self.pair_need[pidx].to(q.dtype)
|
||||
gm = torch.full((B * 49,), float(cap), dtype=q.dtype, device=q.device)
|
||||
gm = gm.scatter_reduce(0, rep_b * 49 + self.pair_gp[pidx], gap.clamp(max=cap), reduce='amin', include_self=True)
|
||||
return {'gm': gm.view(B, 49), 'b': rep_b, 'i': pi, 'j': pj, 'gap': gap}
|
||||
|
||||
def min_gap(self, q, cap=40.0):
|
||||
return self.gaps(q, cap)['gm'].min(-1).values
|
||||
|
||||
def unwrap_to(self, a, b):
|
||||
"""连续轴取离 a 最近的等价角（与 JS unwrapTo 同口径）"""
|
||||
d = b - a
|
||||
dw = torch.remainder(d + 180.0, 360.0) - 180.0
|
||||
return torch.where(self.cont, a + dw, b)
|
||||
|
||||
def clamp_box(self, q):
|
||||
return torch.where(self.cont, q, torch.maximum(torch.minimum(q, self.hi), self.lo))
|
||||
@@ -0,0 +1,10 @@
|
||||
{
|
||||
"apg": {
|
||||
"batch": 128, "lr": 1e-4, "steps": 3000, "grad_clip": 1.0, "amp": "bf16",
|
||||
"plans": 60, "seg_plans": 10,
|
||||
"margin_train_mm": 5.0,
|
||||
"w_tgt": 1.0, "w_rot": 0.3, "w_col": 2.0, "w_smooth": 0.02, "w_len": 0.001, "w_bc": 0.5,
|
||||
"cat_weights": {"A": 0.4, "B": 1.0, "C": 1.6, "D": 0.6, "Dr": 0.6},
|
||||
"eval_every": 500, "eval_tasks": 512
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,10 @@
|
||||
{
|
||||
"model": {"d": 192, "heads": 8, "ff": 768, "enc": 4, "dec": 4},
|
||||
"train": {
|
||||
"batch": 1024, "lr": 1e-3, "warmup": 1000, "steps": 60000, "wd": 0.01, "betas": [0.9, 0.95],
|
||||
"amp": "bf16", "grad_clip": 1.0,
|
||||
"w_mf": 1.0, "w_fk": 0.5, "w_smooth": 0.05, "w_value": 0.1, "w_sub": 0.1,
|
||||
"p_neq": 0.25, "fk_caps": 96,
|
||||
"eval_every": 2000, "eval_tasks": 512, "log_every": 50, "patience_evals": 5
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,10 @@
|
||||
{
|
||||
"model": {"d": 192, "heads": 8, "ff": 768, "enc": 4, "dec": 4, "head": "wta", "K": 8},
|
||||
"train": {
|
||||
"batch": 1024, "lr": 1e-3, "warmup": 1000, "steps": 60000, "wd": 0.01, "betas": [0.9, 0.95],
|
||||
"amp": "bf16", "grad_clip": 1.0,
|
||||
"w_mf": 1.0, "w_fk": 0.5, "w_smooth": 0.05, "w_value": 0.1, "w_sub": 0.1,
|
||||
"p_neq": 0.25, "fk_caps": 96,
|
||||
"eval_every": 2000, "eval_tasks": 512, "log_every": 50, "patience_evals": 5
|
||||
}
|
||||
}
|
||||
+55
@@ -0,0 +1,55 @@
|
||||
"""样本加载：ml/data/samples/*.{X,Y,V,SG,TID}.npy + *.tasks.jsonl（ml/prep/build_dataset.cjs 产出）
|
||||
python -m ml.data --index ml/data/samples # 汇总样本/任务数，写 index.json
|
||||
全部载入内存（200 万样本约 1.5 GB），按任务 split 字段划分 train / val。"""
|
||||
import argparse, glob, json, os
|
||||
import numpy as np
|
||||
import torch
|
||||
|
||||
|
||||
def load_tasks(dirs):
|
||||
tasks = []
|
||||
for d in dirs:
|
||||
for f in sorted(glob.glob(os.path.join(d, '*.tasks.jsonl'))):
|
||||
for line in open(f, encoding='utf-8'):
|
||||
if line.strip():
|
||||
tasks.append(json.loads(line))
|
||||
return tasks
|
||||
|
||||
|
||||
class Samples:
|
||||
def __init__(self, dirs, device='cpu', split=None):
|
||||
if isinstance(dirs, str): dirs = [dirs]
|
||||
xs, ys, vs, ss, ts = [], [], [], [], []
|
||||
for d in dirs:
|
||||
for fx in sorted(glob.glob(os.path.join(d, '*.X.npy'))):
|
||||
b = fx[:-6]
|
||||
xs.append(np.load(fx)); ys.append(np.load(b + '.Y.npy')); vs.append(np.load(b + '.V.npy'))
|
||||
ss.append(np.load(b + '.SG.npy')); ts.append(np.load(b + '.TID.npy'))
|
||||
if not xs: raise SystemExit(f'没有样本：{dirs}（先跑 ml/prep/run_prep.sh）')
|
||||
self.tasks = load_tasks(dirs)
|
||||
val_tid = {t['tid'] for t in self.tasks if t['split'] == 'val'}
|
||||
X, Y, V, SG, TID = map(np.concatenate, (xs, ys, vs, ss, ts))
|
||||
is_val = np.isin(TID, np.fromiter(val_tid, dtype=np.int64)) if val_tid else np.zeros(len(TID), bool)
|
||||
sel = {'train': ~is_val, 'val': is_val, None: np.ones(len(TID), bool)}[split]
|
||||
t = lambda a: torch.from_numpy(np.ascontiguousarray(a[sel])).to(device)
|
||||
self.X, self.Y, self.V, self.SG, self.TID = t(X), t(Y), t(V), t(SG), t(TID)
|
||||
self.n = self.X.shape[0]
|
||||
|
||||
def batch(self, bs, gen=None):
|
||||
i = torch.randint(0, self.n, (bs,), device=self.X.device, generator=gen)
|
||||
return self.X[i], self.Y[i], self.V[i], self.SG[i]
|
||||
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser(); ap.add_argument('--index', default='ml/data/samples'); a = ap.parse_args()
|
||||
tr, va = Samples(a.index, split='train'), Samples(a.index, split='val')
|
||||
tasks = load_tasks([a.index])
|
||||
cats = {}
|
||||
for t in tasks: cats[t['cat'] + '/' + t['mode']] = cats.get(t['cat'] + '/' + t['mode'], 0) + 1
|
||||
info = {'train_samples': tr.n, 'val_samples': va.n, 'tasks': len(tasks), 'val_tasks': sum(t['split'] == 'val' for t in tasks), 'by_cat_mode': cats}
|
||||
json.dump(info, open(os.path.join(a.index, 'index.json'), 'w'), indent=1)
|
||||
print(json.dumps(info, indent=1))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,58 @@
|
||||
"""对拍：python -m ml.eval.check_parity [--kin ml/assets/kin.json] [--data ml/data/parity.json]
|
||||
检查 PyTorch 版 fk / orient / 组对间隙 / 83 维特征 与 JS 一致，以及间隙梯度与有限差分一致。"""
|
||||
import argparse, json
|
||||
import torch
|
||||
from ml.common.kin_torch import Kin
|
||||
from ml.common.features import features, GAP_CAP
|
||||
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument('--kin', default=None); ap.add_argument('--data', default='ml/data/parity.json')
|
||||
a = ap.parse_args()
|
||||
kin = Kin(a.kin) if a.kin else Kin(dtype=torch.float64)
|
||||
D = json.load(open(a.data, encoding='utf-8'))
|
||||
assert D['fp'] == kin.fp, f"指纹不符：parity {D['fp']} vs kin {kin.fp}（重跑 export_kin.cjs）"
|
||||
R = D['recs']; f64 = torch.float64
|
||||
q = torch.tensor([r['q'] for r in R], dtype=f64)
|
||||
qp = torch.tensor([r['qPrev'] for r in R], dtype=f64)
|
||||
T = torch.tensor([r['T'] for r in R], dtype=f64)
|
||||
Rt = torch.tensor([r['R'] if r['R'] else [0.] * 9 for r in R], dtype=f64)
|
||||
pre = torch.zeros(len(R), 3, 6, dtype=f64); pl = torch.zeros(len(R), dtype=torch.long)
|
||||
for n, r in enumerate(R):
|
||||
for k, d in enumerate(r['prefix']): pre[n, k] = torch.tensor(d, dtype=f64)
|
||||
pl[n] = len(r['prefix'])
|
||||
sub = torch.tensor([r['sub'] if r['sub'] else [0.] * 6 for r in R], dtype=f64)
|
||||
sf = torch.tensor([1. if r['sub'] else 0. for r in R], dtype=f64)
|
||||
kin = Kin(a.kin, dtype=f64) if a.kin else kin
|
||||
xf = kin.cum_xf(q)
|
||||
e_fk = (kin.fk(q, xf) - torch.tensor([r['fk'] for r in R], dtype=f64)).abs().max().item()
|
||||
e_or = (kin.orient(q, xf).reshape(-1, 9) - torch.tensor([r['orient'] for r in R], dtype=f64)).abs().max().item()
|
||||
G = kin.gaps(q, GAP_CAP, xf)
|
||||
gm_js = torch.tensor([r['gm'] for r in R], dtype=f64)
|
||||
e_gm = (G['gm'] - gm_js.clamp(max=GAP_CAP)).abs().max().item()
|
||||
col_js = gm_js.min(-1).values <= 0; col_py = G['gm'].min(-1).values <= 0
|
||||
f = features(kin, q, qp, T, Rt, pre, pl, sub, sf, gm=G['gm'], xf=xf)
|
||||
e_f = (f - torch.tensor([r['feat'] for r in R], dtype=f64)).abs().max().item()
|
||||
# 梯度：最小间隙对 q 的梯度 vs 有限差分（取 50 个无干涉且间隙 < 20 的样本）
|
||||
sel = ((G['gm'].min(-1).values > 0.5) & (G['gm'].min(-1).values < 20)).nonzero().flatten()[:50]
|
||||
rel = []
|
||||
if len(sel):
|
||||
qs = q[sel].clone().requires_grad_(True)
|
||||
g = kin.gaps(qs, GAP_CAP)['gm'].min(-1).values
|
||||
gr, = torch.autograd.grad(g.sum(), qs)
|
||||
h = 1e-4
|
||||
for j in range(6):
|
||||
dq = torch.zeros_like(qs); dq[:, j] = h
|
||||
fd = (kin.gaps(qs.detach() + dq, GAP_CAP)['gm'].min(-1).values - kin.gaps(qs.detach() - dq, GAP_CAP)['gm'].min(-1).values) / (2 * h)
|
||||
rel.append(((gr[:, j] - fd).abs() / (fd.abs() + 1e-3)).max().item())
|
||||
res = {'n': len(R), 'fk_max_abs_mm': e_fk, 'orient_max_abs': e_or, 'gap_max_abs_mm': e_gm,
|
||||
'collide_bool_mismatch': int((col_js != col_py).sum()), 'feat_max_abs': e_f,
|
||||
'grad_rel_err_max': max(rel) if rel else None}
|
||||
ok = e_fk < 1e-6 and e_or < 1e-9 and e_gm < 1e-4 and res['collide_bool_mismatch'] == 0 and e_f < 1e-4 and (not rel or max(rel) < 1e-3)
|
||||
res['PASS'] = bool(ok)
|
||||
print(json.dumps(res, indent=1))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,25 @@
|
||||
/* 对拍数据：JS 侧 fk / orient / gapMatrix / 特征（ik-policy.js）→ ml/data/parity.json
|
||||
* node ml/eval/dump_parity.cjs [N=2000]
|
||||
* 然后 python -m ml.eval.check_parity 比对 PyTorch 版。 */
|
||||
'use strict';
|
||||
const fs = require('fs'), path = require('path');
|
||||
const N = +process.argv[2] || 2000;
|
||||
const { S } = require('../../tests/route_bench_setup.cjs')();
|
||||
const POL = require('../../webapp/js/ik-policy.js');
|
||||
const L = S.LIM, O = S.ORDER;
|
||||
let st = 99; const rnd = () => { st = (st * 1664525 + 1013904223) >>> 0; return st / 4294967296; };
|
||||
const rq = () => O.map(id => L[id].lo + rnd() * (L[id].hi - L[id].lo));
|
||||
const recs = [];
|
||||
for (let n = 0; n < N; n++) {
|
||||
const q = rq(), qp = q.map((v, i) => v - (rnd() - 0.5) * 2);
|
||||
const b = rq(); const T = S.fk(b, 0); const pose = rnd() < 0.5; const R = pose ? S.orient(b) : null;
|
||||
const r = Math.floor(rnd() * 4); const prefix = []; for (let k = 0; k < r; k++) prefix.push(q.map(() => (rnd() - 0.5) * 2));
|
||||
const sub = rnd() < 0.5 ? rq() : null;
|
||||
const st0 = { q, qPrev: qp, T, R, prefix, sub };
|
||||
const f = POL.features(S, st0);
|
||||
recs.push({ q, qPrev: qp, T, R, prefix, sub, fk: S.fk(q, 0), orient: S.orient(q), gm: Array.from(S.gapMatrix(q, 40)), feat: Array.from(f) });
|
||||
}
|
||||
const out = path.join(__dirname, '../data/parity.json');
|
||||
fs.mkdirSync(path.dirname(out), { recursive: true });
|
||||
fs.writeFileSync(out, JSON.stringify({ fp: S.modelFingerprint(), recs }));
|
||||
console.log('wrote', out, recs.length);
|
||||
@@ -0,0 +1,53 @@
|
||||
"""导出部署包（方案 §8.4）：
|
||||
python -m ml.export_onnx --ckpt ml/runs/current.pt --out webapp/models/traj_policy.onnx [--fp16]
|
||||
输出 traj_policy.onnx（输入 feat[B,83]、z[B,16,6]；输出 act[B,16,6] 归一化增量、value[B]、sub[B,13]）
|
||||
与 traj_policy.meta.json（碰撞模型指纹、特征版本、维度）。导出后用 onnxruntime 与 PyTorch 对拍。"""
|
||||
import argparse, json, os
|
||||
import numpy as np
|
||||
import torch
|
||||
|
||||
from ml.common.kin_torch import Kin
|
||||
from ml.common.features import F_DIM, H, FEAT_VER, STEP
|
||||
from ml.model.policy import build
|
||||
|
||||
|
||||
class Wrap(torch.nn.Module):
|
||||
def __init__(self, m):
|
||||
super().__init__(); self.m = m
|
||||
|
||||
def forward(self, feat, z):
|
||||
return self.m.sample(feat, z)
|
||||
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument('--ckpt', required=True); ap.add_argument('--out', default='webapp/models/traj_policy.onnx')
|
||||
ap.add_argument('--fp16', action='store_true'); ap.add_argument('--opset', type=int, default=17)
|
||||
a = ap.parse_args()
|
||||
ck = torch.load(a.ckpt, map_location='cpu')
|
||||
m = build(ck['cfg']); m.load_state_dict(ck['model']); m.eval()
|
||||
kin = Kin()
|
||||
if ck.get('fp') and ck['fp'] != kin.fp: raise SystemExit(f"checkpoint 指纹 {ck['fp']} ≠ 当前碰撞模型 {kin.fp}：须重训")
|
||||
os.makedirs(os.path.dirname(a.out) or '.', exist_ok=True)
|
||||
feat, z = torch.randn(8, F_DIM), torch.randn(8, H, 6)
|
||||
torch.onnx.export(Wrap(m), (feat, z), a.out, input_names=['feat', 'z'], output_names=['act', 'value', 'sub'],
|
||||
dynamic_axes={'feat': {0: 'B'}, 'z': {0: 'B'}, 'act': {0: 'B'}, 'value': {0: 'B'}, 'sub': {0: 'B'}},
|
||||
opset_version=a.opset, dynamo=False)
|
||||
if a.fp16:
|
||||
from onnxconverter_common import float16
|
||||
import onnx
|
||||
mo = float16.convert_float_to_float16(onnx.load(a.out), keep_io_types=True)
|
||||
onnx.save(mo, a.out)
|
||||
import onnxruntime as ort
|
||||
s = ort.InferenceSession(a.out, providers=['CPUExecutionProvider'])
|
||||
o = s.run(None, {'feat': feat.numpy(), 'z': z.numpy()})
|
||||
with torch.no_grad(): t = m.sample(feat, z)
|
||||
err = max(float(np.abs(o[i] - t[i].numpy()).max()) for i in range(3))
|
||||
meta = {'fp': kin.fp, 'tcp0': kin.tcp0.tolist(), 'head': m.head, 'feat_ver': FEAT_VER, 'f_dim': F_DIM, 'h': H, 'step': STEP, 'fp16': a.fp16,
|
||||
'ckpt': os.path.abspath(a.ckpt), 'eval': ck.get('eval'), 'max_abs_err_vs_torch': err}
|
||||
json.dump(meta, open(a.out.replace('.onnx', '.meta.json'), 'w'), indent=1)
|
||||
print(json.dumps({'onnx': a.out, 'MB': os.path.getsize(a.out) / 1e6, 'max_abs_err_vs_torch': err}))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,7 @@
|
||||
# ml/gen — 离线老师数据
|
||||
|
||||
完整流程与命令见 `ml/README.md`（第 3、4 步）。这里的脚本：
|
||||
- `consistency.cjs`：老师一致性门禁（目标 ±5 mm 邻居的同伦类一致率）
|
||||
- `gen_dataset.cjs`：采样 → 确定性老师 → 距离下界认证 → 时间律；`--consensus k` 共识过滤；`--tasks f.jsonl` 按给定任务出解（挖错 E2）
|
||||
- `run_parallel.sh`：多进程生成 + 统计
|
||||
- `stats.cjs`：汇总 `stats.json`
|
||||
@@ -0,0 +1,3 @@
|
||||
/* 距离下界认证：实现放在 webapp/js/ik-policy.js（部署安全层与离线过滤共用同一份代码）。 */
|
||||
'use strict';
|
||||
module.exports = (S, opt) => require('../../webapp/js/ik-policy.js').makeCert(S, opt);
|
||||
@@ -0,0 +1,47 @@
|
||||
/* M1 门禁：老师一致性（方案 §4.7 / R1）
|
||||
* node ml/gen/consistency.cjs [N=100] [seed=4242]
|
||||
* 取 B/C 类（需要绕行）任务，起点不变、目标位置扰动 ±5 mm 造 4 个邻居，与原任务老师路径比较：
|
||||
* 两条路径按巡航时间 τ 各重采样 32 点，逐点取各轴最大差；最大偏差 < 30° 视为同一绕行同伦类。
|
||||
* 门禁：一致率 ≥ 90%。另报告同一任务重复求解的确定性。 */
|
||||
'use strict';
|
||||
const N = +process.argv[2] || 100, SEED = +process.argv[3] || 4242;
|
||||
const { S, ArmIK } = require('../../tests/route_bench_setup.cjs')();
|
||||
const sm = require('./sampler.cjs')(S, SEED);
|
||||
const teach = require('./teacher.cjs')(S, ArmIK);
|
||||
const PD = require('./pathdev.cjs');
|
||||
const fz = (r, t) => (t.mode === 'pos' && r.ptsJ6Frozen) ? r.ptsJ6Frozen : r.pts; // 与 gen 同口径：pos 任务冻结 J6（gen 里认证不过才回退）
|
||||
const { VMAX } = require('./timelaw.cjs');
|
||||
function resample(P, n) {
|
||||
const seg = []; for (let i = 1; i < P.length; i++) { let m = 0; for (let j = 0; j < 6; j++) m = Math.max(m, Math.abs(P[i][j] - P[i - 1][j]) / VMAX[j]); seg.push(m); }
|
||||
const tot = seg.reduce((a, b) => a + b, 0) || 1; const out = [];
|
||||
for (let k = 0; k < n; k++) { let tau = tot * k / (n - 1), i = 0; while (i < seg.length - 1 && tau > seg[i]) { tau -= seg[i]; i++; }
|
||||
const u = seg[i] > 0 ? Math.min(1, tau / seg[i]) : 1; out.push(P[i].map((v, j) => v + (P[i + 1][j] - v) * u)); }
|
||||
return out;
|
||||
}
|
||||
const devAx = (A, B) => { const a = resample(A, 32), b = resample(B, 32); const m = [0, 0, 0, 0, 0, 0]; for (let k = 0; k < 32; k++) for (let j = 0; j < 6; j++) m[j] = Math.max(m[j], Math.abs(a[k][j] - b[k][j])); return m; };
|
||||
const dev = (A, B) => Math.max(...devAx(A, B));
|
||||
const axisHist = [0, 0, 0, 0, 0, 0]; let sameNoJ6 = 0;
|
||||
let tasks = 0, pairs = 0, same = 0, detSame = 0, detN = 0; const devs = [], byCat = {};
|
||||
while (tasks < N) {
|
||||
const t = sm.sample(); if (t.cat !== 'B' && t.cat !== 'C') continue;
|
||||
const base = teach(t); if (!base.ok || !base.reached) continue; base.pts = fz(base, t);
|
||||
tasks++;
|
||||
const again = teach(t); if (again.ok) { detN++; if (dev(base.pts, fz(again, t)) < 1e-6) detSame++; }
|
||||
for (let k = 0; k < 4; k++) {
|
||||
const u = [sm.rnd() - 0.5, sm.rnd() - 0.5, sm.rnd() - 0.5], n = Math.hypot(...u);
|
||||
const T2 = t.T.map((v, i) => v + 5 * u[i] / n);
|
||||
const r = teach(Object.assign({}, t, { T: T2 }));
|
||||
if (!r.ok || !r.reached) continue;
|
||||
const da = devAx(base.pts, fz(r, t)), d = Math.max(...da); devs.push(d); pairs++;
|
||||
if (PD.sameClass(base.pts, fz(r, t), t.mode)) sameNoJ6++;
|
||||
if (d >= 30) axisHist[da.indexOf(d)]++;
|
||||
const c = byCat[t.cat] = byCat[t.cat] || { pairs: 0, same: 0 }; c.pairs++;
|
||||
if (d < 30) { same++; c.same++; }
|
||||
}
|
||||
if (tasks % 10 === 0) process.stderr.write(`${tasks}/${N}\n`);
|
||||
}
|
||||
devs.sort((a, b) => a - b);
|
||||
console.log(JSON.stringify({ tasks, pairs, consistentRate: +(same / pairs).toFixed(3), gate: same / pairs >= 0.9 ? 'PASS' : 'FAIL',
|
||||
devP50: +devs[devs.length >> 1].toFixed(1), devP90: +devs[Math.floor(devs.length * 0.9)].toFixed(1),
|
||||
byCat: Object.fromEntries(Object.entries(byCat).map(([k, v]) => [k, +(v.same / v.pairs).toFixed(3)])),
|
||||
consistentByGateRule: +(sameNoJ6 / pairs).toFixed(3), gateByRule: sameNoJ6 / pairs >= 0.9 ? 'PASS' : 'FAIL', worstAxisOfInconsistent: axisHist, deterministic: detSame + '/' + detN }));
|
||||
@@ -0,0 +1,59 @@
|
||||
/* M1 数据生成器（2026-10-17，方案 §4）
|
||||
* node ml/gen/gen_dataset.cjs --n 500 --seed 1 --out ml/data/shard_0001.jsonl [--consensus 4]
|
||||
* --consensus k：邻域共识过滤。起点不变、目标位置各扰动 5 mm 造 k 个邻居跑老师，原任务路径若与多数邻居
|
||||
* 不在同一绕行类（pathdev.sameClass），整条样本 keep=false（why=consensus）。成本约 (1+k) 倍。
|
||||
* 每行一条任务：采样 → 老师 → 距离下界认证过滤 → 局部时间律（50 Hz）→ 写出。
|
||||
* 认证不过的样本也写出（keep=false + 原因），供 stats 统计丢弃率；训练只用 keep=true。 */
|
||||
'use strict';
|
||||
const fs = require('fs'), path = require('path');
|
||||
const args = {}; for (let i = 2; i < process.argv.length; i += 2) args[process.argv[i].replace(/^--/, '')] = process.argv[i + 1];
|
||||
const N = +args.n || 100, SEED = +args.seed || 1, OUT = args.out || 'ml/data/shard_' + String(SEED).padStart(4, '0') + '.jsonl';
|
||||
const { S, ArmIK } = require('../../tests/route_bench_setup.cjs')();
|
||||
const sampler = require('./sampler.cjs')(S, SEED * 7919 + 13);
|
||||
const teach = require('./teacher.cjs')(S, ArmIK);
|
||||
const { sameClass } = require('./pathdev.cjs');
|
||||
const K_CONS = +args.consensus || 0;
|
||||
/* --tasks file.jsonl：按给定任务（q0/T/R/mode/cat）请老师，不采样（挖错循环 E2 补路径用） */
|
||||
const GIVEN = args.tasks ? fs.readFileSync(args.tasks, 'utf8').split('\n').filter(Boolean).map(l => JSON.parse(l)) : null;
|
||||
const NT = GIVEN ? GIVEN.length : N;
|
||||
const cert = require('./cert.cjs')(S, {});
|
||||
const { timeParam } = require('./timelaw.cjs');
|
||||
const fp = S.modelFingerprint ? S.modelFingerprint() : null;
|
||||
fs.mkdirSync(path.dirname(OUT), { recursive: true });
|
||||
const fd = fs.openSync(OUT, 'w');
|
||||
const r3 = v => Math.round(v * 1000) / 1000;
|
||||
const t00 = performance.now();
|
||||
for (let i = 0; i < NT; i++) {
|
||||
const task = GIVEN ? Object.assign({ cat: 'E2' }, GIVEN[i], { mode: GIVEN[i].R ? 'pose' : 'pos' }) : sampler.sample();
|
||||
const t = teach(task);
|
||||
const rec = { id: 's' + String(SEED).padStart(4, '0') + '_' + String(i).padStart(6, '0'), cat: task.cat, mode: task.mode,
|
||||
q0: task.q0.map(r3), T: task.T.map(r3), R: task.R, fp };
|
||||
if (!t.ok) { rec.keep = false; rec.why = t.why; fs.writeSync(fd, JSON.stringify(rec) + '\n'); continue; }
|
||||
const c0 = performance.now();
|
||||
let c = null;
|
||||
if (t.ptsJ6Frozen) { const cf = cert.path(t.ptsJ6Frozen); if (cf.ok) { t.pts = t.ptsJ6Frozen; t.travel = t.travelOf(t.pts); c = cf; rec.j6Frozen = true; } }
|
||||
if (!c) c = cert.path(t.pts);
|
||||
if (task.cat === 'D' && t.reached) rec.cat = 'Dr'; // "不可达"采样其实可达：单独记类，不当停靠样本
|
||||
Object.assign(rec, { path: t.pts.map(q => q.map(r3)), reached: t.reached, err: r3(t.err), oerr: r3(t.oerr), travel_deg: r3(t.travel),
|
||||
plan_ms: Math.round(t.ms), blocked: t.blocked, poseRelaxed: t.poseRelaxed, detour: t.detour,
|
||||
cert: { ok: c.ok, queries: c.queries, minGap: r3(c.minGap), ms: Math.round(performance.now() - c0), why: c.why || null, failAt: c.failAt == null ? null : c.failAt } });
|
||||
/* 训练可用：认证通过；D 类允许未到达（停靠），其余类必须到达；poseRelaxed 的 pose 样本降为 pos */
|
||||
rec.keep = c.ok && (t.reached || task.cat === 'D');
|
||||
if (rec.keep && K_CONS > 0 && t.reached) {
|
||||
let agree = 0, valid = 0;
|
||||
for (let k = 0; k < K_CONS; k++) {
|
||||
const u = [sampler.rnd() - 0.5, sampler.rnd() - 0.5, sampler.rnd() - 0.5], n = Math.hypot(u[0], u[1], u[2]);
|
||||
const nb = teach(Object.assign({}, task, { T: task.T.map((v, j) => v + 5 * u[j] / n) }));
|
||||
if (!nb.ok || !nb.reached) continue;
|
||||
valid++; if (sameClass(t.pts, nb.ptsJ6Frozen && rec.j6Frozen ? nb.ptsJ6Frozen : nb.pts, task.mode)) agree++;
|
||||
}
|
||||
rec.consensus = { agree, valid };
|
||||
if (valid >= 2 && agree * 2 < valid) { rec.keep = false; rec.why = 'consensus'; }
|
||||
}
|
||||
if (t.poseRelaxed && task.mode === 'pose') { rec.mode = 'pos'; rec.R = null; rec.demoted = true; rec.reached = t.err < 1; }
|
||||
if (rec.keep) { const tp = timeParam(t.pts); rec.traj = tp.traj.map(q => q.map(r3)); rec.exec_s = r3(tp.steps * 0.02); }
|
||||
fs.writeSync(fd, JSON.stringify(rec) + '\n');
|
||||
if ((i + 1) % 25 === 0) process.stderr.write(`[seed ${SEED}] ${i + 1}/${NT} ${((performance.now() - t00) / 1000 / (i + 1)).toFixed(2)} s/条\n`);
|
||||
}
|
||||
fs.closeSync(fd);
|
||||
process.stderr.write(`[seed ${SEED}] done ${NT} in ${((performance.now() - t00) / 1000).toFixed(0)} s\n`);
|
||||
@@ -0,0 +1,16 @@
|
||||
/* 路径偏差（同伦类判定用）：两条展开折线按巡航时间 τ 各重采样 n 点，逐轴取最大差（°）。
|
||||
* 判定口径：pos 任务忽略 J6（位置任务里 J6 是冗余自由度），max < 30° 视为同一绕行类。 */
|
||||
'use strict';
|
||||
const { VMAX } = require('./timelaw.cjs');
|
||||
function resample(P, n) {
|
||||
const seg = []; for (let i = 1; i < P.length; i++) { let m = 0; for (let j = 0; j < 6; j++) m = Math.max(m, Math.abs(P[i][j] - P[i - 1][j]) / VMAX[j]); seg.push(m); }
|
||||
if (!seg.length) return Array.from({ length: n }, () => P[0].slice());
|
||||
const tot = seg.reduce((a, b) => a + b, 0) || 1; const out = [];
|
||||
for (let k = 0; k < n; k++) { let tau = tot * k / (n - 1), i = 0; while (i < seg.length - 1 && tau > seg[i]) { tau -= seg[i]; i++; }
|
||||
const u = seg[i] > 0 ? Math.min(1, tau / seg[i]) : 1; out.push(P[i].map((v, j) => v + (P[i + 1][j] - v) * u)); }
|
||||
return out;
|
||||
}
|
||||
function devAx(A, B, n) { n = n || 32; const a = resample(A, n), b = resample(B, n); const m = [0, 0, 0, 0, 0, 0]; for (let k = 0; k < n; k++) for (let j = 0; j < 6; j++) m[j] = Math.max(m[j], Math.abs(a[k][j] - b[k][j])); return m; }
|
||||
const SAME_DEG = 30;
|
||||
function sameClass(A, B, mode) { const d = devAx(A, B); return Math.max.apply(null, mode === 'pos' ? d.slice(0, 5) : d) < SAME_DEG; }
|
||||
module.exports = { resample, devAx, sameClass, SAME_DEG };
|
||||
Executable
+11
@@ -0,0 +1,11 @@
|
||||
#!/usr/bin/env bash
|
||||
# 并行生成：bash ml/gen/run_parallel.sh <每进程条数> <进程数> [起始种子=1] ["--consensus 4"]
|
||||
# 例：8 核机器 5 万条 → bash ml/gen/run_parallel.sh 6250 8
|
||||
set -e
|
||||
N=${1:-500}; P=${2:-2}; S0=${3:-1}; EXTRA=${4:-}
|
||||
cd "$(dirname "$0")/../.."
|
||||
mkdir -p ml/data
|
||||
for ((i=0;i<P;i++)); do s=$((S0+i)); node ml/gen/gen_dataset.cjs --n $N --seed $s --out ml/data/shard_$(printf %04d $s).jsonl $EXTRA 2> ml/data/log_$s.txt & done
|
||||
wait
|
||||
node ml/gen/stats.cjs ml/data/shard_*.jsonl > ml/data/stats.json
|
||||
cat ml/data/stats.json
|
||||
@@ -0,0 +1,32 @@
|
||||
/* 任务采样（方案 §4.3）：A 均匀 45% / B 直线被挡 30% / C 贴立柱 15% / D 目标不可达 10%；pos/pose 各半（D 只做 pos） */
|
||||
'use strict';
|
||||
module.exports = function makeSampler(S, seed) {
|
||||
const L = S.LIM, O = S.ORDER, w = S.wrap180;
|
||||
let st = (seed >>> 0) || 1; const rnd = () => { st = (st * 1664525 + 1013904223) >>> 0; return st / 4294967296; };
|
||||
const rq = () => O.map(id => { const t = L[id]; return t.lo + rnd() * (t.hi - t.lo); });
|
||||
const d = (a, b) => a.map((v, i) => L[O[i]].cont ? w(b[i] - v) : b[i] - v);
|
||||
const segOK = (a, b) => { const dd = d(a, b); const n = Math.max(1, Math.ceil(Math.max(...dd.map(Math.abs)))); for (let i = 1; i <= n; i++) if (!S.collideFree(a.map((v, k) => v + dd[k] * i / n))) return false; return true; };
|
||||
const hitsColumn = q => (S.collideDetail(q) || []).some(p => /立柱/.test(p.cnA + p.cnB));
|
||||
const nearColumn = q => { for (const k of [0, 1, 2]) for (const sg of [-4, 4]) { const p = q.slice(); p[k] += sg; if (hitsColumn(p)) return true; } return false; };
|
||||
const freeQ = () => { for (;;) { const q = rq(); if (S.collideFree(q)) return q; } };
|
||||
function sample() {
|
||||
const u = rnd();
|
||||
const cat = u < 0.45 ? 'A' : u < 0.75 ? 'B' : u < 0.90 ? 'C' : 'D';
|
||||
let a, b, T, R = null;
|
||||
if (cat === 'A') { a = freeQ(); b = freeQ(); }
|
||||
else if (cat === 'B') { for (;;) { a = freeQ(); b = freeQ(); if (!segOK(a, b)) break; } }
|
||||
else if (cat === 'C') {
|
||||
for (;;) { b = rq(); if (!S.collideFree(b) || !nearColumn(b)) continue; a = null;
|
||||
for (let t = 0; t < 200; t++) { const c = rq(); if (S.collideFree(c) && !segOK(c, b)) { a = c; break; } } if (a) break; }
|
||||
} else {
|
||||
a = freeQ();
|
||||
if (rnd() < 0.6) { for (;;) { b = rq(); if (hitsColumn(b)) break; } T = S.fk(b, 0); } // 目标位置落在立柱附近/内部
|
||||
else { b = rq(); const p = S.fk(b, 0), o = S.fk(O.map(() => 0), 0); T = p.map((v, i) => o[i] + (v - o[i]) * 1.6); } // 工作空间外
|
||||
}
|
||||
const mode = cat === 'D' ? 'pos' : (rnd() < 0.5 ? 'pos' : 'pose');
|
||||
if (!T) T = S.fk(b, 0);
|
||||
if (mode === 'pose') R = S.orient(b);
|
||||
return { cat, mode, q0: a, T, R, qb: b };
|
||||
}
|
||||
return { sample, rnd, segOK };
|
||||
};
|
||||
@@ -0,0 +1,21 @@
|
||||
/* M1 统计：node ml/gen/stats.cjs ml/data/shard_*.jsonl > ml/data/stats.json */
|
||||
'use strict';
|
||||
const fs = require('fs');
|
||||
const files = process.argv.slice(2);
|
||||
const by = {}; let total = 0, keep = 0, steps = 0, samples = 0, padTail = 0;
|
||||
const q = a => { a.sort((x, y) => x - y); return a.length ? { p50: +a[a.length >> 1].toFixed(1), p90: +a[Math.floor(a.length * 0.9)].toFixed(1), max: +a[a.length - 1].toFixed(1) } : null; };
|
||||
const plan = [], certMs = [], travel = [], exec = [], gaps = [];
|
||||
for (const f of files) for (const line of fs.readFileSync(f, 'utf8').split('\n')) {
|
||||
if (!line) continue; const r = JSON.parse(line); total++;
|
||||
const k = r.cat + '/' + r.mode; const b = by[k] = by[k] || { n: 0, reached: 0, certFail: 0, keep: 0, demoted: 0, consensusDrop: 0, j6Frozen: 0 };
|
||||
b.n++; if (r.reached) b.reached++; if (r.cert && !r.cert.ok) b.certFail++; if (r.demoted) b.demoted++; if (r.why === 'consensus') b.consensusDrop++; if (r.j6Frozen) b.j6Frozen++;
|
||||
if (r.plan_ms != null) plan.push(r.plan_ms); if (r.cert) certMs.push(r.cert.ms); if (r.cert) gaps.push(r.cert.minGap);
|
||||
if (r.keep) { b.keep++; keep++; travel.push(r.travel_deg); exec.push(r.exec_s);
|
||||
const n = r.traj.length - 1; steps += n; const s = Math.floor(n / 3) + 1; samples += s;
|
||||
for (let t = 0; t <= n; t += 3) if (t + 16 > n) padTail++; }
|
||||
}
|
||||
const out = { tasks: total, keep, keepRate: +(keep / total).toFixed(3), byCatMode: by,
|
||||
planMs: q(plan), certMs: q(certMs), vertexMinGapMm: q(gaps), travelDeg: q(travel), execS: q(exec),
|
||||
samplesEvery3: samples, samplesPerTask: +(samples / keep).toFixed(1), paddedSampleShare: +(padTail / samples).toFixed(3),
|
||||
projectedSamplesFor50k: Math.round(samples / total * 50000) };
|
||||
console.log(JSON.stringify(out, null, 1));
|
||||
@@ -0,0 +1,34 @@
|
||||
/* 老师（方案 §4.3）：solveDyn 松手口径——有离线路网时内部走 PRM 路网 Lazy A* + 捷径平滑，路网失败再落到 RRT-Connect。
|
||||
* 离线确定性（2026-10-17，M1 一致性门禁 78% 未过后加）：
|
||||
* - 每次求解前 resetSession()：清空路网增广/坏边/已验段等跨调用状态，结果与样本顺序无关；
|
||||
* - setDeterministic：毫秒预算改按碰撞判定次数计（msPerCheck 默认 0.5，即 300 ms 预算 ≈ 600 次判定），与机器负载无关；
|
||||
* - pos 任务冻结 J6：位置任务里 J6 不影响 TCP，路径上 J6 恒取起点值（认证不过才保留老师原路径，由 gen 决定）。 */
|
||||
'use strict';
|
||||
module.exports = function makeTeacher(S, ArmIK, opt) {
|
||||
opt = opt || {};
|
||||
const L = S.LIM, O = S.ORDER, w = S.wrap180;
|
||||
if (S.setDeterministic) S.setDeterministic(true, opt.msPerCheck || 0.5);
|
||||
return function teach(task) {
|
||||
const o = { tool: 0, step: 0.5, ref: task.q0, from: task.q0.slice() };
|
||||
if (task.mode === 'pose') { o.pose = { R: task.R }; o.priority = 'pos'; }
|
||||
if (S.resetSession) S.resetSession(); else if (S.rtReset) S.rtReset();
|
||||
const t0 = performance.now();
|
||||
const r = S.solveDyn(task.T, o);
|
||||
const ms = performance.now() - t0;
|
||||
if (!r || !r.q) return { ok: false, ms, why: 'noq' };
|
||||
const pts = [task.q0.slice()].concat(r.via || [], [r.q]);
|
||||
for (let i = 1; i < pts.length; i++) pts[i] = pts[i - 1].map((v, k) => L[O[k]].cont ? v + w(pts[i][k] - v) : pts[i][k]);
|
||||
const qf = pts[pts.length - 1], tcp = S.fk(qf, 0);
|
||||
const err = Math.hypot(tcp[0] - task.T[0], tcp[1] - task.T[1], tcp[2] - task.T[2]);
|
||||
const oerr = task.mode === 'pose' ? ArmIK.rotErrDeg(S.orient(qf), task.R) : 0;
|
||||
const reached = err < 1 && oerr < 0.5 && !r.blocked;
|
||||
const travelOf = P => { let t = 0; for (let i = 1; i < P.length; i++) for (let k = 0; k < 6; k++) t += Math.abs(P[i][k] - P[i - 1][k]); return t; };
|
||||
const out = { ok: true, pts, reached, err, oerr, travel: travelOf(pts), ms, blocked: !!r.blocked, poseRelaxed: !!r.poseRelaxed, unsafe: !!r.unsafe, detour: r.detour || null };
|
||||
if (task.mode === 'pos') {
|
||||
const j6 = task.q0[5];
|
||||
if (pts.some(p => Math.abs(p[5] - j6) > 1e-6)) out.ptsJ6Frozen = pts.map(p => { const c = p.slice(); c[5] = j6; return c; });
|
||||
}
|
||||
out.travelOf = travelOf;
|
||||
return out;
|
||||
};
|
||||
};
|
||||
@@ -0,0 +1,33 @@
|
||||
/* 局部可观测时间律（2026-10-17，方案 §4.5）
|
||||
* 折线按"巡航时间" τ 参数化：每段 τ = max_j |Δq_j| / VMAX_j（各轴同步、主导轴满速）。
|
||||
* 归一化速度 v∈[0,1]（1 = 巡航）：v ← min(1, v + A·dt, sqrt(2·A·剩余 τ))，只依赖当前速度与剩余路程。
|
||||
* 输出 50 Hz（dt = 20 ms）状态序列，末点精确落在终点。 */
|
||||
'use strict';
|
||||
const VMAX = [60, 60, 90, 90, 120, 180]; // °/s，与 tests/route_bench.cjs 同（轴速比 0.6/0.6/0.9/0.9/1.2/1.8）
|
||||
const A = 5.0; // 1/s²：0→巡航约 0.2 s（a_max 待真机参数）
|
||||
const DT = 0.02;
|
||||
function timeParam(P, opt) {
|
||||
opt = opt || {};
|
||||
const vmax = opt.vmax || VMAX, a = opt.acc || A, dt = opt.dt || DT;
|
||||
const segT = [];
|
||||
for (let i = 1; i < P.length; i++) { let m = 0; for (let j = 0; j < 6; j++) m = Math.max(m, Math.abs(P[i][j] - P[i - 1][j]) / vmax[j]); segT.push(m); }
|
||||
const cum = [0]; for (const s of segT) cum.push(cum[cum.length - 1] + s);
|
||||
const total = cum[cum.length - 1];
|
||||
const at = tau => {
|
||||
if (tau >= total) return P[P.length - 1].slice();
|
||||
let i = 0; while (i < segT.length - 1 && cum[i + 1] < tau) i++;
|
||||
const u = segT[i] > 0 ? (tau - cum[i]) / segT[i] : 1;
|
||||
return P[i].map((v, j) => v + (P[i + 1][j] - v) * u);
|
||||
};
|
||||
const out = [P[0].slice()], taus = [0]; let tau = 0, v = 0, guard = 0;
|
||||
while (tau < total - 1e-9 && guard++ < 100000) {
|
||||
const rem = total - tau;
|
||||
const v1 = Math.min(1, v + a * dt, Math.sqrt(2 * a * Math.max(0, rem - 0.5 * v * dt)) + 1e-9);
|
||||
let step = 0.5 * (v + v1) * dt;
|
||||
if (step <= 1e-9) step = Math.min(rem, a * dt * dt); // 防止末端速度掉到 0 卡住
|
||||
tau = Math.min(total, tau + step); v = v1;
|
||||
out.push(at(tau)); taus.push(tau);
|
||||
}
|
||||
return { traj: out, taus, cum, total, steps: out.length - 1 };
|
||||
}
|
||||
module.exports = { timeParam, VMAX, A, DT };
|
||||
@@ -0,0 +1,53 @@
|
||||
"""门禁（方案 §7.2 第 6 步）：新模型须 ①门禁集到达率不降 ②A 类降幅 ≤ 0.5 个百分点 ③上一轮修好的错题 95% 仍对。
|
||||
python -m ml.mining.gate --old cur.pt --new cand.pt --tasks ml/runs/gate_tasks.jsonl --out gate.json [--fixed prev_fixed.jsonl]
|
||||
通过则把 cand 复制为 --current（默认 ml/runs/current.pt）。gate.json 含 accept / gain_pp，loop.sh 据此决定继续或停止。"""
|
||||
import argparse, json, shutil
|
||||
import torch
|
||||
|
||||
from ml.common.kin_torch import Kin
|
||||
from ml.model.policy import build
|
||||
from ml.sim.closed_loop import evaluate
|
||||
|
||||
|
||||
def load(path, dev):
|
||||
ck = torch.load(path, map_location=dev)
|
||||
m = build(ck['cfg']).to(dev); m.load_state_dict(ck['model']); return m.eval()
|
||||
|
||||
|
||||
def per_task_success(pers):
|
||||
return torch.cat([p['done'] & (p['col'] == 0) for _, p in pers]).tolist()
|
||||
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument('--old', required=True); ap.add_argument('--new', required=True); ap.add_argument('--tasks', required=True)
|
||||
ap.add_argument('--fixed', default=None); ap.add_argument('--out', required=True); ap.add_argument('--current', default='ml/runs/current.pt')
|
||||
ap.add_argument('--device', default='cuda' if torch.cuda.is_available() else 'cpu')
|
||||
a = ap.parse_args()
|
||||
kin = Kin(device=a.device)
|
||||
tasks = [json.loads(l) for l in open(a.tasks) if l.strip()]
|
||||
ro, po = evaluate(load(a.old, a.device), kin, tasks, a.device)
|
||||
rn, pn = evaluate(load(a.new, a.device), kin, tasks, a.device)
|
||||
so, sn = per_task_success(po), per_task_success(pn)
|
||||
acc = lambda s, c: (sum(x for x, t in zip(s, tasks) if t['cat'] == c) / max(1, sum(t['cat'] == c for t in tasks)))
|
||||
gain = (sum(sn) - sum(so)) / len(tasks) * 100
|
||||
a_drop = (acc(so, 'A') - acc(sn, 'A')) * 100
|
||||
keep = None
|
||||
if a.fixed:
|
||||
ft = [json.loads(l) for l in open(a.fixed) if l.strip()]
|
||||
if ft:
|
||||
r2, p2 = evaluate(load(a.new, a.device), kin, ft, a.device)
|
||||
keep = sum(per_task_success(p2)) / len(ft)
|
||||
accept = gain >= 0 and a_drop <= 0.5 and (keep is None or keep >= 0.95)
|
||||
out = {'old': ro, 'new': rn, 'gain_pp': gain, 'A_drop_pp': a_drop, 'fixed_kept': keep, 'accept': accept}
|
||||
# 本轮新修好的题（旧错新对）留给下一轮做"仍对"检查
|
||||
fixed_now = [t for t, x, y in zip(tasks, so, sn) if (not x) and y]
|
||||
with open(a.out.replace('.json', '_fixed.jsonl'), 'w') as f:
|
||||
for t in fixed_now: f.write(json.dumps(t) + '\n')
|
||||
json.dump(out, open(a.out, 'w'), indent=1)
|
||||
if accept: shutil.copy(a.new, a.current)
|
||||
print(json.dumps({k: out[k] for k in ('gain_pp', 'A_drop_pp', 'fixed_kept', 'accept')}))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
Executable
+29
@@ -0,0 +1,29 @@
|
||||
#!/usr/bin/env bash
|
||||
# 后训练主循环（方案 §7）：APG 首轮 → 挖错 3–6 轮（每轮：挖错 → [E2 老师补路径] → APG 微调 → 门禁/回滚）
|
||||
# bash ml/mining/loop.sh [轮数=6] [每轮 APG 步数=1000] [每轮出题数=50000]
|
||||
set -e
|
||||
cd "$(dirname "$0")/../.."
|
||||
ROUNDS=${1:-6}; STEPS=${2:-1000}; N=${3:-50000}
|
||||
mkdir -p ml/runs/mine
|
||||
[ -f ml/runs/gate_tasks.jsonl ] || python -m ml.mining.mine_round --make-gate 2000 --seed 777 --out ml/runs
|
||||
[ -f ml/runs/current.pt ] || cp ml/runs/apg/best.pt ml/runs/current.pt
|
||||
SAMPLES="ml/data/samples"; FIXED=""; small=0
|
||||
for ((r=1; r<=ROUNDS; r++)); do
|
||||
D=ml/runs/mine/r$r; mkdir -p $D
|
||||
python -m ml.mining.mine_round --ckpt ml/runs/current.pt --seed $((1000+r)) --n $N --out $D
|
||||
# E2（局部极小）请老师补少量路径（唯一保留的后训练老师用途）
|
||||
if [ -s $D/e2_tasks.jsonl ]; then
|
||||
node ml/gen/gen_dataset.cjs --tasks $D/e2_tasks.jsonl --seed $((5000+r)) --out ml/data/e2/shard_e2_r$r.jsonl
|
||||
node ml/prep/build_dataset.cjs ml/data/e2/shard_e2_r$r.jsonl ml/data/samples_e2
|
||||
SAMPLES="ml/data/samples ml/data/samples_e2"
|
||||
fi
|
||||
POOLS=$(ls ml/runs/mine/r*/pool.jsonl)
|
||||
python -m ml.apg.apg_finetune --init ml/runs/current.pt --samples $SAMPLES --pool $POOLS --out ml/runs/apg_r$r --steps $STEPS
|
||||
python -m ml.mining.gate --old ml/runs/current.pt --new ml/runs/apg_r$r/best.pt --tasks ml/runs/gate_tasks.jsonl --out $D/gate.json $FIXED
|
||||
FIXED="--fixed $D/gate_fixed.jsonl"
|
||||
gain=$(python -c "import json;print(json.load(open('$D/gate.json'))['gain_pp'])")
|
||||
echo "round $r gain ${gain} pp"
|
||||
if python -c "import sys;sys.exit(0 if $gain < 0.5 else 1)"; then small=$((small+1)); else small=0; fi
|
||||
if [ $small -ge 2 ]; then echo "连续 2 轮提升 < 0.5 pp，停止"; break; fi
|
||||
done
|
||||
echo "最终模型：ml/runs/current.pt"
|
||||
@@ -0,0 +1,78 @@
|
||||
"""无老师挖错一轮（方案 §7.2）：出题 → 闭环作答（关安全层）→ 验算 → 错题分类 → 错题+邻居进 APG 采样池。
|
||||
python -m ml.mining.mine_round --ckpt ml/runs/apg/best.pt --seed 1001 --n 50000 --out ml/runs/mine/r1
|
||||
造固定门禁集（只做一次）：python -m ml.mining.mine_round --make-gate 2000 --seed 777 --out ml/runs
|
||||
输出：pool.jsonl（错题与 ±5 mm/±3° 邻居，E1/E6 ×2）、e2_tasks.jsonl（卡在局部极小，可离线请老师补路径）、report.json/.md
|
||||
错题类：E1 撞 / E2 没到且离目标 > 20 mm（局部极小）/ E3 用时 > 4 s / E4 没到但 < 20 mm（末端精度）/ E5 J1 换向 ≥ 3 次（摇摆）"""
|
||||
import argparse, json, os
|
||||
import torch
|
||||
|
||||
from ml.common.kin_torch import Kin
|
||||
from ml.model.policy import build
|
||||
from ml.sim.closed_loop import rollout, goal_err, tasks_to_tensors
|
||||
from ml.mining.tasks import sample_tasks, neighbours
|
||||
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument('--ckpt'); ap.add_argument('--seed', type=int, default=1001); ap.add_argument('--n', type=int, default=50000)
|
||||
ap.add_argument('--out', required=True); ap.add_argument('--bs', type=int, default=512); ap.add_argument('--neigh', type=int, default=10)
|
||||
ap.add_argument('--make-gate', type=int, default=0)
|
||||
ap.add_argument('--mix', default='0.3,0.3,0.3,0.1', help='A,B,C,D 占比；贴立柱门禁集用 0,0,1,0')
|
||||
ap.add_argument('--name', default='gate_tasks.jsonl')
|
||||
ap.add_argument('--device', default='cuda' if torch.cuda.is_available() else 'cpu')
|
||||
a = ap.parse_args()
|
||||
os.makedirs(a.out, exist_ok=True)
|
||||
kin = Kin(device=a.device)
|
||||
if a.make_gate:
|
||||
ts = sample_tasks(kin, a.make_gate, a.seed, mix=tuple(float(x) for x in a.mix.split(',')))
|
||||
with open(os.path.join(a.out, a.name), 'w') as f:
|
||||
for t in ts: f.write(json.dumps(t) + '\n')
|
||||
print('gate tasks', len(ts)); return
|
||||
ck = torch.load(a.ckpt, map_location=a.device)
|
||||
model = build(ck['cfg']).to(a.device); model.load_state_dict(ck['model']); model.eval()
|
||||
tasks = sample_tasks(kin, a.n, a.seed)
|
||||
g = torch.Generator(); g.manual_seed(a.seed)
|
||||
cls = {k: [] for k in ('E1', 'E2', 'E3', 'E4', 'E5', 'ok')}
|
||||
for i in range(0, len(tasks), a.bs):
|
||||
tb = tasks[i:i + a.bs]
|
||||
_, per = rollout(model, kin, tb, a.device, K=8, safety=False, seed=a.seed + i, max_sec=6.0, return_traj=True)
|
||||
q0, T, R, _, _ = tasks_to_tensors(tb, a.device)
|
||||
tr = per['traj']
|
||||
e, _ = goal_err(kin, tr[:, -1].to(a.device), T, R)
|
||||
d1 = tr[:, 1:, 0] - tr[:, :-1, 0]
|
||||
sg = torch.sign(torch.where(d1.abs() > 0.05, d1, torch.zeros_like(d1)))
|
||||
sw = torch.zeros(len(tb))
|
||||
for b in range(len(tb)):
|
||||
s = sg[b][sg[b] != 0]
|
||||
sw[b] = (s[1:] != s[:-1]).sum() if s.numel() > 1 else 0
|
||||
for b, t in enumerate(tb):
|
||||
if per['col'][b] > 0: cls['E1'].append(t)
|
||||
elif not per['done'][b]: (cls['E2'] if e[b] > 20 else cls['E4']).append(t)
|
||||
elif per['t_done'][b] > 4.0: cls['E3'].append(t)
|
||||
elif sw[b] >= 3: cls['E5'].append(t)
|
||||
else: cls['ok'].append(t)
|
||||
pool = []
|
||||
for k, lst in cls.items():
|
||||
if k == 'ok': continue
|
||||
rep = 2 if k == 'E1' else 1
|
||||
for t in lst:
|
||||
t = dict(t); t['hard'] = True; t['err'] = k
|
||||
for _ in range(rep): pool.append(t); pool += neighbours(t, a.neigh, g)
|
||||
with open(os.path.join(a.out, 'pool.jsonl'), 'w') as f:
|
||||
for t in pool: f.write(json.dumps(t) + '\n')
|
||||
with open(os.path.join(a.out, 'e2_tasks.jsonl'), 'w') as f:
|
||||
for t in cls['E2'][:2000]: f.write(json.dumps(t) + '\n')
|
||||
n = len(tasks)
|
||||
rep = {'n': n, **{k: len(v) / n for k, v in cls.items()}, 'pool': len(pool)}
|
||||
bycat = {}
|
||||
for k, lst in cls.items():
|
||||
for t in lst: bycat.setdefault(t['cat'], {}).setdefault(k, 0); bycat[t['cat']][k] += 1
|
||||
rep['by_cat'] = bycat
|
||||
json.dump(rep, open(os.path.join(a.out, 'report.json'), 'w'), indent=1)
|
||||
md = ['# 挖错报告', '', f'任务 {n}，错题池 {len(pool)}', '', '| 类 | 占比 |', '|---|---:|'] + [f'| {k} | {len(v) / n:.2%} |' for k, v in cls.items()]
|
||||
open(os.path.join(a.out, 'report.md'), 'w').write('\n'.join(md) + '\n')
|
||||
print(json.dumps(rep, indent=1))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,79 @@
|
||||
"""无老师出题（PyTorch）：A 均匀 / B 直线被挡 / C 贴立柱 / D 工作空间外；pos/pose 各半。"""
|
||||
import math
|
||||
import torch
|
||||
|
||||
from ml.common.features import wrap180
|
||||
|
||||
|
||||
def rand_q(kin, n, g):
|
||||
u = torch.rand(n, 6, device=kin.lo.device, generator=g)
|
||||
return kin.lo + u * (kin.hi - kin.lo)
|
||||
|
||||
|
||||
def free_q(kin, n, g, chunk=4096):
|
||||
out = []
|
||||
while sum(x.shape[0] for x in out) < n:
|
||||
q = rand_q(kin, chunk, g)
|
||||
ok = kin.gaps(q, 0.5)['gm'].min(-1).values > 0
|
||||
out.append(q[ok])
|
||||
return torch.cat(out)[:n]
|
||||
|
||||
|
||||
def straight_blocked(kin, a, b, n=24):
|
||||
d = torch.where(kin.cont, wrap180(b - a), b - a)
|
||||
s = torch.linspace(0, 1, n, device=a.device)
|
||||
pts = (a[:, None] + s[None, :, None] * d[:, None]).reshape(-1, 6)
|
||||
return (kin.gaps(pts, 0.5)['gm'].min(-1).values <= 0).view(a.shape[0], n).any(-1)
|
||||
|
||||
|
||||
def column_gap(kin, q):
|
||||
gm = kin.gaps(q, 40)['gm']
|
||||
return torch.stack([gm[:, g] for g in (2, 3, 4, 5, 6)], -1).min(-1).values # 组 0（立柱/底座）× 组 2..6
|
||||
|
||||
|
||||
@torch.no_grad()
|
||||
def sample_tasks(kin, n, seed, mix=(0.3, 0.3, 0.3, 0.1)):
|
||||
g = torch.Generator(device=kin.lo.device); g.manual_seed(seed)
|
||||
want = [int(round(n * m)) for m in mix]; want[0] += n - sum(want)
|
||||
tasks = []
|
||||
def emit(cat, q0, b, T=None):
|
||||
pose = torch.rand(q0.shape[0], device=q0.device, generator=g) < 0.5
|
||||
if cat == 'D': pose[:] = False
|
||||
xf = kin.cum_xf(b)
|
||||
Tb = kin.fk(b, xf) if T is None else T
|
||||
Rb = kin.orient(b, xf).reshape(-1, 9)
|
||||
for i in range(q0.shape[0]):
|
||||
tasks.append({'cat': cat, 'mode': 'pose' if pose[i] else 'pos', 'q0': q0[i].tolist(), 'T': Tb[i].tolist(),
|
||||
'R': Rb[i].tolist() if pose[i] else None, 'exec_s': 3.0, 'travel': 0.0})
|
||||
if want[0]:
|
||||
emit('A', free_q(kin, want[0], g), free_q(kin, want[0], g))
|
||||
for cat, k in (('B', want[1]), ('C', want[2])):
|
||||
got_a, got_b, have = [], [], 0
|
||||
while have < k:
|
||||
a, b = free_q(kin, 2048, g), free_q(kin, 2048, g)
|
||||
m = straight_blocked(kin, a, b)
|
||||
if cat == 'C': m &= column_gap(kin, b) < 8
|
||||
got_a.append(a[m]); got_b.append(b[m]); have += int(m.sum())
|
||||
if k: emit(cat, torch.cat(got_a)[:k], torch.cat(got_b)[:k])
|
||||
if want[3]:
|
||||
a, b = free_q(kin, want[3], g), rand_q(kin, want[3], g)
|
||||
o = kin.fk(torch.zeros(1, 6, device=a.device))
|
||||
T = o + (kin.fk(b) - o) * 1.6
|
||||
emit('D', a, b, T)
|
||||
return tasks
|
||||
|
||||
|
||||
def neighbours(t, k, g):
|
||||
"""±5 mm / ±3° 邻居"""
|
||||
out = []
|
||||
for _ in range(k):
|
||||
u = torch.randn(3, generator=g); u = u / u.norm() * 5.0
|
||||
nt = dict(t); nt['T'] = [x + float(d) for x, d in zip(t['T'], u)]
|
||||
if t.get('R'):
|
||||
ax = torch.randn(3, generator=g); ax = ax / ax.norm(); ang = math.radians(3.0) * float(torch.rand(1, generator=g))
|
||||
K = torch.tensor([[0, -ax[2], ax[1]], [ax[2], 0, -ax[0]], [-ax[1], ax[0], 0]])
|
||||
Rr = torch.eye(3) + math.sin(ang) * K + (1 - math.cos(ang)) * K @ K
|
||||
nt['R'] = (Rr @ torch.tensor(t['R']).view(3, 3)).flatten().tolist()
|
||||
nt['hard'] = True
|
||||
out.append(nt)
|
||||
return out
|
||||
@@ -0,0 +1,34 @@
|
||||
"""MeanFlow 损失（Geng et al. 2025；MP1 配方：25% 样本 r≠t，自适应损失加权）。
|
||||
约定：t=0 为数据、t=1 为噪声；z_t = (1−t)·x + t·ε，瞬时速度 v = ε − x；网络输出平均速度 u(z,r,t)。
|
||||
目标 u_tgt = v − (t − r)·du/dt，du/dt = v·∂_z u + ∂_t u 用 torch.func.jvp 一次前向得到（切向量 (v, 0, 1)）。"""
|
||||
import torch
|
||||
from torch.func import jvp
|
||||
|
||||
|
||||
def sample_rt(B, device, dtype, p_neq=0.25, mu=-0.4, sigma=1.0):
|
||||
t = torch.sigmoid(torch.randn(B, device=device, dtype=dtype) * sigma + mu)
|
||||
r = torch.sigmoid(torch.randn(B, device=device, dtype=dtype) * sigma + mu)
|
||||
t, r = torch.maximum(t, r), torch.minimum(t, r)
|
||||
eq = torch.rand(B, device=device) >= p_neq
|
||||
r = torch.where(eq, t, r)
|
||||
return r, t
|
||||
|
||||
|
||||
def meanflow_loss(model, mem, x, p_neq=0.25, ada_p=1.0, ada_c=1e-3):
|
||||
B = x.shape[0]
|
||||
r, t = sample_rt(B, x.device, x.dtype, p_neq)
|
||||
e = torch.randn_like(x)
|
||||
z = (1 - t)[:, None, None] * x + t[:, None, None] * e
|
||||
v = e - x
|
||||
fn = lambda z_, r_, t_: model.decode(mem, z_, r_, t_)
|
||||
u, dudt = jvp(fn, (z, r, t), (v, torch.zeros_like(r), torch.ones_like(t)))
|
||||
u_tgt = (v - (t - r)[:, None, None] * dudt).detach()
|
||||
err = (u - u_tgt).pow(2).mean(dim=(1, 2))
|
||||
w = 1.0 / (err.detach() + ada_c).pow(ada_p)
|
||||
return (w * err).mean(), err.mean().detach()
|
||||
|
||||
|
||||
def one_step(model, mem, e):
|
||||
B = e.shape[0]
|
||||
one = torch.ones(B, device=e.device, dtype=e.dtype)
|
||||
return e - model.decode(mem, e, torch.zeros_like(one), one)
|
||||
@@ -0,0 +1,159 @@
|
||||
"""策略网络（方案 §6.1）：条件特征 → 8 个条件 token → 4 层 encoder；16 个动作 token（噪声 z + 时间嵌入 (r,t)）
|
||||
→ 4 层 decoder（自注意力 + 交叉注意力，非自回归一次前向）→ 每 token 6 维（平均速度场 u）。
|
||||
价值头 / 子目标头只依赖观测（encoder 输出）。
|
||||
注意力用显式 matmul+softmax 实现：torch.func.jvp（MeanFlow 训练）需要前向模式自动微分，SDPA 融合内核不支持。"""
|
||||
import math
|
||||
import torch
|
||||
import torch.nn as nn
|
||||
import torch.nn.functional as F
|
||||
|
||||
from ml.common.features import F_DIM, H
|
||||
|
||||
|
||||
class Attn(nn.Module):
|
||||
def __init__(self, d, nh):
|
||||
super().__init__()
|
||||
self.nh, self.dh = nh, d // nh
|
||||
self.q, self.k, self.v, self.o = (nn.Linear(d, d) for _ in range(4))
|
||||
|
||||
def forward(self, x, mem=None):
|
||||
mem = x if mem is None else mem
|
||||
B, Lq, D = x.shape; Lk = mem.shape[1]
|
||||
q = self.q(x).view(B, Lq, self.nh, self.dh).transpose(1, 2)
|
||||
k = self.k(mem).view(B, Lk, self.nh, self.dh).transpose(1, 2)
|
||||
v = self.v(mem).view(B, Lk, self.nh, self.dh).transpose(1, 2)
|
||||
a = torch.softmax(q @ k.transpose(-1, -2) / math.sqrt(self.dh), dim=-1)
|
||||
return self.o((a @ v).transpose(1, 2).reshape(B, Lq, D))
|
||||
|
||||
|
||||
class FFN(nn.Module):
|
||||
def __init__(self, d, f):
|
||||
super().__init__(); self.a = nn.Linear(d, f); self.b = nn.Linear(f, d)
|
||||
|
||||
def forward(self, x):
|
||||
return self.b(F.gelu(self.a(x)))
|
||||
|
||||
|
||||
class EncBlock(nn.Module):
|
||||
def __init__(self, d, nh, f):
|
||||
super().__init__(); self.n1, self.n2 = nn.LayerNorm(d), nn.LayerNorm(d); self.sa, self.ff = Attn(d, nh), FFN(d, f)
|
||||
|
||||
def forward(self, x):
|
||||
x = x + self.sa(self.n1(x))
|
||||
return x + self.ff(self.n2(x))
|
||||
|
||||
|
||||
class DecBlock(nn.Module):
|
||||
def __init__(self, d, nh, f):
|
||||
super().__init__()
|
||||
self.n1, self.n2, self.n3 = nn.LayerNorm(d), nn.LayerNorm(d), nn.LayerNorm(d)
|
||||
self.sa, self.ca, self.ff = Attn(d, nh), Attn(d, nh), FFN(d, f)
|
||||
|
||||
def forward(self, x, mem):
|
||||
x = x + self.sa(self.n1(x))
|
||||
x = x + self.ca(self.n2(x), mem)
|
||||
return x + self.ff(self.n3(x))
|
||||
|
||||
|
||||
def time_emb(t, dim):
|
||||
half = dim // 2
|
||||
fr = torch.exp(-math.log(1000.0) * torch.arange(half, device=t.device, dtype=t.dtype) / half)
|
||||
a = t[:, None] * 1000.0 * fr[None]
|
||||
return torch.cat([torch.sin(a), torch.cos(a)], -1)
|
||||
|
||||
|
||||
class Policy(nn.Module):
|
||||
def __init__(self, d=192, nh=8, ff=768, n_enc=4, n_dec=4, n_cond=8, f_dim=F_DIM, horizon=H, head='meanflow', K=8):
|
||||
super().__init__()
|
||||
self.d, self.n_cond, self.h, self.head, self.K = d, n_cond, horizon, head, K
|
||||
self.cond = nn.Sequential(nn.Linear(f_dim, 2 * d), nn.GELU(), nn.Linear(2 * d, n_cond * d))
|
||||
self.cpos = nn.Parameter(torch.zeros(1, n_cond, d))
|
||||
self.enc = nn.ModuleList(EncBlock(d, nh, ff) for _ in range(n_enc))
|
||||
self.enc_norm = nn.LayerNorm(d)
|
||||
self.ain = nn.Linear(6, d)
|
||||
self.apos = nn.Parameter(torch.zeros(1, horizon, d))
|
||||
self.temb = nn.Sequential(nn.Linear(2 * 64, d), nn.GELU(), nn.Linear(d, d))
|
||||
self.dec = nn.ModuleList(DecBlock(d, nh, ff) for _ in range(n_dec))
|
||||
self.dec_norm = nn.LayerNorm(d)
|
||||
self.aout = nn.Linear(d, 6)
|
||||
self.value = nn.Sequential(nn.Linear(d, d), nn.GELU(), nn.Linear(d, 1))
|
||||
self.sub = nn.Sequential(nn.Linear(d, d), nn.GELU(), nn.Linear(d, 13))
|
||||
if head == 'wta': # 预定回退：同骨干 WTA K 模式头（方案 §6.2）
|
||||
self.mode_emb = nn.Parameter(torch.randn(1, K, 1, d) * 0.02)
|
||||
self.mode_logit = nn.Linear(d, 1)
|
||||
nn.init.normal_(self.cpos, std=0.02); nn.init.normal_(self.apos, std=0.02)
|
||||
nn.init.zeros_(self.aout.weight); nn.init.zeros_(self.aout.bias)
|
||||
|
||||
def encode(self, feat):
|
||||
x = self.cond(feat).view(-1, self.n_cond, self.d) + self.cpos
|
||||
for b in self.enc: x = b(x)
|
||||
return self.enc_norm(x)
|
||||
|
||||
def heads(self, mem):
|
||||
p = mem.mean(1)
|
||||
return self.value(p).squeeze(-1), self.sub(p)
|
||||
|
||||
def decode(self, mem, z, r, t):
|
||||
"""平均速度场 u(z, r, t)；z [B,16,6]，r,t [B]"""
|
||||
te = self.temb(torch.cat([time_emb(t, 64), time_emb(t - r, 64)], -1))
|
||||
x = self.ain(z) + self.apos + te[:, None]
|
||||
for b in self.dec: x = b(x, mem)
|
||||
return self.aout(self.dec_norm(x))
|
||||
|
||||
def wta_modes(self, mem):
|
||||
"""WTA：K 个模式一次解码 → act [B,K,16,6], logits [B,K]"""
|
||||
B = mem.shape[0]
|
||||
x = (self.ain.bias + self.apos)[:, None] + self.mode_emb # [1,K,16,d]
|
||||
x = x.expand(B, -1, -1, -1).reshape(B * self.K, self.h, self.d)
|
||||
m = mem.repeat_interleave(self.K, 0)
|
||||
for b in self.dec: x = b(x, m)
|
||||
x = self.dec_norm(x)
|
||||
act = self.aout(x).view(B, self.K, self.h, 6)
|
||||
return act, self.mode_logit(x.mean(1)).view(B, self.K)
|
||||
|
||||
def sample(self, feat, z):
|
||||
"""部署接口（ONNX 同签名）：返回 (act [B,16,6] 归一化增量, value [B], sub [B,13])
|
||||
meanflow：一步生成 x = z − u(z, 0, 1)；
|
||||
wta：取第 0 行观测的 K 个模式，第 b 行输出模式 b mod K（部署时 8 行观测相同；前瞻只用各行的 value）。"""
|
||||
mem = self.encode(feat)
|
||||
v, s = self.heads(mem)
|
||||
B = feat.shape[0]
|
||||
if self.head == 'wta':
|
||||
act, _ = self.wta_modes(mem[:1])
|
||||
idx = torch.arange(B, device=feat.device) % self.K
|
||||
return act[0, idx] + 0.0 * z, v, s # z 不参与计算，保留为输入以统一 ONNX 签名
|
||||
one, zero = torch.ones(B, device=feat.device, dtype=feat.dtype), torch.zeros(B, device=feat.device, dtype=feat.dtype)
|
||||
x = z - self.decode(mem, z, zero, one)
|
||||
return x, v, s
|
||||
|
||||
def sample_k(self, feat, K, z=None):
|
||||
"""仿真 / APG 用：每个观测 K 条候选 → act [B,K,16,6], value [B], sub [B,13]"""
|
||||
mem = self.encode(feat)
|
||||
v, s = self.heads(mem)
|
||||
B = feat.shape[0]
|
||||
if self.head == 'wta':
|
||||
act, logit = self.wta_modes(mem)
|
||||
if K < self.K:
|
||||
top = logit.topk(K, -1).indices
|
||||
act = act.gather(1, top[:, :, None, None].expand(-1, -1, self.h, 6))
|
||||
return act, v, s
|
||||
z = torch.randn(B * K, self.h, 6, device=feat.device, dtype=feat.dtype) if z is None else z.reshape(B * K, self.h, 6)
|
||||
m = mem.repeat_interleave(K, 0)
|
||||
one = torch.ones(B * K, device=feat.device, dtype=feat.dtype)
|
||||
x = z - self.decode(m, z, torch.zeros_like(one), one)
|
||||
return x.view(B, K, self.h, 6), v, s
|
||||
|
||||
|
||||
def build(cfg):
|
||||
m = cfg.get('model', {})
|
||||
return Policy(d=m.get('d', 192), nh=m.get('heads', 8), ff=m.get('ff', 768), n_enc=m.get('enc', 4), n_dec=m.get('dec', 4),
|
||||
head=m.get('head', 'meanflow'), K=m.get('K', 8))
|
||||
|
||||
|
||||
def wta_loss(model, mem, y, w_all=0.05):
|
||||
"""WTA-L1 + 模式 CE；返回 (loss, 最优模式输出 [B,16,6])"""
|
||||
act, logit = model.wta_modes(mem)
|
||||
l1 = (act - y[:, None]).abs().mean(dim=(2, 3)) # [B,K]
|
||||
best = l1.argmin(-1)
|
||||
loss = l1.gather(1, best[:, None]).mean() + w_all * l1.mean() + 0.1 * F.cross_entropy(logit, best)
|
||||
return loss, act[torch.arange(y.shape[0]), best]
|
||||
Generated
+227
@@ -0,0 +1,227 @@
|
||||
{
|
||||
"name": "robot-arm-ml",
|
||||
"lockfileVersion": 3,
|
||||
"requires": true,
|
||||
"packages": {
|
||||
"": {
|
||||
"name": "robot-arm-ml",
|
||||
"dependencies": {
|
||||
"onnxruntime-node": "^1.20.0"
|
||||
}
|
||||
},
|
||||
"node_modules/adm-zip": {
|
||||
"version": "0.6.1",
|
||||
"resolved": "https://registry.npmjs.org/adm-zip/-/adm-zip-0.6.1.tgz",
|
||||
"integrity": "sha512-Xwrja8nx9e5o2N1my4DsKCeKpdrnACyr1wtbPxBDgGzKzKyE9kRtBFA8mWldI+RVlD7CBZNWY/wQ2+ydwOR6kQ==",
|
||||
"license": "MIT",
|
||||
"engines": {
|
||||
"node": ">=14.0"
|
||||
}
|
||||
},
|
||||
"node_modules/define-data-property": {
|
||||
"version": "1.1.4",
|
||||
"resolved": "https://registry.npmjs.org/define-data-property/-/define-data-property-1.1.4.tgz",
|
||||
"integrity": "sha512-rBMvIzlpA8v6E+SJZoo++HAYqsLrkg7MSfIinMPFhmkorw7X+dOXVJQs+QT69zGkzMyfDnIMN2Wid1+NbL3T+A==",
|
||||
"license": "MIT",
|
||||
"dependencies": {
|
||||
"es-define-property": "^1.0.0",
|
||||
"es-errors": "^1.3.0",
|
||||
"gopd": "^1.0.1"
|
||||
},
|
||||
"engines": {
|
||||
"node": ">= 0.4"
|
||||
},
|
||||
"funding": {
|
||||
"url": "https://github.com/sponsors/ljharb"
|
||||
}
|
||||
},
|
||||
"node_modules/define-properties": {
|
||||
"version": "1.2.1",
|
||||
"resolved": "https://registry.npmjs.org/define-properties/-/define-properties-1.2.1.tgz",
|
||||
"integrity": "sha512-8QmQKqEASLd5nx0U1B1okLElbUuuttJ/AnYmRXbbbGDWh6uS208EjD4Xqq/I9wK7u0v6O08XhTWnt5XtEbR6Dg==",
|
||||
"license": "MIT",
|
||||
"dependencies": {
|
||||
"define-data-property": "^1.0.1",
|
||||
"has-property-descriptors": "^1.0.0",
|
||||
"object-keys": "^1.1.1"
|
||||
},
|
||||
"engines": {
|
||||
"node": ">= 0.4"
|
||||
},
|
||||
"funding": {
|
||||
"url": "https://github.com/sponsors/ljharb"
|
||||
}
|
||||
},
|
||||
"node_modules/es-define-property": {
|
||||
"version": "1.0.1",
|
||||
"resolved": "https://registry.npmjs.org/es-define-property/-/es-define-property-1.0.1.tgz",
|
||||
"integrity": "sha512-e3nRfgfUZ4rNGL232gUgX06QNyyez04KdjFrF+LTRoOXmrOgFKDg4BCdsjW8EnT69eqdYGmRpJwiPVYNrCaW3g==",
|
||||
"license": "MIT",
|
||||
"engines": {
|
||||
"node": ">= 0.4"
|
||||
}
|
||||
},
|
||||
"node_modules/es-errors": {
|
||||
"version": "1.3.0",
|
||||
"resolved": "https://registry.npmjs.org/es-errors/-/es-errors-1.3.0.tgz",
|
||||
"integrity": "sha512-Zf5H2Kxt2xjTvbJvP2ZWLEICxA6j+hAmMzIlypy4xcBg1vKVnx89Wy0GbS+kf5cwCVFFzdCFh2XSCFNULS6csw==",
|
||||
"license": "MIT",
|
||||
"engines": {
|
||||
"node": ">= 0.4"
|
||||
}
|
||||
},
|
||||
"node_modules/escape-string-regexp": {
|
||||
"version": "4.0.0",
|
||||
"resolved": "https://registry.npmjs.org/escape-string-regexp/-/escape-string-regexp-4.0.0.tgz",
|
||||
"integrity": "sha512-TtpcNJ3XAzx3Gq8sWRzJaVajRs0uVxA2YAkdb1jm2YkPz4G6egUFAyA3n5vtEIZefPk5Wa4UXbKuS5fKkJWdgA==",
|
||||
"license": "MIT",
|
||||
"engines": {
|
||||
"node": ">=10"
|
||||
},
|
||||
"funding": {
|
||||
"url": "https://github.com/sponsors/sindresorhus"
|
||||
}
|
||||
},
|
||||
"node_modules/global-agent": {
|
||||
"version": "4.1.3",
|
||||
"resolved": "https://registry.npmjs.org/global-agent/-/global-agent-4.1.3.tgz",
|
||||
"integrity": "sha512-KUJEViiuFT3I97t+GYMikLPJS2Lfo/S2F+DQuBWzuzaMPnvt5yyZePzArx36fBzpGTxZjIpDbXLeySLgh+k76g==",
|
||||
"license": "BSD-3-Clause",
|
||||
"dependencies": {
|
||||
"globalthis": "^1.0.2",
|
||||
"matcher": "^4.0.0",
|
||||
"semver": "^7.3.5",
|
||||
"serialize-error": "^8.1.0"
|
||||
},
|
||||
"engines": {
|
||||
"node": ">=10.0"
|
||||
}
|
||||
},
|
||||
"node_modules/globalthis": {
|
||||
"version": "1.0.4",
|
||||
"resolved": "https://registry.npmjs.org/globalthis/-/globalthis-1.0.4.tgz",
|
||||
"integrity": "sha512-DpLKbNU4WylpxJykQujfCcwYWiV/Jhm50Goo0wrVILAv5jOr9d+H+UR3PhSCD2rCCEIg0uc+G+muBTwD54JhDQ==",
|
||||
"license": "MIT",
|
||||
"dependencies": {
|
||||
"define-properties": "^1.2.1",
|
||||
"gopd": "^1.0.1"
|
||||
},
|
||||
"engines": {
|
||||
"node": ">= 0.4"
|
||||
},
|
||||
"funding": {
|
||||
"url": "https://github.com/sponsors/ljharb"
|
||||
}
|
||||
},
|
||||
"node_modules/gopd": {
|
||||
"version": "1.2.0",
|
||||
"resolved": "https://registry.npmjs.org/gopd/-/gopd-1.2.0.tgz",
|
||||
"integrity": "sha512-ZUKRh6/kUFoAiTAtTYPZJ3hw9wNxx+BIBOijnlG9PnrJsCcSjs1wyyD6vJpaYtgnzDrKYRSqf3OO6Rfa93xsRg==",
|
||||
"license": "MIT",
|
||||
"engines": {
|
||||
"node": ">= 0.4"
|
||||
},
|
||||
"funding": {
|
||||
"url": "https://github.com/sponsors/ljharb"
|
||||
}
|
||||
},
|
||||
"node_modules/has-property-descriptors": {
|
||||
"version": "1.0.2",
|
||||
"resolved": "https://registry.npmjs.org/has-property-descriptors/-/has-property-descriptors-1.0.2.tgz",
|
||||
"integrity": "sha512-55JNKuIW+vq4Ke1BjOTjM2YctQIvCT7GFzHwmfZPGo5wnrgkid0YQtnAleFSqumZm4az3n2BS+erby5ipJdgrg==",
|
||||
"license": "MIT",
|
||||
"dependencies": {
|
||||
"es-define-property": "^1.0.0"
|
||||
},
|
||||
"funding": {
|
||||
"url": "https://github.com/sponsors/ljharb"
|
||||
}
|
||||
},
|
||||
"node_modules/matcher": {
|
||||
"version": "4.0.0",
|
||||
"resolved": "https://registry.npmjs.org/matcher/-/matcher-4.0.0.tgz",
|
||||
"integrity": "sha512-S6x5wmcDmsDRRU/c2dkccDwQPXoFczc5+HpQ2lON8pnvHlnvHAHj5WlLVvw6n6vNyHuVugYrFohYxbS+pvFpKQ==",
|
||||
"license": "MIT",
|
||||
"dependencies": {
|
||||
"escape-string-regexp": "^4.0.0"
|
||||
},
|
||||
"engines": {
|
||||
"node": ">=10"
|
||||
},
|
||||
"funding": {
|
||||
"url": "https://github.com/sponsors/sindresorhus"
|
||||
}
|
||||
},
|
||||
"node_modules/object-keys": {
|
||||
"version": "1.1.1",
|
||||
"resolved": "https://registry.npmjs.org/object-keys/-/object-keys-1.1.1.tgz",
|
||||
"integrity": "sha512-NuAESUOUMrlIXOfHKzD6bpPu3tYt3xvjNdRIQ+FeT0lNb4K8WR70CaDxhuNguS2XG+GjkyMwOzsN5ZktImfhLA==",
|
||||
"license": "MIT",
|
||||
"engines": {
|
||||
"node": ">= 0.4"
|
||||
}
|
||||
},
|
||||
"node_modules/onnxruntime-common": {
|
||||
"version": "1.30.0",
|
||||
"resolved": "https://registry.npmjs.org/onnxruntime-common/-/onnxruntime-common-1.30.0.tgz",
|
||||
"integrity": "sha512-7fdVWjAID1dVhH/G8qK3APARunV4VkBFoCQAP7qp4Wkab0mrorvmc+sqiT+mKXOzDqdjN5j+/Z9nb4gzNPWcyA==",
|
||||
"license": "MIT"
|
||||
},
|
||||
"node_modules/onnxruntime-node": {
|
||||
"version": "1.30.0",
|
||||
"resolved": "https://registry.npmjs.org/onnxruntime-node/-/onnxruntime-node-1.30.0.tgz",
|
||||
"integrity": "sha512-twhs1C2C/BFkz1yc5OY0KIU2GUq6DURO7hD4bx5Q2Qy3nAMJwRXW8xU3NVczE29VA9lolLOYepoD8fjTGOfIqw==",
|
||||
"hasInstallScript": true,
|
||||
"license": "MIT",
|
||||
"os": [
|
||||
"win32",
|
||||
"darwin",
|
||||
"linux"
|
||||
],
|
||||
"dependencies": {
|
||||
"adm-zip": "^0.6.0",
|
||||
"global-agent": "^4.1.3",
|
||||
"onnxruntime-common": "1.30.0"
|
||||
}
|
||||
},
|
||||
"node_modules/semver": {
|
||||
"version": "7.8.5",
|
||||
"resolved": "https://registry.npmjs.org/semver/-/semver-7.8.5.tgz",
|
||||
"integrity": "sha512-Y7/KDsb8LjooZpwaqGyulO6DQlksgCncchHGk+sZIY4SBvUocMBEFH5Ur1fI4dV+Jvl0w6cjvucaIi40puRioA==",
|
||||
"license": "ISC",
|
||||
"bin": {
|
||||
"semver": "bin/semver.js"
|
||||
},
|
||||
"engines": {
|
||||
"node": ">=10"
|
||||
}
|
||||
},
|
||||
"node_modules/serialize-error": {
|
||||
"version": "8.1.0",
|
||||
"resolved": "https://registry.npmjs.org/serialize-error/-/serialize-error-8.1.0.tgz",
|
||||
"integrity": "sha512-3NnuWfM6vBYoy5gZFvHiYsVbafvI9vZv/+jlIigFn4oP4zjNPK3LhcY0xSCgeb1a5L8jO71Mit9LlNoi2UfDDQ==",
|
||||
"license": "MIT",
|
||||
"dependencies": {
|
||||
"type-fest": "^0.20.2"
|
||||
},
|
||||
"engines": {
|
||||
"node": ">=10"
|
||||
},
|
||||
"funding": {
|
||||
"url": "https://github.com/sponsors/sindresorhus"
|
||||
}
|
||||
},
|
||||
"node_modules/type-fest": {
|
||||
"version": "0.20.2",
|
||||
"resolved": "https://registry.npmjs.org/type-fest/-/type-fest-0.20.2.tgz",
|
||||
"integrity": "sha512-Ne+eE4r0/iWnpAxD852z3A+N0Bt5RN//NjJwRd2VFHEmrywxf5vsZlh4R6lixl6B+wz/8d+maTSAkN1FIkI3LQ==",
|
||||
"license": "(MIT OR CC0-1.0)",
|
||||
"engines": {
|
||||
"node": ">=10"
|
||||
},
|
||||
"funding": {
|
||||
"url": "https://github.com/sponsors/sindresorhus"
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,8 @@
|
||||
{
|
||||
"name": "robot-arm-ml",
|
||||
"private": true,
|
||||
"description": "Node 端依赖：tests/policy_bench.cjs 用 onnxruntime-node 跑部署形态评测",
|
||||
"dependencies": {
|
||||
"onnxruntime-node": "^1.20.0"
|
||||
}
|
||||
}
|
||||
Executable
+51
@@ -0,0 +1,51 @@
|
||||
#!/usr/bin/env bash
|
||||
# 一条龙：bash ml/pipeline.sh <阶段> ；阶段按顺序：
|
||||
# setup 安装 Python/Node 依赖（torch 需先按 CUDA 版本装好）
|
||||
# kin 导出运动学/碰撞模型 + JS↔PyTorch 对拍（必须 PASS）
|
||||
# gate1 老师一致性门禁（决定生成时是否加 --consensus 4）
|
||||
# gen 生成 5 万条老师轨迹（NPROC 进程；CONSENSUS=1 时加共识过滤）
|
||||
# prep 打包训练样本
|
||||
# train 预训练（先 2000 步摸吞吐，再正式训练）
|
||||
# m5 MeanFlow 头门禁：贴立柱集关安全层穿柱率 ≤ 2%（不过 → 用 configs/wta.json 重训）
|
||||
# apg APG 首轮微调
|
||||
# mine 挖错循环（3–6 轮，含门禁回滚）
|
||||
# export 导出 ONNX（fp16）到 webapp/models/
|
||||
# bench 部署形态评测（Node，策略 + 安全层）
|
||||
# all 依次执行 kin → bench
|
||||
set -e
|
||||
cd "$(dirname "$0")/.."
|
||||
NPROC=${NPROC:-$(nproc)}
|
||||
N_TASKS=${N_TASKS:-50000}
|
||||
CFG=${CFG:-ml/configs/base.json}
|
||||
stage=${1:-help}
|
||||
run() { echo "== $*"; "$@"; }
|
||||
case $stage in
|
||||
setup) run pip install -r ml/requirements.txt; (cd ml && npm install --no-audit --no-fund) ;;
|
||||
kin) run node ml/prep/export_kin.cjs; run node ml/eval/dump_parity.cjs 2000; run python -m ml.eval.check_parity ;;
|
||||
gate1) mkdir -p ml/data; run node ml/gen/consistency.cjs 300 4242 | tee ml/data/consistency.json ;;
|
||||
gen) per=$(( (N_TASKS + NPROC - 1) / NPROC ))
|
||||
if [ -z "${CONSENSUS:-}" ] && [ -f ml/data/consistency.json ]; then
|
||||
CONSENSUS=$(python -c "import json;print(1 if json.load(open('ml/data/consistency.json'))['consistentByGateRule']<0.9 else 0)")
|
||||
echo "一致性门禁结果 → CONSENSUS=$CONSENSUS"
|
||||
fi
|
||||
extra=""; [ "${CONSENSUS:-0}" = "1" ] && extra="--consensus 4"
|
||||
run bash ml/gen/run_parallel.sh $per $NPROC 1 "$extra" ;;
|
||||
prep) run bash ml/prep/run_prep.sh $NPROC ;;
|
||||
train) run python -m ml.train --samples ml/data/samples --out ml/runs/bench --steps 2000 --no-eval --config $CFG
|
||||
run python -m ml.train --samples ml/data/samples --out ml/runs/base --config $CFG ;;
|
||||
m5) [ -f ml/runs/nc_gate.jsonl ] || run python -m ml.mining.mine_round --make-gate 1000 --seed 4321 --mix 0,0,1,0 --name nc_gate.jsonl --out ml/runs
|
||||
run python -m ml.sim.closed_loop --ckpt ml/runs/base/best.pt --tasks ml/runs/nc_gate.jsonl --n 1000 --out ml/runs/m5_gate.json
|
||||
python - <<'PY'
|
||||
import json; r = json.load(open('ml/runs/m5_gate.json'))
|
||||
print('贴立柱集（关安全层）穿柱率 %.2f%%，到达率 %.2f%%' % (100 * r['collision_rate'], 100 * r['success']))
|
||||
print('M5 门禁：' + ('通过' if r['collision_rate'] <= 0.02 else '不通过 → CFG=ml/configs/wta.json bash ml/pipeline.sh train'))
|
||||
PY
|
||||
;;
|
||||
apg) run python -m ml.apg.apg_finetune --init ml/runs/base/best.pt --samples ml/data/samples --out ml/runs/apg; cp ml/runs/apg/best.pt ml/runs/current.pt ;;
|
||||
mine) run bash ml/mining/loop.sh ${ROUNDS:-6} ${APG_STEPS:-1000} ${MINE_N:-50000} ;;
|
||||
export) ck=ml/runs/current.pt; [ -f $ck ] || ck=ml/runs/base/best.pt
|
||||
run python -m ml.export_onnx --ckpt $ck --out webapp/models/traj_policy.onnx --fp16 ;;
|
||||
bench) run node tests/policy_bench.cjs webapp/models/traj_policy.onnx nc,route --nc1000 --out ml/runs/policy_bench.json ;;
|
||||
all) for s in kin gate1 gen prep train m5 apg mine export bench; do bash ml/pipeline.sh $s; done ;;
|
||||
*) sed -n '2,16p' "$0" ;;
|
||||
esac
|
||||
@@ -0,0 +1,75 @@
|
||||
/* 原始老师轨迹 → 闭环训练样本（方案 §4.5 / §5）
|
||||
* node ml/prep/build_dataset.cjs ml/data/shard_0001.jsonl [outDir=ml/data/samples]
|
||||
* 每条 keep=true 的轨迹按 50 Hz 时间律重放，每 3 步取一个样本：
|
||||
* X [N,83] 特征（webapp/js/ik-policy.js features()，与部署同一份代码）
|
||||
* Y [N,16,6] 未来 16 步每步增量 / STEP（到终点后补 0）
|
||||
* V [N] 剩余时间（s）/ 5
|
||||
* SG [N,13] 子目标标签：剩余两个途经点相对当前 q 的差 / 180（不足用终点补）+ 是否需要
|
||||
* TID[N] 任务编号（tasks.jsonl 行号所在的全局 id，用于划分与闭环验证）
|
||||
* 增广：30% 样本对 q_t 加 σ=1° 噪声，标签改为 8 步内线性收回老师轨迹（噪声态须无干涉且间隙 > 1 mm）；
|
||||
* 已提交前缀长度 r ∈ {0..3} 均匀随机，前缀取老师接下来 r 步的增量。 */
|
||||
'use strict';
|
||||
const fs = require('fs'), path = require('path');
|
||||
const IN = process.argv[2], OUTDIR = process.argv[3] || path.join(__dirname, '../data/samples');
|
||||
if (!IN) { console.error('usage: build_dataset.cjs <shard.jsonl> [outDir]'); process.exit(1); }
|
||||
const { S } = require('../../tests/route_bench_setup.cjs')();
|
||||
const POL = require('../../webapp/js/ik-policy.js');
|
||||
const { timeParam } = require('../gen/timelaw.cjs');
|
||||
const { writeNpy } = require('./npy.cjs');
|
||||
const { F_DIM, STEP, H } = POL;
|
||||
const L = S.LIM, O = S.ORDER;
|
||||
const base = path.basename(IN, '.jsonl');
|
||||
let st = 0; for (const ch of base) st = (st * 31 + ch.charCodeAt(0)) >>> 0;
|
||||
const rnd = () => { st = (st * 1664525 + 1013904223) >>> 0; return st / 4294967296; };
|
||||
const gauss = () => Math.sqrt(-2 * Math.log(rnd() + 1e-12)) * Math.cos(2 * Math.PI * rnd());
|
||||
const hash = s => { let h = 2166136261; for (const c of s) h = Math.imul(h ^ c.charCodeAt(0), 16777619); return h >>> 0; };
|
||||
const X = [], Y = [], V = [], SG = [], TID = [], tasks = [];
|
||||
const lines = fs.readFileSync(IN, 'utf8').split('\n').filter(Boolean);
|
||||
let nAug = 0, nAugRej = 0;
|
||||
for (const line of lines) {
|
||||
const r = JSON.parse(line);
|
||||
if (!r.keep) continue;
|
||||
const tid = hash(r.id) & 0x7fffffff;
|
||||
const split = (hash(r.id + '#split') % 100) < 5 ? 'val' : 'train';
|
||||
const P = r.path;
|
||||
const tp = timeParam(P);
|
||||
const traj = tp.traj, taus = tp.taus, N = traj.length - 1;
|
||||
tasks.push({ tid, id: r.id, cat: r.cat, mode: r.mode, q0: r.q0, T: r.T, R: r.R, exec_s: N * POL.DT, travel: r.travel_deg, reached: r.reached, split });
|
||||
const vt = tp.cum; // 顶点 τ
|
||||
const inter = P.slice(1, -1).map((p, i) => ({ p, tau: vt[i + 1] }));
|
||||
for (let t = 0; t <= N; t += 3) {
|
||||
let q = traj[t].slice(), qPrev = (t > 0 ? traj[t - 1] : traj[0]).slice();
|
||||
let off = [0, 0, 0, 0, 0, 0], aug = false;
|
||||
if (rnd() < 0.3) {
|
||||
const qn = q.map((v, j) => { let x = v + gauss(); const t2 = L[O[j]]; if (!t2.cont) x = Math.max(t2.lo, Math.min(t2.hi, x)); return x; });
|
||||
nAug++;
|
||||
if (S.collideFree(qn) && S.clearance(qn, 5) > 1) { off = qn.map((v, j) => v - q[j]); q = qn; qPrev = qPrev.map((v, j) => v + off[j]); aug = true; }
|
||||
else nAugRej++;
|
||||
}
|
||||
const pos = k => { const i = Math.min(N, t + k); const w = aug ? Math.max(0, 1 - k / 8) : 0; return traj[i].map((v, j) => v + off[j] * w); };
|
||||
const y = new Float32Array(H * 6);
|
||||
let prevP = q;
|
||||
for (let k = 1; k <= H; k++) { const pk = pos(k); for (let j = 0; j < 6; j++) y[(k - 1) * 6 + j] = Math.max(-1, Math.min(1, (pk[j] - prevP[j]) / STEP[j])); prevP = pk; }
|
||||
const rr = Math.floor(rnd() * 4), prefix = [];
|
||||
for (let k = 0; k < rr; k++) { const row = []; for (let j = 0; j < 6; j++) row.push(y[k * 6 + j] * STEP[j]); prefix.push(row); }
|
||||
const remain = inter.filter(v => v.tau > taus[t] + 0.02);
|
||||
const sub = remain.length && rnd() < 0.8 ? remain[0].p : null;
|
||||
const gm = S.gapMatrix(q, POL.GAP_CAP);
|
||||
const f = POL.features(S, { q, qPrev, T: r.T, R: r.R, prefix, sub }, gm);
|
||||
const v1 = remain[0] ? remain[0].p : P[P.length - 1], v2 = remain[1] ? remain[1].p : P[P.length - 1];
|
||||
const sg = new Float32Array(13);
|
||||
for (let j = 0; j < 6; j++) { sg[j] = (v1[j] - q[j]) / 180; sg[6 + j] = (v2[j] - q[j]) / 180; }
|
||||
sg[12] = remain.length ? 1 : 0;
|
||||
X.push(f); Y.push(y); V.push((N - t) * POL.DT / 5); SG.push(sg); TID.push(tid);
|
||||
}
|
||||
}
|
||||
fs.mkdirSync(OUTDIR, { recursive: true });
|
||||
const n = X.length;
|
||||
const cat = (arrs, w) => { const o = new Float32Array(n * w); arrs.forEach((a, i) => o.set(a, i * w)); return o; };
|
||||
writeNpy(path.join(OUTDIR, base + '.X.npy'), cat(X, F_DIM), [n, F_DIM]);
|
||||
writeNpy(path.join(OUTDIR, base + '.Y.npy'), cat(Y, H * 6), [n, H, 6]);
|
||||
writeNpy(path.join(OUTDIR, base + '.V.npy'), Float32Array.from(V), [n]);
|
||||
writeNpy(path.join(OUTDIR, base + '.SG.npy'), cat(SG, 13), [n, 13]);
|
||||
writeNpy(path.join(OUTDIR, base + '.TID.npy'), TID, [n], 'int32');
|
||||
fs.writeFileSync(path.join(OUTDIR, base + '.tasks.jsonl'), tasks.map(t => JSON.stringify(t)).join('\n') + '\n');
|
||||
console.log(JSON.stringify({ shard: base, tasks: tasks.length, samples: n, aug: nAug, augRejected: nAugRej, featVer: POL.FEAT_VER }));
|
||||
@@ -0,0 +1,23 @@
|
||||
/* 导出运动学 + 胶囊碰撞模型给 PyTorch（ml/common/kin_torch.py 读取）
|
||||
* node ml/prep/export_kin.cjs [out=ml/assets/kin.json]
|
||||
* collide.json 重新拟合/重生成 pair2d 后必须重跑（指纹不符时 Python 端拒绝加载）。 */
|
||||
'use strict';
|
||||
const fs = require('fs'), path = require('path');
|
||||
const { S } = require('../../tests/route_bench_setup.cjs')();
|
||||
const OUT = process.argv[2] || path.join(__dirname, '../assets/kin.json');
|
||||
const COL = JSON.parse(fs.readFileSync(path.join(__dirname, '../../docs/collide.json'), 'utf8'));
|
||||
const ce = S.clusterExport();
|
||||
const out = {
|
||||
fp: S.modelFingerprint(),
|
||||
order: S.ORDER,
|
||||
P: S.ORDER.map(id => S.P[id]), D: S.ORDER.map(id => S.D[id]),
|
||||
lim: S.ORDER.map(id => [S.LIM[id].lo, S.LIM[id].hi, S.LIM[id].cont ? 1 : 0]),
|
||||
tcp0: S.TCP0, capm: S.CAPM,
|
||||
caps: COL.caps.map(c => ({ g: c.g, a: c.a, b: c.b, r: c.r })),
|
||||
clusters: ce.clusters, cpairs: ce.cpairs,
|
||||
lever: S.leverBounds(20000),
|
||||
generated: new Date().toISOString()
|
||||
};
|
||||
fs.mkdirSync(path.dirname(OUT), { recursive: true });
|
||||
fs.writeFileSync(OUT, JSON.stringify(out));
|
||||
console.log('wrote', OUT, 'caps', out.caps.length, 'clusters', out.clusters.length, 'cpairs', out.cpairs.length, 'fp', out.fp);
|
||||
@@ -0,0 +1,13 @@
|
||||
/* 最小 .npy 写出（float32 / int32，C 序） */
|
||||
'use strict';
|
||||
const fs = require('fs');
|
||||
function writeNpy(file, arr, shape, dtype) {
|
||||
const descr = dtype === 'int32' ? '<i4' : '<f4';
|
||||
let hdr = `{'descr': '${descr}', 'fortran_order': False, 'shape': (${shape.join(', ')}${shape.length === 1 ? ',' : ''}), }`;
|
||||
const pre = 10, total = Math.ceil((pre + hdr.length + 1) / 64) * 64;
|
||||
hdr = hdr + ' '.repeat(total - pre - hdr.length - 1) + '\n';
|
||||
const h = Buffer.alloc(pre); h.write('\x93NUMPY', 0, 'latin1'); h[6] = 1; h[7] = 0; h.writeUInt16LE(hdr.length, 8);
|
||||
const ta = dtype === 'int32' ? Int32Array.from(arr) : (arr instanceof Float32Array ? arr : Float32Array.from(arr));
|
||||
fs.writeFileSync(file, Buffer.concat([h, Buffer.from(hdr, 'latin1'), Buffer.from(ta.buffer, ta.byteOffset, ta.byteLength)]));
|
||||
}
|
||||
module.exports = { writeNpy };
|
||||
Executable
+8
@@ -0,0 +1,8 @@
|
||||
#!/usr/bin/env bash
|
||||
# 并行打包样本：bash ml/prep/run_prep.sh [进程数=CPU 核数]
|
||||
set -e
|
||||
cd "$(dirname "$0")/../.."
|
||||
P=${1:-$(nproc)}
|
||||
mkdir -p ml/data/samples
|
||||
ls ml/data/shard_*.jsonl | xargs -P "$P" -I{} node ml/prep/build_dataset.cjs {} ml/data/samples
|
||||
python3 -m ml.data --index ml/data/samples
|
||||
@@ -0,0 +1,6 @@
|
||||
# Python ≥ 3.10。torch 请按 CUDA 版本从 https://pytorch.org 安装（RTX 3090 用 cu121/cu124 均可），再装其余：
|
||||
torch>=2.2
|
||||
numpy>=1.24
|
||||
onnx>=1.15
|
||||
onnxruntime>=1.17
|
||||
onnxconverter-common>=1.14
|
||||
+17
@@ -0,0 +1,17 @@
|
||||
"""本地预览查看器（带 COOP/COEP 头 → 页面跨源隔离 → onnxruntime-web 可用多线程 wasm）
|
||||
python ml/serve.py [端口=8000] 然后打开 http://localhost:8000/?policy=1
|
||||
注：跨源隔离下从 CDN 加载 ort.min.js 需要 CDN 返回 CORP 头；jsDelivr 支持。若被拦，用 &ortLocal=1 并把
|
||||
onnxruntime-web 的 dist 文件（ort.min.js、ort-wasm-simd-threaded*.wasm/.mjs）放到 webapp/vendor/ort/。"""
|
||||
import http.server, os, sys
|
||||
os.chdir(os.path.join(os.path.dirname(os.path.abspath(__file__)), '..', 'webapp'))
|
||||
class H(http.server.SimpleHTTPRequestHandler):
|
||||
def end_headers(self):
|
||||
self.send_header('Cross-Origin-Opener-Policy', 'same-origin')
|
||||
self.send_header('Cross-Origin-Embedder-Policy', 'require-corp')
|
||||
self.send_header('Cross-Origin-Resource-Policy', 'cross-origin')
|
||||
self.send_header('Cache-Control', 'no-store')
|
||||
super().end_headers()
|
||||
extensions_map = {**http.server.SimpleHTTPRequestHandler.extensions_map, '.wasm': 'application/wasm', '.mjs': 'text/javascript'}
|
||||
port = int(sys.argv[1]) if len(sys.argv) > 1 else 8000
|
||||
print(f'http://localhost:{port}/?policy=1')
|
||||
http.server.ThreadingHTTPServer(('', port), H).serve_forever()
|
||||
@@ -0,0 +1,202 @@
|
||||
"""闭环仿真（验证 / 挖错 / 门禁共用，PyTorch 批量，无梯度）——按部署节奏运行：
|
||||
50 Hz 执行；队列 ≤ 3 步时推理一次（已提交前缀 r = 队列长度），提交接下来 3 步；
|
||||
冷启动用价值头对候选末态打分（短前瞻），之后按与上一计划的一致性选候选；
|
||||
接近目标 < 5 mm 时用局部 DLS 精修；子目标头在新目标时给出途经点（到达 < 5° 或 1.5 s 超时后切下一个）。
|
||||
safety=False 时不做任何碰撞拦截（暴露真实错误，挖错用）；safety=True 时近似部署安全层（候选提交步有干涉则换下一条，全不过则停一步）。
|
||||
python -m ml.sim.closed_loop --ckpt ml/runs/base/best.pt --samples ml/data/samples --split val --n 1000 [--safety]"""
|
||||
import argparse, json, math, os
|
||||
import torch
|
||||
|
||||
from ml.common.kin_torch import Kin
|
||||
from ml.common.features import features, STEP, H, GAP_CAP, D2R, wrap180
|
||||
|
||||
DT = 0.02
|
||||
|
||||
|
||||
def tasks_to_tensors(tasks, device):
|
||||
q0 = torch.tensor([t['q0'] for t in tasks], dtype=torch.float32, device=device)
|
||||
T = torch.tensor([t['T'] for t in tasks], dtype=torch.float32, device=device)
|
||||
R = torch.tensor([t['R'] if t.get('R') else [0.] * 9 for t in tasks], dtype=torch.float32, device=device)
|
||||
ex = torch.tensor([t.get('exec_s', 3.0) or 3.0 for t in tasks], dtype=torch.float32, device=device)
|
||||
tr = torch.tensor([t.get('travel', 0.0) or 0.0 for t in tasks], dtype=torch.float32, device=device)
|
||||
return q0, T, R, ex, tr
|
||||
|
||||
|
||||
def goal_err(kin, q, T, R):
|
||||
xf = kin.cum_xf(q)
|
||||
p = kin.fk(q, xf); Rq = kin.orient(q, xf).reshape(-1, 9)
|
||||
e = (T - p).norm(dim=-1)
|
||||
pose = R.abs().sum(-1) > 0
|
||||
tr = (Rq * R).sum(-1)
|
||||
oe = torch.where(pose, torch.acos(((tr - 1) / 2).clamp(-1, 1)) / D2R, torch.zeros_like(e))
|
||||
return e, oe
|
||||
|
||||
|
||||
def dls_steps(kin, q, T, R, n=3, scale=0.3, lam=0.5):
|
||||
"""局部 DLS 精修：n 步增量 [B,n,6]"""
|
||||
step = torch.tensor(STEP, device=q.device)
|
||||
pose = (R.abs().sum(-1) > 0).float()
|
||||
Rt = R.view(-1, 3, 3)
|
||||
def err(x):
|
||||
xf = kin.cum_xf(x); p = kin.fk(x, xf); Rq = kin.orient(x, xf)
|
||||
M = Rt @ Rq.transpose(1, 2)
|
||||
rv = 0.5 * torch.stack([M[:, 2, 1] - M[:, 1, 2], M[:, 0, 2] - M[:, 2, 0], M[:, 1, 0] - M[:, 0, 1]], -1) * 100 * pose[:, None]
|
||||
return torch.cat([T - p, rv], -1)
|
||||
out = []; x = q.clone()
|
||||
for _ in range(n):
|
||||
e0 = err(x); J = []
|
||||
for j in range(6):
|
||||
dx = torch.zeros_like(x); dx[:, j] = 1e-3
|
||||
J.append(-(err(x + dx) - e0) / 1e-3)
|
||||
J = torch.stack(J, -1) # [B,6(err),6(q)]
|
||||
A = J @ J.transpose(1, 2) + lam ** 2 * torch.eye(6, device=q.device)
|
||||
y = torch.linalg.solve(A, e0.unsqueeze(-1))
|
||||
dq = (J.transpose(1, 2) @ y).squeeze(-1)
|
||||
sc = (dq.abs() / (step * scale)).max(-1, keepdim=True).values.clamp(min=1)
|
||||
dq = dq / sc
|
||||
out.append(dq); x = x + dq
|
||||
return torch.stack(out, 1)
|
||||
|
||||
|
||||
@torch.no_grad()
|
||||
def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, return_traj=False):
|
||||
g = torch.Generator(device=device); g.manual_seed(seed)
|
||||
q0, T, R, ex, tr = tasks_to_tensors(tasks, device)
|
||||
B = q0.shape[0]
|
||||
step = torch.tensor(STEP, device=device)
|
||||
limit = torch.clamp(2 * ex, min=3.0) if max_sec is None else torch.full_like(ex, max_sec)
|
||||
nmax = int(math.ceil(limit.max().item() / DT))
|
||||
q, qp = q0.clone(), q0.clone()
|
||||
queue = torch.zeros(B, 0, 6, device=device)
|
||||
last = None
|
||||
subs = torch.zeros(B, 2, 6, device=device); nsub = torch.zeros(B, dtype=torch.long, device=device); sub_t = torch.zeros(B, device=device)
|
||||
done = torch.zeros(B, dtype=torch.bool, device=device); t_done = torch.full((B,), float('nan'), device=device)
|
||||
col = torch.zeros(B, device=device); travel = torch.zeros(B, device=device); brakes = torch.zeros(B, device=device)
|
||||
traj = [q.clone()] if return_traj else None
|
||||
cold = True
|
||||
for it in range(nmax):
|
||||
tnow = it * DT
|
||||
if queue.shape[1] <= 3:
|
||||
r = queue.shape[1]
|
||||
pre = torch.zeros(B, 3, 6, device=device); pre[:, :r] = queue[:, :r]
|
||||
qe = q + queue.sum(1)
|
||||
e, oe = goal_err(kin, qe, T, R)
|
||||
near = (e < 5) & ((R.abs().sum(-1) == 0) | (oe < 5))
|
||||
# 子目标推进
|
||||
if (nsub > 0).any():
|
||||
cur = subs[:, 0]
|
||||
dd = torch.where(kin.cont, wrap180(cur - qe), cur - qe).abs().max(-1).values
|
||||
adv = (nsub > 0) & ((dd < 5) | (tnow - sub_t > 1.5))
|
||||
subs = torch.where(adv[:, None, None], torch.cat([subs[:, 1:], subs[:, 1:]], 1), subs)
|
||||
nsub = torch.where(adv, nsub - 1, nsub); sub_t = torch.where(adv, torch.full_like(sub_t, tnow), sub_t)
|
||||
sf = (nsub > 0).float()
|
||||
feat = features(kin, q, qp, T, R, pre, torch.full((B,), r, device=device), subs[:, 0], sf)
|
||||
z = torch.randn(B * K, H, 6, device=device, generator=g)
|
||||
act, val, sub = model.sample_k(feat, K, z)
|
||||
inc = act.clamp(-1, 1) * step
|
||||
if cold:
|
||||
sub0 = sub
|
||||
need = sub0[:, 12] > 0.5
|
||||
subs = q[:, None] + sub0[:, :12].view(B, 2, 6) * 180
|
||||
nsub = torch.where(need, torch.full_like(nsub, 2), torch.zeros_like(nsub)); sub_t.fill_(tnow)
|
||||
fut = qe[:, None, None] + torch.cumsum(inc[:, :, r:], 2) # [B,K,H-r,6]
|
||||
if last is None:
|
||||
# 短前瞻：候选末态的价值
|
||||
qa = fut[:, :, -1].reshape(B * K, 6); qb = fut[:, :, -2].reshape(B * K, 6)
|
||||
f2 = features(kin, qa, qb, T.repeat_interleave(K, 0), R.repeat_interleave(K, 0), torch.zeros(B * K, 3, 6, device=device),
|
||||
torch.zeros(B * K, device=device), qa, torch.zeros(B * K, device=device))
|
||||
v2, _ = model.heads(model.encode(f2))
|
||||
score = -v2.view(B, K)
|
||||
else:
|
||||
n = min(fut.shape[2], last.shape[1] - 3)
|
||||
w = 0.85 ** torch.arange(n, device=device, dtype=torch.float32)
|
||||
d2 = (((fut[:, :, :n] - last[:, None, 3:3 + n]) / step) ** 2).sum(-1)
|
||||
score = -(d2 * w).sum(-1)
|
||||
order = score.argsort(-1, descending=True)
|
||||
pick = order[:, 0]
|
||||
commit = inc[torch.arange(B), pick, r:r + 3]
|
||||
if safety:
|
||||
ok_any = torch.zeros(B, dtype=torch.bool, device=device)
|
||||
chosen = torch.zeros(B, 3, 6, device=device)
|
||||
for kk in range(K):
|
||||
ck = inc[torch.arange(B), order[:, kk], r:r + 3]
|
||||
pts = qe[:, None] + torch.cumsum(ck, 1)
|
||||
mids = torch.cat([qe[:, None], pts[:, :-1]], 1) + 0.5 * ck
|
||||
allq = torch.cat([pts, mids], 1).reshape(-1, 6)
|
||||
okk = (kin.gaps(allq, 0.5)['gm'].min(-1).values > 0).view(B, 6).all(-1)
|
||||
take = okk & ~ok_any
|
||||
chosen = torch.where(take[:, None, None], ck, chosen); ok_any |= okk
|
||||
commit = torch.where(ok_any[:, None, None], chosen, torch.zeros_like(chosen))
|
||||
brakes += (~ok_any).float()
|
||||
if near.any():
|
||||
ref = dls_steps(kin, qe, T, R)
|
||||
commit = torch.where(near[:, None, None], ref, commit)
|
||||
last = torch.cat([qe[:, None], fut[torch.arange(B), pick]], 1)
|
||||
queue = torch.cat([queue, commit], 1)
|
||||
cold = False
|
||||
d = queue[:, 0]; queue = queue[:, 1:]
|
||||
d = torch.where(done[:, None], torch.zeros_like(d), d)
|
||||
qp = q; q = kin.clamp_box(q + d)
|
||||
travel += d.abs().sum(-1)
|
||||
if return_traj: traj.append(q.clone())
|
||||
mid = qp + 0.5 * (q - qp)
|
||||
gm = kin.gaps(torch.cat([q, mid], 0), 0.5)['gm'].min(-1).values
|
||||
hit = ((gm[:B] <= 0) | (gm[B:] <= 0)) & ~done
|
||||
col += hit.float()
|
||||
e, oe = goal_err(kin, q, T, R)
|
||||
newly = (~done) & (e < 1) & (oe < 0.5)
|
||||
t_done = torch.where(newly, torch.full_like(t_done, (it + 1) * DT), t_done); done |= newly
|
||||
if done.all(): break
|
||||
ok = done & (t_done <= limit)
|
||||
res = {'n': B, 'success': ok.float().mean().item(),
|
||||
'time_ok_1p3': (ok & (t_done <= 1.3 * ex)).float().mean().item(),
|
||||
'collision_rate': (col > 0).float().mean().item(), 'collision_steps_mean': col.mean().item(),
|
||||
'travel_ratio_median': (travel[ok] / tr[ok].clamp(min=1)).median().item() if ok.any() and (tr > 0).any() else None,
|
||||
'time_mean_s': t_done[ok].mean().item() if ok.any() else None, 'brakes_mean': brakes.mean().item()}
|
||||
per = {'done': done.cpu(), 't_done': t_done.cpu(), 'col': col.cpu(), 'travel': travel.cpu()}
|
||||
if return_traj: per['traj'] = torch.stack(traj, 1).cpu()
|
||||
return res, per
|
||||
|
||||
|
||||
def evaluate(model, kin, tasks, device, K=8, safety=False, bs=256, seed=0):
|
||||
agg, pers = {}, []
|
||||
for i in range(0, len(tasks), bs):
|
||||
r, p = rollout(model, kin, tasks[i:i + bs], device, K=K, safety=safety, seed=seed + i)
|
||||
w = r['n']; pers.append((r, p))
|
||||
for k, v in r.items():
|
||||
if k == 'n' or v is None: continue
|
||||
agg[k] = agg.get(k, 0.0) + v * w
|
||||
n = sum(r['n'] for r, _ in pers)
|
||||
out = {k: v / n for k, v in agg.items()}; out['n'] = n
|
||||
return out, pers
|
||||
|
||||
|
||||
def main():
|
||||
from ml.model.policy import build
|
||||
from ml.data import load_tasks
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument('--ckpt', required=True); ap.add_argument('--samples', default='ml/data/samples')
|
||||
ap.add_argument('--tasks', default=None, help='任务 jsonl（覆盖 --samples/--split）')
|
||||
ap.add_argument('--split', default='val'); ap.add_argument('--n', type=int, default=1000)
|
||||
ap.add_argument('--safety', action='store_true'); ap.add_argument('--K', type=int, default=8)
|
||||
ap.add_argument('--device', default='cuda' if torch.cuda.is_available() else 'cpu'); ap.add_argument('--out', default=None)
|
||||
a = ap.parse_args()
|
||||
ck = torch.load(a.ckpt, map_location=a.device)
|
||||
model = build(ck['cfg']).to(a.device); model.load_state_dict(ck['model']); model.eval()
|
||||
kin = Kin(device=a.device)
|
||||
assert ck.get('fp') in (None, kin.fp), '碰撞模型指纹与 checkpoint 不符'
|
||||
if a.tasks: tasks = [json.loads(l) for l in open(a.tasks, encoding='utf-8') if l.strip()]
|
||||
else: tasks = [t for t in load_tasks([a.samples]) if t['split'] == a.split]
|
||||
tasks = tasks[:a.n]
|
||||
res, pers = evaluate(model, kin, tasks, a.device, K=a.K, safety=a.safety)
|
||||
by = {}
|
||||
flat_done = torch.cat([p['done'] for _, p in pers]); flat_col = torch.cat([p['col'] for _, p in pers])
|
||||
for t, d, c in zip(tasks, flat_done.tolist(), flat_col.tolist()):
|
||||
k = t.get('cat', '?') + '/' + t.get('mode', '?'); b = by.setdefault(k, [0, 0, 0]); b[0] += 1; b[1] += int(d); b[2] += int(c > 0)
|
||||
res['by_cat_mode'] = {k: {'n': v[0], 'success': v[1] / v[0], 'collision': v[2] / v[0]} for k, v in by.items()}
|
||||
s = json.dumps(res, indent=1); print(s)
|
||||
if a.out: open(a.out, 'w').write(s)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,123 @@
|
||||
# 2D 绕柱玩具实验：比较不同动作头在"左右(上下)双模态绕行"下的闭环表现
|
||||
import torch, torch.nn as nn, numpy as np, math, time, sys, json
|
||||
torch.manual_seed(0); np.random.seed(0); torch.set_num_threads(2)
|
||||
R=1.0; H=16; STEP=0.05
|
||||
def demo(s,g,mode):
|
||||
via=np.array([0.0, 1.35*mode])
|
||||
pts=[s, np.array([-0.9,0.95*mode]) if s[0]<-0.9 else s, via, np.array([0.9,0.95*mode]), g]
|
||||
P=np.array(pts); L=np.r_[0,np.cumsum(np.linalg.norm(np.diff(P,axis=0),axis=1))]
|
||||
n=int(L[-1]/STEP)+1; u=np.linspace(0,L[-1],n)
|
||||
x=np.interp(u,L,P[:,0]); y=np.interp(u,L,P[:,1]); Q=np.c_[x,y]
|
||||
# 平滑
|
||||
for _ in range(20): Q[1:-1]=0.5*Q[1:-1]+0.25*(Q[:-2]+Q[2:])
|
||||
return Q
|
||||
def sample_task(hard=False):
|
||||
sy=np.random.uniform(-0.3,0.3) if hard else np.random.uniform(-1.5,1.5)
|
||||
gy=np.random.uniform(-0.3,0.3) if hard else np.random.uniform(-1.5,1.5)
|
||||
return np.array([np.random.uniform(-2.5,-1.6),sy]), np.array([np.random.uniform(1.6,2.5),gy])
|
||||
def collide_path(Q):
|
||||
return np.any(np.linalg.norm(Q,axis=1)<R)
|
||||
def build(n=3000):
|
||||
O=[];A=[]
|
||||
for _ in range(n):
|
||||
s,g=sample_task(hard=np.random.rand()<0.4)
|
||||
d=s[1]+g[1]
|
||||
mode = np.sign(d) if abs(d)>0.3 else (1 if np.random.rand()<0.5 else -1)
|
||||
Q=demo(s,g,mode)
|
||||
if collide_path(Q): continue
|
||||
D=np.diff(Q,axis=0); D=np.r_[D,np.zeros((H,2))]
|
||||
for i in range(len(Q)-1):
|
||||
O.append(np.r_[Q[i],g]); A.append(D[i:i+H].reshape(-1)/STEP)
|
||||
return torch.tensor(np.array(O),dtype=torch.float32), torch.tensor(np.array(A),dtype=torch.float32)
|
||||
O,A=build(); print('samples',len(O),flush=True)
|
||||
AD=2*H
|
||||
def mlp(i,o,h=256): return nn.Sequential(nn.Linear(i,h),nn.SiLU(),nn.Linear(h,h),nn.SiLU(),nn.Linear(h,h),nn.SiLU(),nn.Linear(h,o))
|
||||
class MSE(nn.Module):
|
||||
def __init__(s): super().__init__(); s.f=mlp(4,AD)
|
||||
def loss(s,o,a): return ((s.f(o)-a)**2).mean()
|
||||
def sample(s,o,k=1): return s.f(o).unsqueeze(1).expand(-1,k,-1)
|
||||
class WTA(nn.Module):
|
||||
K=4
|
||||
def __init__(s): super().__init__(); s.f=mlp(4,AD*s.K+s.K)
|
||||
def out(s,o):
|
||||
y=s.f(o); return y[:,:AD*s.K].view(-1,s.K,AD), y[:,AD*s.K:]
|
||||
def loss(s,o,a):
|
||||
p,l=s.out(o); e=(p-a[:,None]).abs().mean(-1); k=e.argmin(1)
|
||||
return e.gather(1,k[:,None]).mean()+0.5*nn.functional.cross_entropy(l,k)
|
||||
def sample(s,o,k=1):
|
||||
p,l=s.out(o)
|
||||
if k==1: return p.gather(1,l.argmax(1)[:,None,None].expand(-1,1,AD))
|
||||
o_=l.argsort(1,descending=True); return p.gather(1,o_[:,:,None].expand(-1,-1,AD)) # 按概率排序
|
||||
class FM(nn.Module):
|
||||
def __init__(s,steps): super().__init__(); s.f=mlp(4+AD+1,AD); s.steps=steps
|
||||
def loss(s,o,a):
|
||||
e=torch.randn_like(a); t=torch.rand(len(a),1); z=(1-t)*a+t*e
|
||||
return ((s.f(torch.cat([o,z,t],1))-(e-a))**2).mean()
|
||||
def sample(s,o,k=1):
|
||||
B=len(o); oo=o.repeat_interleave(k,0); z=torch.randn(B*k,AD); dt=1/s.steps
|
||||
for i in range(s.steps):
|
||||
t=torch.full((B*k,1),1-i*dt); z=z-dt*s.f(torch.cat([oo,z,t],1))
|
||||
return z.view(B,k,AD)
|
||||
class MF(nn.Module): # MeanFlow (Geng et al. 2025)
|
||||
def __init__(s): super().__init__(); s.f=mlp(4+AD+2,AD)
|
||||
def u(s,o,z,r,t): return s.f(torch.cat([o,z,r,t],1))
|
||||
def loss(s,o,a):
|
||||
B=len(a); e=torch.randn_like(a)
|
||||
t=torch.rand(B,1); r=torch.rand(B,1); t,r=torch.maximum(t,r),torch.minimum(t,r)
|
||||
eq=(torch.rand(B,1)<0.75); r=torch.where(eq,t,r)
|
||||
z=(1-t)*a+t*e; v=e-a
|
||||
fn=lambda z_,r_,t_: s.u(o,z_,r_,t_)
|
||||
u,dudt=torch.func.jvp(fn,(z,r,t),(v,torch.zeros_like(r),torch.ones_like(t)))
|
||||
tgt=(v-(t-r)*dudt).detach()
|
||||
return ((u-tgt)**2).mean()
|
||||
def sample(s,o,k=1):
|
||||
B=len(o); oo=o.repeat_interleave(k,0); e=torch.randn(B*k,AD)
|
||||
x=e-s.u(oo,e,torch.zeros(B*k,1),torch.ones(B*k,1)); return x.view(B,k,AD)
|
||||
def train(m,iters=6000,bs=512):
|
||||
opt=torch.optim.AdamW(m.parameters(),1e-3,weight_decay=1e-4)
|
||||
sch=torch.optim.lr_scheduler.CosineAnnealingLR(opt,iters)
|
||||
for it in range(iters):
|
||||
idx=torch.randint(0,len(O),(bs,)); l=m.loss(O[idx],A[idx])
|
||||
opt.zero_grad(); l.backward(); opt.step(); sch.step()
|
||||
return m
|
||||
def seg_collide(p,q):
|
||||
for a in np.linspace(0,1,6):
|
||||
if np.linalg.norm(p+(q-p)*a)<R: return True
|
||||
return False
|
||||
@torch.no_grad()
|
||||
def rollout(m,s,g,k=1,safety=False,commit=3,T=500):
|
||||
p=s.copy(); hits=0; ups=[]; brakes=0
|
||||
for t in range(0,T,commit):
|
||||
o=torch.tensor(np.r_[p,g],dtype=torch.float32)[None]
|
||||
C=m.sample(o,k)[0].numpy().reshape(-1,H,2)*STEP
|
||||
chosen=None
|
||||
for c in C:
|
||||
path=p+np.cumsum(c[:commit],0)
|
||||
if not safety: chosen=c; break
|
||||
pts=np.r_[p[None],path]
|
||||
if not any(seg_collide(pts[i],pts[i+1]) for i in range(commit)): chosen=c; break
|
||||
if chosen is None: brakes+=1; continue
|
||||
for d in chosen[:commit]:
|
||||
q=p+d
|
||||
if seg_collide(p,q): hits+=1
|
||||
p=q
|
||||
if abs(p[0])<0.5: ups.append(np.sign(p[1]))
|
||||
if np.linalg.norm(p-g)<0.1: return dict(ok=1,hits=hits,brakes=brakes,steps=t,mode=np.mean(ups) if ups else 0)
|
||||
return dict(ok=0,hits=hits,brakes=brakes,steps=T,mode=np.mean(ups) if ups else 0)
|
||||
def evaluate(m,k,safety,n=150,hard=True):
|
||||
np.random.seed(123); res=[rollout(m,*sample_task(hard),k=k,safety=safety) for _ in range(n)]
|
||||
ok=np.mean([r['ok'] for r in res]); col=np.mean([r['hits']>0 for r in res])
|
||||
br=np.mean([r['brakes'] for r in res]); up=np.mean([r['mode']>0 for r in res if r['ok']]) if ok>0 else float('nan')
|
||||
return dict(reach=round(ok,3),collide_ep=round(col,3),brakes=round(br,1),frac_up=round(up,2))
|
||||
methods={'MSE':lambda:MSE(),'WTA4':lambda:WTA(),'FM10':lambda:FM(10),'FM1':lambda:FM(1),'MeanFlow1':lambda:MF()}
|
||||
which=sys.argv[1:] or list(methods)
|
||||
out={}
|
||||
for name in which:
|
||||
torch.manual_seed(0); t0=time.time(); m=train(methods[name]()); tt=time.time()-t0
|
||||
o=torch.tensor(np.r_[[-2,0],[2,0]],dtype=torch.float32)[None]
|
||||
t1=time.time(); [m.sample(o,8) for _ in range(50)]; lat=(time.time()-t1)/50*1000
|
||||
r={'train_s':round(tt),'infer_ms_k8':round(lat,2)}
|
||||
r['hard_raw_k1']=evaluate(m,1,False); r['hard_safe_k8']=evaluate(m,4 if name=='WTA4' else (1 if name=='MSE' else 8),True)
|
||||
r['easy_raw_k1']=evaluate(m,1,False,hard=False)
|
||||
out[name]=r; print(name,json.dumps(r,ensure_ascii=False),flush=True)
|
||||
json.dump(out,open(f'/workspace/work/toy/res_{"_".join(which)}.json','w'),indent=1)
|
||||
+117
@@ -0,0 +1,117 @@
|
||||
"""预训练（方案 §6）：MeanFlow 一步头 + FK 损失 + 平滑 + 价值头 + 子目标头。
|
||||
python -m ml.train --samples ml/data/samples --out ml/runs/base [--config ml/configs/base.json]
|
||||
吞吐摸底：python -m ml.train --samples ml/data/samples --out ml/runs/bench --steps 2000 --no-eval
|
||||
每 eval_every 步做一次闭环验证（ml/sim/closed_loop.py，部署节奏），按 success 存 best.pt；连续 patience_evals 次不升即停。"""
|
||||
import argparse, json, math, os, time
|
||||
import torch
|
||||
|
||||
from ml.common.kin_torch import Kin
|
||||
from ml.common.features import STEP, D2R
|
||||
from ml.data import Samples, load_tasks
|
||||
from ml.model.policy import build, wta_loss
|
||||
from ml.model.meanflow import meanflow_loss, one_step
|
||||
from ml.sim.closed_loop import evaluate
|
||||
|
||||
|
||||
def fk_points(kin, q, idx):
|
||||
"""选定胶囊端点 + TCP 的世界坐标 [B, 2*len(idx)+1, 3]"""
|
||||
xf = kin.cum_xf(q)
|
||||
a, b = kin.caps_world(q, xf)
|
||||
return torch.cat([a[:, idx], b[:, idx], kin.fk(q, xf)[:, None]], 1)
|
||||
|
||||
|
||||
def q_from_feat(X):
|
||||
s, c = X[:, 0:12:2], X[:, 1:12:2]
|
||||
return torch.atan2(s, c) / D2R
|
||||
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument('--samples', nargs='+', default=['ml/data/samples']); ap.add_argument('--out', default='ml/runs/base')
|
||||
ap.add_argument('--config', default='ml/configs/base.json'); ap.add_argument('--steps', type=int, default=None)
|
||||
ap.add_argument('--resume', default=None); ap.add_argument('--no-eval', action='store_true')
|
||||
ap.add_argument('--device', default='cuda' if torch.cuda.is_available() else 'cpu'); ap.add_argument('--seed', type=int, default=0)
|
||||
a = ap.parse_args()
|
||||
cfg = json.load(open(a.config)); tc = cfg['train']
|
||||
if a.steps: tc['steps'] = a.steps
|
||||
os.makedirs(a.out, exist_ok=True)
|
||||
torch.manual_seed(a.seed)
|
||||
dev = a.device
|
||||
kin = Kin(device=dev)
|
||||
data = Samples(a.samples, device=dev, split='train')
|
||||
val_tasks = [t for t in load_tasks(a.samples) if t['split'] == 'val'][:tc['eval_tasks']]
|
||||
print(f'train samples {data.n}, val tasks {len(val_tasks)}, device {dev}', flush=True)
|
||||
model = build(cfg).to(dev)
|
||||
print(f'params {sum(p.numel() for p in model.parameters()) / 1e6:.2f} M', flush=True)
|
||||
opt = torch.optim.AdamW(model.parameters(), lr=tc['lr'], betas=tuple(tc['betas']), weight_decay=tc['wd'])
|
||||
step0, best = 0, -1.0
|
||||
if a.resume:
|
||||
ck = torch.load(a.resume, map_location=dev); model.load_state_dict(ck['model'])
|
||||
if 'opt' in ck: opt.load_state_dict(ck['opt'])
|
||||
step0, best = ck.get('step', 0), ck.get('best', -1.0)
|
||||
total = tc['steps']
|
||||
lr_at = lambda s: tc['lr'] * (s / tc['warmup'] if s < tc['warmup'] else 0.5 * (1 + math.cos(math.pi * (s - tc['warmup']) / max(1, total - tc['warmup']))))
|
||||
amp = dev.startswith('cuda') and tc.get('amp') == 'bf16'
|
||||
stepv = torch.tensor(STEP, device=dev)
|
||||
fk_idx = torch.linspace(0, kin.cap_a.shape[0] - 1, tc['fk_caps'], device=dev).long()
|
||||
log = open(os.path.join(a.out, 'log.jsonl'), 'a')
|
||||
bad_evals, t0, seen = 0, time.time(), 0
|
||||
def save(name, step, extra=None):
|
||||
d = {'model': model.state_dict(), 'opt': opt.state_dict(), 'cfg': cfg, 'step': step, 'best': best, 'fp': kin.fp}
|
||||
if extra: d.update(extra)
|
||||
torch.save(d, os.path.join(a.out, name))
|
||||
for step in range(step0, total):
|
||||
for gpar in opt.param_groups: gpar['lr'] = lr_at(step)
|
||||
X, Y, V, SG = data.batch(tc['batch'])
|
||||
model.train()
|
||||
with torch.autocast('cuda', dtype=torch.bfloat16, enabled=amp):
|
||||
mem = model.encode(X)
|
||||
if model.head == 'wta':
|
||||
l_mf, xh = wta_loss(model, mem, Y); mf_raw = l_mf.detach(); xh = xh.float()
|
||||
else:
|
||||
l_mf, mf_raw = meanflow_loss(model, mem, Y, p_neq=tc['p_neq'])
|
||||
e = torch.randn_like(Y)
|
||||
xh = one_step(model, mem, e).float()
|
||||
v, s = model.heads(mem)
|
||||
l_val = (v.float() - V).abs().mean(); l_sub = (s.float() - SG).abs().mean()
|
||||
d2 = xh[:, 2:] - 2 * xh[:, 1:-1] + xh[:, :-2]
|
||||
l_sm = d2.pow(2).mean()
|
||||
l_fk = torch.zeros((), device=dev)
|
||||
if tc['w_fk'] > 0:
|
||||
q = q_from_feat(X)
|
||||
ks = torch.tensor([3, 7, 11, 15], device=dev)
|
||||
qh = q[:, None] + torch.cumsum(xh.clamp(-1, 1) * stepv, 1)[:, ks]
|
||||
qt = q[:, None] + torch.cumsum(Y * stepv, 1)[:, ks]
|
||||
B4 = qh.shape[0] * 4
|
||||
ph = fk_points(kin, qh.reshape(B4, 6), fk_idx); pt = fk_points(kin, qt.reshape(B4, 6), fk_idx)
|
||||
l_fk = ((ph - pt).norm(dim=-1) / 10).mean()
|
||||
loss = tc['w_mf'] * l_mf + tc['w_fk'] * l_fk + tc['w_smooth'] * l_sm + tc['w_value'] * l_val + tc['w_sub'] * l_sub
|
||||
opt.zero_grad(set_to_none=True)
|
||||
loss.backward()
|
||||
gn = torch.nn.utils.clip_grad_norm_(model.parameters(), tc['grad_clip'])
|
||||
if not torch.isfinite(loss):
|
||||
print('非有限损失 → 训练发散（M5 门禁：触发 WTA 回退条件）', flush=True); save('diverged.pt', step); break
|
||||
opt.step(); seen += X.shape[0]
|
||||
if step % tc['log_every'] == 0:
|
||||
el = time.time() - t0
|
||||
rec = {'step': step, 'loss': loss.item(), 'mf': mf_raw.item(), 'fk_cm': l_fk.item(), 'smooth': l_sm.item(),
|
||||
'value': l_val.item(), 'sub': l_sub.item(), 'gnorm': gn.item(), 'lr': lr_at(step), 'samples_per_s': seen / max(el, 1e-6)}
|
||||
print(json.dumps(rec), flush=True); log.write(json.dumps(rec) + '\n'); log.flush()
|
||||
if not a.no_eval and (step + 1) % tc['eval_every'] == 0:
|
||||
model.eval()
|
||||
res, _ = evaluate(model, kin, val_tasks, dev)
|
||||
res['step'] = step + 1
|
||||
print('EVAL', json.dumps(res), flush=True); log.write(json.dumps({'eval': res}) + '\n'); log.flush()
|
||||
save('last.pt', step + 1)
|
||||
if res['success'] > best + 1e-4:
|
||||
best = res['success']; bad_evals = 0; save('best.pt', step + 1, {'eval': res})
|
||||
else:
|
||||
bad_evals += 1
|
||||
if bad_evals >= tc['patience_evals']: print('闭环到达率不再上升，停止', flush=True); break
|
||||
save('last.pt', step + 1)
|
||||
if a.no_eval: save('best.pt', step + 1)
|
||||
print(f'done, best success {best:.4f}, {(time.time() - t0) / 3600:.2f} h', flush=True)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,66 @@
|
||||
/* 贴立柱目标基准（2026-10-17，用户报告"立柱另一侧 → 贴立柱目标误判在立柱区内"）
|
||||
* 目标位形 b：本身无干涉，但 J1/J2/J3 任一轴 ±4° 就撞立柱（= 贴着立柱）。
|
||||
* 起点 a：无干涉，且 a→b 关节直线被挡（立柱另一侧）。
|
||||
* 模式：pos = 松手位置任务；pose = 松手位置+姿态任务（姿态取 b 的真实姿态，b 本身就是安全精确解）；
|
||||
* rt = 拖拽实时管线，目标不动连续重规划直到到达或 3 s。
|
||||
* 判据：到达（TCP 误差 < 1 mm，pose 另要姿态 < 0.5°）且整条路径 1° 细验无干涉。
|
||||
* node tests/near_column_bench.cjs <solver.js> [N=30] [modes=pos,pose,rt] */
|
||||
const setup = require('./route_bench_setup.cjs');
|
||||
const { S, ArmIK } = setup(process.argv[2]);
|
||||
const N = +process.argv[3] || 30, MODES = (process.argv[4] || 'pos,pose,rt').split(',');
|
||||
const L = S.LIM, O = S.ORDER, w = S.wrap180;
|
||||
let s = 20261017; const rnd = () => { s = (s * 1664525 + 1013904223) >>> 0; return s / 4294967296; };
|
||||
const rq = () => O.map(id => { const t = L[id]; return t.lo + rnd() * (t.hi - t.lo); });
|
||||
const d = (a, b) => a.map((v, i) => L[O[i]].cont ? w(b[i] - v) : b[i] - v);
|
||||
const segOK = (a, b) => { const dd = d(a, b); const m = Math.max(...dd.map(Math.abs)); const n = Math.max(1, Math.ceil(m)); for (let i = 1; i <= n; i++) if (!S.collideFree(a.map((v, k) => v + dd[k] * i / n))) return false; return true; };
|
||||
const hitsColumn = q => { const det = S.collideDetail(q) || []; return det.some(p => /立柱/.test(p.cnA + p.cnB)); };
|
||||
const nearColumn = q => { for (const k of [0, 1, 2]) for (const sg of [-4, 4]) { const p = q.slice(); p[k] += sg; if (hitsColumn(p)) return true; } return false; };
|
||||
const cases = []; let tries = 0;
|
||||
while (cases.length < N && tries < 400000) {
|
||||
tries++;
|
||||
const b = rq(); if (!S.collideFree(b) || !nearColumn(b)) continue;
|
||||
let a = null;
|
||||
for (let t = 0; t < 200; t++) { const c = rq(); if (S.collideFree(c) && !segOK(c, b)) { a = c; break; } }
|
||||
if (a) cases.push({ a, b });
|
||||
}
|
||||
if (process.env.DUMP) require('fs').writeFileSync(process.env.DUMP, JSON.stringify(cases));
|
||||
const out = {};
|
||||
for (const mode of MODES) {
|
||||
let ok = 0, reached = 0, valid = 0, unsafe = 0, relaxed = 0, ms = 0, worst = 0; const fails = [];
|
||||
cases.forEach((c, ci) => {
|
||||
const T = S.fk(c.b, 0), R = S.orient(c.b);
|
||||
const opt = { tool: 0, step: 0.5, ref: c.a, from: c.a.slice() };
|
||||
if (mode === 'pose') { opt.pose = { R }; opt.priority = 'pos'; }
|
||||
if (S.rtReset) S.rtReset();
|
||||
const t0 = performance.now();
|
||||
let pts = [c.a.slice()], r = null;
|
||||
if (mode === 'rt') {
|
||||
let q = c.a.slice();
|
||||
for (let it = 0; it < 40; it++) { // 每次重规划后沿路径走到头（目标不动；最多 40 次重规划）
|
||||
r = S.solveRoute(T, Object.assign({}, opt, { from: q.slice(), ref: q, warm: q, noVia: true, quick: true }));
|
||||
if (!r || !r.q) break;
|
||||
const seg = (r.via || []).concat([r.q]);
|
||||
for (const p of seg) pts.push(p);
|
||||
const nq = r.q.slice();
|
||||
const tcp = S.fk(nq, 0);
|
||||
if (Math.hypot(tcp[0] - T[0], tcp[1] - T[1], tcp[2] - T[2]) < 1) { q = nq; break; }
|
||||
q = nq;
|
||||
}
|
||||
} else {
|
||||
r = S.solveDyn(T, opt);
|
||||
pts = pts.concat(r.via || [], [r.q]);
|
||||
}
|
||||
const t = performance.now() - t0; ms += t; worst = Math.max(worst, t);
|
||||
for (let i = 1; i < pts.length; i++) pts[i] = pts[i - 1].map((v, k) => L[O[k]].cont ? v + w(pts[i][k] - v) : pts[i][k]);
|
||||
let pv = true; for (let i = 1; i < pts.length; i++) if (!segOK(pts[i - 1], pts[i])) { pv = false; break; }
|
||||
const qf = pts[pts.length - 1], tcp = S.fk(qf, 0);
|
||||
const err = Math.hypot(tcp[0] - T[0], tcp[1] - T[1], tcp[2] - T[2]);
|
||||
const oerr = mode === 'pose' ? ArmIK.rotErrDeg(S.orient(qf), R) : 0;
|
||||
const rc = err < 1 && oerr < 0.5;
|
||||
if (rc) reached++; if (pv) valid++; if (rc && pv) ok++;
|
||||
if (r && r.unsafe) unsafe++; if (r && r.poseRelaxed) relaxed++;
|
||||
if (!(rc && pv)) fails.push({ ci, err: +err.toFixed(1), oerr: +oerr.toFixed(1), pathOK: pv, unsafe: !!(r && r.unsafe), blocked: !!(r && r.blocked) });
|
||||
});
|
||||
out[mode] = { n: cases.length, ok, reached, validPath: valid, unsafe, poseRelaxed: relaxed, msMean: Math.round(ms / cases.length), msMax: Math.round(worst), fails: fails.slice(0, 12) };
|
||||
}
|
||||
console.log(JSON.stringify(out, null, 1));
|
||||
@@ -0,0 +1,91 @@
|
||||
/* 部署形态评测（方案 §9）：策略 + 安全层（webapp/js/ik-policy.js 的 Runner，与浏览器同一份代码）
|
||||
* node tests/policy_bench.cjs [model=webapp/models/traj_policy.onnx] [sets=nc,route] [--nc1000] [--out file.json]
|
||||
* 集合：nc = near_column_bench 同款 30 个贴立柱任务（pos 与 pose 各跑一遍）
|
||||
* route = route_bench 同款 40 个直线被挡任务（pos）
|
||||
* nc1000= 一次性扩展贴立柱集 1000 例（新种子，验收用；加 --nc1000 开启）
|
||||
* 判据：到达（TCP < 1 mm，pose 另需姿态 < 0.5°）且执行轨迹无干涉帧（每步端点+中点 collideFree，外加距离认证复核）；
|
||||
* 超时 10 s。另报告推理 / 安全层耗时 P50/P99（Node CPU，仅作参考；浏览器延迟用查看器 ?policy=1 实测）。
|
||||
* 依赖：cd ml && npm install（onnxruntime-node） */
|
||||
'use strict';
|
||||
const fs = require('fs'), path = require('path');
|
||||
const argv = process.argv.slice(2);
|
||||
const flag = k => argv.includes(k);
|
||||
const opt = k => { const i = argv.indexOf(k); return i >= 0 ? argv[i + 1] : null; };
|
||||
const pos = argv.filter((a, i) => !a.startsWith('--') && !(i > 0 && argv[i - 1].startsWith('--out')));
|
||||
const MODEL = pos[0] || path.join(__dirname, '../webapp/models/traj_policy.onnx');
|
||||
const SETS = (pos[1] || 'nc,route').split(',').concat(flag('--nc1000') ? ['nc1000'] : []);
|
||||
let ort; try { ort = require(path.join(__dirname, '../ml/node_modules/onnxruntime-node')); } catch (e) { ort = require('onnxruntime-node'); }
|
||||
const { S, ArmIK } = require('./route_bench_setup.cjs')();
|
||||
const POL = require('../webapp/js/ik-policy.js');
|
||||
const L = S.LIM, O = S.ORDER, w = S.wrap180;
|
||||
const meta = JSON.parse(fs.readFileSync(MODEL.replace('.onnx', '.meta.json'), 'utf8'));
|
||||
if (meta.fp !== S.modelFingerprint()) { console.error('模型碰撞指纹', meta.fp, '≠ 当前', S.modelFingerprint(), '：须重训'); process.exit(2); }
|
||||
if (meta.feat_ver !== POL.FEAT_VER) { console.error('特征版本不符'); process.exit(2); }
|
||||
|
||||
function lcg(seed) { let s = seed >>> 0; return () => { s = (s * 1664525 + 1013904223) >>> 0; return s / 4294967296; }; }
|
||||
function ncCases(N, seed) {
|
||||
const rnd = lcg(seed);
|
||||
const rq = () => O.map(id => { const t = L[id]; return t.lo + rnd() * (t.hi - t.lo); });
|
||||
const d = (a, b) => a.map((v, i) => L[O[i]].cont ? w(b[i] - v) : b[i] - v);
|
||||
const segOK = (a, b) => { const dd = d(a, b); const m = Math.max(...dd.map(Math.abs)); const n = Math.max(1, Math.ceil(m)); for (let i = 1; i <= n; i++) if (!S.collideFree(a.map((v, k) => v + dd[k] * i / n))) return false; return true; };
|
||||
const hitsColumn = q => (S.collideDetail(q) || []).some(p => /立柱/.test(p.cnA + p.cnB));
|
||||
const nearColumn = q => { for (const k of [0, 1, 2]) for (const sg of [-4, 4]) { const p = q.slice(); p[k] += sg; if (hitsColumn(p)) return true; } return false; };
|
||||
const cases = []; let tries = 0;
|
||||
while (cases.length < N && tries < 4000000) {
|
||||
tries++;
|
||||
const b = rq(); if (!S.collideFree(b) || !nearColumn(b)) continue;
|
||||
let a = null;
|
||||
for (let t = 0; t < 200; t++) { const c = rq(); if (S.collideFree(c) && !segOK(c, b)) { a = c; break; } }
|
||||
if (a) cases.push({ q0: a, T: S.fk(b, 0), R: S.orient(b) });
|
||||
}
|
||||
return cases;
|
||||
}
|
||||
function routeCases(N) {
|
||||
const rnd = lcg(12345);
|
||||
const rq = () => O.map(id => { const t = L[id]; return t.lo + rnd() * (t.hi - t.lo); });
|
||||
const d = (a, b) => a.map((v, i) => L[O[i]].cont ? w(b[i] - v) : b[i] - v);
|
||||
const segOK = (a, b) => { const dd = d(a, b); const m = Math.max(...dd.map(Math.abs)); const n = Math.max(1, Math.ceil(m / 1)); for (let i = 1; i <= n; i++) if (!S.collideFree(a.map((v, k) => v + dd[k] * i / n))) return false; return true; };
|
||||
const cases = []; let tries = 0;
|
||||
while (cases.length < N && tries < 20000) { tries++; const a = rq(), b = rq(); if (!S.collideFree(a) || !S.collideFree(b)) continue; if (segOK(a, b)) continue; cases.push({ q0: a, T: S.fk(b, 0), R: null }); }
|
||||
return cases;
|
||||
}
|
||||
const pct = (a, p) => { if (!a.length) return null; const s = a.slice().sort((x, y) => x - y); return +s[Math.min(s.length - 1, Math.floor(s.length * p))].toFixed(2); };
|
||||
|
||||
(async () => {
|
||||
const sess = await ort.InferenceSession.create(MODEL, { executionProviders: ['cpu'], intraOpNumThreads: 1 });
|
||||
const infer = POL.ortInfer(ort, sess);
|
||||
const cert = POL.makeCert(S, {});
|
||||
const out = { model: MODEL, meta: { fp: meta.fp, eval: meta.eval } };
|
||||
const runSet = async (name, cases, mode) => {
|
||||
let ok = 0, reached = 0, colTasks = 0, colFrames = 0, uncert = 0, brakes = 0, locks = 0; const inf = [], saf = [], times = [], fails = [];
|
||||
for (let ci = 0; ci < cases.length; ci++) {
|
||||
const c = cases[ci];
|
||||
const R = mode === 'pose' ? c.R : null;
|
||||
const r = new POL.Runner(S, infer, { seed: 1000 + ci });
|
||||
r.reset(c.q0); r.setGoal(c.T, R);
|
||||
let prev = c.q0.slice(), cf = 0;
|
||||
for (let k = 0; k < 500 && !r.done; k++) {
|
||||
if (r.needPlan()) await r.plan();
|
||||
const q = r.tick();
|
||||
const mid = prev.map((v, j) => 0.5 * (v + q[j]));
|
||||
if (!S.collideFree(q) || !S.collideFree(mid)) cf++;
|
||||
else if (!cert.seg(prev, q)) uncert++;
|
||||
prev = q.slice();
|
||||
}
|
||||
const { e, oe } = r.err(r.q);
|
||||
const rc = r.done && e < 1 && oe < 0.5;
|
||||
if (rc) reached++; if (cf) { colTasks++; colFrames += cf; } if (rc && !cf) { ok++; times.push(r.t); } else fails.push({ ci, err: +e.toFixed(1), oerr: +oe.toFixed(1), colFrames: cf, brakes: r.stats.brakes });
|
||||
inf.push(...r.stats.inferMs); saf.push(...r.stats.safetyMs); brakes += r.stats.brakes; locks += r.stats.locks;
|
||||
if ((ci + 1) % 50 === 0) process.stderr.write(`${name} ${ci + 1}/${cases.length}\n`);
|
||||
}
|
||||
out[name] = { n: cases.length, ok, reached, collisionTasks: colTasks, collisionFrames: colFrames, uncertifiedSegs: uncert, brakes, locks,
|
||||
timeP50: pct(times, 0.5), inferMsP50: pct(inf, 0.5), inferMsP99: pct(inf, 0.99), safetyMsP50: pct(saf, 0.5), safetyMsP99: pct(saf, 0.99), fails: fails.slice(0, 15) };
|
||||
console.error(name, JSON.stringify({ ok, n: cases.length, collisionFrames: colFrames }));
|
||||
};
|
||||
if (SETS.includes('nc')) { const c = ncCases(+opt('--n') || 30, 20261017); await runSet('near_column_pos', c, 'pos'); await runSet('near_column_pose', c, 'pose'); }
|
||||
if (SETS.includes('route')) await runSet('route40', routeCases(+opt('--n') || 40), 'pos');
|
||||
if (SETS.includes('nc1000')) await runSet('near_column_1000', ncCases(1000, 1), 'pos');
|
||||
const s = JSON.stringify(out, null, 1);
|
||||
console.log(s);
|
||||
if (opt('--out')) fs.writeFileSync(opt('--out'), s);
|
||||
})();
|
||||
@@ -0,0 +1,43 @@
|
||||
const setup=require('./route_bench_setup.cjs');
|
||||
const solverPath=process.argv[2];const N=+process.argv[3]||40;const live=process.argv[4]==='live';
|
||||
const {S}=setup(solverPath);
|
||||
const L=S.LIM,O=S.ORDER,w=S.wrap180;
|
||||
let s=12345;const rnd=()=>{s=(s*1664525+1013904223)>>>0;return s/4294967296};
|
||||
const rq=()=>O.map(id=>{const t=L[id];return t.lo+rnd()*(t.hi-t.lo)});
|
||||
const d=(a,b)=>a.map((v,i)=>L[O[i]].cont?w(b[i]-v):b[i]-v);
|
||||
function segOK(a,b){const dd=d(a,b);const m=Math.max(...dd.map(Math.abs));const n=Math.max(1,Math.ceil(m/1));for(let i=1;i<=n;i++){if(!S.collideFree(a.map((v,k)=>v+dd[k]*i/n)))return false}return true}
|
||||
// build cases: free start, free goal config, straight blocked
|
||||
const cases=[];let tries=0;
|
||||
while(cases.length<N&&tries<20000){tries++;const a=rq(),b=rq();if(!S.collideFree(a)||!S.collideFree(b))continue;
|
||||
if(segOK(a,b))continue; // only obstacle cases
|
||||
cases.push({from:a,T:S.fk(b,0)});}
|
||||
const W=[1,1,1,1,1,1];
|
||||
let res={n:cases.length,reached:0,valid:0,blocked:0,len:0,lenDirect:0,ms:0,rev:0,segs:0,timeExec:0,fail:[]};
|
||||
const VMAX=[60,60,90,90,120,180]; // deg/s assumed
|
||||
function execTime(pts){let t=0;for(let i=1;i<pts.length;i++){const dd=d(pts[i-1],pts[i]);t+=Math.max(...dd.map((x,k)=>Math.abs(x)/VMAX[k]))+ (pts.length>2&&i<pts.length-1?0.25:0)}return t}
|
||||
for(const c of cases){
|
||||
const t0=performance.now();
|
||||
const opt={tool:0,step:0.5,ref:c.from,from:c.from.slice()};
|
||||
if(live){opt.noVia=true;opt.quick=true;opt.warm=c.from}
|
||||
const r=live?S.solveRoute(c.T,opt):S.solveDyn(c.T,opt);
|
||||
const ms=performance.now()-t0;res.ms+=ms;
|
||||
const pts=[c.from].concat(r.via||[],[r.q]);
|
||||
// unwrap
|
||||
for(let i=1;i<pts.length;i++)pts[i]=pts[i-1].map((v,k)=>L[O[k]].cont?v+w(pts[i][k]-v):pts[i][k]);
|
||||
let ok=true;for(let i=1;i<pts.length;i++)if(!segOK(pts[i-1],pts[i])){ok=false;break}
|
||||
const tcp=S.fk(r.q,0);const err=Math.hypot(tcp[0]-c.T[0],tcp[1]-c.T[1],tcp[2]-c.T[2]);
|
||||
const reached=err<1&&!r.blocked;
|
||||
if(reached)res.reached++; if(ok)res.valid++; if(r.blocked)res.blocked++;
|
||||
let len=0;for(let i=1;i<pts.length;i++)len+=d(pts[i-1],pts[i]).reduce((a,x)=>a+Math.abs(x),0);
|
||||
const direct=d(c.from,r.q).reduce((a,x)=>a+Math.abs(x),0);
|
||||
// reversals
|
||||
let rev=0;for(let k=0;k<6;k++){let sg=0;for(let i=1;i<pts.length;i++){const x=d(pts[i-1],pts[i])[k];if(Math.abs(x)<2)continue;const s2=Math.sign(x);if(sg&&s2!==sg)rev++;sg=s2}}
|
||||
res.per=res.per||[];res.per.push({ms:Math.round(ms),reached,ok,len:Math.round(len),rev,segs:pts.length-1,exec:+execTime(pts).toFixed(2),detour:r.detour||null});
|
||||
if(reached&&ok){res.len+=len;res.lenDirect+=direct;res.rev+=rev;res.segs+=pts.length-1;res.timeExec+=execTime(pts)}
|
||||
else res.fail.push({reached,ok,err:+err.toFixed(1),detour:r.detour||null,blocked:!!r.blocked});
|
||||
}
|
||||
const g=res.reached;const both=cases.length-res.fail.length;
|
||||
require('fs').writeFileSync(process.argv[5]||require('path').join(require('os').tmpdir(),'route_bench_per.json'),JSON.stringify(res.per));console.log(JSON.stringify({n:res.n,reached:res.reached,validPath:res.valid,goodBoth:both,blocked:res.blocked,
|
||||
avgPlanMs:+(res.ms/res.n).toFixed(0),avgJointTravel:+(res.len/both).toFixed(0),avgDirect:+(res.lenDirect/both).toFixed(0),
|
||||
avgReversals:+(res.rev/both).toFixed(2),avgSegs:+(res.segs/both).toFixed(2),avgExecS:+(res.timeExec/both).toFixed(2),
|
||||
failTypes:res.fail.slice(0,12)}));
|
||||
@@ -0,0 +1,12 @@
|
||||
/* 路径规划基准的求解器装配（Node）：node tests/route_bench.cjs [solver.js] [N] */
|
||||
const fs=require('fs'),path=require('path');
|
||||
const ROOT=path.resolve(__dirname,'..');
|
||||
module.exports=function(solverPath,collidePath){
|
||||
const ArmIK=require(solverPath||path.join(ROOT,'webapp/js/ik-solver.js'));
|
||||
const JMap=/* 关节表取自 arm/kinematics.py JOINTS（无需 mesh_arm.json） */ {"J1": {"rear": [-51.0, 128.0, 610.0], "d": [1, 0, 0]}, "J2": {"rear": [19.5, 100.0, 490.0], "d": [0, 1, 0]}, "J3": {"rear": [19.5, 128.0, 445.0], "d": [0, 0, -1]}, "J4": {"rear": [19.5, 100.0, 332.0], "d": [0, 1, 0]}, "J5": {"rear": [19.5, 100.0, 222.0], "d": [0, 1, 0]}, "J6": {"rear": [19.5, 128.0, 176.0], "d": [0, 0, -1]}};
|
||||
const travel=JSON.parse(fs.readFileSync(ROOT+'/docs/travel.json','utf8'));
|
||||
const COL=JSON.parse(fs.readFileSync(collidePath||ROOT+'/docs/collide.json','utf8'));
|
||||
const TR={};for(const t of travel){const a=COL.adj[t.axis];TR[t.axis]={lo:a?a[0]:t.lo,hi:a?a[1]:t.hi,continuous:!!t.continuous};}
|
||||
const S=ArmIK.make({joints:JMap,travel:TR,tcp:[19.5,128,93],collide:COL});
|
||||
return {S,ArmIK};
|
||||
};
|
||||
@@ -0,0 +1,49 @@
|
||||
/* 实时拖拽仿真基准：目标沿直线从 A 的 TCP 拖到 B 的 TCP（中间被立柱挡住），每帧 16.7 ms。
|
||||
* 每帧：手臂沿当前路径按各轴速度上限前进；求解器空闲就用"此刻位形"作起点重规划（quick 模式）。
|
||||
* 统计：重规划耗时分布、穿模帧（1° 细验）、拖完 6 s 后的到达误差。
|
||||
* node tests/rt_drag_bench.cjs <solver.js> [场景数=12] [帧数=120] */
|
||||
const setup = require('./route_bench_setup.cjs');
|
||||
const { S } = setup(process.argv[2]);
|
||||
const NC = +process.argv[3] || 12, NF = +process.argv[4] || 120, DT = 1 / 60, TAIL = 360;
|
||||
const L = S.LIM, O = S.ORDER, w = S.wrap180;
|
||||
let s = 777; const rnd = () => { s = (s * 1664525 + 1013904223) >>> 0; return s / 4294967296; };
|
||||
const rq = () => O.map(id => { const t = L[id]; return t.lo + rnd() * (t.hi - t.lo); });
|
||||
const d = (a, b) => a.map((v, i) => L[O[i]].cont ? w(b[i] - v) : b[i] - v);
|
||||
const segOK = (a, b) => { const dd = d(a, b); const m = Math.max(...dd.map(Math.abs)); const n = Math.max(1, Math.ceil(m / 1)); for (let i = 1; i <= n; i++) if (!S.collideFree(a.map((v, k) => v + dd[k] * i / n))) return false; return true; };
|
||||
const VMAX = [60, 60, 90, 90, 120, 180];
|
||||
const cases = []; let tries = 0;
|
||||
while (cases.length < NC && tries < 50000) { tries++; const a = rq(), b = rq(); if (!S.collideFree(a) || !S.collideFree(b) || segOK(a, b)) continue; cases.push([a, b]); }
|
||||
const lat = []; let badFrames = 0, frames = 0, reached = 0, stuck = 0;
|
||||
for (const [a, b] of cases) {
|
||||
const TA = S.fk(a, 0), TB = S.fk(b, 0);
|
||||
let q = a.slice(), path = [], busyUntil = 0, tNow = 0;
|
||||
for (let f = 0; f < NF + TAIL; f++) {
|
||||
tNow = f * DT * 1000;
|
||||
const u = Math.min(1, f / NF), T = TA.map((v, i) => v + (TB[i] - v) * u);
|
||||
if (tNow >= busyUntil) { // 求解器空闲：派发一次重规划（结果在耗时后才生效）
|
||||
const t0 = performance.now();
|
||||
const r = S.solveRoute(T, { tool: 0, step: 0.5, ref: q, warm: q, from: q.slice(), noVia: true, quick: true });
|
||||
const ms = performance.now() - t0; lat.push(ms);
|
||||
busyUntil = tNow + ms;
|
||||
if (r && r.q) { path = (r.via || []).concat([r.q]); }
|
||||
}
|
||||
/* 按轴速上限沿折线前进 */
|
||||
let budget = 1;
|
||||
const q0 = q.slice();
|
||||
while (path.length && budget > 1e-9) {
|
||||
const tgt = q.map((v, i) => L[O[i]].cont ? v + w(path[0][i] - v) : path[0][i]);
|
||||
const dd = tgt.map((v, i) => v - q[i]);
|
||||
const need = Math.max(...dd.map((x, i) => Math.abs(x) / (VMAX[i] * DT)));
|
||||
if (need <= budget) { q = tgt; path.shift(); budget -= need; }
|
||||
else { q = q.map((v, i) => v + dd[i] * budget / need); budget = 0; }
|
||||
}
|
||||
frames++;
|
||||
if (!segOK(q0, q)) badFrames++;
|
||||
}
|
||||
const tcp = S.fk(q, 0), err = Math.hypot(tcp[0] - TB[0], tcp[1] - TB[1], tcp[2] - TB[2]);
|
||||
if (err < 1) reached++; else stuck++;
|
||||
}
|
||||
lat.sort((x, y) => x - y);
|
||||
const pct = p => +lat[Math.min(lat.length - 1, Math.floor(lat.length * p))].toFixed(1);
|
||||
console.log(JSON.stringify({ cases: cases.length, replans: lat.length, latP50: pct(0.5), latP90: pct(0.9), latP99: pct(0.99), latMax: pct(1),
|
||||
latMean: +(lat.reduce((x, y) => x + y, 0) / lat.length).toFixed(1), collidingFrames: badFrames, frames, reachedAfter6s: reached }));
|
||||
@@ -360,5 +360,7 @@
|
||||
<script src="js/urlparams.js"></script>
|
||||
<script src="js/ik-solver.js"></script>
|
||||
<script src="js/ik-glue.js"></script>
|
||||
<script src="js/ik-policy.js"></script>
|
||||
<script src="js/ik-policy-glue.js"></script>
|
||||
</body>
|
||||
</html>
|
||||
|
||||
+57
-8
@@ -330,9 +330,30 @@ function setJoints(q, doAnim, via, ms) {
|
||||
refreshTarget();
|
||||
if (COL) refreshJointLabelsLazy(ORDER.map(id => ang[id]));
|
||||
}
|
||||
/* 实时碰撞守护（2026-10-15）：动画每帧对当前显示位形做一次碰撞判定（~1.2 ms/帧），
|
||||
命中立即停住并亮红标。规划给的折线本已按 1.5° 细验，这里是最后一道独立防线，
|
||||
也让"此刻是否碰撞"实时可见。统计供测试/控制台：window.__RT_GUARD()。 */
|
||||
const RTG = { frames: 0, hits: 0, ms: 0, last: null };
|
||||
window.__RT_GUARD = () => Object.assign({}, RTG, { avgMs: RTG.frames ? RTG.ms / RTG.frames : 0 });
|
||||
function rtGuard(qNow) {
|
||||
if (!S || !S.collideFree) return true;
|
||||
const t = performance.now();
|
||||
const ok = S.collideFree(qNow);
|
||||
RTG.ms += performance.now() - t; RTG.frames++;
|
||||
if (!ok) {
|
||||
RTG.hits++; RTG.last = qNow.slice();
|
||||
const b = document.getElementById('ikBadge');
|
||||
if (b) { b.textContent = '实时碰撞守护：检测到干涉，已停住'; b.className = 'badge badb'; }
|
||||
}
|
||||
return ok;
|
||||
}
|
||||
function animTo(q, ms, via) {
|
||||
/* 实时跟随：上一段动画还在走时被新规划打断，新动画从"正在运动"接续——
|
||||
不再从零速缓入（否则每次重规划都顿一下），只在终点缓出。 */
|
||||
const moving = !!anim;
|
||||
if (anim) { anim = null; }
|
||||
const from = ORDER.map(id => ang[id]);
|
||||
const guardOn = !S.collideFree || S.collideFree(from); // 起点已在干涉里（脱困动作）不设守护
|
||||
/* 连续轴（J1 肩抬举 / J3 大臂自转 / J6 腕自转）走「短弧」。
|
||||
三者物理上无机械限位，θ 与 θ±360 是同一物理位形（applyJoint 走 cos/sin）；
|
||||
若按数值直接线性插值，+151.9° → −167.0° 会反向扫 318.9° 绕一整圈（实测）。
|
||||
@@ -340,7 +361,7 @@ function animTo(q, ms, via) {
|
||||
end 保存求解器给的代表角，末帧落回它——与插值终点恰好相差 360° 的整数倍，
|
||||
同一物理角，视觉零跳变，同时保证滑块/关节表数值与求解结果一致。
|
||||
via：绕行中间位形（solveRoute 判直线被挡时给的两段式路径），逐段同样按短弧折算，
|
||||
每段各 ms 毫秒——渲染走的段与碰撞校验走的段是同一批，不存在"校验过了但动画穿模"。 */
|
||||
渲染走的段与碰撞校验走的段是同一批，不存在"校验过了但动画穿模"。 */
|
||||
const segs = [];
|
||||
let p = from;
|
||||
for (const raw of (via || []).concat([q])) {
|
||||
@@ -352,17 +373,37 @@ function animTo(q, ms, via) {
|
||||
segs.push(t); p = t;
|
||||
}
|
||||
const end = q.slice();
|
||||
const total = ms * segs.length;
|
||||
/* 时间参数化（2026-10-15）：旧版每段固定 ms 且每段各自 smoothstep —— 每个中间位形都
|
||||
减速到 0 再起步（顿挫），且 5° 的小段和 150° 的大段用同样时间（小段慢吞吞、大段猛甩）。
|
||||
现在：各段时长 ∝ 该段"最慢轴所需时间"（按轴速比 VMAX 折算，近端轴更慢），
|
||||
整条折线只做一次全局 smoothstep：只在起点/终点加减速，途经中间位形不停顿。
|
||||
走的仍是校验过的那条折线（中间位形经捷径平滑后通常只剩 1~2 个，折角很少）。 */
|
||||
const VREL = [0.6, 0.6, 0.9, 0.9, 1.2, 1.8];
|
||||
const pts = [from].concat(segs), dur = [];
|
||||
for (let i = 1; i < pts.length; i++) {
|
||||
let t = 0;
|
||||
for (let j = 0; j < 6; j++) t = Math.max(t, Math.abs(pts[i][j] - pts[i - 1][j]) / VREL[j]);
|
||||
dur.push(Math.max(t, 1e-3));
|
||||
}
|
||||
const sum = dur.reduce((a, b) => a + b, 0);
|
||||
/* 总时长：ms 视为"标准 90° 移动"的时长，按路径实际长度伸缩，夹在 [0.6ms, 3.5ms] */
|
||||
const total = Math.max(ms * 0.6, Math.min(ms * 3.5, ms * sum / 150));
|
||||
const cum = [0]; for (const d of dur) cum.push(cum[cum.length - 1] + d / sum);
|
||||
anim = { from: from, to: segs[segs.length - 1], t0: performance.now(), ms: total };
|
||||
const tick = () => {
|
||||
if (!anim) return;
|
||||
const el = performance.now() - anim.t0;
|
||||
const si = Math.min(segs.length - 1, Math.floor(el / ms));
|
||||
const k = Math.min(1, (el - si * ms) / ms);
|
||||
const e = k * k * (3 - 2 * k);
|
||||
const a0 = si === 0 ? from : segs[si - 1], a1 = segs[si];
|
||||
const u = Math.min(1, el / total);
|
||||
const k = u;
|
||||
const e = moving ? 1 - (1 - u) * (1 - u) // 接续运动：只缓出
|
||||
: u * u * (3 - 2 * u); // 静止起步：全局一次缓入缓出
|
||||
let si = 0; while (si < dur.length - 1 && e > cum[si + 1]) si++;
|
||||
const f = Math.min(1, Math.max(0, (e - cum[si]) / (cum[si + 1] - cum[si])));
|
||||
const a0 = pts[si], a1 = pts[si + 1];
|
||||
const qNow = a0.map((v, i) => v + (a1[i] - v) * f);
|
||||
if (guardOn && !rtGuard(qNow)) { anim = null; return; } // 停在上一帧（已判无干涉）的位形
|
||||
for (let i = 0; i < 6; i++) {
|
||||
const id = ORDER[i], a = a0[i] + (a1[i] - a0[i]) * e;
|
||||
const id = ORDER[i], a = qNow[i];
|
||||
ang[id] = a;
|
||||
const sl = $('s' + id), v = $('v' + id);
|
||||
if (sl) sl.value = a;
|
||||
@@ -492,10 +533,12 @@ function renderResult(res, src) {
|
||||
: res.blocked
|
||||
? '路径被立柱挡住 · 已停在边界（目标差 ' + dsc.toFixed(1) + ' mm）'
|
||||
: res.detour
|
||||
? (res.detour === 'retract_pass' ? '避障到达 · 模拟人类回退绕过立柱'
|
||||
? (res.detour === 'planned' ? '避障到达 · 规划绕行（' + ((res.via || []).length) + ' 个中间位形）'
|
||||
: res.detour === 'retract_pass' ? '避障到达 · 模拟人类回退绕过立柱'
|
||||
: (res.detour === 'gentle' ? '避障到达 · 微调协同避开立柱'
|
||||
: (res.detour === 'coordinated' ? '避障到达 · 平滑协同跨越立柱'
|
||||
: '绕行立柱后到达')))
|
||||
: (res.poseRelaxed && posOk) ? '位置到达 · 精确姿态会撞立柱，已取最接近的无碰撞姿态（偏差 ' + dori.toFixed(1) + '\u00B0）'
|
||||
: (posOk && oriOk) ? '已到达'
|
||||
: oriPrio ? '姿态到达·位置最近 ' + dsc.toFixed(1) + ' mm'
|
||||
: mixMid ? '混合折中 · 位置差 ' + dsc.toFixed(1) + ' mm · 姿态残差 ' + dori.toFixed(2) + '\u00B0'
|
||||
@@ -665,6 +708,10 @@ function runSolve() {
|
||||
TGT = [X, Y, Z];
|
||||
lastTool = +$('ikTool').value || 0;
|
||||
refreshTarget();
|
||||
/* 策略模式（?policy=1，ik-policy-glue.js）：目标交给闭环策略执行，不调用规划器（2026-10-17） */
|
||||
if (window.__POLICY && window.__POLICY.active() && window.__POLICY.setGoal(TGT.slice(), poseOn ? Rsolver() : null, lastTool)) {
|
||||
badge.textContent = '策略执行中…'; badge.className = 'badge wait'; return;
|
||||
}
|
||||
badge.textContent = '计算中…'; badge.className = 'badge wait';
|
||||
setTimeout(() => {
|
||||
const opt = { tool: lastTool, step: +$('ikStep').value || 0.25, ref: curRef(),
|
||||
@@ -703,6 +750,7 @@ function runSolveLive() {
|
||||
};
|
||||
try {
|
||||
lastTool = +$('ikTool').value || 0;
|
||||
if (window.__POLICY && window.__POLICY.active() && window.__POLICY.setGoal(TGT.slice(), poseOn ? Rsolver() : null, lastTool)) { settle(true); return; }
|
||||
/* 实时追踪原来走 solveTo —— 一个碰撞判定都不做：实测把目标球横拖过立柱，
|
||||
1300 帧里 870 帧处于穿模状态。改用 solveRoute：按"从当前位形这条直线能不能走"
|
||||
挑解，走不通就贴着障碍停（blocked），不再把手势直接映射进立柱里。 */
|
||||
@@ -1128,6 +1176,7 @@ function boot() {
|
||||
boot();
|
||||
|
||||
window.IK = { solver: S, setTarget: p => { TGT = p.slice(); syncInputs(); refreshTarget(); },
|
||||
setJoints: setJoints, curQ: () => ORDER.map(id => ang[id]),
|
||||
clampAxis: clampAxis, refreshJointLabels: refreshJointLabelsLazy,
|
||||
getTarget: () => TGT.slice(), run: runSolve, TCP0: TCP0, LIM: LIM,
|
||||
setView: setView, getView: () => VIEW,
|
||||
|
||||
@@ -0,0 +1,73 @@
|
||||
/* ============================================================================
|
||||
* ik-policy-glue.js —— 查看器接入 Transformer 策略（URL 加 ?policy=1 启用）
|
||||
* ?policy=1 用 models/traj_policy.onnx
|
||||
* ?policy=models/xxx.onnx 指定模型
|
||||
* &ep=webgpu 改用 WebGPU 后端（默认 wasm-SIMD 多线程）
|
||||
* 目标变化（松手求解 / 实时追踪）都交给 ArmPolicy.Runner：50 Hz 执行时钟放在 Web Worker（不受 rAF 60 Hz 与后台节流影响），
|
||||
* 每 3 步推理一次，安全层认证后才执行；ik-glue 的逐帧碰撞守卫不参与（策略执行不走 animTo），由 Runner 安全层保证。
|
||||
* onnxruntime-web 默认从 jsDelivr 加载；离线部署把 ort.min.js 与 *.wasm 放到 vendor/ort/ 并加 &ortLocal=1。
|
||||
* 多线程 wasm 需要页面跨源隔离（COOP/COEP 头，见 ml/README.md）；没有时自动退回单线程。
|
||||
* ==========================================================================*/
|
||||
(function () {
|
||||
'use strict';
|
||||
const qs = new URLSearchParams(location.search);
|
||||
if (!qs.has('policy')) return;
|
||||
const MODEL = qs.get('policy') && qs.get('policy') !== '1' ? qs.get('policy') : 'models/traj_policy.onnx';
|
||||
const ORT_BASE = qs.get('ortLocal') === '1' ? 'vendor/ort/' : 'https://cdn.jsdelivr.net/npm/onnxruntime-web@1.20.1/dist/';
|
||||
const badge = () => document.getElementById('ikBadge');
|
||||
const say = (t, cls) => { const b = badge(); if (b) { b.textContent = t; b.className = 'badge ' + (cls || 'wait'); } console.log('[policy]', t); };
|
||||
let ready = false, runner = null, running = false, worker = null;
|
||||
window.__POLICY = { active: () => ready, setGoal: () => false, stats: () => runner && runner.stats };
|
||||
|
||||
function loadScript(src) { return new Promise((res, rej) => { const s = document.createElement('script'); s.src = src; s.onload = res; s.onerror = () => rej(new Error('load ' + src)); document.head.appendChild(s); }); }
|
||||
async function waitIK() { for (let i = 0; i < 200 && !(window.IK && window.IK.solver && window.ArmPolicy); i++) await new Promise(r => setTimeout(r, 50)); if (!window.IK) throw new Error('IK 未就绪'); }
|
||||
|
||||
async function boot() {
|
||||
try {
|
||||
await waitIK();
|
||||
const S = window.IK.solver;
|
||||
const meta = await (await fetch(MODEL.replace('.onnx', '.meta.json'))).json();
|
||||
if (meta.fp !== S.modelFingerprint()) throw new Error('模型碰撞指纹 ' + meta.fp + ' ≠ 当前 ' + S.modelFingerprint() + '：collide.json 已变，须重训');
|
||||
if (meta.feat_ver !== ArmPolicy.FEAT_VER) throw new Error('特征版本不符：模型 ' + meta.feat_ver + ' / 运行时 ' + ArmPolicy.FEAT_VER);
|
||||
if (meta.tcp0 && meta.tcp0.some((v, i) => Math.abs(v - S.TCP0[i]) > 0.01)) throw new Error('TCP 与训练时不同：' + S.TCP0 + ' vs ' + meta.tcp0);
|
||||
if (!window.ort) await loadScript(ORT_BASE + 'ort.min.js');
|
||||
ort.env.wasm.wasmPaths = ORT_BASE;
|
||||
ort.env.wasm.numThreads = self.crossOriginIsolated ? Math.min(4, navigator.hardwareConcurrency || 1) : 1;
|
||||
const ep = qs.get('ep') === 'webgpu' ? ['webgpu', 'wasm'] : ['wasm'];
|
||||
const sess = await ort.InferenceSession.create(MODEL, { executionProviders: ep, graphOptimizationLevel: 'all' });
|
||||
runner = new ArmPolicy.Runner(S, ArmPolicy.ortInfer(ort, sess), {});
|
||||
/* 预热：首次推理有编译开销 */
|
||||
await ArmPolicy.ortInfer(ort, sess)(new Float32Array(8 * ArmPolicy.F_DIM), new Float32Array(8 * ArmPolicy.H * 6), 8);
|
||||
const tick = URL.createObjectURL(new Blob(['let h=null;onmessage=e=>{clearInterval(h);if(e.data>0)h=setInterval(()=>postMessage(0),e.data)}'], { type: 'text/javascript' }));
|
||||
worker = new Worker(tick);
|
||||
worker.onmessage = onTick;
|
||||
window.__POLICY = {
|
||||
active: () => ready,
|
||||
setGoal: (T, R, tool) => {
|
||||
if (tool) { say('策略模式暂不支持工具偏移 ≠ 0，已交回规划器', 'badb'); return false; }
|
||||
if (!running || !runner.q) runner.reset(window.IK.curQ());
|
||||
runner.setGoal(T, R); start(); return true;
|
||||
},
|
||||
stop, stats: () => runner.stats, runner: () => runner,
|
||||
latency: () => { const p = (a, x) => { const s = a.slice().sort((u, v) => u - v); return s.length ? s[Math.min(s.length - 1, Math.floor(s.length * x))] : null; };
|
||||
return { inferP50: p(runner.stats.inferMs, .5), inferP99: p(runner.stats.inferMs, .99), safetyP50: p(runner.stats.safetyMs, .5), safetyP99: p(runner.stats.safetyMs, .99), threads: ort.env.wasm.numThreads, ep: ep[0] }; }
|
||||
};
|
||||
ready = true;
|
||||
say('策略模式就绪（' + ep[0] + '，' + ort.env.wasm.numThreads + ' 线程）：拖动目标即由策略执行', 'ok');
|
||||
} catch (e) { say('策略模式不可用：' + e.message, 'badb'); console.error(e); }
|
||||
}
|
||||
function start() { if (running) return; running = true; worker.postMessage(ArmPolicy.DT * 1000); }
|
||||
function stop() { running = false; if (worker) worker.postMessage(0); }
|
||||
function onTick() {
|
||||
if (!running) return;
|
||||
if (runner.needPlan()) runner.plan().catch(e => { console.error(e); stop(); say('策略推理出错：' + e.message, 'badb'); });
|
||||
const q = runner.tick();
|
||||
if (q) window.IK.setJoints(q, false);
|
||||
if (runner.done && !runner.queue.length) {
|
||||
stop();
|
||||
const { e, oe } = runner.err(runner.q);
|
||||
say('策略到达 · 误差 ' + e.toFixed(2) + ' mm' + (runner.goal.R ? ' / ' + oe.toFixed(2) + '°' : '') + ' · 刹停 ' + runner.stats.brakes + ' 次', 'ok');
|
||||
} else if (runner.brakeRun >= 2) say('策略：安全层连续拦截，减速重规划中…', 'wait');
|
||||
}
|
||||
boot();
|
||||
})();
|
||||
@@ -0,0 +1,346 @@
|
||||
/* ============================================================================
|
||||
* ik-policy.js —— Transformer 避障策略的闭环运行时（浏览器 / Node 通用，UMD）
|
||||
* 方案文档 §5 / §8：50 Hz 执行，每 3 步（60 ms）推理一次，8 个噪声 → 8 条 16 步动作块，
|
||||
* 1 步胶囊梯度投影 → 排序 → 距离下界认证（前 3 步 + 刹停段）→ 提交；全不过则沿已认证刹停段停。
|
||||
* 不依赖任何规划器（solveDyn / routeRT / 路网）：只用求解器的 fk / orient / gapMatrix / 胶囊对间隙。
|
||||
* 特征定义与 ml/common/features.py 逐项一致（ml/eval 对拍）。改特征必须同时改两边并升 FEAT_VER。
|
||||
* ==========================================================================*/
|
||||
(function (root, factory) {
|
||||
if (typeof module === 'object' && module.exports) module.exports = factory();
|
||||
else root.ArmPolicy = factory();
|
||||
}(typeof self !== 'undefined' ? self : this, function () {
|
||||
'use strict';
|
||||
|
||||
const FEAT_VER = 1;
|
||||
const VMAX = [60, 60, 90, 90, 120, 180]; // °/s（与 ml/gen/timelaw.cjs 同）
|
||||
const DT = 0.02;
|
||||
const STEP = VMAX.map(v => v * DT); // 每步最大增量（°）
|
||||
const ACC = 5.0; // 归一化加速度 1/s²（与时间律同）
|
||||
const H = 16, NJ = 6, PREFIX = 3;
|
||||
const GP = [[0, 2], [0, 3], [0, 4], [0, 5], [0, 6], [2, 4], [2, 5], [3, 5]]; // 特征里的 8 个组对
|
||||
const GAP_CAP = 40;
|
||||
const F_DIM = 83;
|
||||
const D2R = Math.PI / 180;
|
||||
const wrap180 = a => { a = ((a + 180) % 360 + 360) % 360 - 180; return a; };
|
||||
|
||||
/* ---------------- 特征（83 维） ---------------- */
|
||||
function features(S, st, gm) {
|
||||
const q = st.q, L = S.LIM, O = S.ORDER, f = new Float32Array(F_DIM);
|
||||
let o = 0;
|
||||
for (let j = 0; j < 6; j++) { f[o++] = Math.sin(q[j] * D2R); f[o++] = Math.cos(q[j] * D2R); }
|
||||
for (const j of [1, 3, 4]) { const t = L[O[j]]; f[o++] = 2 * (q[j] - t.lo) / (t.hi - t.lo) - 1; }
|
||||
const qp = st.qPrev || q;
|
||||
for (let j = 0; j < 6; j++) { const d = L[O[j]].cont ? wrap180(q[j] - qp[j]) : q[j] - qp[j]; f[o++] = d / STEP[j]; }
|
||||
const p = S.fk(q, 0), R = S.orient(q);
|
||||
for (let k = 0; k < 3; k++) f[o++] = p[k] / 600;
|
||||
for (const k of [0, 3, 6, 1, 4, 7]) f[o++] = R[k];
|
||||
for (let k = 0; k < 3; k++) f[o++] = st.T[k] / 600;
|
||||
if (st.R) for (const k of [0, 3, 6, 1, 4, 7]) f[o++] = st.R[k]; else o += 6;
|
||||
for (let k = 0; k < 3; k++) f[o++] = (st.T[k] - p[k]) / 600;
|
||||
for (let k = 0; k < 3; k++) f[o++] = Math.max(-1, Math.min(1, (st.T[k] - p[k]) / 50));
|
||||
f[o++] = st.R ? 0 : 1; f[o++] = st.R ? 1 : 0;
|
||||
const pre = st.prefix || [];
|
||||
for (let k = 0; k < PREFIX; k++) for (let j = 0; j < 6; j++) f[o++] = k < pre.length ? pre[k][j] / STEP[j] : 0;
|
||||
for (let k = 0; k < PREFIX; k++) f[o++] = k < pre.length ? 1 : 0;
|
||||
if (st.sub) { for (let j = 0; j < 6; j++) { const d = L[O[j]].cont ? wrap180(st.sub[j] - q[j]) : st.sub[j] - q[j]; f[o++] = d / 180; } f[o++] = 1; }
|
||||
else o += 7;
|
||||
const G = gm || S.gapMatrix(q, GAP_CAP);
|
||||
for (const [a, b] of GP) f[o++] = Math.min(G[a * 7 + b], GAP_CAP) / GAP_CAP;
|
||||
if (o !== F_DIM) throw new Error('feature dim ' + o);
|
||||
return f;
|
||||
}
|
||||
|
||||
/* ---------------- 距离下界认证（ml/gen/cert.cjs 复用本实现） ---------------- */
|
||||
function makeCert(S, opt) {
|
||||
opt = opt || {};
|
||||
const CAP = opt.cap || 40, MIN_DEG = opt.minDeg || 0.002;
|
||||
/* 分组杠杆表 TL[k][g]：组对 (gA,gB) 只用关节 gA..gB−1 带动组 gB 的杠杆（比全局上界紧得多） */
|
||||
const TL = opt.leverTable || S.leverTable(4000);
|
||||
const L = S.LIM, O = S.ORDER;
|
||||
const memo = new Map();
|
||||
const key = q => q.map(v => v.toFixed(5)).join(',');
|
||||
let queries = 0, budgetLeft = Infinity;
|
||||
const gm = q => { const k = key(q); let M = memo.get(k); if (!M) { M = S.gapMatrix(q, CAP); queries++; budgetLeft--; if (memo.size > 50000) memo.clear(); memo.set(k, M); } return M; };
|
||||
const minGap = M => { let m = Infinity; for (let a = 0; a < 7; a++) for (let b = a + 2; b < 7; b++) m = Math.min(m, M[a * 7 + b]); return m; };
|
||||
const inBox = q => O.every((id, i) => L[id].cont || (q[i] >= L[id].lo - 1e-6 && q[i] <= L[id].hi + 1e-6));
|
||||
function seg(a, b, depth) {
|
||||
if (budgetLeft <= 0) return false;
|
||||
const Ma = gm(a), Mb = gm(b);
|
||||
const dq = a.map((v, i) => Math.abs(b[i] - v));
|
||||
let ok = true;
|
||||
for (let ga = 0; ga < 7 && ok; ga++) for (let gb = ga + 2; gb < 7; gb++) {
|
||||
const lhs = Ma[ga * 7 + gb] + Mb[ga * 7 + gb];
|
||||
let rhs = 0; for (let k = ga; k < gb; k++) rhs += TL[k][gb] * dq[k];
|
||||
if (!(lhs > rhs)) { ok = false; break; }
|
||||
}
|
||||
if (ok) return true;
|
||||
if (minGap(Ma) <= 0 || minGap(Mb) <= 0) return false;
|
||||
if (Math.max.apply(null, dq) < MIN_DEG || depth > 40) return false;
|
||||
const m = a.map((v, i) => 0.5 * (v + b[i]));
|
||||
return seg(a, m, depth + 1) && seg(m, b, depth + 1);
|
||||
}
|
||||
/* 折线（相邻点按短弧展开）→ {ok, queries, minGap, failAt, why}；budget = 本次最多新增距离查询数（超出判不过，why='budget'） */
|
||||
function path(pts, budget) {
|
||||
const q0 = queries; let mg = Infinity;
|
||||
budgetLeft = budget || Infinity;
|
||||
const P = [pts[0].slice()];
|
||||
for (let i = 1; i < pts.length; i++) P.push(P[i - 1].map((v, k) => L[O[k]].cont ? v + wrap180(pts[i][k] - v) : pts[i][k]));
|
||||
for (let i = 0; i < P.length; i++) { if (!inBox(P[i])) return { ok: false, queries: queries - q0, minGap: mg, failAt: i, why: 'box' }; }
|
||||
for (let i = 0; i < P.length; i++) mg = Math.min(mg, minGap(gm(P[i])));
|
||||
for (let i = 1; i < P.length; i++) if (!seg(P[i - 1], P[i], 0)) return { ok: false, queries: queries - q0, minGap: mg, failAt: i, why: budgetLeft <= 0 ? 'budget' : 'cert' };
|
||||
budgetLeft = Infinity;
|
||||
return { ok: true, queries: queries - q0, minGap: mg, unwrapped: P };
|
||||
}
|
||||
return { seg: (a, b) => { budgetLeft = Infinity; return seg(a, b, 0); }, path, leverTable: TL, gapMin: q => minGap(gm(q)), gm, clear: () => memo.clear() };
|
||||
}
|
||||
|
||||
/* ---------------- 刹停段：从 q 以每步增量 v 起，按 ACC 均匀减速到 0 ---------------- */
|
||||
function brakeSteps(v) {
|
||||
let n = 0; for (let j = 0; j < 6; j++) n = Math.max(n, Math.ceil(Math.abs(v[j]) / (STEP[j] * ACC * DT) - 1e-9));
|
||||
const out = [];
|
||||
for (let k = 1; k <= n; k++) out.push(v.map(x => x * (1 - k / (n + 1))));
|
||||
return out;
|
||||
}
|
||||
|
||||
/* ---------------- 1 步胶囊梯度投影（每轴 ≤ maxDeg，仅 gap < margin） ---------------- */
|
||||
/* pairs：本次推理开始时在 qe 处取的近距胶囊对（S.criticalPairs(qe, margin+10)），候选各步只对这些对求间隙与有限差分（微秒级） */
|
||||
function project(S, q, margin, maxDeg, pairs) {
|
||||
if (!pairs || !pairs.length) return q;
|
||||
const gs = pairs.map(p => S.pairGap(q, p[0], p[1]));
|
||||
const g0 = Math.min.apply(null, gs);
|
||||
if (g0 >= margin) return q;
|
||||
pairs = pairs.filter((p, i) => gs[i] < margin + 2).slice(0, 6);
|
||||
const h = 0.01, grad = [0, 0, 0, 0, 0, 0];
|
||||
for (let j = 0; j < 6; j++) {
|
||||
const qa = q.slice(); qa[j] += h;
|
||||
const g1 = Math.min.apply(null, pairs.map(p => S.pairGap(qa, p[0], p[1])));
|
||||
grad[j] = (g1 - g0) / h;
|
||||
}
|
||||
const n2 = grad.reduce((a, x) => a + x * x, 0);
|
||||
if (n2 < 1e-12) return q;
|
||||
const need = margin - g0;
|
||||
return q.map((v, j) => v + Math.max(-maxDeg, Math.min(maxDeg, need * grad[j] / n2)));
|
||||
}
|
||||
|
||||
/* ---------------- 局部 DLS 精修（接近目标时） ---------------- */
|
||||
function refineStep(S, q, T, Rt, maxScale) {
|
||||
const e = x => {
|
||||
const p = S.fk(x, 0), r = [T[0] - p[0], T[1] - p[1], T[2] - p[2]];
|
||||
if (!Rt) return r;
|
||||
const R = S.orient(x); // 旋转误差向量 ≈ 0.5·vee(R_t Rᵀ − R Rtᵀ)，按 100 mm/rad 折算到 mm 量级
|
||||
const M = [0, 0, 0, 0, 0, 0, 0, 0, 0];
|
||||
for (let i = 0; i < 3; i++) for (let k = 0; k < 3; k++) { let s = 0; for (let m = 0; m < 3; m++) s += Rt[i * 3 + m] * R[k * 3 + m]; M[i * 3 + k] = s; }
|
||||
return r.concat([0.5 * (M[7] - M[5]) * 100, 0.5 * (M[2] - M[6]) * 100, 0.5 * (M[3] - M[1]) * 100]);
|
||||
};
|
||||
const e0 = e(q), m = e0.length, h = 1e-3, J = [];
|
||||
for (let j = 0; j < 6; j++) { const qa = q.slice(); qa[j] += h; const e1 = e(qa); J.push(e1.map((v, i) => -(v - e0[i]) / h)); }
|
||||
// Δq = Jᵀ (J Jᵀ + λ²I)⁻¹ e，m ≤ 6
|
||||
const lam = 0.5, A = [];
|
||||
for (let i = 0; i < m; i++) { A.push([]); for (let k = 0; k < m; k++) { let s = i === k ? lam * lam : 0; for (let j = 0; j < 6; j++) s += J[j][i] * J[j][k]; A[i].push(s); } }
|
||||
const y = solveLin(A, e0.slice());
|
||||
const dq = [0, 0, 0, 0, 0, 0];
|
||||
for (let j = 0; j < 6; j++) { let s = 0; for (let i = 0; i < m; i++) s += J[j][i] * y[i]; dq[j] = s; }
|
||||
let sc = 1; for (let j = 0; j < 6; j++) sc = Math.max(sc, Math.abs(dq[j]) / (STEP[j] * (maxScale || 0.5)));
|
||||
return { dq: dq.map(v => v / sc), err: Math.hypot(e0[0], e0[1], e0[2]) };
|
||||
}
|
||||
function solveLin(A, b) {
|
||||
const n = b.length;
|
||||
for (let c = 0; c < n; c++) {
|
||||
let p = c; for (let r = c + 1; r < n; r++) if (Math.abs(A[r][c]) > Math.abs(A[p][c])) p = r;
|
||||
[A[c], A[p]] = [A[p], A[c]]; [b[c], b[p]] = [b[p], b[c]];
|
||||
for (let r = c + 1; r < n; r++) { const f = A[r][c] / A[c][c]; for (let k = c; k < n; k++) A[r][k] -= f * A[c][k]; b[r] -= f * b[c]; }
|
||||
}
|
||||
const x = new Array(n).fill(0);
|
||||
for (let r = n - 1; r >= 0; r--) { let s = b[r]; for (let k = r + 1; k < n; k++) s -= A[r][k] * x[k]; x[r] = s / A[r][r]; }
|
||||
return x;
|
||||
}
|
||||
|
||||
/* ---------------- 正态噪声 ---------------- */
|
||||
function makeRng(seed) {
|
||||
let s = (seed >>> 0) || 1;
|
||||
const u = () => { s = (s * 1664525 + 1013904223) >>> 0; return (s + 0.5) / 4294967296; };
|
||||
return { u, n: () => Math.sqrt(-2 * Math.log(u())) * Math.cos(2 * Math.PI * u()) };
|
||||
}
|
||||
|
||||
/* ============================================================================
|
||||
* 闭环运行器
|
||||
* infer(feat Float32Array[B*F], z Float32Array[B*H*6], B) → Promise<{act[B*H*6], value[B], sub[B*13]}>
|
||||
* 用法：r = new Runner(S, infer, opts); r.reset(q0); r.setGoal(T, R);
|
||||
* 每 20 ms 调 r.tick()；r.needPlan() 为真时 await r.plan()（tick 与 plan 可并行：plan 只在队列 ≥3 步时发起）
|
||||
* ==========================================================================*/
|
||||
class Runner {
|
||||
constructor(S, infer, opts) {
|
||||
this.S = S; this.infer = infer;
|
||||
this.o = Object.assign({ K: 8, commit: 3, margin: 5, projMax: 0.5, refineMm: 5, arriveMm: 1, arriveDeg: 0.5,
|
||||
stuckPlans: 5, lockSec: 1.0, subReachDeg: 5, subTimeout: 1.5, seed: 12345, lookahead: true,
|
||||
certBudget: 40, safetyMs: 30 }, opts || {}); // 每条候选最多 40 次距离查询；每次推理安全层总预算 30 ms（超出按刹停处理）
|
||||
this.cert = makeCert(S, { leverTable: this.o.leverTable, cap: 20 });
|
||||
this.rng = makeRng(this.o.seed);
|
||||
this.stats = { plans: 0, inferMs: [], safetyMs: [], brakes: 0, refine: 0, lookahead: 0, locks: 0, projected: 0 };
|
||||
this.reset(null);
|
||||
}
|
||||
reset(q0) {
|
||||
this.q = q0 ? q0.slice() : null; this.qPrev = this.q ? this.q.slice() : null;
|
||||
this.queue = []; this.brakeTail = []; this.lastPlan = null; this.goal = null;
|
||||
this.subs = []; this.subT = 0; this.vals = []; this.brakeRun = 0; this.lock = null; this.t = 0; this.cold = true; this.done = false;
|
||||
}
|
||||
setGoal(T, R) {
|
||||
const jump = !this.goal || Math.hypot(T[0] - this.goal.T[0], T[1] - this.goal.T[1], T[2] - this.goal.T[2]) > 50 || (!!R !== !!this.goal.R);
|
||||
this.goal = { T: T.slice(), R: R ? R.slice() : null };
|
||||
if (jump) { this.needSub = true; this.subs = []; this.cold = true; this.vals = []; this.lock = null; }
|
||||
this.done = false;
|
||||
}
|
||||
qEnd() { let q = this.q.slice(); for (const d of this.queue) q = q.map((v, j) => v + d[j]); return q; }
|
||||
err(q) {
|
||||
const p = this.S.fk(q, 0), T = this.goal.T;
|
||||
const e = Math.hypot(T[0] - p[0], T[1] - p[1], T[2] - p[2]);
|
||||
let oe = 0;
|
||||
if (this.goal.R) { const R = this.S.orient(q); let tr = 0; for (let i = 0; i < 9; i++) tr += R[i] * this.goal.R[i]; oe = Math.acos(Math.max(-1, Math.min(1, (tr - 1) / 2))) / D2R; }
|
||||
return { e, oe };
|
||||
}
|
||||
needPlan() { return !!this.goal && !this.done && !this.planning && this.queue.length <= this.o.commit; }
|
||||
tick() {
|
||||
if (!this.q) return null;
|
||||
const d = this.queue.shift();
|
||||
this.qPrev = this.q.slice();
|
||||
if (d) this.q = this.q.map((v, j) => v + d[j]);
|
||||
this.t += DT;
|
||||
if (this.goal && !this.queue.length) {
|
||||
const { e, oe } = this.err(this.q);
|
||||
if (e < this.o.arriveMm && oe < this.o.arriveDeg) this.done = true;
|
||||
}
|
||||
return this.q;
|
||||
}
|
||||
_cand(act, k, r) { // 第 k 条候选：增量（°），丢掉模型的前 r 步（已提交前缀），取接下来的步
|
||||
const inc = [];
|
||||
for (let s = 0; s < H; s++) { const row = []; for (let j = 0; j < 6; j++) row.push(Math.max(-1, Math.min(1, act[(k * H + s) * 6 + j])) * STEP[j]); inc.push(row); }
|
||||
return inc;
|
||||
}
|
||||
_certCommit(qe, steps) { // 认证：提交步 + 刹停段（先用离散判定快速剔除，再做距离下界认证）
|
||||
const pts = [qe.slice()]; let q = qe.slice();
|
||||
for (const d of steps) { q = q.map((v, j) => v + d[j]); pts.push(q.slice()); }
|
||||
for (let i = 1; i < pts.length; i++) if (!this.S.collideFree(pts[i])) return { ok: false, tail: [] };
|
||||
const tail = brakeSteps(steps.length ? steps[steps.length - 1] : [0, 0, 0, 0, 0, 0]);
|
||||
/* 刹停段各步同向等比减速 → 关节空间里是一条直线，认证终点一段即可 */
|
||||
if (tail.length) { for (const d of tail) q = q.map((v, j) => v + d[j]); pts.push(q.slice()); }
|
||||
const c = this.cert.path(pts, this.o.certBudget);
|
||||
return { ok: c.ok, tail };
|
||||
}
|
||||
async plan() {
|
||||
if (!this.needPlan()) return;
|
||||
this.planning = true;
|
||||
try { await this._plan(); } finally { this.planning = false; }
|
||||
}
|
||||
async _plan() {
|
||||
const S = this.S, o = this.o, K = o.K;
|
||||
const r = Math.min(this.queue.length, PREFIX), prefix = this.queue.slice(0, r);
|
||||
const qe = this.qEnd();
|
||||
this.stats.plans++;
|
||||
/* 接近目标：局部 DLS 精修代替模型 */
|
||||
const ee = this.err(qe);
|
||||
if (ee.e < o.refineMm && (!this.goal.R || ee.oe < 5)) {
|
||||
if (ee.e < o.arriveMm && ee.oe < o.arriveDeg) return;
|
||||
const steps = []; let q = qe.slice();
|
||||
for (let k = 0; k < o.commit; k++) { const rs = refineStep(S, q, this.goal.T, this.goal.R, 0.3); steps.push(rs.dq); q = q.map((v, j) => v + rs.dq[j]); }
|
||||
const c = this._certCommit(qe, steps);
|
||||
if (c.ok) { this.queue.push(...steps); this.brakeTail = c.tail; this.stats.refine++; return; }
|
||||
}
|
||||
/* 子目标推进 */
|
||||
if (this.subs.length) {
|
||||
const sg = this.subs[0]; let m = 0; for (let j = 0; j < 6; j++) m = Math.max(m, Math.abs((S.LIM[S.ORDER[j]].cont ? wrap180(sg[j] - qe[j]) : sg[j] - qe[j])));
|
||||
if (m < o.subReachDeg || this.t - this.subT > o.subTimeout) { this.subs.shift(); this.subT = this.t; }
|
||||
}
|
||||
const gm = S.gapMatrix(this.q, GAP_CAP);
|
||||
const st = { q: this.q, qPrev: this.qPrev, T: this.goal.T, R: this.goal.R, prefix, sub: this.subs[0] || null };
|
||||
const f = features(S, st, gm);
|
||||
const feat = new Float32Array(K * F_DIM), z = new Float32Array(K * H * 6);
|
||||
for (let k = 0; k < K; k++) feat.set(f, k * F_DIM);
|
||||
for (let i = 0; i < z.length; i++) z[i] = this.rng.n();
|
||||
const t0 = now();
|
||||
const out = await this.infer(feat, z, K);
|
||||
this.stats.inferMs.push(now() - t0);
|
||||
const t1 = now();
|
||||
/* 子目标：新目标/跳变后取价值最好的那条输出 */
|
||||
let best = 0; for (let k = 1; k < K; k++) if (out.value[k] < out.value[best]) best = k;
|
||||
if (this.needSub) {
|
||||
this.needSub = false; this.subT = this.t;
|
||||
const sb = out.sub.subarray(best * 13, best * 13 + 13);
|
||||
if (sb[12] > 0.5) this.subs = [0, 1].map(v => this.q.map((x, j) => x + sb[v * 6 + j] * 180));
|
||||
}
|
||||
/* 候选：去掉前缀后的提交步，投影，打分 */
|
||||
const cands = [], near = S.criticalPairs(qe, o.margin + 10).slice(0, 64);
|
||||
for (let k = 0; k < K; k++) {
|
||||
const inc = this._cand(out.act, k, r);
|
||||
let steps = inc.slice(r, r + o.commit), q = qe.slice();
|
||||
const adj = [];
|
||||
for (const d of steps) {
|
||||
let qn = q.map((v, j) => v + d[j]);
|
||||
const qp = project(S, qn, o.margin, o.projMax, near);
|
||||
if (qp !== qn) this.stats.projected++;
|
||||
adj.push(qp.map((v, j) => v - q[j])); q = qp;
|
||||
}
|
||||
steps = adj;
|
||||
const fut = []; let qq = qe.slice(); for (let s = r; s < H; s++) { qq = qq.map((v, j) => v + inc[s][j]); fut.push(qq); }
|
||||
let j1 = 0; for (let s = 0; s < H; s++) j1 += inc[s][0];
|
||||
let score = -2.0 * out.value[k];
|
||||
if (this.lastPlan) { // 与上一条已选计划的未来重叠步对齐（时间平移 3 步）
|
||||
let c = 0, w = 1;
|
||||
for (let s = 0; s < fut.length && s + o.commit < this.lastPlan.length; s++, w *= 0.85) {
|
||||
let d2 = 0; for (let j = 0; j < 6; j++) d2 += Math.pow((fut[s][j] - this.lastPlan[s + o.commit][j]) / STEP[j], 2);
|
||||
c += w * d2;
|
||||
}
|
||||
score -= 0.02 * c;
|
||||
}
|
||||
cands.push({ k, steps, fut, j1, score, value: out.value[k] });
|
||||
}
|
||||
/* 卡死检测：价值 0.3 s 内不降 或 连续 2 次刹停 → 锁定另一绕行方向 1 s */
|
||||
const vbest = out.value[best]; this.vals.push(vbest); if (this.vals.length > o.stuckPlans) this.vals.shift();
|
||||
const stuck = this.brakeRun >= 2 || (this.vals.length >= o.stuckPlans && this.vals[this.vals.length - 1] > this.vals[0] * 0.99 && ee.e > 20);
|
||||
if (stuck && !(this.lock && this.t < this.lock.until)) {
|
||||
const lastDir = this.lastPlan ? Math.sign(this.lastPlan[this.lastPlan.length - 1][0] - this.lastPlan[0][0]) || 1 : 1;
|
||||
this.lock = { side: -lastDir, until: this.t + o.lockSec }; this.stats.locks++; this.vals = [];
|
||||
}
|
||||
let pool = cands;
|
||||
if (this.lock && this.t < this.lock.until) { const f2 = cands.filter(c => Math.abs(c.j1) < 2 || Math.sign(c.j1) === this.lock.side); if (f2.length) pool = f2; }
|
||||
/* 短前瞻：冷启动 / 卡死时，各候选块末态再推一块，用其价值打分 */
|
||||
if (o.lookahead && (this.cold || stuck)) {
|
||||
this.stats.lookahead++;
|
||||
const feat2 = new Float32Array(pool.length * F_DIM), z2 = new Float32Array(pool.length * H * 6);
|
||||
pool.forEach((c, i) => {
|
||||
const qa = c.fut[c.fut.length - 1], qb = c.fut[Math.max(0, c.fut.length - 2)];
|
||||
feat2.set(features(S, { q: qa, qPrev: qb, T: this.goal.T, R: this.goal.R, prefix: [], sub: null }), i * F_DIM);
|
||||
});
|
||||
for (let i = 0; i < z2.length; i++) z2[i] = this.rng.n();
|
||||
const o2 = await this.infer(feat2, z2, pool.length);
|
||||
pool.forEach((c, i) => { c.score = -2.0 * o2.value[i] - 0.5 * c.value; });
|
||||
}
|
||||
this.cold = false;
|
||||
pool.sort((a, b) => b.score - a.score);
|
||||
/* 安全层：依次认证，第一个通过即提交 */
|
||||
let picked = null;
|
||||
for (const c of pool) {
|
||||
if (now() - t1 > o.safetyMs && picked === null && c !== pool[0]) break;
|
||||
const v = this._certCommit(qe, c.steps); if (v.ok) { picked = c; this.brakeTail = v.tail; break; }
|
||||
}
|
||||
this.stats.safetyMs.push(now() - t1);
|
||||
if (picked) {
|
||||
this.queue.push(...picked.steps); this.lastPlan = [qe.slice()].concat(picked.fut); this.brakeRun = 0;
|
||||
} else {
|
||||
this.stats.brakes++; this.brakeRun++;
|
||||
if (this.brakeTail.length) { this.queue.push(...this.brakeTail); this.brakeTail = []; }
|
||||
this.lastPlan = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
const now = () => (typeof performance !== 'undefined' ? performance.now() : Date.now());
|
||||
|
||||
/* ---------------- onnxruntime 适配（web: ort = window.ort；node: require('onnxruntime-node')） ---------------- */
|
||||
function ortInfer(ort, session) {
|
||||
return async (feat, z, B) => {
|
||||
const r = await session.run({ feat: new ort.Tensor('float32', feat, [B, F_DIM]), z: new ort.Tensor('float32', z, [B, H, 6]) });
|
||||
return { act: r.act.data, value: r.value.data, sub: r.sub.data };
|
||||
};
|
||||
}
|
||||
|
||||
return { FEAT_VER, VMAX, DT, STEP, ACC, H, F_DIM, GP, GAP_CAP, features, makeCert, brakeSteps, project, refineStep, Runner, ortInfer, wrap180 };
|
||||
}));
|
||||
+953
-32
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,5 @@
|
||||
策略模型放这里（由 `python -m ml.export_onnx --fp16` 生成）：
|
||||
- `traj_policy.onnx`
|
||||
- `traj_policy.meta.json`（碰撞模型指纹 / 特征版本 / TCP，查看器加载时校验）
|
||||
|
||||
查看器地址加 `?policy=1` 启用策略模式，详见 `ml/README.md` 第 8 步。
|
||||
@@ -0,0 +1,40 @@
|
||||
/* 生成两关节零件对类的间隙表，写入 docs/collide.json 的 pair2d 字段。
|
||||
* node webapp/tools/gen_pair2d.cjs 默认生成 (2,4) 类（P05/M3 × P07/M5，J3×J4）
|
||||
* 重新拟合胶囊（scratch/fit_capsules.py 重写 collide.json）后必须重跑；
|
||||
* 求解器按指纹校验，不符会自动弃表回退精算（只是变慢，不会算错）。 */
|
||||
const fs = require('fs'), path = require('path');
|
||||
const ROOT = path.resolve(__dirname, '..', '..');
|
||||
const setup = require(path.join(ROOT, 'tests/route_bench_setup.cjs'));
|
||||
const COLF = path.join(ROOT, 'docs/collide.json');
|
||||
/* 文本级追加/替换，保持 fit_capsules.py 写出的原格式不动（避免整文件 diff） */
|
||||
let text = fs.readFileSync(COLF, 'utf8');
|
||||
const cut = text.indexOf(',\n "pair2d":');
|
||||
if (cut >= 0) text = text.slice(0, cut) + '\n}\n';
|
||||
fs.writeFileSync(COLF + '.tmp', text);
|
||||
const { S } = setup(undefined, COLF + '.tmp');
|
||||
fs.unlinkSync(COLF + '.tmp');
|
||||
const H = 1.0, classes = [[2, 4]];
|
||||
const out = {};
|
||||
for (const [ga, gb] of classes) {
|
||||
const ci = ga * 7 + gb, ja = ga, jb = ga + 1;
|
||||
const la = S.LIM[S.ORDER[ja]], lb = S.LIM[S.ORDER[jb]];
|
||||
const lo = [la.cont ? -180 : la.lo, lb.cont ? -180 : lb.lo];
|
||||
const n = [la.cont ? Math.round(360 / H) : Math.round((la.hi - la.lo) / H) + 1,
|
||||
lb.cont ? Math.round(360 / H) : Math.round((lb.hi - lb.lo) / H) + 1];
|
||||
const buf = Buffer.alloc(n[0] * n[1] * 2);
|
||||
const t0 = Date.now();
|
||||
for (let i = 0; i < n[0]; i++) {
|
||||
for (let j = 0; j < n[1]; j++) {
|
||||
const q = [0, 0, 0, 0, 0, 0];
|
||||
q[ja] = lo[0] + i * H; q[jb] = lo[1] + j * H;
|
||||
const g = S.classGap(q, ci);
|
||||
buf.writeInt16LE(Math.max(-32767, Math.min(32767, Math.floor(g * 100))), 2 * (i * n[1] + j)); // 向下取整：保守
|
||||
}
|
||||
if (i % 60 === 0) console.error('class', ga + '' + gb, i + '/' + n[0], ((Date.now() - t0) / 1000).toFixed(0) + 's');
|
||||
}
|
||||
out[ci] = { joints: [S.ORDER[ja], S.ORDER[jb]], lo: lo, h: H, n: n, fp: S.classFingerprint(ci), b64: buf.toString('base64') };
|
||||
}
|
||||
const body = text.replace(/\s*\}\s*$/, '');
|
||||
fs.writeFileSync(COLF, body + ',\n "pair2d": ' + JSON.stringify(out) +
|
||||
',\n "pair2d_generated": ' + JSON.stringify(new Date().toISOString()) + '\n}\n');
|
||||
console.log('pair2d written', Object.keys(out));
|
||||
@@ -0,0 +1,82 @@
|
||||
/* 离线生成关节空间安全路网（PRM），写入 docs/collide.json 的 roadmap 字段。
|
||||
* node webapp/tools/gen_roadmap.cjs [节点数=2500] [k=10]
|
||||
* 障碍是静态的（立柱 + 自身零件），路网只依赖碰撞模型与限位；重新拟合胶囊 / 改限位后必须重跑
|
||||
* （求解器按指纹校验，不符自动弃用路网，退回在线 RRT —— 只是变慢，不会算错）。
|
||||
* 多线程：worker_threads 按 CPU 数并行细验边。 */
|
||||
const fs = require('fs'), path = require('path'), os = require('os');
|
||||
const { Worker, isMainThread, parentPort, workerData } = require('worker_threads');
|
||||
const ROOT = path.resolve(__dirname, '..', '..');
|
||||
const COLF = path.join(ROOT, 'docs/collide.json');
|
||||
function stripped() { // 去掉旧 roadmap 字段（文本级，保持原格式）
|
||||
let text = fs.readFileSync(COLF, 'utf8');
|
||||
const cut = text.indexOf(',\n "roadmap":');
|
||||
if (cut >= 0) text = text.slice(0, cut) + '\n}\n';
|
||||
return text;
|
||||
}
|
||||
if (!isMainThread) {
|
||||
const { S } = require(path.join(ROOT, 'tests/route_bench_setup.cjs'))(undefined, workerData.tmp);
|
||||
parentPort.on('message', ({ Q, edges }) => {
|
||||
const ok = edges.map(([a, b]) => S.edgeFree(Q[a], S.unwrapTo ? S.unwrapTo(Q[a], Q[b]) : Q[b]) ? 1 : 0);
|
||||
parentPort.postMessage(ok);
|
||||
});
|
||||
return;
|
||||
}
|
||||
(async () => {
|
||||
const N = +process.argv[2] || 2500, K = +process.argv[3] || 10;
|
||||
const text = stripped(), tmp = COLF + '.rmtmp';
|
||||
fs.writeFileSync(tmp, text);
|
||||
const { S } = require(path.join(ROOT, 'tests/route_bench_setup.cjs'))(undefined, tmp);
|
||||
const O = S.ORDER, L = S.LIM;
|
||||
let seed = 20261015;
|
||||
const rnd = () => { seed = (seed * 1664525 + 1013904223) >>> 0; return seed / 4294967296; };
|
||||
const Q = [];
|
||||
let tries = 0;
|
||||
while (Q.length < N) {
|
||||
tries++;
|
||||
const q = O.map(id => { const t = L[id]; return t.cont ? -180 + 360 * rnd() : t.lo + (t.hi - t.lo) * rnd(); });
|
||||
const qr = q.map(v => Math.round(v * 100) / 100);
|
||||
if (S.collideFree(qr)) Q.push(qr);
|
||||
}
|
||||
console.error('nodes', N, 'free ratio', (N / tries).toFixed(3));
|
||||
const E = new Set();
|
||||
for (let i = 0; i < N; i++) {
|
||||
const d = [];
|
||||
for (let j = 0; j < N; j++) if (j !== i) d.push([j, S.wd(Q[i], Q[j])]);
|
||||
d.sort((a, b) => a[1] - b[1]);
|
||||
for (let m = 0; m < K; m++) { const j = d[m][0]; E.add(i < j ? i * 65536 + j : j * 65536 + i); }
|
||||
}
|
||||
const edges = [...E].map(k => [Math.floor(k / 65536), k % 65536]);
|
||||
console.error('candidate edges', edges.length);
|
||||
const nw = Math.max(1, os.cpus().length), ws = [];
|
||||
for (let w = 0; w < nw; w++) ws.push(new Worker(__filename, { workerData: { tmp } }));
|
||||
const CH = 200, okAll = new Uint8Array(edges.length);
|
||||
let next = 0, done = 0; const t0 = Date.now();
|
||||
await new Promise(res => {
|
||||
const feed = w => {
|
||||
if (next >= edges.length) { if (done >= edges.length) res(); return; }
|
||||
const lo = next; next = Math.min(edges.length, next + CH);
|
||||
w.once('message', ok => { okAll.set(ok, lo); done += ok.length;
|
||||
if ((done / CH) % 10 === 0) console.error('edges', done + '/' + edges.length, ((Date.now() - t0) / 1000).toFixed(0) + 's');
|
||||
if (done >= edges.length) res(); else feed(w); });
|
||||
w.postMessage({ Q, edges: edges.slice(lo, next) });
|
||||
};
|
||||
ws.forEach(feed);
|
||||
});
|
||||
ws.forEach(w => w.terminate());
|
||||
fs.unlinkSync(tmp);
|
||||
const adjL = Array.from({ length: N }, () => []);
|
||||
let ne = 0;
|
||||
edges.forEach(([a, b], i) => { if (okAll[i]) { adjL[a].push(b); adjL[b].push(a); ne++; } });
|
||||
/* 连通分量统计 */
|
||||
const comp = new Int32Array(N).fill(-1); let nc = 0, big = 0;
|
||||
for (let i = 0; i < N; i++) if (comp[i] < 0) { let st = [i], sz = 0; comp[i] = nc; while (st.length) { const u = st.pop(); sz++; for (const v of adjL[u]) if (comp[v] < 0) { comp[v] = nc; st.push(v); } } big = Math.max(big, sz); nc++; }
|
||||
console.error('valid edges', ne, 'components', nc, 'largest', big);
|
||||
const qb = Buffer.alloc(N * 12); Q.forEach((q, i) => q.forEach((v, k) => qb.writeInt16LE(Math.round(v * 100), (i * 6 + k) * 2)));
|
||||
const off = Buffer.alloc((N + 1) * 4), adj = Buffer.alloc(ne * 2 * 2);
|
||||
let p = 0; for (let i = 0; i < N; i++) { off.writeUInt32LE(p, i * 4); for (const v of adjL[i]) { adj.writeUInt16LE(v, p * 2); p++; } } off.writeUInt32LE(p, N * 4);
|
||||
const rm = { n: N, k: K, res: 1.5, fp: S.modelFingerprint(), q: qb.toString('base64'), off: off.toString('base64'), adj: adj.toString('base64'),
|
||||
generated: new Date().toISOString(), stats: { edges: ne, components: nc, largest: big } };
|
||||
const body = text.replace(/\s*\}\s*$/, '');
|
||||
fs.writeFileSync(COLF, body + ',\n "roadmap": ' + JSON.stringify(rm) + '\n}\n');
|
||||
console.log('roadmap written', rm.stats);
|
||||
})();
|
||||
Reference in New Issue
Block a user