50 lines
3.2 KiB
JavaScript
50 lines
3.2 KiB
JavaScript
/* 实时拖拽仿真基准：目标沿直线从 A 的 TCP 拖到 B 的 TCP（中间被立柱挡住），每帧 16.7 ms。
|
|
* 每帧：手臂沿当前路径按各轴速度上限前进；求解器空闲就用"此刻位形"作起点重规划（quick 模式）。
|
|
* 统计：重规划耗时分布、穿模帧（1° 细验）、拖完 6 s 后的到达误差。
|
|
* node tests/rt_drag_bench.cjs <solver.js> [场景数=12] [帧数=120] */
|
|
const setup = require('./route_bench_setup.cjs');
|
|
const { S } = setup(process.argv[2]);
|
|
const NC = +process.argv[3] || 12, NF = +process.argv[4] || 120, DT = 1 / 60, TAIL = 360;
|
|
const L = S.LIM, O = S.ORDER, w = S.wrap180;
|
|
let s = 777; 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 / 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 VMAX = [60, 60, 90, 90, 120, 180];
|
|
const cases = []; let tries = 0;
|
|
while (cases.length < NC && tries < 50000) { tries++; const a = rq(), b = rq(); if (!S.collideFree(a) || !S.collideFree(b) || segOK(a, b)) continue; cases.push([a, b]); }
|
|
const lat = []; let badFrames = 0, frames = 0, reached = 0, stuck = 0;
|
|
for (const [a, b] of cases) {
|
|
const TA = S.fk(a, 0), TB = S.fk(b, 0);
|
|
let q = a.slice(), path = [], busyUntil = 0, tNow = 0;
|
|
for (let f = 0; f < NF + TAIL; f++) {
|
|
tNow = f * DT * 1000;
|
|
const u = Math.min(1, f / NF), T = TA.map((v, i) => v + (TB[i] - v) * u);
|
|
if (tNow >= busyUntil) { // 求解器空闲：派发一次重规划（结果在耗时后才生效）
|
|
const t0 = performance.now();
|
|
const r = S.solveRoute(T, { tool: 0, step: 0.5, ref: q, warm: q, from: q.slice(), noVia: true, quick: true });
|
|
const ms = performance.now() - t0; lat.push(ms);
|
|
busyUntil = tNow + ms;
|
|
if (r && r.q) { path = (r.via || []).concat([r.q]); }
|
|
}
|
|
/* 按轴速上限沿折线前进 */
|
|
let budget = 1;
|
|
const q0 = q.slice();
|
|
while (path.length && budget > 1e-9) {
|
|
const tgt = q.map((v, i) => L[O[i]].cont ? v + w(path[0][i] - v) : path[0][i]);
|
|
const dd = tgt.map((v, i) => v - q[i]);
|
|
const need = Math.max(...dd.map((x, i) => Math.abs(x) / (VMAX[i] * DT)));
|
|
if (need <= budget) { q = tgt; path.shift(); budget -= need; }
|
|
else { q = q.map((v, i) => v + dd[i] * budget / need); budget = 0; }
|
|
}
|
|
frames++;
|
|
if (!segOK(q0, q)) badFrames++;
|
|
}
|
|
const tcp = S.fk(q, 0), err = Math.hypot(tcp[0] - TB[0], tcp[1] - TB[1], tcp[2] - TB[2]);
|
|
if (err < 1) reached++; else stuck++;
|
|
}
|
|
lat.sort((x, y) => x - y);
|
|
const pct = p => +lat[Math.min(lat.length - 1, Math.floor(lat.length * p))].toFixed(1);
|
|
console.log(JSON.stringify({ cases: cases.length, replans: lat.length, latP50: pct(0.5), latP90: pct(0.9), latP99: pct(0.99), latMax: pct(1),
|
|
latMean: +(lat.reduce((x, y) => x + y, 0) / lat.length).toFixed(1), collidingFrames: badFrames, frames, reachedAfter6s: reached }));
|