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