feat(ik): 避障路径改为多目标 Lazy RRT-Connect + 规划提速 32%

- 避障路径改为多目标 Lazy RRT-Connect + 捷径平滑;碰撞窄相位预测提速。
- 规划提速 32%:(2,4) 零件对类离线间隙表、撞点前移、判定记忆、routeFrom 惰性回退、平滑预算收紧。
This commit is contained in:
Hark
2026-10-18 06:40:22 +08:00
committed by cbb
parent 1dfdd48792
commit 318ad0e264
7 changed files with 541 additions and 21 deletions
+4 -2
View File
File diff suppressed because one or more lines are too long
+68
View File
@@ -0,0 +1,68 @@
# 避障路径规划优化报告(2026-10-15,基于 1dfdd48)
## 现状问题(旧 routeOrVia 阶梯)
1. **穿模**:`routeAroundPlan` 每段只查 ≤8 点(长段采样间隔 15°+),`coordinated` 二分松弛只查 6 点。
60 个"直线被挡"随机场景里 18 条路径在 1° 细验下实际穿模(13 条为 coordinated)。
2. **怪动作**:`CRUISE` 巡航位形把 J3/J5/J6 强制拉到固定值(0/-60/0),与障碍无关的腕部和大臂自转也被
"归零再转回";回退绕行(retract_pass)先收臂再伸展,路径绕远。
3. **单目标**:绕行只认一个目标解 qT,换个肘/腕构型就能直走的情况被忽略。
4. **动画顿挫**:`animTo` 每个中间位形各占固定 ms 并各自 smoothstep —— 每过一个 via 都减速到 0;
5° 小段与 150° 大段用同样时间。
## 改动
- `ik-solver.js` 新增 6c 节:
- `gatherGoals`:候选表里端点安全的 IK 解按加权关节距离取前 6 个(同盆地去重)作为目标集;
- 先试多目标直线(1.5° 细验)→ 再试弦中点 J2/J4 微调单中间点 → 再上 **Lazy RRT-Connect**
(起点树 ↔ 多根目标树;生长时只查节点 + 边中点,连通后整条路径细验,坏边判死子树);
- `shortcut`:随机捷径 + 贪心最远可达 + 逐轴拉直(去掉与障碍无关轴的来回摆);
- 加权度量 `PW=[1.6,1.4,1,1,0.7,0.5]`(近端轴代价高),定种子 LCG,结果可复现;
- 连续轴段内变化 <179°,保证与查看器短弧插值走的是同一条线;
- 规划失败时仍回落旧阶梯与 routeVia,最坏行为不劣于旧版。
- 碰撞窄相位前加**胶囊级包围球预筛**(保守、零漏判):`collideFree` 2.9 → 1.8 ms/次,
4000 个随机位形与旧实现判定 0 差异。
- `ik-glue.js animTo`:各段时长按"最慢轴所需时间"(轴速比 VREL)分配,整条折线一次全局缓入缓出,
中间位形不停顿;总时长按路径长度伸缩。
## 基准(`node tests/route_bench.cjs [solver] 60`,同一批 60 个直线被挡场景)
| 指标 | 旧 | 新 |
|---|---|---|
| 1° 细验无穿模的路径 | 42/60 | 58/60(剩 2 条最大侵入 0.28/0.08 mm,在 0.3 mm 安全裕度内) |
| 到达且无穿模 | 37 | 53 |
| 两版都成功的 37 例:平均关节总行程 | 417° | 388°(−7%) |
| 同上:估算执行时间 | 2.56 s | 2.46 s |
| 平均规划耗时(Node 单线程) | 0.72 s | 1.23 s(全集)/ 0.94 s(37 例) |
到不了的 5 例两版相同,属于目标超出安全可达区(IK 残差 4~235 mm),不是规划问题。
## 已知取舍 / 后续
- 规划耗时上升约 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%。
+43
View File
@@ -0,0 +1,43 @@
const setup=require('./route_bench_setup.cjs');
const solverPath=process.argv[2];const N=+process.argv[3]||40;const live=process.argv[4]==='live';
const {S}=setup(solverPath);
const L=S.LIM,O=S.ORDER,w=S.wrap180;
let s=12345;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);
function 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}
// build cases: free start, free goal config, straight blocked
const cases=[];let tries=0;
while(cases.length<N&&tries<20000){tries++;const a=rq(),b=rq();if(!S.collideFree(a)||!S.collideFree(b))continue;
if(segOK(a,b))continue; // only obstacle cases
cases.push({from:a,T:S.fk(b,0)});}
const W=[1,1,1,1,1,1];
let res={n:cases.length,reached:0,valid:0,blocked:0,len:0,lenDirect:0,ms:0,rev:0,segs:0,timeExec:0,fail:[]};
const VMAX=[60,60,90,90,120,180]; // deg/s assumed
function execTime(pts){let t=0;for(let i=1;i<pts.length;i++){const dd=d(pts[i-1],pts[i]);t+=Math.max(...dd.map((x,k)=>Math.abs(x)/VMAX[k]))+ (pts.length>2&&i<pts.length-1?0.25:0)}return t}
for(const c of cases){
const t0=performance.now();
const opt={tool:0,step:0.5,ref:c.from,from:c.from.slice()};
if(live){opt.noVia=true;opt.quick=true;opt.warm=c.from}
const r=live?S.solveRoute(c.T,opt):S.solveDyn(c.T,opt);
const ms=performance.now()-t0;res.ms+=ms;
const pts=[c.from].concat(r.via||[],[r.q]);
// unwrap
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 ok=true;for(let i=1;i<pts.length;i++)if(!segOK(pts[i-1],pts[i])){ok=false;break}
const tcp=S.fk(r.q,0);const err=Math.hypot(tcp[0]-c.T[0],tcp[1]-c.T[1],tcp[2]-c.T[2]);
const reached=err<1&&!r.blocked;
if(reached)res.reached++; if(ok)res.valid++; if(r.blocked)res.blocked++;
let len=0;for(let i=1;i<pts.length;i++)len+=d(pts[i-1],pts[i]).reduce((a,x)=>a+Math.abs(x),0);
const direct=d(c.from,r.q).reduce((a,x)=>a+Math.abs(x),0);
// reversals
let rev=0;for(let k=0;k<6;k++){let sg=0;for(let i=1;i<pts.length;i++){const x=d(pts[i-1],pts[i])[k];if(Math.abs(x)<2)continue;const s2=Math.sign(x);if(sg&&s2!==sg)rev++;sg=s2}}
res.per=res.per||[];res.per.push({ms:Math.round(ms),reached,ok,len:Math.round(len),rev,segs:pts.length-1,exec:+execTime(pts).toFixed(2),detour:r.detour||null});
if(reached&&ok){res.len+=len;res.lenDirect+=direct;res.rev+=rev;res.segs+=pts.length-1;res.timeExec+=execTime(pts)}
else res.fail.push({reached,ok,err:+err.toFixed(1),detour:r.detour||null,blocked:!!r.blocked});
}
const g=res.reached;const both=cases.length-res.fail.length;
require('fs').writeFileSync(process.argv[5]||require('path').join(require('os').tmpdir(),'route_bench_per.json'),JSON.stringify(res.per));console.log(JSON.stringify({n:res.n,reached:res.reached,validPath:res.valid,goodBoth:both,blocked:res.blocked,
avgPlanMs:+(res.ms/res.n).toFixed(0),avgJointTravel:+(res.len/both).toFixed(0),avgDirect:+(res.lenDirect/both).toFixed(0),
avgReversals:+(res.rev/both).toFixed(2),avgSegs:+(res.segs/both).toFixed(2),avgExecS:+(res.timeExec/both).toFixed(2),
failTypes:res.fail.slice(0,12)}));
+12
View File
@@ -0,0 +1,12 @@
/* 路径规划基准的求解器装配(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,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(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};
};
+26 -8
View File
@@ -340,7 +340,7 @@ function animTo(q, ms, via) {
end 保存求解器给的代表角,末帧落回它——与插值终点恰好相差 360° 的整数倍,
同一物理角,视觉零跳变,同时保证滑块/关节表数值与求解结果一致。
via:绕行中间位形(solveRoute 判直线被挡时给的两段式路径),逐段同样按短弧折算,
每段各 ms 毫秒——渲染走的段与碰撞校验走的段是同一批,不存在"校验过了但动画穿模"。 */
渲染走的段与碰撞校验走的段是同一批,不存在"校验过了但动画穿模"。 */
const segs = [];
let p = from;
for (const raw of (via || []).concat([q])) {
@@ -352,17 +352,34 @@ function animTo(q, ms, via) {
segs.push(t); p = t;
}
const end = q.slice();
const total = ms * segs.length;
/* 时间参数化(2026-10-15):旧版每段固定 ms 且每段各自 smoothstep —— 每个中间位形都
减速到 0 再起步(顿挫),且 5° 的小段和 150° 的大段用同样时间(小段慢吞吞、大段猛甩)。
现在:各段时长 ∝ 该段"最慢轴所需时间"(按轴速比 VMAX 折算,近端轴更慢),
整条折线只做一次全局 smoothstep:只在起点/终点加减速,途经中间位形不停顿。
走的仍是校验过的那条折线(中间位形经捷径平滑后通常只剩 1~2 个,折角很少)。 */
const VREL = [0.6, 0.6, 0.9, 0.9, 1.2, 1.8];
const pts = [from].concat(segs), dur = [];
for (let i = 1; i < pts.length; i++) {
let t = 0;
for (let j = 0; j < 6; j++) t = Math.max(t, Math.abs(pts[i][j] - pts[i - 1][j]) / VREL[j]);
dur.push(Math.max(t, 1e-3));
}
const sum = dur.reduce((a, b) => a + b, 0);
/* 总时长:ms 视为"标准 90° 移动"的时长,按路径实际长度伸缩,夹在 [0.6ms, 3.5ms] */
const total = Math.max(ms * 0.6, Math.min(ms * 3.5, ms * sum / 150));
const cum = [0]; for (const d of dur) cum.push(cum[cum.length - 1] + d / sum);
anim = { from: from, to: segs[segs.length - 1], t0: performance.now(), ms: total };
const tick = () => {
if (!anim) return;
const el = performance.now() - anim.t0;
const si = Math.min(segs.length - 1, Math.floor(el / ms));
const k = Math.min(1, (el - si * ms) / ms);
const e = k * k * (3 - 2 * k);
const a0 = si === 0 ? from : segs[si - 1], a1 = segs[si];
const u = Math.min(1, el / total);
const k = u;
const e = u * u * (3 - 2 * u); // 全局一次缓入缓出
let si = 0; while (si < dur.length - 1 && e > cum[si + 1]) si++;
const f = Math.min(1, Math.max(0, (e - cum[si]) / (cum[si + 1] - cum[si])));
const a0 = pts[si], a1 = pts[si + 1];
for (let i = 0; i < 6; i++) {
const id = ORDER[i], a = a0[i] + (a1[i] - a0[i]) * e;
const id = ORDER[i], a = a0[i] + (a1[i] - a0[i]) * f;
ang[id] = a;
const sl = $('s' + id), v = $('v' + id);
if (sl) sl.value = a;
@@ -492,7 +509,8 @@ function renderResult(res, src) {
: res.blocked
? '路径被立柱挡住 · 已停在边界(目标差 ' + dsc.toFixed(1) + ' mm)'
: res.detour
? (res.detour === 'retract_pass' ? '避障到达 · 模拟人类回退绕过立柱'
? (res.detour === 'planned' ? '避障到达 · 规划绕行(' + ((res.via || []).length) + ' 个中间位形)'
: res.detour === 'retract_pass' ? '避障到达 · 模拟人类回退绕过立柱'
: (res.detour === 'gentle' ? '避障到达 · 微调协同避开立柱'
: (res.detour === 'coordinated' ? '避障到达 · 平滑协同跨越立柱'
: '绕行立柱后到达')))
+348 -11
View File
@@ -175,6 +175,8 @@ function make(cfg) {
const HARD = (COL && COL.hard) || {};
const ADJ = (COL && COL.adj) || {}; // 相邻打印件对的 1D 自由角区间(度)
const PART = [], PARTPAIR = []; // 零件级(宽相位)与零件对
/* 胶囊半长(窄相位前的胶囊级包围球预筛用,2026-10-15) */
const HLEN = CAPG ? CAPG.map(c => 0.5 * Math.hypot(c.b[0] - c.a[0], c.b[1] - c.a[1], c.b[2] - c.a[2])) : null;
if (COL) {
const pidx = {};
for (const nm in COL.parts) {
@@ -260,12 +262,86 @@ function make(cfg) {
const ca = A.clus[ia], cb = B.clus[ib];
const pr = [];
for (const i of ca.caps) for (const j of cb.caps)
pr.push([i, j, CAPG[i].r + CAPG[j].r + CAPM]);
pr.push([i, j, CAPG[i].r + CAPG[j].r + CAPM, HLEN[i] + HLEN[j] + CAPG[i].r + CAPG[j].r + CAPM]);
if (pr.length) cl.push([ia, ib, pr, ca.rad + cb.rad, ca.rmax + cb.rmax]);
}
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)——
@@ -292,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) => {
@@ -340,34 +423,60 @@ 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];
/* 胶囊级包围球预筛:中点距 > 半长和 + 半径和 + margin 时线段距离必然 ≥ need(保守、无漏判),
实测 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 + 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 升序
(最贴合的排前面)。空数组 = 无动态干涉。 */
@@ -1994,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 = [];
@@ -2025,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) {
@@ -2193,11 +2305,236 @@ function make(cfg) {
}
return null;
}
/* ---------- 6c. 采样式避障规划:多目标 RRT-Connect + 捷径平滑(2026-10-15) ----------
取代原先"微调偏移 → 固定巡航位形 → 回退绕行 → 收臂转 J1"的手工阶梯:
· 旧阶梯的 CRUISE 位形把 J3/J5/J6 强制拉回固定值(0/-60/0),哪怕它们和障碍无关,
所以每次绕行都会出现"腕部无故甩一圈、大臂自转归零再转回去"的怪动作;
· 旧阶梯每段只查 ≤8 个点(长段间隔可达 15°+),且只认一个目标解;
· 找不到就贴边停(blocked),用户看到的是"卡住/抖动"。
新做法:
1) 目标集 = 候选表里所有端点安全的 IK 解(按加权关节距离取前 GOAL_K 个),
而不是单一 qT —— 换一个肘/腕构型往往就有直线可走;
2) 双向 RRT-Connect(起点树 ↔ 多根目标树),关节空间按加权距离扩展,
连续轴(J1/J3/J6)在起点 ±180° 的展开坐标里采样,天然走短弧;
3) 边检测按 EDGE_RES 度二分顺序(先中点再细分,撞点通常 1~3 次就被否决);
4) 捷径平滑:随机两点直连 + 贪心最远可达,去掉 RRT 的锯齿,
最终路径通常只剩 1~2 个中间位形,且只动"必须动"的轴;
5) 定种子 LCG:同一起点/目标结果可复现,不会每次松手走不同的路。 */
const PW = [1.6, 1.4, 1.0, 1.0, 0.7, 0.5]; // 加权:近端轴惯量大、扫掠体积大,动它代价更高
const EDGE_RES = 1.5, RRT_STEP = 20, GOAL_K = 6;
function dq(a, b) { // 展开坐标下的差(连续轴取短弧)
const o = new Array(6);
for (let i = 0; i < 6; i++) o[i] = LIM[ORDER[i]].cont ? wrap180(b[i] - a[i]) : b[i] - a[i];
return o;
}
function wdist(a, b) { // 加权 L2(最近邻 / 代价)
let s = 0;
for (let i = 0; i < 6; i++) { const x = (a[i] - b[i]) * PW[i]; s += x * x; }
return Math.sqrt(s);
}
function pathCost(pts) { let c = 0; for (let i = 1; i < pts.length; i++) c += wdist(pts[i - 1], pts[i]); return c; }
/* 二分顺序的边检测:端点由调用侧保证;先查中点、再 1/4、3/4……撞点早否决 */
function edgeFree(a, b, res) {
res = res || EDGE_RES;
let m = 0;
for (let i = 0; i < 6; i++) {
const d = Math.abs(b[i] - a[i]);
if (d >= 179 && LIM[ORDER[i]].cont) return false; // 动画按短弧插值:段内连续轴不得 ≥180°
m = Math.max(m, d);
}
const n = Math.max(1, Math.ceil(m / res));
if (n <= 1) return true;
const q = new Array(6), Q = [[0, n]];
for (let h = 0; h < Q.length; h++) { // 广度优先二分:中点 → 四分点 → …
const [lo, hi] = Q[h];
if (hi - lo < 2) continue;
const k = (lo + hi) >> 1, t = k / n;
for (let i = 0; i < 6; i++) q[i] = a[i] + (b[i] - a[i]) * t;
if (!collideFree(q)) return false;
Q.push([lo, k], [k, hi]);
}
return true;
}
/* 把 q 展开到离 ref 最近的等价角(连续轴),供规划在连续坐标里工作 */
function unwrapTo(ref, q) { return q.map((v, i) => LIM[ORDER[i]].cont ? ref[i] + wrap180(v - ref[i]) : v); }
function inBox(q) {
for (let i = 0; i < 6; i++) { const t = LIM[ORDER[i]]; if (!t.cont && (q[i] < t.lo - 1e-6 || q[i] > t.hi + 1e-6)) return false; }
return true;
}
function gatherGoals(cur, from) {
const seen = new Set(), out = [];
const add = q => {
if (!q || q.length !== 6) return;
const u = unwrapTo(from, q);
const key = u.map(v => Math.round(v)).join(',');
if (seen.has(key)) return;
seen.add(key); out.push(u);
};
add(cur.q);
const L = cur.list || [];
for (let i = 0; i < Math.min(L.length, 160); i++) add(L[i] && L[i].q);
out.sort((a, b) => wdist(from, a) - wdist(from, b));
const goals = [];
for (const g of out) {
if (goals.length >= GOAL_K) break;
if (goals.some(h => wdist(g, h) < 8)) continue; // 同一盆地只留一个
if (collideFree(g)) goals.push(g);
}
return goals;
}
function shortcut(pts, deadline, rnd) {
pts = pts.map(p => p.slice());
/* 随机捷径:在两段上各取一点直连 */
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);
const ta = rnd(), tb = rnd();
const pa = pts[a].map((v, k) => v + (pts[a + 1][k] - v) * ta);
const pb = pts[b].map((v, k) => v + (pts[b + 1][k] - v) * tb);
if (wdist(pa, pb) >= pathCost(pts.slice(a, b + 2).concat()) - 1e-6) continue;
if (!edgeFree(pa, pb)) continue;
pts = pts.slice(0, a + 1).concat([pa, pb], pts.slice(b + 1));
}
/* 贪心:从当前点直连最远可达点,删掉多余中间位形 */
const out = [pts[0]];
let i = 0;
while (i < pts.length - 1) {
let j = pts.length - 1;
while (j > i + 1 && !edgeFree(pts[i], pts[j])) j--;
out.push(pts[j]); i = j;
}
/* 逐轴拉直:中间位形的每个轴若能取"前后两点按段长插值"的值且两侧边仍无碰撞,就拉直它。
去掉 RRT 留下的"某轴来回摆"(与障碍无关的轴不该动),这正是视觉上"怪动作"的来源。 */
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);
for (let k = 0; k < 6; k++) {
const v = a[k] + (b[k] - a[k]) * t;
if (Math.abs(v - out[i][k]) < 0.5) continue;
const q = out[i].slice(); q[k] = v;
if (collideFree(q) && edgeFree(a, q) && edgeFree(q, b)) out[i] = q;
}
}
}
return out;
}
/* Lazy RRT-Connect:生长时只查新节点本身 + 边中点(2 次判定),连通后再对整条路径
逐边 2° 细验;不通的边把子节点判死(连同其子树),继续生长。碰撞判定是唯一的大头
(~3 ms/次),惰性验证把每次迭代从 ~25 ms 降到 ~6 ms。 */
function rrtConnect(from, goals, budgetMs, seed) {
const t0 = nowMs(), deadline = t0 + budgetMs;
let s = seed >>> 0 || 1;
const rnd = () => { s = (s * 1664525 + 1013904223) >>> 0; return s / 4294967296; };
const A = [{ q: from.slice(), p: -1, ok: true, dead: false }];
const B = goals.map(g => ({ q: g.slice(), p: -1, ok: true, dead: false }));
const alive = (T, i) => { while (i >= 0) { if (T[i].dead) return false; i = T[i].p; } return true; };
const nearest = (T, q) => {
let bi = -1, bd = Infinity;
for (let i = 0; i < T.length; i++) { const d = wdist(T[i].q, q); if (d < bd && alive(T, i)) { bd = d; bi = i; } }
return bi;
};
const steer = (a, b) => {
let m = 0; for (let i = 0; i < 6; i++) m = Math.max(m, Math.abs(b[i] - a[i]));
if (m <= RRT_STEP) return b.slice();
const k = RRT_STEP / m; return a.map((v, i) => v + (b[i] - v) * k);
};
const extend = (T, q) => { // 0=trapped 1=advanced 2=reached
const ni = nearest(T, q);
if (ni < 0) return [0, -1];
const qn = steer(T[ni].q, q);
if (!inBox(qn) || !collideFree(qn)) return [0, -1];
const mid = T[ni].q.map((v, i) => 0.5 * (v + qn[i]));
if (!collideFree(mid)) return [0, -1];
T.push({ q: qn, p: ni, ok: false, dead: false }); // ok=false:边尚未细验
return [wdist(qn, q) < 1e-6 ? 2 : 1, T.length - 1];
};
/* 细验一条树枝(叶→根),返回 true=全通;失败则判死第一条坏边的子端 */
const verify = (T, i) => {
while (i >= 0 && T[i].p >= 0) {
const n = T[i];
if (!n.ok) {
if (!edgeFree(T[n.p].q, n.q)) { n.dead = true; return false; }
n.ok = true;
}
i = n.p;
}
return true;
};
const trace = (T, i) => { const o = []; while (i >= 0) { o.push(T[i].q); i = T[i].p; } return o; };
let Ta = A, Tb = B, swapped = false, it = 0;
while (nowMs() < deadline) {
it++;
let qr;
if (rnd() < 0.1) qr = Tb[Math.floor(rnd() * Tb.length)].q.slice();
else qr = ORDER.map((id, i) => { const t = LIM[id]; return t.cont ? from[i] - 180 + 360 * rnd() : t.lo + (t.hi - t.lo) * rnd(); });
const [st, ia] = extend(Ta, qr);
if (st) {
const qa = Ta[ia].q;
let r = [1, -1];
while (r[0] === 1) r = extend(Tb, qa);
if (r[0] === 2) {
/* 连接点在两棵树里是同一位形:两侧树枝都细验通过才算路径 */
const okA = verify(Ta, ia), okB = okA && verify(Tb, r[1]);
if (okA && okB) {
const pa = trace(Ta, ia), pb = trace(Tb, r[1]);
let path = pa.reverse().concat(pb.slice(1));
if (swapped) path = path.reverse();
return { path: path, iters: it, ms: nowMs() - t0, rnd: rnd };
}
}
}
const t = Ta; Ta = Tb; Tb = t; swapped = !swapped;
if (Ta.length + Tb.length > 6000) break;
}
return null;
}
function planAround(from, cur, quick) {
const goals = gatherGoals(cur, from);
if (!goals.length) return null;
const budget = quick ? 150 : 1500;
/* 先试直线(多目标、2° 细验):换个构型能直走就不绕 */
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, 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;
if (!inBox(m) || !collideFree(m)) continue;
if (edgeFree(from, m) && edgeFree(m, g)) return { goal: g, via: [m], detour: 'planned' };
}
}
let seed = 2166136261;
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 : 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;
const around = opt.noAround ? null : 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' });
/* 首选:采样规划(多目标 + 平滑),给出的路径已按 EDGE_RES 细验 */
if (!opt.noAround) {
const pl = planAround(from, cur, opt.quick);
if (pl) {
const res = Object.assign({}, cur, { q: pl.goal.map((v, i) => LIM[ORDER[i]].cont ? wrap180(v) : v), routed: true, blocked: false, kept: false });
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' });
}
if (opt.noVia) return r;
return routeVia(from, target, opt) || r;
}
@@ -2313,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,
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,
+40
View File
@@ -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));