Robot_Arm/tests/ik_baseline.mjs

/**
 * IK 数值基线探针（Node，不需要浏览器）。
 *
 *   node tests/ik_baseline.mjs                          生成基线到 stdout
 *   node tests/ik_baseline.mjs --out scratch/_gate/ik.json
 *   node tests/ik_baseline.mjs --compare scratch/_gate/ik.json   重构后跑这条，FAIL 就退出码 1
 *   node tests/ik_baseline.mjs --solver web/arm-ik-solver.js     指定求解器文件
 *
 * 输出分两段，因为它们的判定方式不同：
 *   exact  —— 件数/顶点数、TCP 推断、限位、固定 FK、固定目标的解向量、selfTest 误差与命中数。
 *            这些必须逐字节相同；用定种子 RNG 保证可复现（selfTest 内部原本走 Math.random）。
 *   timing —— solveDyn 中位耗时。机器负载会变，只按 ±10% 报 WARN，不作 FAIL。
 *
 * 求解器配置构造与查看器里的 ik-glue 逐行对齐（JMap/COL/TR/TCP0），
 * 否则两边算的不是同一个问题，对比无意义。
 */
import fs from 'fs';
import path from 'path';
import { pathToFileURL } from 'url';

const ROOT = path.resolve(import.meta.dirname, '..');
const first = (...cands) => cands.find(p => fs.existsSync(p));

const argv = process.argv.slice(2);
const argVal = (name, dflt) => {
  const i = argv.indexOf('--' + name);
  return i >= 0 && argv[i + 1] && !argv[i + 1].startsWith('--') ? argv[i + 1] : dflt;
};

const SOLVER = argVal('solver', first(
  path.join(ROOT, 'webapp/js/ik/solver.js'),
  path.join(ROOT, 'web/arm-ik-solver.js')));
const MESH = first(path.join(ROOT, 'webapp/data/mesh_arm.json'),
                   path.join(ROOT, 'web/mesh_arm.json'));
const TRAVEL = path.join(ROOT, 'docs/travel.json');
const COLLIDE = path.join(ROOT, 'docs/collide.json');

const mod = await import(pathToFileURL(SOLVER).href);
const ArmIK = mod.ArmIK || mod.default;
if (!ArmIK) throw new Error('求解器没有导出 ArmIK: ' + SOLVER);

const mesh = JSON.parse(fs.readFileSync(MESH, 'utf8'));
const travel = JSON.parse(fs.readFileSync(TRAVEL, 'utf8'));
const collide = fs.existsSync(COLLIDE) ? JSON.parse(fs.readFileSync(COLLIDE, 'utf8')) : null;

/* ---- 与 ik-glue.js:16-52 同构的配置 ---- */
const JO = mesh.joints;
const JJ = {}; for (const j of JO) JJ[j.id] = j;
const JMap = {}; for (const j of JO) JMap[j.id] = { rear: j.rear, d: j.d };
const COL = (collide && collide.caps && collide.caps.length) ? collide : null;
const ADJ = (COL && COL.adj) || {};
const MOTOR = +((COL && COL.motor_limit_deg) || 360);
const TR = {}; const noAdj = [];
for (const t of travel) {
  const a = ADJ[t.axis];
  if (!a && !t.continuous) noAdj.push(t.axis);
  TR[t.axis] = { lo: a ? a[0] : t.lo, hi: a ? a[1] : t.hi, continuous: !!t.continuous };
}
/* TCP 高度：J6 组内实打印件（排除 ENV_）顶点的最小查看器 y，查看器 y = CAD z。
   与 ik-glue.js:17-27 同语义：只有真找到才覆盖默认值 93，不是拿 93 当下界。 */
let tcpZ = 93, loFound = Infinity;
for (const p of mesh.parts) {
  if (p.grp !== 'J6' || !p.printed || String(p.name).slice(0, 4) === 'ENV_') continue;
  if (!p.v || !p.v.length) continue;
  for (const q of p.v) loFound = Math.min(loFound, q[2]);
}
if (isFinite(loFound)) tcpZ = loFound;
const TCP0 = [JJ.J6.rear[0], JJ.J6.rear[1], tcpZ];

const S = ArmIK.make({ joints: JMap, travel: TR, tcp: TCP0, collide: COL });
const ORDER = ArmIK.ORDER;

/* selfTest/poseSelfTest 内部用 Math.random()，不锁种子的话两次跑的误差数字就不同，
   关卡会退化成"看大概"。这里换成定LCG，让 exact 段逐字节可复现。 */
const _rand = Math.random;
function seeded(seed, fn) {
  let s = seed >>> 0;
  Math.random = () => { s = (s * 1664525 + 1013904223) >>> 0; return s / 4294967296; };
  try { return fn(); } finally { Math.random = _rand; }
}

const parts = mesh.parts;
const r6 = v => +v.toFixed(6);

/* ---- 固定输入（不含随机）的可复现输出 ---- */
const Q0 = [10, -20, 30, 40, -50, 15];
const zeros = ORDER.map(() => 0);
const targets = [[150, 128, 260], [180, 128, 300], [120, -150, 320], [60, 128, 420]];

