feat(ik): 避障路径改为多目标 Lazy RRT-Connect + 规划提速 32%
- 避障路径改为多目标 Lazy RRT-Connect + 捷径平滑;碰撞窄相位预测提速。 - 规划提速 32%:(2,4) 零件对类离线间隙表、撞点前移、判定记忆、routeFrom 惰性回退、平滑预算收紧。
This commit is contained in:
@@ -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