Robot_Arm/tests/policy_bench.cjs

/* 部署形态评测（方案 §9）：策略 + 安全层（webapp/js/ik-policy.js 的 Runner，与浏览器同一份代码）
 *   node tests/policy_bench.cjs [model=webapp/models/traj_policy.onnx] [sets=nc,route,r8] [--nc1000] [--nc-n 30] [--route-n 40] [--r8-n 50] [--out file.json]
 * 集合：nc    = near_column_bench 同款 30 个贴立柱任务（pos 与 pose 各跑一遍）
 *       route = route_bench 同款 40 个直线被挡任务（pos）
 *       nc1000= 一次性扩展贴立柱集 1000 例（新种子，验收用；加 --nc1000 开启）
 *       r8    = 方案 R8 对抗集 50 例：J1 起终差 ≥ 140°、直线被挡、终点贴立柱（两侧都要绕远）；
 *               恢复 = 首次换向锁后 3 s 内到达（无锁则按到达计）；活锁 = 超时未到达且锁 ≥ 2 次
 * 2026-10-18：--n 拆成 --nc-n / --route-n / --r8-n；输出 brakeRate（刹停次数 / 推理次数，验收 < 2%）。
 * 判据：到达（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 VALOPT = new Set(['--out', '--nc-n', '--route-n', '--r8-n']);
const pos = argv.filter((a, i) => !a.startsWith('--') && !(i > 0 && VALOPT.has(argv[i - 1])));
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 r8Cases(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;
    for (let t = 0; t < 200; t++) {
      const a = rq(); if (!S.collideFree(a) || !nearColumn(a)) continue;   // 2026-10-18：起点也贴柱 → 两侧都要绕远（方案 R8 口径）
      if (Math.abs(d(a, b)[0]) < 140 || segOK(a, b)) continue;
      cases.push({ q0: a, T: S.fk(b, 0), R: null }); break;
    }
  }
  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, plans = 0, recovered = 0, lockTasks = 0, livelock = 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, tLock = null;
      for (let k = 0; k < 500 && !r.done; k++) {
        if (r.needPlan()) await r.plan();
        if (tLock == null && r.stats.locks > 0) tLock = r.t;
        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; plans += r.stats.plans;
      if (tLock != null) { lockTasks++; if (rc && !cf && r.t - tLock <= 3.0) recovered++; } else if (rc && !cf) recovered++;
      if (!rc && r.stats.locks >= 2) livelock++;
      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,
      plans, brakeRate: plans ? +(brakes / plans).toFixed(4) : null, lockTasks, recovered3s: recovered, recoveryRate: cases.length ? +(recovered / cases.length).toFixed(4) : null, livelock,
      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('--nc-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('--route-n') || 40), 'pos');
  if (SETS.includes('r8')) await runSet('r8_adversarial', r8Cases(+opt('--r8-n') || 50, 8888), '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);
})();