const solveRuns = targets.map(T => {
  const xs = [];
  for (let k = 0; k < 9; k++) {
    const t0 = performance.now();
    const r = S.solveDyn(T.slice(), { tool: 0, step: 0.25, ref: zeros, from: zeros.slice() });
    xs.push({ ms: performance.now() - t0, q: r && r.q ? r.q.map(v => +v.toFixed(3)) : null,
              ok: !!(r && r.q), err: r && r.err != null ? +r.err.toFixed(9) : null });
  }
  /* 头 3 次是 JIT 预热，留中位数也去不掉进程间的预热差；实测取 9 次里后 6 次的中位，
     自比抖动从 >50% 降到可直接判定的量级。 */
  const warm = xs.slice(3).sort((a, b) => a.ms - b.ms);
  const m = warm[Math.floor(warm.length / 2)];
  return { T, medianMs: +m.ms.toFixed(2), q: m.q, ok: m.ok, err: m.err };
});

const st = seeded(20261015, () => S.selfTest(400, 8));
const pst = seeded(777, () => S.poseSelfTest(8));

const exact = {
  counts: {
    parts: parts.length,
    verts: parts.reduce((n, p) => n + ((p.v && p.v.length) || 0), 0),
    tris: parts.reduce((n, p) => n + ((p.t && p.t.length) || 0), 0),
    printed: parts.filter(p => p.printed).length,
    env: parts.filter(p => String(p.name).startsWith('ENV_')).length,
    joints: JO.length,
  },
  tcpZ: +tcpZ.toFixed(3),
  TCP0: TCP0.map(v => +v.toFixed(3)),
  motorTravel: MOTOR,
  noAdjAxes: noAdj,
  caps: COL ? COL.caps.length : 0,
  lim: Object.fromEntries(ORDER.map(id => [id, [r6(S.LIM[id].lo), r6(S.LIM[id].hi), !!S.LIM[id].cont]])),
  fkQ0: S.fk(Q0, 0).map(r6),
  fkChainQ0: S.fkChain(Q0, 0).map(r6),
  orientQ0: S.orient(Q0).map(r6),
  solveDyn: solveRuns.map(r => ({ T: r.T, ok: r.ok, err: r.err, q: r.q })),
  selfTest: { maxFkErr: st.maxFkErr, maxIkErr: st.maxIkErr, ikHit: st.ikHit, ikTried: st.ikTried,
              structWarn: st.structWarn, poseHit: st.poseHit, poseTried: st.poseTried,
              poseErr: st.poseErr, K: st.K, LIM: st.LIM },
  poseSelfTest: { poseHit: pst.poseHit, poseTried: pst.poseTried, poseErr: pst.poseErr },
};
const timing = {
  solveDynMedianMs: solveRuns.map(r => r.medianMs),
  poseSelfTestAvgMs: +pst.poseMs.toFixed(2),
};

const doc = { meta: { solver: path.relative(ROOT, SOLVER).replace(/\\/g, '/'),
                      mesh: path.relative(ROOT, MESH).replace(/\\/g, '/') },
              exact, timing };
const text = JSON.stringify(doc, null, 1);

const out = argVal('out', null);
if (out) fs.writeFileSync(out, text + '\n');

const base = argVal('compare', null);
if (!base) { if (!out) console.log(text); process.exit(0); }

const b = JSON.parse(fs.readFileSync(base, 'utf8'));
const diffs = [];
(function walk(x, y, p) {
  if (x === null || y === null || typeof x !== 'object' || typeof y !== 'object') {
    if (JSON.stringify(x) !== JSON.stringify(y)) diffs.push(p + ': ' + JSON.stringify(y) + ' -> ' + JSON.stringify(x));
    return;
  }
  for (const k of new Set([...Object.keys(x), ...Object.keys(y)])) walk(x[k], y[k], p ? p + '.' + k : k);
})(doc.exact, b.exact, '');

console.log('== exact（必须逐字节相同）==');
console.log(diffs.length ? diffs.map(d => '  DIFF ' + d).join('\n') : '  全部一致 ✓');
console.log('== timing（±10% 只报 WARN）==');
const tb = b.timing;
let warn = 0;
doc.timing.solveDynMedianMs.forEach((v, i) => {
  const o = tb.solveDynMedianMs[i];
  const d = (v - o) / o * 100;
  if (Math.abs(d) > 10) { warn++; console.log(`  WARN solveDyn#${i}: ${o} -> ${v} ms (${d.toFixed(1)}%)`); }
  else console.log(`  ok   solveDyn#${i}: ${o} -> ${v} ms (${d.toFixed(1)}%)`);
});
if (diffs.length) { console.log('RESULT: FAIL（exact 有 %d 处差异）', diffs.length); process.exit(1); }
console.log('RESULT: PASS' + (warn ? `（timing ${warn} 项超 ±10%，需人判）` : ''));