perf(ik): 规划提速 32%：(2,4) 零件对类离线间隙表、撞点前移、判定记忆、routeFrom 惰性回退、平滑预算收紧
This commit is contained in:
Hark
+124
-13
@@ -268,6 +268,80 @@ function make(cfg) {
|
||||
if (cl.length) PARTPAIR.push([A, B, cl, A.rad + B.rad, A.rmax + B.rmax]);
|
||||
}
|
||||
}
|
||||
/* ---------- 0c. 两关节零件对类的预计算间隙表（2026-10-15） ----------
|
||||
零件对类 (ga, gb) 且 gb−ga=2 时，两组的相对位姿只由中间两个关节决定（如 (2,4)：P05/M3 ×
|
||||
P07/M5 只取决于 J3、J4）。实测 (2,4) 类长期相距 ~5 mm、平面相对，簇级宽相位筛不掉，
|
||||
独占每次 collideFree 约 80% 的线段距离计算。离线把该类最小间隙按 1° 网格打成表
|
||||
（collide.json 的 pair2d 字段，webapp/tools/gen_pair2d.cjs 生成），运行时取最近网格点，
|
||||
按 Lipschitz 界给出间隙上下界：下界 > 0 → 该类整类跳过；上界 < 0 → 直接判撞；
|
||||
夹在中间才回退精算。结果与精算逐位一致（保守），只省时间。
|
||||
指纹不匹配（重新拟合过胶囊但没重生成表）时整表弃用，自动回退精算。 */
|
||||
function classOf(A, B) { return A.g < B.g ? A.g * 7 + B.g : B.g * 7 + A.g; }
|
||||
function classFingerprint(ci) {
|
||||
let h = 2166136261 >>> 0;
|
||||
const mix = v => { h = Math.imul(h ^ (Math.round(v * 1000) | 0), 16777619) >>> 0; };
|
||||
mix(CAPM);
|
||||
const inCls = new Set();
|
||||
for (const pp of PARTPAIR) if (classOf(pp[0], pp[1]) === ci) { inCls.add(pp[0]._i); inCls.add(pp[1]._i); }
|
||||
for (const pt of PART) { // 按 PART 固定顺序（PARTPAIR 运行期会前移重排）
|
||||
if (!inCls.has(pt._i)) continue;
|
||||
for (const i of pt.caps) { const c = CAPG[i]; mix(c.g); mix(c.r); for (const v of c.a) mix(v); for (const v of c.b) mix(v); }
|
||||
}
|
||||
return h.toString(16);
|
||||
}
|
||||
const P2D = {}; // ci → 表
|
||||
if (COL && COL.pair2d) {
|
||||
for (const key in COL.pair2d) {
|
||||
const T = COL.pair2d[key], ci = +key;
|
||||
const ga = Math.floor(ci / 7), gb = ci % 7;
|
||||
if (gb - ga !== 2) continue;
|
||||
if (T.fp !== classFingerprint(ci)) { if (typeof console !== 'undefined') console.warn('[IK] pair2d 类 ' + key + ' 指纹不符，弃用（重跑 webapp/tools/gen_pair2d.cjs）'); continue; }
|
||||
let gap;
|
||||
if (typeof atob === 'function') { const bin = atob(T.b64); gap = new Int16Array(bin.length >> 1); for (let i = 0; i < gap.length; i++) gap[i] = (bin.charCodeAt(2 * i) | (bin.charCodeAt(2 * i + 1) << 8)) << 16 >> 16; }
|
||||
else { const buf = Buffer.from(T.b64, 'base64'); gap = new Int16Array(buf.length >> 1); for (let i = 0; i < gap.length; i++) gap[i] = buf.readInt16LE(2 * i); }
|
||||
/* 杠杆界（mm/°）：高组胶囊轴端点 p 在低组坐标系里的速度 ≤ |ω_a|·dist(p, 轴a) + |ω_b|·dist(p, 轴b)，
|
||||
轴b 与高组同刚体 → dist(p,轴b) 不变；dist(p,轴a) ≤ dist(枢轴b, 轴a) + |p − 枢轴b|（两者对两关节转动都不变）。
|
||||
只用轴端点（半径不随运动变），距离对线段凸 → 端点取极值即可。 */
|
||||
const ja = ga, jb = ga + 1; // 关节下标（0 基）：带动组 ga+1 与 ga+2
|
||||
const Pa = P[ORDER[ja]], Da = D[ORDER[ja]], Pb = P[ORDER[jb]], Db = D[ORDER[jb]];
|
||||
const perp = (p, P0, d) => { const v = [p[0] - P0[0], p[1] - P0[1], p[2] - P0[2]]; const t = v[0] * d[0] + v[1] * d[1] + v[2] * d[2]; return Math.hypot(v[0] - t * d[0], v[1] - t * d[1], v[2] - t * d[2]); };
|
||||
const pbA = perp(Pb, Pa, Da);
|
||||
let La = 0, Lb = 0;
|
||||
for (const pp of PARTPAIR) {
|
||||
if (classOf(pp[0], pp[1]) !== ci) continue;
|
||||
const hi = pp[0].g === gb ? pp[0] : pp[1];
|
||||
for (const i of hi.caps) for (const p of [CAPG[i].a, CAPG[i].b]) {
|
||||
La = Math.max(La, pbA + Math.hypot(p[0] - Pb[0], p[1] - Pb[1], p[2] - Pb[2]));
|
||||
Lb = Math.max(Lb, perp(p, Pb, Db));
|
||||
}
|
||||
}
|
||||
P2D[ci] = { ja: ja, jb: jb, lo: T.lo, h: T.h, n: T.n, ca: !!LIM[ORDER[ja]].cont, gap: gap,
|
||||
La: La * D2R, Lb: Lb * D2R, st: 0, gen: -1 };
|
||||
}
|
||||
}
|
||||
/* 查表：1=该类必无干涉，−1=必干涉，0=不确定（需精算） */
|
||||
function p2dStatus(T, q) {
|
||||
let a = q[T.ja], b = q[T.jb];
|
||||
if (T.ca) a = wrap180(a);
|
||||
let ia = Math.round((a - T.lo[0]) / T.h), ib = Math.round((b - T.lo[1]) / T.h);
|
||||
if (T.ca) ia = ((ia % T.n[0]) + T.n[0]) % T.n[0];
|
||||
if (ia < 0 || ia >= T.n[0] || ib < 0 || ib >= T.n[1]) return 0;
|
||||
let da = Math.abs(a - (T.lo[0] + ia * T.h)); if (T.ca) da = Math.abs(wrap180(a - (T.lo[0] + ia * T.h)));
|
||||
const db = Math.abs(b - (T.lo[1] + ib * T.h));
|
||||
const g = T.gap[ia * T.n[1] + ib] * 0.01, sl = T.La * da + T.Lb * db + 1e-6;
|
||||
if (g - sl > 0) return 1;
|
||||
if (g + sl < 0 && g > -327) return -1;
|
||||
return 0;
|
||||
}
|
||||
/* 某类在姿态 q 下的精确最小间隙（生成表用）：需 cumXf 之后的变换缓冲，故走一次独立扫描 */
|
||||
function classGap(q, ci) {
|
||||
let best = Infinity;
|
||||
const save = P2D[ci]; delete P2D[ci];
|
||||
try {
|
||||
scanPairs(q, (na, nb, d, need) => {}, ci, g => { if (g < best) best = g; });
|
||||
} finally { if (save) P2D[ci] = save; }
|
||||
return Math.min(best, 20); // ≥20 mm 一律记 20（下界，保守）
|
||||
}
|
||||
/* 组累积刚体变换 cum[k]：(R,t) 使 world = R·local + t，k=1..6（0 为恒等） */
|
||||
/* 碰撞热路径预分配（一次性）：替代每次 check 重建 clc/cache/pcc（约 2500 个小数组/次，
|
||||
GC 占单次判定 ~13%）。普通数组 + 代际戳，元素类型统一（数字 + 一个字符串 cn）——
|
||||
@@ -294,9 +368,16 @@ function make(cfg) {
|
||||
}
|
||||
/* 逐对扫描（簇级宽相位 → 胶囊线段距离）。visit(cnA,cnB,d,need) 对每个距离
|
||||
< need 的胶囊对回调；无回调（collideFree）时首对即短路返回。 */
|
||||
function scanPairs(q, visit) {
|
||||
function scanPairs(q, visit, onlyCls, gapCb) {
|
||||
if (!COL) return false;
|
||||
const { R: Rs, T: Ts } = cumXf(q);
|
||||
/* 两关节类查表（只在纯判定时用；明细/生成表走精算） */
|
||||
const INF = gapCb ? 20 : 0; // 生成表：宽相位外扩 20 mm，表内 <20 mm 的间隙为精确值
|
||||
if (!visit) for (const k in P2D) {
|
||||
const T = P2D[k], st = p2dStatus(T, q);
|
||||
if (st < 0) return true;
|
||||
T.st = st;
|
||||
}
|
||||
/* clc/cache/pcc 全部预分配（声明见 cumXf 前）：CGEN 代际戳标记"本 check 已变换" */
|
||||
CGEN++;
|
||||
const clOf = (pi, ci) => {
|
||||
@@ -342,16 +423,18 @@ function make(cfg) {
|
||||
let hit = false;
|
||||
for (let k = 0; k < PARTPAIR.length; k++) {
|
||||
const [A, B, cl, rsum0, rmax0] = PARTPAIR[k];
|
||||
if (onlyCls !== undefined) { if (classOf(A, B) !== onlyCls) continue; }
|
||||
else if (!visit) { const T = P2D[classOf(A, B)]; if (T && T.st === 1) continue; }
|
||||
const pa = pcOf(A._i), pb = pcOf(B._i);
|
||||
const ddx = pa[0] - pb[0], ddy = pa[1] - pb[1], ddz = pa[2] - pb[2];
|
||||
const lim0 = rsum0 + rmax0 + CAPM;
|
||||
if (ddx * ddx + ddy * ddy + ddz * ddz > lim0 * lim0) continue; // 零件级分离
|
||||
if (ddx * ddx + ddy * ddy + ddz * ddz > (lim0 + INF) * (lim0 + INF)) continue; // 零件级分离
|
||||
for (let m = 0; m < cl.length; m++) {
|
||||
const [ia, ib, pr, rsum, rmaxS] = cl[m];
|
||||
const p1 = clOf(A._i, ia), p2 = clOf(B._i, ib);
|
||||
const dx = p1[0] - p2[0], dy = p1[1] - p2[1], dz = p1[2] - p2[2];
|
||||
const lim = rsum + rmaxS + CAPM;
|
||||
if (dx * dx + dy * dy + dz * dz > lim * lim) continue; // 簇球分离
|
||||
if (dx * dx + dy * dy + dz * dz > (lim + INF) * (lim + INF)) continue; // 簇球分离
|
||||
for (let n = 0; n < pr.length; n++) {
|
||||
const e1 = xf(pr[n][0]), e2 = xf(pr[n][1]);
|
||||
const need = pr[n][2];
|
||||
@@ -359,22 +442,41 @@ function make(cfg) {
|
||||
实测 segSegDistRaw 占碰撞判定 ~45% 耗时，绝大多数胶囊对在这一步就能跳过 */
|
||||
const mx = 0.5 * (e1[0] + e1[3] - e2[0] - e2[3]), my = 0.5 * (e1[1] + e1[4] - e2[1] - e2[4]),
|
||||
mz = 0.5 * (e1[2] + e1[5] - e2[2] - e2[5]), bl = pr[n][3];
|
||||
if (mx * mx + my * my + mz * mz > bl * bl) continue;
|
||||
if (mx * mx + my * my + mz * mz > (bl + INF) * (bl + INF)) continue;
|
||||
const d = segSegDistRaw(e1[0], e1[1], e1[2], e1[3], e1[4], e1[5],
|
||||
e2[0], e2[1], e2[2], e2[3], e2[4], e2[5]);
|
||||
if (gapCb) { gapCb(d - need); continue; }
|
||||
if (d < need) {
|
||||
hit = true;
|
||||
if (visit) visit(e1[6], e2[6], d, need);
|
||||
else return true;
|
||||
else {
|
||||
/* 前移启发：撞上的零件对 / 簇对挪到表头。规划中大量判定落在同一处障碍
|
||||
（典型：立柱 × 腕/法兰），下次首查即命中、提前短路。只改顺序，不改结果。 */
|
||||
if (k > 0) { const e = PARTPAIR[k]; PARTPAIR.copyWithin(1, 0, k); PARTPAIR[0] = e; }
|
||||
if (m > 0) { const e = cl[m]; cl.copyWithin(1, 0, m); cl[0] = e; }
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return hit;
|
||||
}
|
||||
/* 间隙下界（mm，封顶 cap）：所有零件对 min(线段距 − need)。宽相位被筛掉的对按
|
||||
"球心距 − 阈值"计下界，所以返回值 ≤ 真实最小间隙（保守）。< 0 即干涉。
|
||||
供 edgeFree 做保守连续检测：间隙 c 内，任何关节运动只要扫掠位移 < c 就不可能撞。 */
|
||||
/* 全姿态动态碰撞判定：True = 无动态干涉（模型近似，含 margin 外扩） */
|
||||
const PST = { n: 0 };
|
||||
/* 单次求解内的判定记忆：routeFrom / gatherGoals / 规划器会对同一批 IK 候选反复判定 */
|
||||
let CFMEMO = null;
|
||||
function collideFree(q) {
|
||||
return !COL || !scanPairs(q, null);
|
||||
if (!COL) return true;
|
||||
let key;
|
||||
if (CFMEMO) { key = q[0] + ',' + q[1] + ',' + q[2] + ',' + q[3] + ',' + q[4] + ',' + q[5]; const v = CFMEMO.get(key); if (v !== undefined) return v; }
|
||||
PST.n++;
|
||||
const v = !scanPairs(q, null);
|
||||
if (CFMEMO && CFMEMO.size < 20000) CFMEMO.set(key, v);
|
||||
return v;
|
||||
}
|
||||
/* 干涉明细（调参/界面提示用）：返回 [{cnA, cnB, d, need}]，按 d−need 升序
|
||||
（最贴合的排前面）。空数组 = 无动态干涉。 */
|
||||
@@ -2001,7 +2103,7 @@ function make(cfg) {
|
||||
}
|
||||
return null;
|
||||
}
|
||||
function routeFrom(from, cur) {
|
||||
function routeFrom(from, cur, lazy) {
|
||||
if (!COL || !cur || !cur.q || !from) return cur;
|
||||
if (!collideFree(from)) return cur; // 起点已在干涉里：先把自己拔出来再说
|
||||
const seen = new Set(), cand = [];
|
||||
@@ -2032,6 +2134,9 @@ function make(cfg) {
|
||||
}
|
||||
/* 没有一条直线能干净抵达：沿最优候选（优先端点安全的那个）方向二分退回安全边界，
|
||||
让臂贴着障碍停住，而不是穿过去。 */
|
||||
/* lazy：调用方接下来要跑规划器，贴边停的回退位形先不算（findSafeTarget + 二分 + 细验
|
||||
约占被挡场景 routeFrom 耗时的一半），规划失败时再补算。 */
|
||||
if (lazy) return Object.assign({}, cur, { routed: true, blocked: true, lazyBlocked: true });
|
||||
const safeT = findSafeTarget(cur, from);
|
||||
let bestQ = safeT ? safeT.q : null;
|
||||
if (!bestQ) {
|
||||
@@ -2280,7 +2385,7 @@ function make(cfg) {
|
||||
function shortcut(pts, deadline, rnd) {
|
||||
pts = pts.map(p => p.slice());
|
||||
/* 随机捷径：在两段上各取一点直连 */
|
||||
for (let it = 0; it < 80 && pts.length > 2 && nowMs() < deadline; it++) {
|
||||
for (let it = 0; it < 50 && pts.length > 2 && nowMs() < deadline; it++) {
|
||||
const i = Math.floor(rnd() * (pts.length - 1)), j = Math.floor(rnd() * (pts.length - 1));
|
||||
if (Math.abs(i - j) < 1) continue;
|
||||
const a = Math.min(i, j), b = Math.max(i, j);
|
||||
@@ -2301,7 +2406,7 @@ function make(cfg) {
|
||||
}
|
||||
/* 逐轴拉直：中间位形的每个轴若能取"前后两点按段长插值"的值且两侧边仍无碰撞，就拉直它。
|
||||
去掉 RRT 留下的"某轴来回摆"（与障碍无关的轴不该动），这正是视觉上"怪动作"的来源。 */
|
||||
for (let pass = 0; pass < 2 && out.length > 2; pass++) {
|
||||
for (let pass = 0; pass < 1 && out.length > 2; pass++) {
|
||||
for (let i = 1; i < out.length - 1 && nowMs() < deadline; i++) {
|
||||
const a = out[i - 1], b = out[i + 1];
|
||||
const la = wdist(a, out[i]), lb = wdist(out[i], b), t = la / (la + lb || 1);
|
||||
@@ -2393,7 +2498,7 @@ function make(cfg) {
|
||||
for (const g of goals) if (edgeFree(from, g)) return { goal: g, via: [], detour: null };
|
||||
/* 再试"单中间点微调"：弦中点抬肩 / 收肘（只动 J2/J4，其余轴保持直线插值），2° 细验。
|
||||
贴边擦碰的大多数场景这里就能解决，动作最接近直线、最像人。 */
|
||||
for (const g of goals.slice(0, 3)) {
|
||||
for (const g of goals.slice(0, 2)) {
|
||||
const mid = from.map((v, i) => 0.5 * (v + g[i]));
|
||||
for (const [d2, d4] of GENTLE_OFFSETS) {
|
||||
const m = mid.slice(); m[1] += d2; m[3] += d4;
|
||||
@@ -2405,12 +2510,17 @@ function make(cfg) {
|
||||
for (const v of from.concat(goals[0])) seed = Math.imul(seed ^ Math.round(v * 10), 16777619);
|
||||
const r = rrtConnect(from, goals, budget, seed);
|
||||
if (!r) return null;
|
||||
const sm = shortcut(r.path, nowMs() + (quick ? 40 : 250), r.rnd);
|
||||
const sm = shortcut(r.path, nowMs() + (quick ? 40 : 160), r.rnd);
|
||||
const goal = sm[sm.length - 1];
|
||||
return { goal: goal, via: sm.slice(1, -1), detour: 'planned', iters: r.iters, ms: r.ms };
|
||||
}
|
||||
function routeOrVia(from, target, cur, opt) {
|
||||
const r = routeFrom(from, cur);
|
||||
CFMEMO = new Map();
|
||||
try { return routeOrViaImpl(from, target, cur, opt); } finally { CFMEMO = null; }
|
||||
}
|
||||
function routeOrViaImpl(from, target, cur, opt) {
|
||||
const lazy = !opt.noAround;
|
||||
let r = routeFrom(from, cur, lazy);
|
||||
if (!r.blocked || !from) return r;
|
||||
/* 首选：采样规划（多目标 + 平滑），给出的路径已按 EDGE_RES 细验 */
|
||||
if (!opt.noAround) {
|
||||
@@ -2420,6 +2530,7 @@ function make(cfg) {
|
||||
if (pl.via.length) Object.assign(res, { via: pl.via, detour: pl.detour, planIters: pl.iters, planMs: pl.ms });
|
||||
return res;
|
||||
}
|
||||
if (r.lazyBlocked) r = routeFrom(from, cur, false);
|
||||
/* 规划预算内未连通（极少）：退回旧的启发式阶梯 */
|
||||
const around = routeAroundPlan(from, cur, opt.quick, r.safeT);
|
||||
if (around) return Object.assign({}, around.res, { via: around.via, routed: true, blocked: false, detour: around.detour || 'around' });
|
||||
@@ -2539,7 +2650,7 @@ function make(cfg) {
|
||||
}
|
||||
|
||||
return { fk: fk, fkChain: fkChain, solve: solve, solveTo: solveTo, solveWarm: solveWarm, solveDyn: solveDyn,
|
||||
solveRoute: solveRoute, routeFrom: routeFrom, transitPt: transitPt, planAround: planAround, edgeFree: edgeFree,
|
||||
solveRoute: solveRoute, routeFrom: routeFrom, transitPt: transitPt, planAround: planAround, edgeFree: edgeFree, PST: PST, classGap: classGap, classFingerprint: classFingerprint, P2D: P2D,
|
||||
mixSolve: mixSolve, closest: closest, closest6: closest6, closestOri: closestOri, orient: orient,
|
||||
branch: branch, selfTest: selfTest, poseSelfTest: poseSelfTest,
|
||||
collideFree: collideFree, collideDetail: collideDetail,
|
||||
|
||||
Reference in New Issue
Block a user