67 lines
4.3 KiB
JavaScript
67 lines
4.3 KiB
JavaScript
/* 贴立柱目标基准（2026-10-17，用户报告"立柱另一侧 → 贴立柱目标误判在立柱区内"）
|
||
* 目标位形 b：本身无干涉，但 J1/J2/J3 任一轴 ±4° 就撞立柱（= 贴着立柱）。
|
||
* 起点 a：无干涉，且 a→b 关节直线被挡（立柱另一侧）。
|
||
* 模式：pos = 松手位置任务；pose = 松手位置+姿态任务（姿态取 b 的真实姿态，b 本身就是安全精确解）；
|
||
* rt = 拖拽实时管线，目标不动连续重规划直到到达或 3 s。
|
||
* 判据：到达（TCP 误差 < 1 mm，pose 另要姿态 < 0.5°）且整条路径 1° 细验无干涉。
|
||
* node tests/near_column_bench.cjs <solver.js> [N=30] [modes=pos,pose,rt] */
|
||
const setup = require('./route_bench_setup.cjs');
|
||
const { S, ArmIK } = setup(process.argv[2]);
|
||
const N = +process.argv[3] || 30, MODES = (process.argv[4] || 'pos,pose,rt').split(',');
|
||
const L = S.LIM, O = S.ORDER, w = S.wrap180;
|
||
let s = 20261017; const rnd = () => { s = (s * 1664525 + 1013904223) >>> 0; return s / 4294967296; };
|
||
const rq = () => O.map(id => { const t = L[id]; return t.lo + rnd() * (t.hi - t.lo); });
|
||
const d = (a, b) => a.map((v, i) => L[O[i]].cont ? w(b[i] - v) : b[i] - v);
|
||
const segOK = (a, b) => { const dd = d(a, b); const m = Math.max(...dd.map(Math.abs)); const n = Math.max(1, Math.ceil(m)); for (let i = 1; i <= n; i++) if (!S.collideFree(a.map((v, k) => v + dd[k] * i / n))) return false; return true; };
|
||
const hitsColumn = q => { const det = S.collideDetail(q) || []; return det.some(p => /立柱/.test(p.cnA + p.cnB)); };
|
||
const nearColumn = q => { for (const k of [0, 1, 2]) for (const sg of [-4, 4]) { const p = q.slice(); p[k] += sg; if (hitsColumn(p)) return true; } return false; };
|
||
const cases = []; let tries = 0;
|
||
while (cases.length < N && tries < 400000) {
|
||
tries++;
|
||
const b = rq(); if (!S.collideFree(b) || !nearColumn(b)) continue;
|
||
let a = null;
|
||
for (let t = 0; t < 200; t++) { const c = rq(); if (S.collideFree(c) && !segOK(c, b)) { a = c; break; } }
|
||
if (a) cases.push({ a, b });
|
||
}
|
||
if (process.env.DUMP) require('fs').writeFileSync(process.env.DUMP, JSON.stringify(cases));
|
||
const out = {};
|
||
for (const mode of MODES) {
|
||
let ok = 0, reached = 0, valid = 0, unsafe = 0, relaxed = 0, ms = 0, worst = 0; const fails = [];
|
||
cases.forEach((c, ci) => {
|
||
const T = S.fk(c.b, 0), R = S.orient(c.b);
|
||
const opt = { tool: 0, step: 0.5, ref: c.a, from: c.a.slice() };
|
||
if (mode === 'pose') { opt.pose = { R }; opt.priority = 'pos'; }
|
||
if (S.rtReset) S.rtReset();
|
||
const t0 = performance.now();
|
||
let pts = [c.a.slice()], r = null;
|
||
if (mode === 'rt') {
|
||
let q = c.a.slice();
|
||
for (let it = 0; it < 40; it++) { // 每次重规划后沿路径走到头（目标不动；最多 40 次重规划）
|
||
r = S.solveRoute(T, Object.assign({}, opt, { from: q.slice(), ref: q, warm: q, noVia: true, quick: true }));
|
||
if (!r || !r.q) break;
|
||
const seg = (r.via || []).concat([r.q]);
|
||
for (const p of seg) pts.push(p);
|
||
const nq = r.q.slice();
|
||
const tcp = S.fk(nq, 0);
|
||
if (Math.hypot(tcp[0] - T[0], tcp[1] - T[1], tcp[2] - T[2]) < 1) { q = nq; break; }
|
||
q = nq;
|
||
}
|
||
} else {
|
||
r = S.solveDyn(T, opt);
|
||
pts = pts.concat(r.via || [], [r.q]);
|
||
}
|
||
const t = performance.now() - t0; ms += t; worst = Math.max(worst, t);
|
||
for (let i = 1; i < pts.length; i++) pts[i] = pts[i - 1].map((v, k) => L[O[k]].cont ? v + w(pts[i][k] - v) : pts[i][k]);
|
||
let pv = true; for (let i = 1; i < pts.length; i++) if (!segOK(pts[i - 1], pts[i])) { pv = false; break; }
|
||
const qf = pts[pts.length - 1], tcp = S.fk(qf, 0);
|
||
const err = Math.hypot(tcp[0] - T[0], tcp[1] - T[1], tcp[2] - T[2]);
|
||
const oerr = mode === 'pose' ? ArmIK.rotErrDeg(S.orient(qf), R) : 0;
|
||
const rc = err < 1 && oerr < 0.5;
|
||
if (rc) reached++; if (pv) valid++; if (rc && pv) ok++;
|
||
if (r && r.unsafe) unsafe++; if (r && r.poseRelaxed) relaxed++;
|
||
if (!(rc && pv)) fails.push({ ci, err: +err.toFixed(1), oerr: +oerr.toFixed(1), pathOK: pv, unsafe: !!(r && r.unsafe), blocked: !!(r && r.blocked) });
|
||
});
|
||
out[mode] = { n: cases.length, ok, reached, validPath: valid, unsafe, poseRelaxed: relaxed, msMean: Math.round(ms / cases.length), msMax: Math.round(worst), fails: fails.slice(0, 12) };
|
||
}
|
||
console.log(JSON.stringify(out, null, 1));
|