diff --git a/tests/near_column_bench.cjs b/tests/near_column_bench.cjs new file mode 100644 index 0000000..b2c5f33 --- /dev/null +++ b/tests/near_column_bench.cjs @@ -0,0 +1,66 @@ +/* 贴立柱目标基准（2026-10-17，用户报告"立柱另一侧 → 贴立柱目标误判在立柱区内"） + * 目标位形 b：本身无干涉，但 J1/J2/J3 任一轴 ±4° 就撞立柱（= 贴着立柱）。 + * 起点 a：无干涉，且 a→b 关节直线被挡（立柱另一侧）。 + * 模式：pos = 松手位置任务；pose = 松手位置+姿态任务（姿态取 b 的真实姿态，b 本身就是安全精确解）； + * rt = 拖拽实时管线，目标不动连续重规划直到到达或 3 s。 + * 判据：到达（TCP 误差 < 1 mm，pose 另要姿态 < 0.5°）且整条路径 1° 细验无干涉。 + * node tests/near_column_bench.cjs [N=30] [modes=pos,pose,rt] */ +const setup = require('./route_bench_setup.cjs'); +const { S, ArmIK } = setup(process.argv[2]); +const N = +process.argv[3] || 30, MODES = (process.argv[4] || 'pos,pose,rt').split(','); +const L = S.LIM, O = S.ORDER, w = S.wrap180; +let s = 20261017; 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); +const segOK = (a, b) => { const dd = d(a, b); const m = Math.max(...dd.map(Math.abs)); const n = Math.max(1, Math.ceil(m)); for (let i = 1; i <= n; i++) if (!S.collideFree(a.map((v, k) => v + dd[k] * i / n))) return false; return true; }; +const hitsColumn = q => { const det = S.collideDetail(q) || []; return det.some(p => /立柱/.test(p.cnA + p.cnB)); }; +const nearColumn = q => { for (const k of [0, 1, 2]) for (const sg of [-4, 4]) { const p = q.slice(); p[k] += sg; if (hitsColumn(p)) return true; } return false; }; +const cases = []; let tries = 0; +while (cases.length < N && tries < 400000) { + tries++; + const b = rq(); if (!S.collideFree(b) || !nearColumn(b)) continue; + let a = null; + for (let t = 0; t < 200; t++) { const c = rq(); if (S.collideFree(c) && !segOK(c, b)) { a = c; break; } } + if (a) cases.push({ a, b }); +} +if (process.env.DUMP) require('fs').writeFileSync(process.env.DUMP, JSON.stringify(cases)); +const out = {}; +for (const mode of MODES) { + let ok = 0, reached = 0, valid = 0, unsafe = 0, relaxed = 0, ms = 0, worst = 0; const fails = []; + cases.forEach((c, ci) => { + const T = S.fk(c.b, 0), R = S.orient(c.b); + const opt = { tool: 0, step: 0.5, ref: c.a, from: c.a.slice() }; + if (mode === 'pose') { opt.pose = { R }; opt.priority = 'pos'; } + if (S.rtReset) S.rtReset(); + const t0 = performance.now(); + let pts = [c.a.slice()], r = null; + if (mode === 'rt') { + let q = c.a.slice(); + for (let it = 0; it < 40; it++) { // 每次重规划后沿路径走到头（目标不动；最多 40 次重规划） + r = S.solveRoute(T, Object.assign({}, opt, { from: q.slice(), ref: q, warm: q, noVia: true, quick: true })); + if (!r || !r.q) break; + const seg = (r.via || []).concat([r.q]); + for (const p of seg) pts.push(p); + const nq = r.q.slice(); + const tcp = S.fk(nq, 0); + if (Math.hypot(tcp[0] - T[0], tcp[1] - T[1], tcp[2] - T[2]) < 1) { q = nq; break; } + q = nq; + } + } else { + r = S.solveDyn(T, opt); + pts = pts.concat(r.via || [], [r.q]); + } + const t = performance.now() - t0; ms += t; worst = Math.max(worst, t); + 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 pv = true; for (let i = 1; i < pts.length; i++) if (!segOK(pts[i - 1], pts[i])) { pv = false; break; } + const qf = pts[pts.length - 1], tcp = S.fk(qf, 0); + const err = Math.hypot(tcp[0] - T[0], tcp[1] - T[1], tcp[2] - T[2]); + const oerr = mode === 'pose' ? ArmIK.rotErrDeg(S.orient(qf), R) : 0; + const rc = err < 1 && oerr < 0.5; + if (rc) reached++; if (pv) valid++; if (rc && pv) ok++; + if (r && r.unsafe) unsafe++; if (r && r.poseRelaxed) relaxed++; + if (!(rc && pv)) fails.push({ ci, err: +err.toFixed(1), oerr: +oerr.toFixed(1), pathOK: pv, unsafe: !!(r && r.unsafe), blocked: !!(r && r.blocked) }); + }); + out[mode] = { n: cases.length, ok, reached, validPath: valid, unsafe, poseRelaxed: relaxed, msMean: Math.round(ms / cases.length), msMax: Math.round(worst), fails: fails.slice(0, 12) }; +} +console.log(JSON.stringify(out, null, 1)); diff --git a/webapp/js/ik-glue.js b/webapp/js/ik-glue.js index d839a6e..6d274a6 100644 --- a/webapp/js/ik-glue.js +++ b/webapp/js/ik-glue.js @@ -538,6 +538,7 @@ function renderResult(res, src) { : (res.detour === 'gentle' ? '避障到达 · 微调协同避开立柱' : (res.detour === 'coordinated' ? '避障到达 · 平滑协同跨越立柱' : '绕行立柱后到达'))) + : (res.poseRelaxed && posOk) ? '位置到达 · 精确姿态会撞立柱，已取最接近的无碰撞姿态（偏差 ' + dori.toFixed(1) + '\u00B0）' : (posOk && oriOk) ? '已到达' : oriPrio ? '姿态到达·位置最近 ' + dsc.toFixed(1) + ' mm' : mixMid ? '混合折中 · 位置差 ' + dsc.toFixed(1) + ' mm · 姿态残差 ' + dori.toFixed(2) + '\u00B0' diff --git a/webapp/js/ik-solver.js b/webapp/js/ik-solver.js index 408b016..ac7e7e0 100644 --- a/webapp/js/ik-solver.js +++ b/webapp/js/ik-solver.js @@ -803,6 +803,19 @@ function make(cfg) { const R = Math.hypot(BY, BZ), ph = Math.atan2(BZ, BY); const tol = opt.tol || 1e-6, tol2 = tol * tol; const KEEP = 4000; + /* 分层保留（2026-10-17）：候选表原先"按行程代价截前 KEEP 条"。行程以当前位形 ref 为参照， + 手臂在立柱另一侧时，前 4000 条全挤在"离当前最近、但贴着立柱"的那几个盆地里——实测目标 + (-136.8, 272.3, 245.5)：无 ref 时 4000 条里 3318 条无干涉，ref=立柱另一侧时 0 条， + 于是判「目标在立柱区内」，换个位置再来又能到（用户报告的现象）。 + 改为：前一半按代价取最优，后一半在其余候选里按代价顺序等距抽样，保住远端盆地的代表。 */ + const thinKeep = (arr, keep, cmp) => { + arr.sort(cmp); + if (arr.length <= keep) return arr; + const h = keep >> 1, out = arr.slice(0, h), rest = arr.length - h, m = keep - h; + for (let k = 0; k < m; k++) out.push(arr[h + Math.floor(k * rest / m)]); + return out; + }; + let altPose = null; /* 参考位姿（ref）：交互求解时以当前姿态为参考，选解判据取「所有电机需要转过的 角度之和」最小 —— 即这一步行程最短的构型（取代原先的 Σ|θ| 最小）。 动机：等价多分支（θ3 的 ±σ/±(π−σ)、肘上/肘下、以及 J1/J3 同号反号的镜像族） @@ -882,7 +895,6 @@ function make(cfg) { } return best; }; - /* 采样轴：含上下限位端点，避免恰在限位上的最优解被漏掉 */ const axis = (lo, hi, step) => { const v = []; @@ -955,9 +967,8 @@ function make(cfg) { if (sv) { all.push(sv); nSol++; if (POSE && sv.oerr <= 1) band.push(sv); } } } - if (!POSE && all.length > KEEP * 12) { // 有界保留：只留代价最优者 - all.sort((x, y) => x.rcost - y.rcost); - all.length = KEEP; + if (!POSE && all.length > KEEP * 12) { // 有界保留：代价最优一半 + 其余分层抽样（见 thinKeep） + all = thinKeep(all, KEEP, (x, y) => x.rcost - y.rcost); trimmed = true; } } @@ -965,7 +976,10 @@ function make(cfg) { band.sort((x, y) => x.oerr - y.oerr); const keepBand = band.slice(0, KEEP); const keepSet = new Set(keepBand); - const rest = all.filter(s => !keepSet.has(s)).sort((x, y) => x.rcost - y.rcost).slice(0, KEEP); + /* 位置精确族按姿态残差排的分层样本：精确姿态的解全撞立柱时，供上层退而取 + "位置精确、姿态最接近的无干涉解"（findSafeTarget），不再整单判不可达 */ + altPose = thinKeep(all.slice(), 800, (x, y) => x.oerr - y.oerr).map(s => ({ q: s.q.slice(), oerr: s.oerr, cost: s.cost, err: 0 })); + const rest = thinKeep(all.filter(s => !keepSet.has(s)), KEEP, (x, y) => x.rcost - y.rcost); all = keepBand.concat(rest); trimmed = band.length + rest.length < nSol; } else all.sort((x, y) => x.rcost - y.rcost); @@ -1268,7 +1282,7 @@ function make(cfg) { pool.sort((x, y) => poolByOerr ? ((x.oerr - y.oerr) || (x.rc0 - y.rc0)) : (POSE ? x.rc0 - y.rc0 : x.rcost - y.rcost)); const best = pool.length ? pool[0] : null; - const list = pool.slice().sort((x, y) => POSE ? x.rc0 - y.rc0 : x.rcost - y.rcost).slice(0, KEEP); + const list = thinKeep(pool.slice(), KEEP, (x, y) => POSE ? x.rc0 - y.rc0 : x.rcost - y.rcost); if (best && list.indexOf(best) < 0) list.unshift(best); /* 真残差补算：热循环里延后的 fk 校验在这里对入选候选统一补一次（err 为真值） */ if (best) { @@ -1299,7 +1313,7 @@ function make(cfg) { })() : null, err: best ? best.err : null, oerr: best ? best.oerr : null, nSol: nSol, truncated: trimmed, - samples: samples, ms: nowMs() - t0, list: list + samples: samples, ms: nowMs() - t0, list: list, altPose: altPose }; } @@ -2090,13 +2104,29 @@ function make(cfg) { if (best) return best; const len = (cur.list || []).length; if (len > 120) { - const stride = Math.max(1, Math.floor((len - 120) / 40)); + /* 尾部分层样本（thinKeep 保留的远端盆地代表）：全部过一遍取离起点最近的安全者， + 而不是第一个碰到的（2026-10-17：原先取首个，常落到行程远大的盆地） */ + const stride = Math.max(1, Math.floor((len - 120) / 60)); for (let i = 120; i < len; i += stride) { const s = cur.list[i]; if (s && s.q && collideFree(s.q)) { - return { q: s.q, res: Object.assign({}, cur, { q: s.q.slice(), cost: s.cost, err: s.err, oerr: s.oerr }) }; + const cst = costOf(s.q, null, from); + if (cst < bestCost) { bestCost = cst; best = { q: s.q, res: Object.assign({}, cur, { q: s.q.slice(), cost: s.cost, err: s.err, oerr: s.oerr }) }; } } } + if (best) return best; + } + /* 姿态任务：精确姿态的解全撞（目标贴着立柱、姿态又朝立柱时常见）→ 位置精确族里取 + 姿态残差最小的无干涉解，标 poseRelaxed（界面显示"位置到达·姿态残差 x°"）， + 不再判「目标在立柱区内」（2026-10-17 用户报告） */ + const alt = cur.altPose || []; + if (alt.length && cur.priority !== 'ori') { + const lim = Math.min(alt.length, 400); + for (let i = 0; i < lim; i += (i < 40 ? 1 : 4)) { + const s = alt[i]; + if (s && s.q && collideFree(s.q)) + return { q: s.q, res: Object.assign({}, cur, { q: s.q.slice(), cost: s.cost, err: 0, oerr: s.oerr, poseRelaxed: true }) }; + } } return null; } @@ -2172,6 +2202,9 @@ function make(cfg) { else if (near) cur = Object.assign({}, FULLIK.cur, { approx: true }); // 拖动中：先朝附近目标的安全解走，目标停下再精解（避免每帧全量 IK 尖峰） else { const o2 = Object.assign({}, opt); delete o2.warm; o2.quick = false; + /* 只要目标解，不要路由：带着 from 时 solveDyn 会顺带整套 routeOrVia（慢），且返回的 q 可能是 + "贴边停住"的位形——再被缓存成 FULLIK 目标，实时管线就朝停靠点规划（2026-10-17 贴立柱测试发现） */ + delete o2.from; /* solveDyn：按动态限位挑"本身无干涉"的解（全量表按代价截断在 4000 条，安全支常排在很后面甚至被截掉） */ const full = solveDyn(target, o2); if (full && full.q) { cur = Object.assign({}, full); FULLIK.key = key; FULLIK.cur = cur; FULLIK.T = target.slice(); FULLIK.at = nowMs(); } @@ -2363,6 +2396,7 @@ function make(cfg) { } const n = Math.max(1, Math.ceil(m / res)); if (n <= 1) return true; + if (BADE.size && BADE.has(segKey(a, b))) return false; // 半格复验判过的坏段（pathFine） const q = new Array(6), Q = [[0, n]]; for (let h = 0; h < Q.length; h++) { // 广度优先二分：中点 → 四分点 → … const [lo, hi] = Q[h]; @@ -2395,6 +2429,13 @@ function make(cfg) { for (let i = 0; i < Math.min(L.length, listMax); i++) add(L[i] && L[i].q); out.sort((a, b) => wdist(from, a) - wdist(from, b)); const goals = [], tried = []; + /* IK 选定的目标解（solveDyn/findSafeTarget 已保证它无干涉）永远先入选：原先它和候选表一起按 + "离起点距离"排队，起点在立柱另一侧时，限量模式（cfMax=30）的判定额度全耗在离起点近、 + 贴立柱必撞的盆地上，轮不到它 → goals 为空 → 每帧"停在边界"（2026-10-17 贴立柱测试 ci5） */ + if (cur.q && cur.q.length === 6) { + const u = unwrapTo(from, cur.q); + if (collideFree(u)) { goals.push(u); tried.push(u); cfMax--; } + } for (const g of out) { if (goals.length >= kMax || cfMax <= 0) break; if (goals.some(h => wdist(g, h) < 8)) continue; // 同一盆地只留一个 @@ -2565,6 +2606,20 @@ function make(cfg) { return best; } const RM_K = 12; + /* 跨帧坏边表（2026-10-17）：原先 bad 只在单次查询内有效。目标贴着立柱时接入边大多不通， + 40 ms 预算刚好耗在"重新验同一批坏边"上，下一帧又从头验——拖拽松手前永远规划不到， + 实测 30 个贴立柱目标里 12 个一直停在边界。碰撞模型是静态的，坏边可以永久记住； + 节点用位形坐标做键（起点/目标/增广节点的编号每次查询都会变）。 */ + const BADE = new Set(); + const DEAD = new Set(); // 半格复验否决的图边（路网/增广节点编号对） + function rmIdsAt(q) { // 位形 → 与之重合的路网/增广节点编号 + const ids = []; + if (!RM) return ids; + const M = RM.N + RMX.Q.length; + for (let i = 0; i < M; i++) if (wd(q, i < RM.N ? RM.Q[i] : RMX.Q[i - RM.N]) < 1e-3) ids.push(i); + return ids; + } + const pkey = (i, p) => i < (RM ? RM.N : 0) ? 'r' + i : p.map(v => v.toFixed(2)).join(','); /* 返回 { path:[from, ..., goal] (展开坐标) } 或 null */ function rmPlan(from, goals, budgetMs) { if (!RM) return null; @@ -2582,16 +2637,22 @@ function make(cfg) { const isGoal = i => i >= G0; const h = i => { let m = Infinity; const p = pos(i); for (const g of goals) m = Math.min(m, wd(p, g)); return m; }; let timedOut = false; + const skey = (a, b) => { const x = pkey(a, pos(a)), y = pkey(b, pos(b)); return x < y ? x + '|' + y : y + '|' + x; }; const edgeOk = (a, b) => { // 只对本次查询的待验边做在线细验 const k = ekey(a, b); if (good.has(k)) return true; if (bad.has(k)) return false; + const sk = skey(a, b); + if (BADE.has(sk)) { bad.add(k); return false; } if (nowMs() > deadline) { timedOut = true; return false; } const pa = pos(a), pb = unwrapTo(pa, pos(b)); const ok = edgeFree(pa, pb); (ok ? good : bad).add(k); + if (!ok) { if (BADE.size > 300000) BADE.clear(); BADE.add(sk); } return ok; }; + /* 已知坏的接入边直接不进图：省掉 A* 反复选中它们再被否决的轮次 */ + for (const [u, lst] of extra) for (let j = lst.length - 1; j >= 0; j--) if (BADE.has(skey(u, lst[j][0]))) { bad.add(ekey(u, lst[j][0])); } const augment = () => { /* 增广（成功或超时都做：已验的接入边是下一帧的资产，跨帧"随时可停"地累积进度）：起点 / 用到的目标并入，带上所有已验通过的接入边 */ if (RMX.Q.length + 2 <= RMX_MAX) { @@ -2626,6 +2687,7 @@ function make(cfg) { if (isGoal(u)) { found = u; break; } const relax = (v, c, lazy) => { if (lazy && bad.has(ekey(u, v))) return; + if (!lazy && DEAD.size && DEAD.has(ekey(u, v))) return; const gv = gu + c; if (gv < (gsc.has(v) ? gsc.get(v) : Infinity)) { gsc.set(v, gv); par.set(v, [u, lazy]); push(gv + h(v), v); } }; @@ -2674,6 +2736,7 @@ function make(cfg) { 于是一次重规划通常只验 1~2 条短边（几毫秒），而不是重新接入路网。 路径尾巴会累积小折线：每次先试"倒数第二点 → 新目标"直连，能连就替换掉旧目标。 */ let RT = null; // { path:[展开位形...] } + const RTSTALL = { key: null, n: 0 }; // 同一目标连续规划失败的次数（预算递增用） function onPath(path, q, tol) { // q 是否在折线上：返回 [段号, 参数] 或 null for (let i = 0; i + 1 < path.length; i++) { const a = path[i], b = unwrapTo(a, path[i + 1]); @@ -2711,6 +2774,37 @@ function make(cfg) { /* 实时管线（拖拽 quick 模式且路网可用时）：只走"每步都细验过"的分支，并且每一步都有硬时限。 直连 → 热启动 → 路网 Lazy A*（40 ms）→ 都不行就沿直线走到最后一个安全点停住。 不再进入旧的启发式阶梯（慢，且其段只抽查少量点，拖拽仿真里实测会穿模）。 */ + /* 交付前半格复验（2026-10-17）：规划期按 EDGE_RES=1.5° 细验，贴立柱时薄薄一层干涉能整个夹在 + 两个采样点之间（贴立柱测试 ci0：119.5° 的长边在 0.76 处 1° 复验撞）。交付给动画前把每段的 + 采样点错开半格再验一遍（等效 0.75°）；验过的段记住，拖拽中重复交付同一段不再付费。 */ + const FINE = new Set(); + function segKey(a, b) { return a.map(v => v.toFixed(2)).join(',') + '>' + b.map(v => v.toFixed(2)).join(','); } + function pathFine(path) { + for (let i = 1; i < path.length; i++) { + const a = path[i - 1], b = unwrapTo(a, path[i]), k = segKey(a, b); + if (FINE.has(k)) continue; + let m = 0; for (let j = 0; j < 6; j++) m = Math.max(m, Math.abs(b[j] - a[j])); + const n = Math.max(1, Math.ceil(m / EDGE_RES)); + if (n > 1 || m > EDGE_RES * 0.5) { + const q = new Array(6); + for (let t = 0; t < n; t++) { + const u = (t + 0.5) / n; + for (let j = 0; j < 6; j++) q[j] = a[j] + (b[j] - a[j]) * u; + if (!collideFree(q)) { + if (BADE.size > 300000) BADE.clear(); + BADE.add(segKey(a, b)); BADE.add(segKey(b, a)); + /* 这段若是图里的已验边（路网边或增广边，A* 不再细验），把它判死，否则下一帧 A* 原样再走 */ + const ia = rmIdsAt(a), ib = rmIdsAt(b); + for (const x of ia) for (const y of ib) DEAD.add(x < y ? x * 65536 + y : y * 65536 + x); + return false; + } + } + } + if (FINE.size > 20000) FINE.clear(); + FINE.add(k); + } + return true; + } function routeRT(from, cur) { if (!COL || !cur || !cur.q || !from) return cur; if (!collideFree(from)) return cur; // 起点已在干涉里：交给脱困逻辑 @@ -2719,17 +2813,32 @@ function make(cfg) { const t0 = nowMs(); const goals = gatherGoals(cur, from, 3, 4000, 30); const done = (path, detour, blocked) => { + /* 半格复验不过：这一帧不交付这条路径——沿旧路径继续/原地等，下一帧重规划（坏段已记入 BADE） */ + if (!blocked && path.length >= 2 && !pathFine(path)) { + RT = null; + return Object.assign({}, cur, { q: from.map((v, i) => LIM[ORDER[i]].cont ? wrap180(v) : v), via: [], + routed: true, blocked: false, kept: false, detour: 'continue', pending: true, planMs: nowMs() - t0 }); + } RT = { path: path }; const q = path[path.length - 1]; return Object.assign({}, cur, { q: q.map((v, i) => LIM[ORDER[i]].cont ? wrap180(v) : v), via: path.slice(1, -1), routed: true, blocked: !!blocked, kept: false, detour: detour, planMs: nowMs() - t0 }); }; if (goals.length) { - if (edgeFree(from, goals[0])) return done([from.slice(), goals[0]], null); + if (!BADE.has(segKey(from, goals[0])) && edgeFree(from, goals[0])) { + const r0 = done([from.slice(), goals[0]], null); + if (!r0.pending) return r0; + } const w = rtWarm(from, goals, t0 + 25); if (w) return done(w, w.length > 2 ? 'planned' : null); - const rp = rmPlan(from, goals, 40); - if (rp) return done(rmSmooth(rp.path, nowMs() + 15), 'planned'); + /* 目标停住而规划连续失败时逐帧加预算 40→80→160→240 ms（2026-10-17）：贴立柱目标的接入边 + 又长又多半不通，单帧 40 ms 只够验一两条；目标不动时延迟无所谓，按住不放也能很快规划出来。 + 目标一挪（键变）立即回到 40 ms，拖动跟手不受影响。 */ + const gk = goals[0].map(v => Math.round(v)).join(','); + const stall = RTSTALL.key === gk ? RTSTALL.n : 0; + const rp = rmPlan(from, goals, Math.min(240, 40 << stall)); + if (rp) { RTSTALL.key = null; RTSTALL.n = 0; return done(rmSmooth(rp.path, nowMs() + 15), 'planned'); } + RTSTALL.key = gk; RTSTALL.n = Math.min(3, stall + 1); /* 本帧没规划完：沿上一条（已验）路径继续走，下一帧接着规划——不急停、不乱走 */ if (RT && RT.path.length >= 2) { const hit = onPath(RT.path, from, 0.05); @@ -2846,6 +2955,52 @@ function make(cfg) { +108.75°（肩侧抬 90°）。若静态解的某轴贴在边界上，就按当前姿态二分出 该轴的动态边界，临时放宽该轴限位后重解；新解必须通过全姿态碰撞判定、 且总关节角不升才采用，否则原样返回（最坏即静态行为，无回退风险）。 */ + /* 姿态放宽兜底（2026-10-17）：目标贴着立柱时，指定姿态的解族（只保留姿态残差 ≤POSETOL 的） + 可能整族都撞立柱，但这个位置本身完全够得到。位置优先/混合模式下改解一次纯位置任务， + 对每个候选按 J6 闭式最优滚转（TCP 在 J6 轴上，J6 不改位置）算姿态残差，按残差升序取第一个 + 无干涉解。只在"无安全解"的兜底分支里跑（多一次全量 IK），姿态优先模式不放宽。 */ + function bestRoll(q, Rt) { + const t = q.slice(); + let bv = Infinity, bj = t[5]; + for (let a = -180; a < 180; a += 4) { t[5] = a; const e = rotErrDeg(orient(t), Rt); if (e < bv) { bv = e; bj = a; } } + let lo = bj - 4, hi = bj + 4; + for (let k = 0; k < 30; k++) { + const m1 = lo + (hi - lo) / 3, m2 = hi - (hi - lo) / 3; + t[5] = m1; const e1 = rotErrDeg(orient(t), Rt); t[5] = m2; const e2 = rotErrDeg(orient(t), Rt); + if (e1 < e2) hi = m2; else lo = m1; + } + t[5] = 0.5 * (lo + hi); + const L6 = LIM[ORDER[5]]; + if (L6.cont) t[5] = wrap180(t[5]); else t[5] = Math.min(L6.hi, Math.max(L6.lo, t[5])); + return { q: t, oerr: rotErrDeg(orient(t), Rt) }; + } + function poseRelax(target, opt, cur) { + if (!opt.pose || !opt.pose.R || opt.priority === 'ori') return null; + const o2 = Object.assign({}, opt); delete o2.pose; delete o2.priority; delete o2.mixW; delete o2.warm; + delete o2.ref; // 不带行程参照：候选表不偏向起点那一侧（按姿态残差挑） + const r = solve(target, o2); + if (!r || !r.list || !r.list.length) return null; + const Rt = opt.pose.R; + const cand = r.list.map(s => bestRoll(s.q, Rt)); + cand.sort((x, y) => x.oerr - y.oerr); + /* 去重：J1~J5 按 6° 栅格分桶，同桶只验姿态残差最小的那个（撞立柱的候选成片相邻， + 不分桶时实测要验上千个才碰到第一个安全解） */ + const seenB = new Set(); + let checks = 0; + for (const c of cand) { + if (checks >= 300) break; + const key = c.q.slice(0, 5).map(v => Math.round(v / 6)).join(','); + if (seenB.has(key)) continue; + seenB.add(key); + checks++; + if (collideFree(c.q)) { + return { q: c.q, res: Object.assign({}, cur, { q: c.q.slice(), cost: costOf(c.q), err: 0, oerr: c.oerr, + reachable: true, ok: true, poseRelaxed: true, poseFine: false, poseTier: c.oerr <= 2 ? 'approx' : 'poor', + list: r.list }) }; + } + } + return null; + } function solveDyn(target, opt) { opt = opt || {}; if (opt.from && !opt.ref) opt.ref = opt.from; @@ -2855,7 +3010,7 @@ function make(cfg) { sweep_v2 的组合扫描只记录不收紧滑块）。这里对最终解做全姿态碰撞校验： 撞了就沿候选表找安全的解，优先采用最贴近起点的安全位形。 */ if (!collideFree(cur.q)) { - const safeT = findSafeTarget(cur, opt.from); + const safeT = findSafeTarget(cur, opt.from || opt.ref) || poseRelax(target, opt, cur); if (safeT) cur = Object.assign({}, safeT.res, { swaped: true }); else cur = Object.assign({}, cur, { unsafe: true }); }