ml: 数据生成 + 可微运动学对拍 + MeanFlow/WTA 策略与闭环仿真 + APG/挖错/ONNX 导出与运行手册
- M1 数据生成器：采样/老师/距离下界认证/时间律/统计/一致性门禁；确定性老师（碰撞次数计时 + resetSession）、pos 任务冻结 J6、--consensus 邻域共识过滤、Dr 类。 - 全流程代码与运行手册：数据打包/可微运动学与对拍/MeanFlow+WTA 模型/闭环仿真/APG/挖错循环/ONNX 导出；webapp 策略运行时 ik-policy.js、?policy=1 接入、tests/policy_bench.cjs；ik-solver 增 pairList/clusterExport/criticalPairs/pairGap/leverTable。
This commit is contained in:
Hark
+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/
|
||||
定位图/
|
||||
|
||||
@@ -431,7 +431,7 @@ MeanFlow
|
||||
<tbody>
|
||||
<tr class="odd">
|
||||
<td>模型规模</td>
|
||||
<td>约 4.1M 参数编码器-解码器 Transformer，不用 VLA 大模型</td>
|
||||
<td>约 4.9M 参数编码器-解码器 Transformer，不用 VLA 大模型</td>
|
||||
<td>A</td>
|
||||
<td>IMPACT 4.67M、300 Hz；π0 3B 单次预填充 46 ms（4090）</td>
|
||||
</tr>
|
||||
@@ -997,14 +997,16 @@ a_max 为待定参数，真机型号确定后设定），dt = 20 ms。</li>
|
||||
</ul></li>
|
||||
<li>每条路径约 40–50 个样本 → 约 200 万样本（以 <code>stats.json</code>
|
||||
为准）。</li>
|
||||
<li>协变量偏移对策：关节噪声增广（σ =
|
||||
1°，噪声状态由老师离线重标）；偏离状态的进一步覆盖靠 §7。</li>
|
||||
<li>协变量偏移对策：关节噪声增广（30% 样本，σ = 1°；噪声态须无干涉且间隙
|
||||
> 1 mm，标签为 8 步内线性收回老师轨迹的增量）；偏离状态的进一步覆盖靠
|
||||
§7。</li>
|
||||
</ol>
|
||||
<h3 id="46-耗时与存储">4.6 耗时与存储</h3>
|
||||
<ul>
|
||||
<li>路网 A* 每条数十毫秒，耗时主要在过滤认证；按 <code>solveDyn</code>
|
||||
0.7 s/条上限估算，沙箱 2 进程不超过 5 小时。</li>
|
||||
<li>200 万样本原始约 1 GB；Parquet 约 200 MB。</li>
|
||||
<li>200 万样本以 float32 <code>.npy</code> 存放约 1.5
|
||||
GB（训练时整体载入内存）。</li>
|
||||
</ul>
|
||||
<h3 id="47-检查点">4.7 检查点</h3>
|
||||
<ul class="task-list">
|
||||
@@ -1018,8 +1020,13 @@ type="checkbox" /><code>ml/data/stats.json</code>：类别、到达率、行程
|
||||
</ul>
|
||||
<hr />
|
||||
<h2 id="5-表示与预处理">5. 表示与预处理</h2>
|
||||
<p>文件 <code>ml/prep/build_dataset.py</code> →
|
||||
<code>train/val/test.parquet</code>、<code>norm.json</code>。</p>
|
||||
<p>文件
|
||||
<code>ml/prep/build_dataset.cjs</code>（Node，特征直接调用部署运行时
|
||||
<code>webapp/js/ik-policy.js</code> 的同一份实现）→
|
||||
<code>ml/data/samples/*.{X,Y,V,SG,TID}.npy</code> +
|
||||
<code>*.tasks.jsonl</code>；按任务 5% 划为验证集。PyTorch 侧特征
|
||||
<code>ml/common/features.py</code> 与 JS 逐项对拍（闭环仿真与 APG
|
||||
使用）。</p>
|
||||
<h3 id="51-输入">5.1 输入</h3>
|
||||
<table>
|
||||
<thead>
|
||||
@@ -1123,7 +1130,7 @@ type="checkbox" /><code>ml/data/stats.json</code>：类别、到达率、行程
|
||||
</tr>
|
||||
<tr class="odd">
|
||||
<td>参数量</td>
|
||||
<td>约 4.1M</td>
|
||||
<td>约 4.9M</td>
|
||||
</tr>
|
||||
</tbody>
|
||||
</table>
|
||||
@@ -1275,10 +1282,15 @@ Hz）与 20 ms 不整除及后台节流造成的抖动。</p>
|
||||
<h3 id="83-安全层唯一的硬保证">8.3 安全层（唯一的硬保证）</h3>
|
||||
<ul>
|
||||
<li><strong>距离下界认证</strong>：对一段运动 q_a →
|
||||
q_b，若两端到障碍的最小距离之和 d(q_a) + d(q_b) > Σ_j r_j·|Δθ_j|（r_j
|
||||
为每轴实测杠杆上界，J1 约 9.0 mm/°，六轴合计最大约 24.3
|
||||
mm/°），则整段安全；否则二分，直到全部子段通过或判不安全。臂×立柱/台架用胶囊距离；自碰撞对复用
|
||||
<code>pair2d</code> 杠杆界（J3 3.24、J4 2.54 mm/°）。外加关节限位。</li>
|
||||
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)。外加关节限位。先用离散
|
||||
<code>collideFree</code> 剔除明显干涉的候选；运行时每条候选最多 40
|
||||
次距离查询、每次推理安全层总预算 30 ms，超出按刹停处理。</li>
|
||||
<li>离散采样检查（现有 <code>pathFine</code>，1.5° +
|
||||
半格）只作快速预筛，不作保证依据。</li>
|
||||
<li><strong>可刹停不变式</strong>：只有当"候选前 3 步 + 从第 3
|
||||
@@ -1299,8 +1311,8 @@ ms，否则放 Web Worker 与推理流水线并行。</li>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr class="odd">
|
||||
<td><code>traj_policy.onnx</code>（fp16，约 8.3 MB）+
|
||||
<code>norm.json</code></td>
|
||||
<td><code>traj_policy.onnx</code>（fp16，约 9.8 MB）+
|
||||
<code>traj_policy.meta.json</code>（指纹 / 特征版本 / TCP）</td>
|
||||
<td>PRM 路网、IK 候选大表</td>
|
||||
</tr>
|
||||
<tr class="even">
|
||||
@@ -1545,8 +1557,18 @@ rt）</td>
|
||||
例到达但路径细验不过；§4.4 的认证过滤会丢弃这类样本。</li>
|
||||
<li><code>ci27</code> 姿态根枚举漏分支（姿态偏差约 97.5°）会影响 pose
|
||||
数据；APG 姿态项可部分纠正，必要时先修老师。</li>
|
||||
<li>胶囊模型或立柱变动即需重训：数据约 5 h + 预训练 1.5–5.6 h + 后训练
|
||||
2–4 h。</li>
|
||||
<li>胶囊模型或立柱变动即需重训：数据（用户机器 16 核约 2 h）+ 预训练
|
||||
1.5–6 h + 后训练数小时。</li>
|
||||
<li>老师一致性沙箱首测 78%（100 任务、373 对邻居，门禁
|
||||
90%）：不一致主要来自 J6
|
||||
冗余与时间预算造成的非确定性。已改为确定性老师（预算按碰撞判定次数计、每条前清空跨调用缓存）并在
|
||||
pos 任务冻结 J6，待在用户机器复测；仍不达标则生成时启用 4
|
||||
邻居共识过滤。</li>
|
||||
<li>安全层耗时：未训练模型（候选为噪声）在 Node
|
||||
单线程实测每次推理安全层约 20–60 ms，高于 5 ms
|
||||
目标；训练后候选平滑会下降，以 <code>policy_bench</code> 与浏览器
|
||||
<code>__POLICY.latency()</code> 实测为准，超标则把安全层移入 Web Worker
|
||||
与推理流水线并行。</li>
|
||||
</ol>
|
||||
<h3 id="103-需要用户确认">10.3 需要用户确认</h3>
|
||||
<ol type="1">
|
||||
@@ -1556,21 +1578,25 @@ a_max、插补频率与提交步数。</li>
|
||||
</ol>
|
||||
<hr />
|
||||
<h2 id="11-目录规划">11. 目录规划</h2>
|
||||
<p>运行步骤、命令与耗时见 <code>ml/README.md</code>。</p>
|
||||
<pre><code>ml/
|
||||
gen/ gen_dataset.cjs sampler.cjs run_parallel.sh
|
||||
data/ （不入库）shard_*.jsonl stats.json *.parquet norm.json
|
||||
prep/ build_dataset.py
|
||||
model/ policy_transformer.py # MeanFlow 一步头
|
||||
sim/ closed_loop.py # 闭环仿真（验证、挖错、APG 共用）
|
||||
eval/ collide_torch.py # 可微胶囊距离，与 JS 对拍
|
||||
apg/ apg_finetune.py
|
||||
mining/ mine_round.py gate.py loop.sh
|
||||
toy/ toy_bimodal.py # 2D 双模态对照实验
|
||||
configs/ base.yaml apg.yaml
|
||||
train.py export_onnx.py README.md
|
||||
webapp/js/ik-policy.js # 闭环推理 + 排序 + 安全认证 + 刹停
|
||||
webapp/models/traj_policy.onnx traj_policy.norm.json
|
||||
tests/policy_bench.cjs</code></pre>
|
||||
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 部署形态评测</code></pre>
|
||||
<h2 id="12-里程碑">12. 里程碑</h2>
|
||||
<table>
|
||||
<thead>
|
||||
@@ -1596,9 +1622,9 @@ tests/policy_bench.cjs</code></pre>
|
||||
</tr>
|
||||
<tr class="odd">
|
||||
<td>M3</td>
|
||||
<td>5 万条老师数据 → 约 200 万样本</td>
|
||||
<td>Parquet</td>
|
||||
<td>沙箱约 5 h 或用户机器</td>
|
||||
<td>5 万条老师数据 → 约 180 万样本</td>
|
||||
<td><code>samples/*.npy</code></td>
|
||||
<td>用户机器</td>
|
||||
</tr>
|
||||
<tr class="even">
|
||||
<td>M4</td>
|
||||
|
||||
+30
-23
@@ -25,7 +25,7 @@
|
||||
|
||||
| 模块 | 最终选择 | 证据等级 | 关键依据 |
|
||||
|---|---|---|---|
|
||||
| 模型规模 | 约 4.1M 参数编码器-解码器 Transformer，不用 VLA 大模型 | A | IMPACT 4.67M、300 Hz；π0 3B 单次预填充 46 ms（4090） |
|
||||
| 模型规模 | 约 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（位置式优于速度式） |
|
||||
@@ -195,12 +195,12 @@ pip install numpy pyarrow tqdm tensorboard onnx onnxruntime
|
||||
- 输入：§5.1 全部字段（含当前速度、已提交前缀）
|
||||
- 标签：未来 16 步 Δq_{t+k} = q_{t+k} − q_t（到终点后补零）
|
||||
3. 每条路径约 40–50 个样本 → 约 200 万样本（以 `stats.json` 为准）。
|
||||
4. 协变量偏移对策：关节噪声增广（σ = 1°，噪声状态由老师离线重标）；偏离状态的进一步覆盖靠 §7。
|
||||
4. 协变量偏移对策：关节噪声增广（30% 样本，σ = 1°；噪声态须无干涉且间隙 > 1 mm，标签为 8 步内线性收回老师轨迹的增量）；偏离状态的进一步覆盖靠 §7。
|
||||
|
||||
### 4.6 耗时与存储
|
||||
|
||||
- 路网 A* 每条数十毫秒，耗时主要在过滤认证；按 `solveDyn` 0.7 s/条上限估算，沙箱 2 进程不超过 5 小时。
|
||||
- 200 万样本原始约 1 GB；Parquet 约 200 MB。
|
||||
- 200 万样本以 float32 `.npy` 存放约 1.5 GB（训练时整体载入内存）。
|
||||
|
||||
### 4.7 检查点
|
||||
|
||||
@@ -213,7 +213,7 @@ pip install numpy pyarrow tqdm tensorboard onnx onnxruntime
|
||||
|
||||
## 5. 表示与预处理
|
||||
|
||||
文件 `ml/prep/build_dataset.py` → `train/val/test.parquet`、`norm.json`。
|
||||
文件 `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 输入
|
||||
|
||||
@@ -262,7 +262,7 @@ J1/J3/J6 统一用 `unwrapTo` 展开后再做增量；预处理与部署共用
|
||||
|---|---|
|
||||
| d_model / 头数 / FFN | 192 / 8 / 768 |
|
||||
| encoder / decoder 层数 | 4 / 4 |
|
||||
| 参数量 | 约 4.1M |
|
||||
| 参数量 | 约 4.9M |
|
||||
|
||||
子目标触发：新目标或目标跳变 > 50 mm 时输出；到达当前子目标 < 5° 后切下一个。
|
||||
|
||||
@@ -364,7 +364,7 @@ for 每个 batch（随机采样 q0 和目标，不需要老师）:
|
||||
|
||||
### 8.3 安全层（唯一的硬保证）
|
||||
|
||||
- **距离下界认证**：对一段运动 q_a → q_b，若两端到障碍的最小距离之和 d(q_a) + d(q_b) > Σ_j r_j·|Δθ_j|（r_j 为每轴实测杠杆上界，J1 约 9.0 mm/°，六轴合计最大约 24.3 mm/°），则整段安全；否则二分，直到全部子段通过或判不安全。臂×立柱/台架用胶囊距离；自碰撞对复用 `pair2d` 杠杆界（J3 3.24、J4 2.54 mm/°）。外加关节限位。
|
||||
- **距离下界认证**：对一段运动 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 与推理流水线并行。
|
||||
@@ -374,7 +374,7 @@ for 每个 batch（随机采样 q0 和目标，不需要老师）:
|
||||
|
||||
| 有 | 没有 |
|
||||
|---|---|
|
||||
| `traj_policy.onnx`（fp16，约 8.3 MB）+ `norm.json` | PRM 路网、IK 候选大表 |
|
||||
| `traj_policy.onnx`（fp16，约 9.8 MB）+ `traj_policy.meta.json`（指纹 / 特征版本 / TCP） | PRM 路网、IK 候选大表 |
|
||||
| 胶囊距离与认证（`collideFree`、包围球预筛、`pair2d`） | `solveDyn` / `routeRT` / RRT / A* |
|
||||
| `fk` / `orient`、局部牛顿精修 `solveTo` 单步 | 全量 IK 枚举 |
|
||||
|
||||
@@ -454,7 +454,9 @@ for 每个 batch（随机采样 q0 和目标，不需要老师）:
|
||||
|
||||
1. 现有规划器（老师）在 `tests/route_bench.cjs` 上有 1/40 例到达但路径细验不过；§4.4 的认证过滤会丢弃这类样本。
|
||||
2. `ci27` 姿态根枚举漏分支（姿态偏差约 97.5°）会影响 pose 数据；APG 姿态项可部分纠正，必要时先修老师。
|
||||
3. 胶囊模型或立柱变动即需重训：数据约 5 h + 预训练 1.5–5.6 h + 后训练 2–4 h。
|
||||
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 需要用户确认
|
||||
|
||||
@@ -465,22 +467,27 @@ for 每个 batch（随机采样 q0 和目标，不需要老师）:
|
||||
|
||||
## 11. 目录规划
|
||||
|
||||
运行步骤、命令与耗时见 `ml/README.md`。
|
||||
|
||||
```
|
||||
ml/
|
||||
gen/ gen_dataset.cjs sampler.cjs run_parallel.sh
|
||||
data/ （不入库）shard_*.jsonl stats.json *.parquet norm.json
|
||||
prep/ build_dataset.py
|
||||
model/ policy_transformer.py # MeanFlow 一步头
|
||||
sim/ closed_loop.py # 闭环仿真（验证、挖错、APG 共用）
|
||||
eval/ collide_torch.py # 可微胶囊距离，与 JS 对拍
|
||||
apg/ apg_finetune.py
|
||||
mining/ mine_round.py gate.py loop.sh
|
||||
toy/ toy_bimodal.py # 2D 双模态对照实验
|
||||
configs/ base.yaml apg.yaml
|
||||
train.py export_onnx.py README.md
|
||||
webapp/js/ik-policy.js # 闭环推理 + 排序 + 安全认证 + 刹停
|
||||
webapp/models/traj_policy.onnx traj_policy.norm.json
|
||||
tests/policy_bench.cjs
|
||||
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. 里程碑
|
||||
@@ -489,7 +496,7 @@ tests/policy_bench.cjs
|
||||
|---|---|---|---|
|
||||
| M1 | 数据生成器 + 时间律 + 1000 条试跑；老师一致性门禁 | `ml/gen/*`、stats.json | 沙箱 |
|
||||
| M2 | 可微胶囊距离（PyTorch）与 JS 对拍；距离认证实现与 10⁵ 段审计 | `collide_torch.py`、认证模块 | 沙箱 |
|
||||
| M3 | 5 万条老师数据 → 约 200 万样本 | Parquet | 沙箱约 5 h 或用户机器 |
|
||||
| M3 | 5 万条老师数据 → 约 180 万样本 | `samples/*.npy` | 用户机器 |
|
||||
| M4 | 模型 + 训练工程 + 闭环仿真；小数据过拟合；**导出 ONNX 测浏览器延迟** | `train.py`、`closed_loop.py`、延迟报告 | 沙箱 + 浏览器 |
|
||||
| M5 | 预训练 + MeanFlow 头门禁 | checkpoint、门禁报告 | 用户 GPU |
|
||||
| M6 | APG 微调 + 挖错循环 3–6 轮 | 每轮 report.md | 用户 GPU |
|
||||
|
||||
+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()
|
||||
+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,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);
|
||||
})();
|
||||
@@ -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>
|
||||
|
||||
@@ -708,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(),
|
||||
@@ -746,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），不再把手势直接映射进立柱里。 */
|
||||
@@ -1171,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 };
|
||||
}));
|
||||
+132
-4
@@ -368,11 +368,12 @@ function make(cfg) {
|
||||
}
|
||||
/* 逐对扫描（簇级宽相位 → 胶囊线段距离）。visit(cnA,cnB,d,need) 对每个距离
|
||||
< need 的胶囊对回调；无回调（collideFree）时首对即短路返回。 */
|
||||
let GAPCAP = 20;
|
||||
function scanPairs(q, visit, onlyCls, gapCb) {
|
||||
if (!COL) return false;
|
||||
const { R: Rs, T: Ts } = cumXf(q);
|
||||
/* 两关节类查表（只在纯判定时用；明细/生成表走精算） */
|
||||
const INF = gapCb ? 20 : 0; // 生成表：宽相位外扩 20 mm，表内 <20 mm 的间隙为精确值
|
||||
const INF = gapCb ? GAPCAP : 0; // 生成表：宽相位外扩 GAPCAP（默认 20 mm），表内 <GAPCAP 的间隙为精确值
|
||||
if (!visit) for (const k in P2D) {
|
||||
const T = P2D[k], st = p2dStatus(T, q);
|
||||
if (st < 0) return true;
|
||||
@@ -445,7 +446,7 @@ function make(cfg) {
|
||||
if (mx * mx + my * my + mz * mz > (bl + INF) * (bl + INF)) continue;
|
||||
const d = segSegDistRaw(e1[0], e1[1], e1[2], e1[3], e1[4], e1[5],
|
||||
e2[0], e2[1], e2[2], e2[3], e2[4], e2[5]);
|
||||
if (gapCb) { gapCb(d - need); continue; }
|
||||
if (gapCb) { gapCb(d - need, pr[n][0], pr[n][1]); continue; }
|
||||
if (d < need) {
|
||||
hit = true;
|
||||
if (visit) visit(e1[6], e2[6], d, need);
|
||||
@@ -462,6 +463,129 @@ function make(cfg) {
|
||||
}
|
||||
return hit;
|
||||
}
|
||||
/* 胶囊对清单 [[i, j, need]]（与 scanPairs 判定的对完全一致；ml/ 导出给 PyTorch 可微版用） */
|
||||
function pairList() {
|
||||
const out = [];
|
||||
for (const [, , cl] of PARTPAIR) for (const c of cl) for (const pr of c[2]) out.push([pr[0], pr[1], pr[2]]);
|
||||
return out;
|
||||
}
|
||||
/* 宽相位结构导出（ml/ PyTorch 版复刻同一套簇球筛选，保证 min(gap, cap) 与 JS 逐位同口径）：
|
||||
clusters[k] = {g, c(局部簇心), rad, rmax, caps}；cpairs = [[ka, kb, rsum, rmaxS]]，簇对内胶囊对取全组合，need = r_i + r_j + CAPM */
|
||||
function clusterExport() {
|
||||
const clusters = [], base = new Map();
|
||||
for (const pt of PART) { base.set(pt, clusters.length); for (const c of pt.clus) clusters.push({ g: pt.g, c: c.c.slice(), rad: c.rad, rmax: c.rmax, caps: c.caps.slice() }); }
|
||||
const cpairs = [];
|
||||
for (const [A, B, cl] of PARTPAIR) for (const c of cl) cpairs.push([base.get(A) + c[0], base.get(B) + c[1], c[3], c[4]]);
|
||||
return { clusters: clusters, cpairs: cpairs, capm: CAPM };
|
||||
}
|
||||
/* 间隙 < thr 的胶囊对 [[i, j, gap]]，按 gap 升序（安全层投影用：只对这几对做有限差分） */
|
||||
function criticalPairs(q, thr) {
|
||||
const out = [];
|
||||
if (!COL) return out;
|
||||
const save = GAPCAP; GAPCAP = thr;
|
||||
try { scanPairs(q, () => {}, undefined, (g, i, j) => { if (g < thr) out.push([i, j, g]); }); } finally { GAPCAP = save; }
|
||||
out.sort((x, y) => x[2] - y[2]);
|
||||
return out;
|
||||
}
|
||||
/* 单个胶囊对的间隙（距离 − need），只变换两根胶囊，微秒级 */
|
||||
function pairGap(q, i, j) {
|
||||
const { R: Rs, T: Ts } = cumXf(q);
|
||||
const tf = (c, e) => { const R = Rs[c.g], T = Ts[c.g]; return [R[0] * e[0] + R[1] * e[1] + R[2] * e[2] + T[0], R[3] * e[0] + R[4] * e[1] + R[5] * e[2] + T[1], R[6] * e[0] + R[7] * e[1] + R[8] * e[2] + T[2]]; };
|
||||
const A = CAPG[i], B = CAPG[j];
|
||||
const a0 = tf(A, A.a), a1 = tf(A, A.b), b0 = tf(B, B.a), b1 = tf(B, B.b);
|
||||
return segSegDistRaw(a0[0], a0[1], a0[2], a1[0], a1[1], a1[2], b0[0], b0[1], b0[2], b1[0], b1[1], b1[2]) - (A.r + B.r + CAPM);
|
||||
}
|
||||
function setDeterministic(on, msPerCheck) { VCLK = on ? { base: 0, per: msPerCheck || 0.5, tick: 0 } : null; }
|
||||
/* 清空所有跨调用状态（路网增广、坏边/死边/已验段缓存、实时路径、全量 IK 缓存），使下一次求解与历史无关 */
|
||||
function resetSession() {
|
||||
RMX.Q.length = 0; RMX.adj.length = 0; RMX.adjR = null;
|
||||
BADE.clear(); DEAD.clear(); FINE.clear();
|
||||
RT = null; RTSTALL.key = null; RTSTALL.n = 0;
|
||||
FULLIK.key = null; FULLIK.cur = null; FULLIK.T = null; FULLIK.at = 0;
|
||||
}
|
||||
/* 全胶囊对最小间隙（mm，= 距离 − need，负值为干涉；≥cap 一律记 cap，下界、保守）（2026-10-17，
|
||||
ml/gen 距离下界认证用）：不走 pair2d 查表、不提前退出，比 collideFree 慢，只给离线/安全层用。 */
|
||||
function clearance(q, cap) {
|
||||
if (!COL) return Infinity;
|
||||
cap = cap || 20;
|
||||
let best = cap; const save = GAPCAP; GAPCAP = cap;
|
||||
try { scanPairs(q, () => {}, undefined, g => { if (g < best) best = g; }); } finally { GAPCAP = save; }
|
||||
return best;
|
||||
}
|
||||
/* 按组对的最小间隙表（7×7 扁平，[gA*7+gB]，gA<gB；未进入窄相位的对记 cap）。
|
||||
组对 (gA,gB) 的相对位姿只受关节 gA..gB−1 影响，认证时只用这几轴的杠杆和。 */
|
||||
function gapMatrix(q, cap) {
|
||||
cap = cap || 20;
|
||||
const M = new Float64Array(49).fill(cap);
|
||||
if (!COL) return M;
|
||||
const save = GAPCAP; GAPCAP = cap;
|
||||
try {
|
||||
scanPairs(q, () => {}, undefined, (g, i, j) => {
|
||||
let a = CAPG[i].g, b = CAPG[j].g; if (a > b) { const t = a; a = b; b = t; }
|
||||
if (g < M[a * 7 + b]) M[a * 7 + b] = g;
|
||||
});
|
||||
} finally { GAPCAP = save; }
|
||||
return M;
|
||||
}
|
||||
/* 每轴杠杆上界 r_j（mm/°）：轴 j 转 1° 时，j 之后任一胶囊端点相对 j 之前部件的最大位移。
|
||||
随机采样 n 个位形取最大值再放大 (1+pad)。胶囊间距对端点位移 1-Lipschitz，故
|
||||
Σ r_j|Δθ_j| 是关节直线段上任意胶囊对间隙变化量的上界（用于 d(a)+d(b) > Σ r_j|Δθ_j| 认证）。 */
|
||||
/* 分组杠杆表 T[k][g]（mm/°）：组 g 的胶囊端点到轴 k 的最大距离（g > k），比 leverBounds 的"全部远端"上界紧：
|
||||
组对 (gA,gB) 的相对运动只由关节 gA..gB−1 带动组 gB 产生，故 Σ_k T[k][gB]·|Δθ_k| 是该组对间隙变化上界。 */
|
||||
function leverTable(n, pad, seed) {
|
||||
n = n || 4000; pad = pad === undefined ? 0.05 : pad;
|
||||
let st = (seed || 7) >>> 0; const rnd = () => { st = (st * 1664525 + 1013904223) >>> 0; return st / 4294967296; };
|
||||
const T = [0, 1, 2, 3, 4, 5].map(() => [0, 0, 0, 0, 0, 0, 0]);
|
||||
for (let t = 0; t < n; t++) {
|
||||
const q = ORDER.map(id => { const L = LIM[id]; return L.cont ? -180 + 360 * rnd() : L.lo + (L.hi - L.lo) * rnd(); });
|
||||
const { R: Rs, T: Ts } = cumXf(q);
|
||||
for (let j = 0; j < 6; j++) {
|
||||
const Rj = Rs[j], tj = Ts[j], Pj = P[ORDER[j]], Dj = D[ORDER[j]];
|
||||
const o = [Rj[0] * Pj[0] + Rj[1] * Pj[1] + Rj[2] * Pj[2] + tj[0], Rj[3] * Pj[0] + Rj[4] * Pj[1] + Rj[5] * Pj[2] + tj[1], Rj[6] * Pj[0] + Rj[7] * Pj[1] + Rj[8] * Pj[2] + tj[2]];
|
||||
const u = [Rj[0] * Dj[0] + Rj[1] * Dj[1] + Rj[2] * Dj[2], Rj[3] * Dj[0] + Rj[4] * Dj[1] + Rj[5] * Dj[2], Rj[6] * Dj[0] + Rj[7] * Dj[1] + Rj[8] * Dj[2]];
|
||||
const un = Math.hypot(u[0], u[1], u[2]);
|
||||
for (const c of CAPG) {
|
||||
if (c.g < j + 1) continue;
|
||||
const R = Rs[c.g], Tt = Ts[c.g];
|
||||
for (const e of [c.a, c.b]) {
|
||||
const w = [R[0] * e[0] + R[1] * e[1] + R[2] * e[2] + Tt[0] - o[0], R[3] * e[0] + R[4] * e[1] + R[5] * e[2] + Tt[1] - o[1], R[6] * e[0] + R[7] * e[1] + R[8] * e[2] + Tt[2] - o[2]];
|
||||
const dp = (w[0] * u[0] + w[1] * u[1] + w[2] * u[2]) / un;
|
||||
const dd = Math.sqrt(Math.max(0, w[0] * w[0] + w[1] * w[1] + w[2] * w[2] - dp * dp));
|
||||
if (dd > T[j][c.g]) T[j][c.g] = dd;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
/* 组 g 的端点距离是该组上界；但组对 (gA,gB) 里远端组 gB 之外，gB 与 gA 之间的组不参与该对，
|
||||
而 gB 自身被 j<gB 的所有关节带动——直接取 T[j][gB] 即可 */
|
||||
return T.map(row => row.map(v => v * D2R * (1 + pad)));
|
||||
}
|
||||
function leverBounds(n, pad, seed) {
|
||||
n = n || 4000; pad = pad === undefined ? 0.05 : pad;
|
||||
let st = (seed || 7) >>> 0; const rnd = () => { st = (st * 1664525 + 1013904223) >>> 0; return st / 4294967296; };
|
||||
const r = [0, 0, 0, 0, 0, 0];
|
||||
for (let t = 0; t < n; t++) {
|
||||
const q = ORDER.map(id => { const L = LIM[id]; return L.cont ? -180 + 360 * rnd() : L.lo + (L.hi - L.lo) * rnd(); });
|
||||
const { R: Rs, T: Ts } = cumXf(q);
|
||||
for (let j = 0; j < 6; j++) {
|
||||
const Rj = Rs[j], tj = Ts[j], Pj = P[ORDER[j]], Dj = D[ORDER[j]];
|
||||
const o = [Rj[0] * Pj[0] + Rj[1] * Pj[1] + Rj[2] * Pj[2] + tj[0], Rj[3] * Pj[0] + Rj[4] * Pj[1] + Rj[5] * Pj[2] + tj[1], Rj[6] * Pj[0] + Rj[7] * Pj[1] + Rj[8] * Pj[2] + tj[2]];
|
||||
const u = [Rj[0] * Dj[0] + Rj[1] * Dj[1] + Rj[2] * Dj[2], Rj[3] * Dj[0] + Rj[4] * Dj[1] + Rj[5] * Dj[2], Rj[6] * Dj[0] + Rj[7] * Dj[1] + Rj[8] * Dj[2]];
|
||||
const un = Math.hypot(u[0], u[1], u[2]);
|
||||
for (const c of CAPG) {
|
||||
if (c.g < j + 1) continue;
|
||||
const R = Rs[c.g], T = Ts[c.g];
|
||||
for (const e of [c.a, c.b]) {
|
||||
const w = [R[0] * e[0] + R[1] * e[1] + R[2] * e[2] + T[0] - o[0], R[3] * e[0] + R[4] * e[1] + R[5] * e[2] + T[1] - o[1], R[6] * e[0] + R[7] * e[1] + R[8] * e[2] + T[2] - o[2]];
|
||||
const dp = (w[0] * u[0] + w[1] * u[1] + w[2] * u[2]) / un;
|
||||
const dd = Math.sqrt(Math.max(0, w[0] * w[0] + w[1] * w[1] + w[2] * w[2] - dp * dp));
|
||||
if (dd > r[j]) r[j] = dd;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return r.map(v => v * D2R * (1 + pad));
|
||||
}
|
||||
/* 全姿态动态碰撞判定：True = 无动态干涉（模型近似，含 margin 外扩） */
|
||||
const PST = { n: 0 };
|
||||
/* 单次求解内的判定记忆：routeFrom / gatherGoals / 规划器会对同一批 IK 候选反复判定 */
|
||||
@@ -715,7 +839,11 @@ function make(cfg) {
|
||||
for (let i = 0; i < 6; i++) { const w = W ? W[i] : 1; t += Math.abs(wrap180(q[i] - ref[i])) * w; }
|
||||
return t + 1e-6 * c;
|
||||
};
|
||||
const nowMs = () => (typeof performance !== 'undefined' ? performance.now() : Date.now());
|
||||
/* 确定性时钟（2026-10-17，ml/gen 离线老师用）：开启后各处毫秒预算改按"碰撞判定次数 × msPerCheck"计，
|
||||
另每次读时钟 +1 µs 防纯计算循环不前进。规划结果只取决于输入，不随机器快慢/负载变化。默认关闭。 */
|
||||
let VCLK = null;
|
||||
const nowMs = () => VCLK ? VCLK.base + PST.n * VCLK.per + (VCLK.tick += 0.001)
|
||||
: (typeof performance !== 'undefined' ? performance.now() : Date.now());
|
||||
|
||||
/* 姿态求解原语工厂：位置闭式支把 θ1..θ3 吸收掉之后，姿态任务只剩「工具轴指向 =
|
||||
目标轴」这 2 个方程、2 个未知数 (θ4,θ5)，解是孤立点，滚转再由 θ6 闭式补上。
|
||||
@@ -3109,7 +3237,7 @@ function make(cfg) {
|
||||
solveRoute: solveRoute, routeFrom: routeFrom, transitPt: transitPt, planAround: planAround, edgeFree: edgeFree, PST: PST, unwrapTo: unwrapTo, rtWarm: rtWarm, rtReset: () => { RT = null; }, rmPlan: rmPlan, rmSmooth: rmSmooth, modelFingerprint: modelFingerprint, RM: RM, wd: wd, classGap: classGap, classFingerprint: classFingerprint, P2D: P2D,
|
||||
mixSolve: mixSolve, closest: closest, closest6: closest6, closestOri: closestOri, orient: orient,
|
||||
branch: branch, selfTest: selfTest, poseSelfTest: poseSelfTest,
|
||||
collideFree: collideFree, collideDetail: collideDetail,
|
||||
collideFree: collideFree, collideDetail: collideDetail, clearance: clearance, gapMatrix: gapMatrix, leverBounds: leverBounds, leverTable: leverTable, setDeterministic: setDeterministic, resetSession: resetSession, pairList: pairList, clusterExport: clusterExport, criticalPairs: criticalPairs, pairGap: pairGap, TCP0: TCP0, CAPM: CAPM,
|
||||
dynLimit: dynLimit, dynLimits: dynLimits,
|
||||
K: K, LIM: LIM, P: P, D: D, ORDER: ORDER, wrap180: wrap180, costOf: costOf };
|
||||
}
|
||||
|
||||
@@ -0,0 +1,5 @@
|
||||
策略模型放这里（由 `python -m ml.export_onnx --fp16` 生成）：
|
||||
- `traj_policy.onnx`
|
||||
- `traj_policy.meta.json`（碰撞模型指纹 / 特征版本 / TCP，查看器加载时校验）
|
||||
|
||||
查看器地址加 `?policy=1` 启用策略模式，详见 `ml/README.md` 第 8 步。
|
||||
Reference in New Issue
Block a user