fix(ik): 贴立柱目标不再误判为立柱区内：候选表分层保留 + 姿态放宽兜底 + 实时管线跨帧坏边/目标解优先/预算递增/半格复验；新增贴立柱基准 tests/near_column_bench.cjs
This commit is contained in:
@@ -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 <solver.js> [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));
|
||||
@@ -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'
|
||||
|
||||
+168
-13
@@ -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 });
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user