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

- 避障路径改为多目标 Lazy RRT-Connect + 捷径平滑；碰撞窄相位预测提速。
- 规划提速 32%：(2,4) 零件对类离线间隙表、撞点前移、判定记忆、routeFrom 惰性回退、平滑预算收紧。

tests/route_bench.cjs

@@ -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

@@ -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};
+};