ml/gen: M1 数据生成器（采样/老师/距离下界认证/时间律/统计/一致性门禁）；ik-solver 增 clearance/gapMatrix/leverBounds

ml/gen/cert.cjs

@@ -0,0 +1,43 @@
+/* 距离下界认证（2026-10-17，方案 §8.3 / §4.4）
+ * 关节直线段 a→b 安全 ⇔ 对每个组对 (gA,gB)（gB ≥ gA+2）：
+ *     gap_ab(a) + gap_ab(b) > Σ_{k=gA}^{gB-1} r_k·|Δθ_k|
+ * gap 取 S.gapMatrix（距离 − need，含 margin），r_k 为 S.leverBounds 的每轴杠杆上界（mm/°）。
+ * 不满足就二分，子段 < MIN_DEG 仍不过则判不安全。连续轴按 unwrapTo 口径展开后再插值。 */
+'use strict';
+module.exports = function makeCert(S, opt) {
+  opt = opt || {};
+  const CAP = opt.cap || 40, MIN_DEG = opt.minDeg || 0.002;
+  const R = opt.lever || S.leverBounds(4000);
+  const L = S.LIM, O = S.ORDER, w = S.wrap180;
+  const memo = new Map();
+  const key = q => q.map(v => v.toFixed(5)).join(',');
+  let queries = 0;
+  const gm = q => { const k = key(q); let M = memo.get(k); if (!M) { M = S.gapMatrix(q, CAP); queries++; 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) {
+    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 += R[k] * 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} */
+  function path(pts) {
+    const q0 = queries; let mg = 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 + w(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' }; 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: 'cert' };
+    return { ok: true, queries: queries - q0, minGap: mg, unwrapped: P };
+  }
+  return { seg: (a, b) => seg(a, b, 0), path, lever: R, gapMin: q => minGap(gm(q)) };
+};

ml/gen/consistency.cjs

@@ -0,0 +1,45 @@
+/* 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 { 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;
+  tasks++;
+  const again = teach(t); if (again.ok) { detN++; if (dev(base.pts, again.pts) < 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, r.pts), d = Math.max(...da); devs.push(d); pairs++;
+    if (Math.max(...da.slice(0, 5)) < 30) 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)])),
+  consistentIgnoringJ6: +(sameNoJ6 / pairs).toFixed(3), worstAxisOfInconsistent: axisHist, deterministic: detSame + '/' + detN }));

ml/gen/gen_dataset.cjs

@@ -0,0 +1,39 @@
+/* M1 数据生成器（2026-10-17，方案 §4）
+ *   node ml/gen/gen_dataset.cjs --n 500 --seed 1 --out ml/data/shard_0001.jsonl
+ * 每行一条任务：采样 → 老师 → 距离下界认证过滤 → 局部时间律（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 LEVER = S.leverBounds(4000);
+const cert = require('./cert.cjs')(S, { lever: LEVER });
+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 < N; i++) {
+  const task = 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();
+  const c = cert.path(t.pts);
+  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 (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}/${N}  ${((performance.now() - t00) / 1000 / (i + 1)).toFixed(2)} s/条\n`);
+}
+fs.closeSync(fd);
+process.stderr.write(`[seed ${SEED}] done ${N} in ${((performance.now() - t00) / 1000).toFixed(0)} s\n`);

ml/gen/run_parallel.sh

@@ -0,0 +1,11 @@
+#!/usr/bin/env bash
+# 并行生成：bash ml/gen/run_parallel.sh <每进程条数> <进程数> [起始种子=1]
+# 例：8 核机器 5 万条 → bash ml/gen/run_parallel.sh 6250 8
+set -e
+N=${1:-500}; P=${2:-2}; S0=${3:-1}
+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 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

ml/gen/sampler.cjs

@@ -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 };
+};

ml/gen/stats.cjs

@@ -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 };
+  b.n++; if (r.reached) b.reached++; if (r.cert && !r.cert.ok) b.certFail++; if (r.demoted) b.demoted++;
+  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));

ml/gen/teacher.cjs

@@ -0,0 +1,23 @@
+/* 老师（方案 §4.3）：solveDyn 松手口径——有离线路网时内部走 PRM 路网 Lazy A* + 捷径平滑（种子由起点决定，确定性），
+ * 路网失败再落到 RRT-Connect。返回展开后的折线与到达判据。 */
+'use strict';
+module.exports = function makeTeacher(S, ArmIK) {
+  const L = S.LIM, O = S.ORDER, w = S.wrap180;
+  return function teach(task) {
+    const opt = { tool: 0, step: 0.5, ref: task.q0, from: task.q0.slice() };
+    if (task.mode === 'pose') { opt.pose = { R: task.R }; opt.priority = 'pos'; }
+    if (S.rtReset) S.rtReset();
+    const t0 = performance.now();
+    const r = S.solveDyn(task.T, opt);
+    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;
+    let travel = 0; for (let i = 1; i < pts.length; i++) for (let k = 0; k < 6; k++) travel += Math.abs(pts[i][k] - pts[i - 1][k]);
+    return { ok: true, pts, reached, err, oerr, travel, ms, blocked: !!r.blocked, poseRelaxed: !!r.poseRelaxed, unsafe: !!r.unsafe, detour: r.detour || null };
+  };
+};

ml/gen/timelaw.cjs

@@ -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()]; 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));
+  }
+  return { traj: out, total, steps: out.length - 1 };
+}
+module.exports = { timeParam, VMAX, A, DT };