cbb/Robot_Arm
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(ik): 避障路径改为多目标 Lazy RRT-Connect + 捷径平滑；碰撞窄相位预筛提速 38%；动画按轴速比时间参数化、途经点不停顿

Browse Source
This commit is contained in:
Hark
2026-10-15 05:05:24 +00:00
parent 1dfdd48792
commit 8ad5b3b4fa
5 changed files with 351 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
docs/路径规划优化报告.md
+40
View File
@@ -0,0 +1,40 @@
# 避障路径规划优化报告（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）+ 拐角抛物线过渡，
过渡段需重新做碰撞校验。
tests/route_bench.cjs
+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)}));
tests/route_bench_setup.cjs
+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){
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 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};
};
webapp/js/ik-glue.js
+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' ? '避障到达 · 平滑协同跨越立柱'
: '绕行立柱后到达')))
webapp/js/ik-solver.js
+230 -4
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,7 +262,7 @@ 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]);
@@ -353,6 +355,11 @@ function make(cfg) {
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 * bl) 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 (d < need) {
@@ -2193,11 +2200,230 @@ 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 < 80 && 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 < 2 && 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, 3)) {
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 : 250), 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);
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;
}
/* 规划预算内未连通（极少）：退回旧的启发式阶梯 */
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 +2539,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,
mixSolve: mixSolve, closest: closest, closest6: closest6, closestOri: closestOri, orient: orient,
branch: branch, selfTest: selfTest, poseSelfTest: poseSelfTest,
collideFree: collideFree, collideDetail: collideDetail,