perf(ik): 规划提速 32%:(2,4) 零件对类离线间隙表、撞点前移、判定记忆、routeFrom 惰性回退、平滑预算收紧
This commit is contained in:
+4
-2
File diff suppressed because one or more lines are too long
@@ -38,3 +38,31 @@
|
||||
- 规划耗时上升约 0.3~0.5 s(Worker 里跑,不卡界面);拖拽实时模式预算 150 ms,超时回落旧阶梯。
|
||||
- 执行层仍是关节空间折线;若要上真机,下一步应做带加速度约束的时间最优参数化(TOPP)+ 拐角抛物线过渡,
|
||||
过渡段需重新做碰撞校验。
|
||||
|
||||
## 二期:规划提速(2026-10-15)
|
||||
|
||||
逐段计时(20 个被挡场景):时间主要花在"单中间点微调"(~350 ms/例)、RRT(~280 ms)和平滑(~250 ms 截止);
|
||||
每次碰撞判定约 1.5 ms,其中 80% 的线段距离计算来自 (2,4) 类零件对(P05/M3 × P07/M5,长期相距 ~5 mm)。
|
||||
|
||||
改动:
|
||||
1. **两关节零件对类间隙表**:(2,4) 类的相对位姿只取决于 J3、J4,离线按 1° 网格生成最小间隙表
|
||||
(`webapp/tools/gen_pair2d.cjs` → `docs/collide.json` 的 `pair2d`)。运行时取最近网格点,按杠杆界
|
||||
(J3 3.24 mm/°、J4 2.54 mm/°)给出上下界,能定论就跳过/判撞,否则回退精算。2 万随机姿态与精算判定 0 差异;
|
||||
单次判定 1.60 → 1.25 ms。指纹校验:重新拟合胶囊后未重生成表会自动弃表(只变慢不出错)。
|
||||
2. **前移启发**:撞上的零件对/簇对移到表头,规划中反复撞同一处时首查即短路。
|
||||
3. **单次求解内判定记忆**:routeFrom / gatherGoals / 规划器对同一批 IK 候选不再重复判定。
|
||||
4. **routeFrom 惰性回退**:被挡时先不算"贴边停"位形(findSafeTarget + 二分),规划失败才补算。
|
||||
5. 微调只试前 2 个目标;平滑 50 次随机捷径 / 160 ms 截止,逐轴拉直 1 轮。
|
||||
|
||||
同一批 60 例(`node tests/route_bench.cjs <solver> 60`):
|
||||
|
||||
| 指标 | 一期 | 二期 |
|
||||
|---|---:|---:|
|
||||
| 平均规划耗时 | 1239 ms | 837 ms(−32%) |
|
||||
| 到达目标 | 55 | 58 |
|
||||
| 1° 细验不穿模 | 58 | 58 |
|
||||
| 关节总行程均值 | 438° | 436° |
|
||||
| 往返次数均值 | 2.08 | 1.98 |
|
||||
|
||||
尝试过但未采用:基于间隙 + 杠杆界的保守连续检测。(2,4) 类常年仅 ~5 mm 间隙把步长压到 <1°,
|
||||
单次间隙查询又比布尔判定贵,实测比 1.5° 离散检测慢 25%。
|
||||
|
||||
@@ -1,11 +1,11 @@
|
||||
/* 路径规划基准的求解器装配(Node):node tests/route_bench.cjs [solver.js] [N] */
|
||||
const fs=require('fs'),path=require('path');
|
||||
const ROOT=path.resolve(__dirname,'..');
|
||||
module.exports=function(solverPath){
|
||||
module.exports=function(solverPath,collidePath){
|
||||
const ArmIK=require(solverPath||path.join(ROOT,'webapp/js/ik-solver.js'));
|
||||
const JMap=/* 关节表取自 arm/kinematics.py JOINTS(无需 mesh_arm.json) */ {"J1": {"rear": [-51.0, 128.0, 610.0], "d": [1, 0, 0]}, "J2": {"rear": [19.5, 100.0, 490.0], "d": [0, 1, 0]}, "J3": {"rear": [19.5, 128.0, 445.0], "d": [0, 0, -1]}, "J4": {"rear": [19.5, 100.0, 332.0], "d": [0, 1, 0]}, "J5": {"rear": [19.5, 100.0, 222.0], "d": [0, 1, 0]}, "J6": {"rear": [19.5, 128.0, 176.0], "d": [0, 0, -1]}};
|
||||
const travel=JSON.parse(fs.readFileSync(ROOT+'/docs/travel.json','utf8'));
|
||||
const COL=JSON.parse(fs.readFileSync(ROOT+'/docs/collide.json','utf8'));
|
||||
const COL=JSON.parse(fs.readFileSync(collidePath||ROOT+'/docs/collide.json','utf8'));
|
||||
const TR={};for(const t of travel){const a=COL.adj[t.axis];TR[t.axis]={lo:a?a[0]:t.lo,hi:a?a[1]:t.hi,continuous:!!t.continuous};}
|
||||
const S=ArmIK.make({joints:JMap,travel:TR,tcp:[19.5,128,93],collide:COL});
|
||||
return {S,ArmIK};
|
||||
|
||||
+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,
|
||||
|
||||
@@ -0,0 +1,40 @@
|
||||
/* 生成两关节零件对类的间隙表,写入 docs/collide.json 的 pair2d 字段。
|
||||
* node webapp/tools/gen_pair2d.cjs 默认生成 (2,4) 类(P05/M3 × P07/M5,J3×J4)
|
||||
* 重新拟合胶囊(scratch/fit_capsules.py 重写 collide.json)后必须重跑;
|
||||
* 求解器按指纹校验,不符会自动弃表回退精算(只是变慢,不会算错)。 */
|
||||
const fs = require('fs'), path = require('path');
|
||||
const ROOT = path.resolve(__dirname, '..', '..');
|
||||
const setup = require(path.join(ROOT, 'tests/route_bench_setup.cjs'));
|
||||
const COLF = path.join(ROOT, 'docs/collide.json');
|
||||
/* 文本级追加/替换,保持 fit_capsules.py 写出的原格式不动(避免整文件 diff) */
|
||||
let text = fs.readFileSync(COLF, 'utf8');
|
||||
const cut = text.indexOf(',\n "pair2d":');
|
||||
if (cut >= 0) text = text.slice(0, cut) + '\n}\n';
|
||||
fs.writeFileSync(COLF + '.tmp', text);
|
||||
const { S } = setup(undefined, COLF + '.tmp');
|
||||
fs.unlinkSync(COLF + '.tmp');
|
||||
const H = 1.0, classes = [[2, 4]];
|
||||
const out = {};
|
||||
for (const [ga, gb] of classes) {
|
||||
const ci = ga * 7 + gb, ja = ga, jb = ga + 1;
|
||||
const la = S.LIM[S.ORDER[ja]], lb = S.LIM[S.ORDER[jb]];
|
||||
const lo = [la.cont ? -180 : la.lo, lb.cont ? -180 : lb.lo];
|
||||
const n = [la.cont ? Math.round(360 / H) : Math.round((la.hi - la.lo) / H) + 1,
|
||||
lb.cont ? Math.round(360 / H) : Math.round((lb.hi - lb.lo) / H) + 1];
|
||||
const buf = Buffer.alloc(n[0] * n[1] * 2);
|
||||
const t0 = Date.now();
|
||||
for (let i = 0; i < n[0]; i++) {
|
||||
for (let j = 0; j < n[1]; j++) {
|
||||
const q = [0, 0, 0, 0, 0, 0];
|
||||
q[ja] = lo[0] + i * H; q[jb] = lo[1] + j * H;
|
||||
const g = S.classGap(q, ci);
|
||||
buf.writeInt16LE(Math.max(-32767, Math.min(32767, Math.floor(g * 100))), 2 * (i * n[1] + j)); // 向下取整:保守
|
||||
}
|
||||
if (i % 60 === 0) console.error('class', ga + '' + gb, i + '/' + n[0], ((Date.now() - t0) / 1000).toFixed(0) + 's');
|
||||
}
|
||||
out[ci] = { joints: [S.ORDER[ja], S.ORDER[jb]], lo: lo, h: H, n: n, fp: S.classFingerprint(ci), b64: buf.toString('base64') };
|
||||
}
|
||||
const body = text.replace(/\s*\}\s*$/, '');
|
||||
fs.writeFileSync(COLF, body + ',\n "pair2d": ' + JSON.stringify(out) +
|
||||
',\n "pair2d_generated": ' + JSON.stringify(new Date().toISOString()) + '\n}\n');
|
||||
console.log('pair2d written', Object.keys(out));
|
||||
Reference in New Issue
Block a user