/* ============================================================================ * arm-ik-solver.js — 六轴手臂逆运动学求解器（最小总关节角） * * 正运动学：World = A1(θ1)·A2(θ2)·A3(θ3)·A4(θ4)·A5(θ5)·A6(θ6)·TCP， * Ai(θ) = T(Pi)·R(axis_i, θ)·T(−Pi)，与查看器场景图完全一致。 * 逆运动学：以 (θ4,θ5) 为二维冗余参数全域扫描，其余三轴闭式求解。 * * MIT / 无依赖。可在浏览器（window.ArmIK）或 Node（module.exports）中加载。 * ==========================================================================*/ (function (root, factory) { if (typeof module === 'object' && module.exports) module.exports = factory(); else root.ArmIK = factory(); }(typeof self !== 'undefined' ? self : this, function () { 'use strict'; const D2R = Math.PI / 180, R2D = 180 / Math.PI; const ORDER = ['J1', 'J2', 'J3', 'J4', 'J5', 'J6']; /* ---------- 小工具 ---------- */ const wrap180 = a => { a = (a + 180) % 360; if (a < 0) a += 360; return a - 180; }; const nrm = v => { const n = Math.hypot(v[0], v[1], v[2]) || 1; return [v[0] / n, v[1] / n, v[2] / n]; }; function rotAxis(d, a) { // 3x3 旋转：绕单位轴 d 转 a 弧度（右手法） const c = Math.cos(a), s = Math.sin(a); /* 轴对齐特化：查看器 6 个关节轴均为 ±X/±Y/±Z，Rodrigues 通用式退化为每轴 4 个非零元素 （数学恒等；绕 −X 转 a = 绕 X 转 −a）。姿态路径热点（rotAxis 曾占 ~11%）。 */ const x = d[0], y = d[1], z = d[2]; if (y === 0 && z === 0) { if (x > 0) return [1, 0, 0, 0, c, -s, 0, s, c]; return [1, 0, 0, 0, c, s, 0, -s, c]; } if (x === 0 && z === 0) { if (y > 0) return [c, 0, s, 0, 1, 0, -s, 0, c]; return [c, 0, -s, 0, 1, 0, s, 0, c]; } if (x === 0 && y === 0) { if (z > 0) return [c, -s, 0, s, c, 0, 0, 0, 1]; return [c, s, 0, -s, c, 0, 0, 0, 1]; } const C = 1 - c; return [ x * x * C + c, x * y * C - z * s, x * z * C + y * s, y * x * C + z * s, y * y * C + c, y * z * C - x * s, z * x * C - y * s, z * y * C + x * s, z * z * C + c ]; } const mul3 = (A, B) => { const C = new Array(9); for (let i = 0; i < 3; i++) for (let j = 0; j < 3; j++) C[i * 3 + j] = A[i * 3] * B[j] + A[i * 3 + 1] * B[3 + j] + A[i * 3 + 2] * B[6 + j]; return C; }; const mv3 = (A, v) => [ A[0] * v[0] + A[1] * v[1] + A[2] * v[2], A[3] * v[0] + A[4] * v[1] + A[5] * v[2], A[6] * v[0] + A[7] * v[1] + A[8] * v[2] ]; const cross3 = (a, b) => [a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0]]; const T3 = M => [M[0], M[3], M[6], M[1], M[4], M[7], M[2], M[5], M[8]]; // 3x3 转置 const clamp1 = v => v < -1 ? -1 : (v > 1 ? 1 : v); /* 两线段最近距离（Ericson《Real-Time Collision Detection》标准式）。 动态限位的度量基础：解析式必须敢被数值校验——node 侧对随机线段与 暴力密集采样对比（scratch 自检脚本），别只看"数值像是对的"。 内核收标量直传（碰撞热循环每次调用上万次，建数组的分配开销实测占一半）， 数组版仅作对外包装/测试用。 */ function segSegDistRaw(x1, y1, z1, x2, y2, z2, x3, y3, z3, x4, y4, z4) { const d1x = x2 - x1, d1y = y2 - y1, d1z = z2 - z1; const d2x = x4 - x3, d2y = y4 - y3, d2z = z4 - z3; const rx = x1 - x3, ry = y1 - y3, rz = z1 - z3; const a = d1x * d1x + d1y * d1y + d1z * d1z; const e = d2x * d2x + d2y * d2y + d2z * d2z; const f = d2x * rx + d2y * ry + d2z * rz; const cl01 = v => v < 0 ? 0 : (v > 1 ? 1 : v); let s = 0, t = 0; if (a <= 1e-12 && e <= 1e-12) { s = 0; t = 0; } else if (a <= 1e-12) { s = 0; t = cl01(f / e); } else { const c = d1x * rx + d1y * ry + d1z * rz; if (e <= 1e-12) { t = 0; s = cl01(-c / a); } else { const b = d1x * d2x + d1y * d2y + d1z * d2z; const denom = a * e - b * b; s = denom > 1e-12 ? cl01((b * f - c * e) / denom) : 0; t = (b * s + f) / e; if (t < 0) { t = 0; s = cl01(-c / a); } else if (t > 1) { t = 1; s = cl01((b - c) / a); } } } const cx = x1 + d1x * s - (x3 + d2x * t); const cy = y1 + d1y * s - (y3 + d2y * t); const cz = z1 + d1z * s - (z3 + d2z * t); return Math.sqrt(cx * cx + cy * cy + cz * cz); } function segSegDist(p1, q1, p2, q2) { return segSegDistRaw(p1[0], p1[1], p1[2], q1[0], q1[1], q1[2], p2[0], p2[1], p2[2], q2[0], q2[1], q2[2]); } /* 两旋转矩阵间的夹角（度）：θ = acos((tr(AᵀB) − 1) / 2)，A、B 均为正交阵。 tr(AᵀB) = Σᵢⱼ AᵢⱼBᵢⱼ —— 行主序即两数组逐元素点积。 */ function rotErrDeg(A, B) { let tr = 0; for (let i = 0; i < 9; i++) tr += A[i] * B[i]; return Math.acos(clamp1((tr - 1) / 2)) * R2D; } /* 姿态参数（CAD 系）→ 工具坐标系旋转矩阵（行主序 9 数）。 az：工具轴（法兰面 +Z）绕 CAD Z 轴的方位角；el：工具轴自 CAD +Z 抬起的仰角（0=朝上，180=朝下）； roll：绕工具轴自身的滚转角（右手系）。 列向量构成工具坐标系：z = 工具轴；x0 = 子午面基（−sin az, cos az, 0），恒垂直于 z； x = 绕 z 滚转 roll 后的 x0；y = z × x 保证右手系。 */ function poseMat(azDeg, elDeg, rollDeg) { const az = azDeg * D2R, el = elDeg * D2R, ro = rollDeg * D2R; const ca = Math.cos(az), sa = Math.sin(az); const z = [Math.sin(el) * ca, Math.sin(el) * sa, Math.cos(el)]; const x0 = [-sa, ca, 0]; const x = mv3(rotAxis(z, ro), x0); const y = cross3(z, x); return [x[0], y[0], z[0], x[1], y[1], z[1], x[2], y[2], z[2]]; } /* 姿态矩阵 → {az, el, roll}（poseMat 的逆分解，度）。el ∈ [0,180]，az ∈ (−180,180]，roll ∈ (−180,180] R = R0·Rx(roll)（R0 = poseMat(az,el,0)，列为 x0/y0/z）⇒ 工具 x 列 = cos(roll)·x0 + sin(roll)·y0 */ function poseAng(R) { const z = [R[2], R[5], R[8]], x = [R[0], R[3], R[6]]; // 行主序：列 2 = 工具轴，列 0 = 工具 x const az = Math.atan2(z[1], z[0]) * R2D; const el = Math.atan2(Math.hypot(z[0], z[1]), z[2]) * R2D; const x0 = [-Math.sin(az * D2R), Math.cos(az * D2R), 0]; const y0 = cross3(z, x0); const d0 = x[0] * x0[0] + x[1] * x0[1] + x[2] * x0[2]; const d1 = x[0] * y0[0] + x[1] * y0[1] + x[2] * y0[2]; return { az: wrap180(az), el: el, roll: wrap180(Math.atan2(d1, d0) * R2D) }; } /* ==========================================================================*/ function make(cfg) { /* ---------- 0. 关节表 → 运动学常量 ---------- */ const JJ = cfg.joints, TR = cfg.travel || {}; const P = {}, D = {}; for (const id of ORDER) { if (!JJ[id]) throw new Error('缺少关节 ' + id); P[id] = JJ[id].rear.slice(); D[id] = nrm(JJ[id].d); } const LIM = {}; for (const id of ORDER) { const t = TR[id] || { lo: -360, hi: 360, continuous: true }; /* 注意：LIM 是【求解器的规范代表角区间】，不是界面允许范围。 连续轴（J1/J3/J6）物理上无机械限位——界面滑块/动态限位已放到 ±360°（见下面的 joint sliders 与 dynLimit），但解一律取 |θ| 最小的等价角，故规范区间仍保持 ±180°。 若此处也放到 ±360°：closest 的随机重启按 [LIM.lo, LIM.hi] 撒点，会散布到 ±360°， 实测不可达兜底的连续轴最大 |θ| 从 176° 涨到 356°、平均 Σ|θ| +24%，动画会绕远路。 非连续轴仍用真实行程。 */ LIM[id] = { lo: +t.lo, hi: +t.hi, cont: !!t.continuous }; } /* ---------- 0b. 动态碰撞模型（胶囊近似，cfg.collide 可选） ---------- 数据由 scratch/fit_capsules.py 从 CAD 网格拟合写 docs/collide.json： caps[i] = {g, cn, a, b, r}，g=0(root 固定)..6(J6)，端点为零位 CAD 世界坐标。 组 g 的胶囊受 A1..Ag 嵌套旋转（与查看器场景图/此处 fkChain 同一约定）。 只检 |Δg|≥2 的零件对：同组相对静止（零位单实体检查已保证不干涉）， 相邻组相对位姿只由中间关节决定、与其它关节无关（sweep_v2 单关节硬限位 即此类，如 P04×P05/P06×P07/P07×P08），非相邻对（含撞固定立柱）才随姿态 大变——这正是静态限位过松/过紧的来源。 两级宽相位：零件级包围球（变换 19 个球心）先筛，球对分离即跳过该零件对 的全部胶囊对；只有邻近零件对才变换胶囊端点做线段距离。 */ const COL = cfg.collide || null; const CAPG = COL ? COL.caps : null; // [{g,cn,a,b,r}] const CAPM = COL ? +(COL.margin_mm != null ? COL.margin_mm : 1.0) : 0; const MOTOR = COL ? +(COL.motor_limit_deg || 0) : 0; // 电机机械限位（度） /* 静态限位每侧硬度（fit_capsules.py 按 travel_detail 首个卡死对判定）： 硬边 = 相邻组零件自碰撞，与其它关节无关 → 任何姿态都取静态值，不放宽； 可放宽 = 卡死对跨 ≥2 组（典型：腕/法兰撞固定立柱）→ 只在零位成立， 由动态模型按当前姿态重算。 */ 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) { const caps = COL.parts[nm]; if (!caps.length) continue; const g = CAPG[caps[0]].g; let cx = 0, cy = 0, cz = 0; for (const i of caps) { cx += (CAPG[i].a[0] + CAPG[i].b[0]) / 2; cy += (CAPG[i].a[1] + CAPG[i].b[1]) / 2; cz += (CAPG[i].a[2] + CAPG[i].b[2]) / 2; } cx /= caps.length; cy /= caps.length; cz /= caps.length; let rad = 0, rmax = 0; for (const i of caps) { const c = CAPG[i]; rad = Math.max(rad, Math.hypot((c.a[0] + c.b[0]) / 2 - cx, (c.a[1] + c.b[1]) / 2 - cy, (c.a[2] + c.b[2]) / 2 - cz) + 0.5 * Math.hypot(c.b[0] - c.a[0], c.b[1] - c.a[1], c.b[2] - c.a[2]) + c.r); rmax = Math.max(rmax, c.r); } pidx[nm] = PART.length; PART.push({ cn: CAPG[caps[0]].cn, g: g, caps: caps, c: [cx, cy, cz], rad: rad, rmax: rmax, _i: PART.length }); } /* 簇级宽相位：每件胶囊按主轴投影分桶（每桶 ≤CAPS_PER 枚）。桶球半径 ~15-25， 零位臂-立柱间隙 ~44mm 也只能筛掉一部分——实测再细分（目标半径 20mm）会让 簇球对测试量爆炸（62 万次，27ms/check），得不偿失；速度靠戳记缓存与用法层 的边界缓存解决（见 dynLimit 与 glue 的滑块夹取）。 */ const CAPS_PER = 4; for (const pt of PART) { const mids = pt.caps.map(i => [(CAPG[i].a[0] + CAPG[i].b[0]) / 2, (CAPG[i].a[1] + CAPG[i].b[1]) / 2, (CAPG[i].a[2] + CAPG[i].b[2]) / 2]); let u = [1, 0, 0]; // 幂迭代求主轴（5 轮足够定向） for (let it = 0; it < 5; it++) { const sm = [0, 0, 0]; for (const m of mids) { const d = m[0] * u[0] + m[1] * u[1] + m[2] * u[2]; sm[0] += d * m[0]; sm[1] += d * m[1]; sm[2] += d * m[2]; } const n = Math.hypot(sm[0], sm[1], sm[2]); if (n < 1e-9) { u = [1, 0, 0]; break; } u = [sm[0] / n, sm[1] / n, sm[2] / n]; } const ord = pt.caps.map((c, k) => [mids[k][0] * u[0] + mids[k][1] * u[1] + mids[k][2] * u[2], k]) .sort((a, b) => a[0] - b[0]).map(x => x[1]); const G = Math.max(1, Math.ceil(pt.caps.length / CAPS_PER)); pt.clus = []; for (let k = 0; k < G; k++) { const cs = ord.slice(Math.floor(k * pt.caps.length / G), Math.floor((k + 1) * pt.caps.length / G)); if (!cs.length) continue; let mx = 0, my = 0, mz = 0; for (const k2 of cs) { mx += mids[k2][0]; my += mids[k2][1]; mz += mids[k2][2]; } mx /= cs.length; my /= cs.length; mz /= cs.length; let rad = 0, rmax = 0; for (const k2 of cs) { const c = CAPG[pt.caps[k2]]; rad = Math.max(rad, Math.hypot(mids[k2][0] - mx, mids[k2][1] - my, mids[k2][2] - mz) + 0.5 * Math.hypot(c.b[0] - c.a[0], c.b[1] - c.a[1], c.b[2] - c.a[2]) + c.r); rmax = Math.max(rmax, c.r); } pt.clus.push({ c: [mx, my, mz], rad: rad, rmax: rmax, caps: cs.map(k2 => pt.caps[k2]) }); } } const names = Object.keys(COL.parts).filter(n => COL.parts[n].length); for (let x = 0; x < names.length; x++) for (let y = x + 1; y < names.length; y++) { const A = PART[pidx[names[x]]], B = PART[pidx[names[y]]]; const dg = Math.abs(A.g - B.g); if (dg < 2) continue; // 同组/相邻组 → 静态限位或 1D 间隔表 /* 相邻组打印件对（P04×P05 / P06×P07 / P07×P08）**不能**塞进胶囊模型：两件 同心跨在同一关节轴上，胶囊轴距≈0，给多少重叠容忍都会在零位误报（实测 P04×P03 超报 15mm、P06×P07 超报 14mm）。它们是"只由中间关节决定的 1D 约束"，由 fit_capsules 生成的 ADJ 区间表（collide.json 的 adj 字段） 在 dynLimit 里按角度查表处理。 */ /* 相邻组打印件对（P04×P05 / P06×P07 / P07×P08）纳入动态模型：其相对位姿只由 中间关节决定、与姿态无关，但在另一侧可能被动态对（撞立柱）抢先卡死，静态 单轴扫描只会记下先撞的那一对——不含它们就会在另一侧漏判（实测 J5+ 在 J4=-100 时放到电机限 106，真值 96，就是 P07×P08 在 +96 侧的自碰撞）。 */ const cl = []; for (let ia = 0; ia < A.clus.length; ia++) for (let ib = 0; ib < B.clus.length; ib++) { 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, 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）—— 实测优于 Float64Array 方案（混入字符串会污染元素类型反而更慢）。 */ const XFBUF = COL ? CAPG.map(c => { const e = new Array(8); e[6] = c.cn; e[7] = -1; return e; }) : null; const CLBUF = COL ? PART.map(pt => pt.clus.map(() => { const e = new Array(4); e[3] = -1; return e; })) : null; const PCBUF = COL ? PART.map(() => { const e = new Array(4); e[3] = -1; return e; }) : null; let CGEN = 0; function cumXf(q) { let R = [1, 0, 0, 0, 1, 0, 0, 0, 1], T = [0, 0, 0]; const Rs = [R.slice()], Ts = [T.slice()]; for (let k = 0; k < 6; k++) { const Pk = P[ORDER[k]], Rk = rotAxis(D[ORDER[k]], q[k] * D2R); const rp = mv3(Rk, Pk); // A_k = T(Pk)·Rk·T(−Pk) const tk = [Pk[0] - rp[0], Pk[1] - rp[1], Pk[2] - rp[2]]; const nR = mul3(R, Rk); const nT = [R[0] * tk[0] + R[1] * tk[1] + R[2] * tk[2] + T[0], R[3] * tk[0] + R[4] * tk[1] + R[5] * tk[2] + T[1], R[6] * tk[0] + R[7] * tk[1] + R[8] * tk[2] + T[2]]; R = nR; T = nT; Rs.push(R.slice()); Ts.push(T.slice()); } return { R: Rs, T: Ts }; } /* 逐对扫描（簇级宽相位 → 胶囊线段距离）。visit(cnA,cnB,d,need) 对每个距离 < need 的胶囊对回调；无回调（collideFree）时首对即短路返回。 */ 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) => { const e = CLBUF[pi][ci]; if (e[3] !== CGEN && e[3] !== -2) { const pt = PART[pi], R = Rs[pt.g], t = Ts[pt.g], c = pt.clus[ci].c; e[0] = R[0] * c[0] + R[1] * c[1] + R[2] * c[2] + t[0]; e[1] = R[3] * c[0] + R[4] * c[1] + R[5] * c[2] + t[1]; e[2] = R[6] * c[0] + R[7] * c[1] + R[8] * c[2] + t[2]; e[3] = pt.g === 0 ? -2 : CGEN; // 组 0（立柱/底座）静止：变换一次永久有效 } return e; }; const pcOf = pi => { const e = PCBUF[pi]; if (e[3] !== CGEN && e[3] !== -2) { const pt = PART[pi], R = Rs[pt.g], t = Ts[pt.g], c = pt.c; e[0] = R[0] * c[0] + R[1] * c[1] + R[2] * c[2] + t[0]; e[1] = R[3] * c[0] + R[4] * c[1] + R[5] * c[2] + t[1]; e[2] = R[6] * c[0] + R[7] * c[1] + R[8] * c[2] + t[2]; e[3] = pt.g === 0 ? -2 : CGEN; } return e; }; const xf = i => { const e = XFBUF[i]; if (e[7] === CGEN || e[7] === -2) return e; const c = CAPG[i], R = Rs[c.g], t = Ts[c.g]; e[0] = R[0] * c.a[0] + R[1] * c.a[1] + R[2] * c.a[2] + t[0]; e[1] = R[3] * c.a[0] + R[4] * c.a[1] + R[5] * c.a[2] + t[1]; e[2] = R[6] * c.a[0] + R[7] * c.a[1] + R[8] * c.a[2] + t[2]; e[3] = R[0] * c.b[0] + R[1] * c.b[1] + R[2] * c.b[2] + t[0]; e[4] = R[3] * c.b[0] + R[4] * c.b[1] + R[5] * c.b[2] + t[1]; e[5] = R[6] * c.b[0] + R[7] * c.b[1] + R[8] * c.b[2] + t[2]; e[7] = c.g === 0 ? -2 : CGEN; return e; }; /* 宽相位阈值必须保守：球心距 > rad_A+rad_B+rmax_A+rmax_B+margin 才可筛除。 推导：胶囊轴上任点都在球内，故轴距 ≥ 球心距−rad_A−rad_B；要保证它对所有 胶囊对都 > r_i+r_j+margin（=need），阈值得带上双方最大胶囊半径与 margin。 早期版本只加 rad（漏了 rmax 与 margin），实测把 P01_C×P06 在 J1=20/J2=49 的 1618mm³ 深撞整对筛掉——球相交但轴距仍小于半径和的对被误跳。 */ 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 + 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 + 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 { /* 前移启发：撞上的零件对 / 簇对挪到表头。规划中大量判定落在同一处障碍 （典型：立柱 × 腕/法兰），下次首查即命中、提前短路。只改顺序，不改结果。 */ 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; } /* 全姿态动态碰撞判定：True = 无动态干涉（模型近似，含 margin 外扩） */ const PST = { n: 0 }; /* 单次求解内的判定记忆：routeFrom / gatherGoals / 规划器会对同一批 IK 候选反复判定 */ let CFMEMO = null; function collideFree(q) { 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 升序 （最贴合的排前面）。空数组 = 无动态干涉。 */ function collideDetail(q) { const out = []; if (!COL) return out; scanPairs(q, (na, nb, d, need) => out.push({ cnA: na, cnB: nb, d: d, need: need })); out.sort((x, y) => (x.d - x.need) - (y.d - y.need)); return out; } /* 某轴在给定姿态下的动态边界（度）：从 q[id] 向 dir 方向二分至首次干涉， 上限为电机机械限位。返回可安全抵达的极值。 */ function dynLimit(id, q, dir, tol) { tol = tol || 0.25; const k = ORDER.indexOf(id); if (!COL || k < 0 || !dir) return q[k]; const sign = dir > 0 ? 1 : -1; const side = dir > 0 ? 'hi' : 'lo'; const hard = HARD[id] && HARD[id][side]; const hi0 = dir > 0 ? LIM[id].hi : LIM[id].lo; /* 连续轴的搜索端点取 ±180：转过的角度以 360 为周期，360° 与 0° 是同一个位姿， 拿它当端点探一次必然"安全"，二分会把区间内真实存在的撞点整段跳过（实测 J3 在 J4=-17.875 的折肘位形下首撞 148°，而旧口径直接返回 360）。 */ let hi = sign * (LIM[id].cont ? 180 : Math.min(MOTOR || 180, 180)); /* 相邻打印件对的 1D 自由区间（adj 表，精确 CAD 实测、与姿态无关）封顶： 典型漏判场景——J4=-100 时 J5+ 被 P07×P08 卡在 96°，而该对落在静态扫描的 视野外（零位时立柱先把 J5 卡在 34°），动态胶囊模型也看不见相邻对。 */ const adj = ADJ[id]; if (adj && dir > 0) hi = Math.min(hi, adj[1]); if (adj && dir < 0) hi = Math.max(hi, adj[0]); /* 硬边侧（静态限位由相邻打印件对决定、与姿态无关，胶囊模型看不见那一对）： 静态值只当区间上限，区间内仍要按当前姿态二分——否则折肘位形下动态检查被整个 跳过，滑块放到静态 -93°，而实测 J5 在 J3=-180° 时 -28° 末端法兰已撞立柱下段。 */ if (hard && Math.abs(hi0) < Math.abs(hi)) hi = hi0; if (hi === q[k]) return q[k]; const at = v => { const qq = q.slice(); qq[k] = v; return collideFree(qq); }; if (!at(q[k])) return q[k]; // 当前姿已干涉：不放大 let lo = q[k]; if (!LIM[id].cont) { // 限位轴：折过去就是折过去，区间内单峰，二分即可 if (at(hi)) return hi; while (Math.abs(hi - lo) > tol) { const mid = 0.5 * (lo + hi); if (at(mid)) lo = mid; else hi = mid; } return lo; } /* 连续轴（J1/J3/J6）：整周内可行集不连续——末段绕过立柱后会重新变安全， "端点安全"根本不代表中间没撞（实测 J1=-81 位形向 + 方向 180° 端点安全， 但 -26° 起撞立柱中段）。所以先按固定步长粗扫出首个干涉角，再在该段内二分。 步长 4° 起（区间 360° 时约 40 次 collideFree ≈ 150 ms，只在滑块缓存未命中时发生）。 */ const step = (hi > q[k] ? 1 : -1) * Math.max(4, Math.abs(hi - q[k]) / 40); let jam = null; for (let v = lo + step; (hi > q[k] ? v < hi : v > hi); v += step) { if (at(v)) lo = v; else { jam = v; break; } } if (jam === null) { if (at(hi)) return hi; jam = hi; } while (Math.abs(jam - lo) > tol) { const mid = 0.5 * (lo + jam); if (at(mid)) lo = mid; else jam = mid; } return lo; } /* 六轴动态限位表（UI 显示/滑块夹取用） */ function dynLimits(q, tol) { const out = {}; for (const id of ORDER) out[id] = { lo: dynLimit(id, q, -1, tol), hi: dynLimit(id, q, +1, tol) }; return out; } const TCP0 = (cfg.tcp || [P.J6[0], P.J6[1], 93]).slice(); // 缺省 TCP 落在 J6 轴线上（随整机前移自动更新） const ax = D.J6; // 工具沿 J6 轴向外伸 const K = { X0: P.J3[0], Y0: P.J6[1], Z1: P.J1[2], Z2: P.J2[2], Z3: P.J3[2], Z4: P.J4[2], Z5: P.J5[2], TCP0: TCP0, ax: ax }; K.C0 = K.Z2 - K.Z1; K.A43 = K.Z4 - K.Z3; K.A32 = K.Z3 - K.Z2; K.A54 = K.Z5 - K.Z4; K.L5 = K.Z5 - TCP0[2]; // J5 轴 → TCP（沿 −Z） /* 结构前提断言：J2/J4/J5 同向平行；J1 轴为 X；J3/J6 轴为 Z；TCP 落在 J6 轴线上 */ const AXP = v => Math.abs(Math.abs(v[2]) - 1) < 1e-9; const WARN = []; if (Math.abs(D.J1[0]) < 1e-9 || Math.abs(Math.abs(D.J1[0]) - 1) > 1e-9) WARN.push('J1 轴不是 X'); for (const id of ['J2', 'J4', 'J5']) if (Math.abs(Math.abs(D[id][1]) - 1) > 1e-9) WARN.push(id + ' 轴不是 Y'); if (!AXP(D.J3)) WARN.push('J3 轴不是 Z'); if (!AXP(D.J6)) WARN.push('J6 轴不是 Z'); if (Math.abs(P.J3[0] - P.J2[0]) > 1e-9 || Math.abs(P.J4[0] - P.J2[0]) > 1e-9 || Math.abs(P.J5[0] - P.J2[0]) > 1e-9) WARN.push('J2/J4/J5 不在同一 x 平面'); if (Math.abs(TCP0[0] - P.J6[0]) > 1e-9 || Math.abs(TCP0[1] - P.J6[1]) > 1e-9) WARN.push('TCP 不在 J6 轴线上（θ6 将影响位置）'); /* ---------- 1. 严格矩阵链正运动学（对照基准） ---------- */ function mul4(A, B) { const C = new Array(16); for (let i = 0; i < 4; i++) for (let j = 0; j < 4; j++) { let s = 0; for (let k = 0; k < 4; k++) s += A[i * 4 + k] * B[k * 4 + j]; C[i * 4 + j] = s; } return C; } function mv4(M, v) { return [0, 1, 2, 3].map(i => M[i * 4] * v[0] + M[i * 4 + 1] * v[1] + M[i * 4 + 2] * v[2] + M[i * 4 + 3] * v[3]); } /* 标准 4x4 同质链 World = A1·A2·…·A6·TCP，Ai = T(Pi)·R(轴,θ)·T(−Pi) —— 与查看器场景图（逐级左乘的 matrix 局部变换）逐位等价 */ function fkChain(q, tool) { const tcp = [TCP0[0], TCP0[1], TCP0[2] + (+tool || 0) * ax[2]]; let M = [1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1]; for (let i = 0; i < 6; i++) { const id = ORDER[i], p = P[id], R = rotAxis(D[id], q[i] * D2R); const rp = mv3(R, p); // Ai 的旋转=R，平移=p−R·p const A = [R[0], R[1], R[2], p[0] - rp[0], R[3], R[4], R[5], p[1] - rp[1], R[6], R[7], R[8], p[2] - rp[2], 0, 0, 0, 1]; M = mul4(M, A); } const w = mv4(M, [tcp[0], tcp[1], tcp[2], 1]); return [w[0], w[1], w[2]]; } /* ---------- 1b. 工具姿态链：前 thru 个关节旋转之积（CAD 系，右乘法向） ---------- */ /* 工具坐标系姿态 = R1·R2·…·R6。因 R6 只绕工具自身 z 轴转动： · 工具轴（法兰面 +Z 指向）= (R1…R5)·ẑ —— 与 θ6 无关，这正是姿态任务可解的关键； · θ6 只提供绕工具轴的滚转。 */ function orient(q, thru) { let M = [1, 0, 0, 0, 1, 0, 0, 0, 1]; const n = thru == null ? 6 : thru; for (let i = 0; i < n; i++) M = mul3(M, rotAxis(D[ORDER[i]], q[i] * D2R)); return M; } /* M5 第 3 列（工具轴）列向量传播：J4/J5 同轴 ⇒ R4·R5=R(θ4+θ5) 合并为一次旋转， 只乘单位向量、不构造 3×3、免 T3/mul3/rotErrDeg/acos。与 orient(q,5) 列 2 一致（≤1e-15）。 */ const SAMEAX = D.J4[0] === D.J5[0] && D.J4[1] === D.J5[1] && D.J4[2] === D.J5[2]; function axisCol(q) { /* 零分配特化：J4/J5 同轴合并为 R(Y,θ4+θ5) 作用在 ẑ 上（= (sin,0,cos)），J3 为 −Z、 J2 为 Y、J1 为 X，逐级展开向量传播（与通用矩阵链数学恒等；每级系数为 0 的项直接省去）。 不满足结构前提时回落通用矩阵链。 */ if (SAMEAX && D.J4[1] === 1 && D.J1[0] === 1 && D.J2[1] === 1 && D.J3[2] === -1) { const r45 = (q[3] + q[4]) * D2R; const x5 = Math.sin(r45), z5 = Math.cos(r45); const c3 = Math.cos(q[2] * D2R), s3 = Math.sin(q[2] * D2R); const x3 = c3 * x5, y3 = -s3 * x5; // J3：R(−Z,θ3)·(x5,0,z5) const c2 = Math.cos(q[1] * D2R), s2 = Math.sin(q[1] * D2R); const x2 = c2 * x3 + s2 * z5, z2 = -s2 * x3 + c2 * z5; // J2：R(Y,θ2) const c1 = Math.cos(q[0] * D2R), s1 = Math.sin(q[0] * D2R); return [x2, c1 * y3 - s1 * z2, s1 * y3 + c1 * z2]; // J1：R(X,θ1) } let v; if (SAMEAX) v = mv3(rotAxis(D.J4, (q[3] + q[4]) * D2R), [0, 0, 1]); else v = mv3(mul3(rotAxis(D.J4, q[3] * D2R), rotAxis(D.J5, q[4] * D2R)), [0, 0, 1]); v = mv3(rotAxis(D.J3, q[2] * D2R), v); v = mv3(rotAxis(D.J2, q[1] * D2R), v); v = mv3(rotAxis(D.J1, q[0] * D2R), v); return v; } /* ---------- 2. 闭式正运动学（快速，用于求解内部） ---------- */ function fk(q, tool) { const L = K.L5 + (+tool || 0); const s1 = Math.sin(q[0] * D2R), c1 = Math.cos(q[0] * D2R); const s2 = Math.sin(q[1] * D2R), c2 = Math.cos(q[1] * D2R); const s3 = Math.sin(q[2] * D2R), c3 = Math.cos(q[2] * D2R); const s4 = Math.sin(q[3] * D2R), c4 = Math.cos(q[3] * D2R); const s5 = Math.sin(q[4] * D2R), c5 = Math.cos(q[4] * D2R); const u = -L * s5, w = K.A54 - L * c5; const dx = u * c4 + w * s4; const dz = K.A43 - u * s4 + w * c4; const Vx = dx, Vz = dz + K.A32; const b = -Vx * s3; const c = K.C0 + (-Vx * c3 * s2 + Vz * c2); const a = Vx * c3 * c2 + Vz * s2; return [K.X0 + a, K.Y0 + b * c1 - c * s1, K.Z1 + b * s1 + c * c1]; } /* ---------- 3. 逆解：给定 (θ4,θ5) 的闭式分支 ---------- */ /* 单槽 sin/cos 缓存：网格扫描外层 θ4 固定，每行只算一次（浮点同值必命中，NaN 不等必重算） */ let _bt4 = NaN, _bt5 = NaN, _bs4 = 0, _bc4 = 0, _bs5 = 0, _bc5 = 0; function branch(th4, th5, A, R, ph, tool) { const L = K.L5 + (+tool || 0); if (th4 !== _bt4) { _bs4 = Math.sin(th4 * D2R); _bc4 = Math.cos(th4 * D2R); _bt4 = th4; } if (th5 !== _bt5) { _bs5 = Math.sin(th5 * D2R); _bc5 = Math.cos(th5 * D2R); _bt5 = th5; } const s5 = _bs5, c5 = _bc5, s4 = _bs4, c4 = _bc4; const u = -L * s5, w = K.A54 - L * c5; const dx = u * c4 + w * s4; const dz = K.A43 - u * s4 + w * c4; const Vx = dx, Vz = dz + K.A32; const out = []; const n = (R * R + A * A - Vx * Vx - Vz * Vz - K.C0 * K.C0) / (2 * K.C0); const cc = K.C0 + n; if (Math.abs(Vx) < 1e-7) { // 退化：θ3 不影响 TCP if (Math.abs(Vz) < 1e-9) return out; const s2 = A / Vz, c2 = n / Vz; if (Math.abs(s2 * s2 + c2 * c2 - 1) > 1e-12) return out; // 收紧：无 fk 复核后须自行保证精确（原松解本就会被 fk 拒） const th1 = (Math.abs(cc) > 1e-12) ? (ph - Math.atan2(cc, 0)) * R2D : 0; out.push([wrap180(th1), wrap180(Math.atan2(s2, c2) * R2D), 0, th4, th5, 0]); return out; } const m2 = (n * n + A * A - Vz * Vz) / (Vx * Vx); if (m2 < -1e-9 || m2 > 1 + 1e-9) return out; const sig = Math.acos(Math.sqrt(Math.max(0, Math.min(1, m2)))); /* θ3 四支（±σ、±(π−σ)）的 cos/sin 只需一对三角函数值，按支下标取符号（原实现每支各算一次） */ const c3s = Math.cos(sig), s3s = Math.sin(sig); for (let k = 0; k < 4; k++) { const c3 = (k < 2) ? c3s : -c3s, s3 = (k === 0 || k === 2) ? s3s : -s3s; const kap = Vx * c3, lam = Vz; const det = kap * kap + lam * lam; if (det < 1e-12) continue; const th2 = Math.atan2((lam * A - kap * n) / det, (kap * A + lam * n) / det) * R2D; const b = -Vx * s3; const th1 = (Math.abs(b) + Math.abs(cc) > 1e-12) ? (ph - Math.atan2(cc, b)) * R2D : 0; out.push([wrap180(th1), wrap180(th2), wrap180(Math.atan2(s3, c3) * R2D), th4, th5, 0]); } return out; } const LJ5 = [LIM.J1, LIM.J2, LIM.J3, LIM.J4, LIM.J5]; const inLimit = q => { // 取否定式：NaN 候选必须落选（限位数组预取，免每点字符串键查表） for (let i = 0; i < 5; i++) { const t = LJ5[i]; if (!(q[i] >= t.lo - 1e-9 && q[i] <= t.hi + 1e-9)) return false; } return true; }; /* 选解代价。 - 不给 ref：Σ|θ|（对外显示口径；离线批算也走这一支） - 给 ref：用户指定的判据「**所有电机需要转过的角度之和**」最小， 即相对当前姿态的行程最小。连续轴（J1/J3/J6）无机械限位，θ 与 θ±360 是同一 物理位形，故角差一律取 wrap180。末尾 1e-6·Σ|θ| 只作并列时的次序稳定项。 （旧实现是 Σ|θ| + lam·Σwrap(θ−ref)² 的二次连续项，已被本判据取代； 参数 lam 保留仅为兼容旧调用，不再参与计算。） */ const costOf = (q, W, ref, lam) => { let c = 0; for (let i = 0; i < 6; i++) { const w = W ? W[i] : 1; c += Math.abs(q[i]) * w; } if (!ref) return c; let t = 0; for (let i = 0; i < 6; i++) { const w = W ? W[i] : 1; t += Math.abs(wrap180(q[i] - ref[i])) * w; } return t + 1e-6 * c; }; const nowMs = () => (typeof performance !== 'undefined' ? performance.now() : Date.now()); /* 姿态求解原语工厂：位置闭式支把 θ1..θ3 吸收掉之后，姿态任务只剩「工具轴指向 = 目标轴」这 2 个方程、2 个未知数 (θ4,θ5)，解是孤立点，滚转再由 θ6 闭式补上。 粗扫根枚举（solve 内）与实时续解（solveWarm）共用同一套。 */ function poseTools(target, tool, Rtar) { const Aa = target[0] - K.X0, Bq = target[1] - K.Y0, Br = target[2] - K.Z1; const Rr = Math.hypot(Bq, Br), phh = Math.atan2(Br, Bq); const L4 = LIM.J4, L5 = LIM.J5; let poseQAt, poseRes, poseNewton; const Ad = [Rtar[2], Rtar[5], Rtar[8]]; const pz = cross3(Ad, [0, 0, 1]); const bu = Math.hypot(pz[0], pz[1], pz[2]) > 0.5 ? nrm(pz) : nrm(cross3(Ad, [1, 0, 0])); const bv = cross3(Ad, bu); poseQAt = (t4, t5, qc) => { // 同支匹配：θ1..θ3 最近 const ss = branch(t4, t5, Aa, Rr, phh, tool); let bb = null, bd = Infinity; for (const s0 of ss) { if (!inLimit(s0)) continue; let d = 0; for (let i = 0; i < 3; i++) d += Math.abs(wrap180(s0[i] - qc[i])); if (d < bd) { bd = d; bb = s0; } } return bb; }; poseRes = q => { const w = axisCol(q); const dp = w[0] * Ad[0] + w[1] * Ad[1] + w[2] * Ad[2]; const tx = w[0] - dp * Ad[0], ty = w[1] - dp * Ad[1], tz = w[2] - dp * Ad[2]; return [tx * bu[0] + ty * bu[1] + tz * bu[2], tx * bv[0] + ty * bv[1] + tz * bv[2], dp]; }; poseNewton = q => { // 返回 {q, ne, exact} if (!q) return null; let lam = 1e-3, bq = q, e = poseRes(q), bn = Math.hypot(e[0], e[1]); /* 切向残差在「工具轴与目标轴平行」处同样为零，反平族也收敛到 0， 所以同向判据只能在收口时验，不能当迭代中止条件（否则每支一进来就废）。 */ for (let it = 0; it < 28; it++) { if (bn < 1e-12) break; const dpx = 0.02; const qa = poseQAt(bq[3] + dpx, bq[4], bq), qb = poseQAt(bq[3], bq[4] + dpx, bq); if (!qa || !qb) break; const ea = poseRes(qa), eb = poseRes(qb); const j00 = (ea[0] - e[0]) / dpx, j01 = (eb[0] - e[0]) / dpx; const j10 = (ea[1] - e[1]) / dpx, j11 = (eb[1] - e[1]) / dpx; const a00 = j00 * j00 + j10 * j10, a01 = j00 * j01 + j10 * j11, a11 = j01 * j01 + j11 * j11; const g0 = -(j00 * e[0] + j10 * e[1]), g1 = -(j01 * e[0] + j11 * e[1]); const d0 = a00 * (1 + lam), d1 = a11 * (1 + lam); const det = d0 * d1 - a01 * a01; if (!(Math.abs(det) > 1e-20)) break; let s0 = (d1 * g0 - a01 * g1) / det, s1 = (-a01 * g0 + d0 * g1) / det; const mx = Math.max(Math.abs(s0), Math.abs(s1)); if (mx > 2) { s0 *= 2 / mx; s1 *= 2 / mx; } const n4 = Math.max(L4.lo, Math.min(L4.hi, bq[3] + s0)); const n5 = Math.max(L5.lo, Math.min(L5.hi, bq[4] + s1)); const nq = poseQAt(n4, n5, bq); if (!nq) { lam *= 8; if (lam > 1e8) break; continue; } const en = poseRes(nq); const nn = Math.hypot(en[0], en[1]); if (nn < bn) { bq = nq; bn = nn; e = en; lam = Math.max(1e-10, lam * 0.4); } else { lam *= 8; if (lam > 1e8) break; } if (Math.abs(s0) < 1e-13 && Math.abs(s1) < 1e-13) break; } return { q: bq, ne: bn, exact: bn < 1e-12 && e[2] > 0.999999 }; }; return { qAt: poseQAt, res: poseRes, newton: poseNewton }; } /* 给定 θ1..θ5 的闭式位置解，闭式补滚转角 θ6 并给出全姿态偏差（度）。 solve 的粗扫与 solveWarm 的续解共用同一份，避免两处判据漂移。 */ function rollClose(q, Rtar) { const M5 = orient(q, 5); const N = mul3(T3(M5), Rtar); // 需满足 N = Rz(−θ6) const th6 = -Math.atan2(N[3], N[0]) * R2D; const q2 = q.slice(); q2[5] = th6; return { q: q2, oerr: rotErrDeg(mul3(M5, rotAxis(D.J6, th6 * D2R)), Rtar) }; } /* ---------- 4. 主求解入口 ---------- */ function solve(target, opt) { opt = opt || {}; const t0 = nowMs(); const tool = +opt.tool || 0; const W = opt.weights || [1, 1, 1, 1, 1, 1]; const h = Math.max(0.02, +(opt.step || 0.25)); const A = target[0] - K.X0; const BY = target[1] - K.Y0, BZ = target[2] - K.Z1; const R = Math.hypot(BY, BZ), ph = Math.atan2(BZ, BY); const tol = opt.tol || 1e-6, tol2 = tol * tol; const KEEP = 4000; /* 参考位姿（ref）：交互求解时以当前姿态为参考，选解判据取「所有电机需要转过的 角度之和」最小 —— 即这一步行程最短的构型（取代原先的 Σ|θ| 最小）。 动机：等价多分支（θ3 的 ±σ/±(π−σ)、肘上/肘下、以及 J1/J3 同号反号的镜像族） 之间只按 Σ|θ| 选优会随目标微动反复横跳（实测目标挪 20 mm 就翻族、绕一大圈）， 而两族 Σ|θ| 常只差 0~20°，判据本身病态；改以行程为准后，宁可多花几度关节角， 也不让手臂绕远路。ref 缺省时退化为 Σ|θ| 最小（离线批算口径不变）。 */ const REF = (opt.ref && opt.ref.length === 6) ? opt.ref.slice() : null; const LAM = +opt.refW || 0; /* 姿态任务：除 TCP 位置外同时指定工具姿态（R 为行主序 9 数，CAD 系）。 解法完全复用位置闭式支：(θ4,θ5) 扫描给出一族精确到点的解，每支的姿态 M = R1…R5 随之确定；对每支解算滚转角 θ6 并只保留工具轴对齐（偏差≤POSETOL）者， 代价加 POSEW·e_ori² 把精修拉向零偏差。θ6 ∈ ±360° 连续，总在限位内。 */ const POSE = (opt.pose && opt.pose.R && opt.pose.R.length === 9) ? opt.pose.R.slice() : null; const PT = POSE ? poseTools(target, tool, POSE) : null; /* 优先级模式（双优先级范式）： 'pos' 位置优先 —— 位置必须精确；姿态取「位置精确解集内」残差最小者。 姿态在 POSETOL 内不可达时**不丢弃候选**，改从轴夹角最小的种子出发做 最小残差搜索，返回位置精确、姿态最近的解并如实上报 oerr。 'ori' 姿态优先 —— 姿态必须精确；位置取「姿态精确解集内」距目标最近者 （由 solveTo 调 closestOri 完成，见 §5c）。 省略（undefined）—— 与改动前逐字一致：POSETOL 硬门、超门即弃并回落 DLS。 既有调用方（pose_ik_check / bench_ik_split / ikopt 系列）行为不变。 */ const PRIO = (opt.priority === 'pos' || opt.priority === 'ori') ? opt.priority : null; const POSETOL = 2.0; // 粗扫接受的最大姿态偏差（度） const POSEFINE = 0.05; // 达标线：偏差 ≤ 此值视为姿态精确到达（机械上已无意义之差） let poseW = 200.0; // 姿态偏差惩罚权重（对 deg²）：0.3° 偏差即折合 Σ|θ| 量级 // （数十度）的代价； escalating 时 ×100 直至出现 POSEFINE 内解 /* 轴夹角预筛：θ6 只提供绕工具轴的滚转、不改工具轴，故全姿态偏差 oerr ≥ 工具轴夹角 （20 万随机样本验证零违反）。轴夹角就超 POSETOL 的候选必被 closeRoll 拒， 先用一次点积挡掉——粗扫里这是姿态开销的大头（每候选省一次 orient 全矩阵 + acos）。 */ const A0 = POSE ? [POSE[2], POSE[5], POSE[8]] : null; const AXREJ = POSE ? 2 * Math.sin(POSETOL * D2R / 2) ** 2 : 0; // = 1 − cos(POSETOL) const closeRoll = q => rollClose(q, POSE); // 给定位置解，解 θ6 并返回姿态偏差（度） /* 位置优先模式的「姿态最近解」种子：工具轴夹角恒 ≤ 全姿态偏差（θ6 只滚转不改轴）， 故轴夹角最小的点最有希望成为 oerr 最小解。AXREJ 那道门只放行轴夹角 ≤POSETOL 的 候选，姿态在 POSETOL 外不可达时会把候选全挡下——这里另存一批廉价种子 （只用已算过的 1−w·A0，不算滚转），供落选后做最小残差搜索。 */ const SEEDN = 24; let axisSeeds = []; const keepSeed = (q, key) => { let wi = -1; if (axisSeeds.length >= SEEDN) { // 先与最差种子比：不比它好就直接丢（避免每次全表扫） wi = 0; for (let i = 1; i < SEEDN; i++) if (axisSeeds[i].key > axisSeeds[wi].key) wi = i; if (key >= axisSeeds[wi].key) return; } for (const s of axisSeeds) if (Math.hypot(s.q[3] - q[3], s.q[4] - q[4]) <= 2 * h) return; if (wi < 0) axisSeeds.push({ q: q, key: key }); else axisSeeds[wi] = { q: q, key: key }; }; const scoreBranch = (q, e) => { // 对一条位置精确支打分（位置误差 e 已由闭式保证） let oerr = 0; if (POSE) { const w = axisCol(q); const dp = w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2]; if (PRIO === 'pos') keepSeed(q, 1 - dp); if (1 - dp > AXREJ) return null; const cl = closeRoll(q); if (cl.oerr > POSETOL) return null; q = cl.q; oerr = cl.oerr; } const c = costOf(q, W); const rc0 = REF ? costOf(q, W, REF, LAM) : c; // 含连续项、不含姿态惩罚（分层选解用） let rc = rc0; if (POSE) rc += poseW * oerr * oerr; return { q: q, cost: c, rc0: rc0, rcost: rc, err: e, oerr: oerr }; }; const evalAt = (t4, t5) => { let best = null; const sols = branch(t4, t5, A, R, ph, tool); for (let k = 0; k < sols.length; k++) { const q = sols[k]; if (!inLimit(q)) continue; const c = scoreBranch(q, 0); // 闭式支已精确命中目标；真残差在返回前对入选候选统一补算（O5） if (c && (!best || c.rcost < best.rcost - 1e-12)) best = c; } return best; }; /* 采样轴：含上下限位端点，避免恰在限位上的最优解被漏掉 */ const axis = (lo, hi, step) => { const v = []; const n = Math.floor((hi - lo) / step + 1e-9); for (let i = 0; i <= n; i++) v.push(lo + i * step); if (v[v.length - 1] < hi - 1e-9) v.push(hi); return v; }; const L4 = LIM.J4, L5 = LIM.J5; const g4 = axis(L4.lo, L4.hi, h); /* 逐行裁剪可行段：位置闭式支只在平面支有解时才存在，限位矩形里只有 7~27% 的网格点能过 branch 判据（实测 5 个目标 × 全部 θ4 行，每行可行集均为单一 连续段、零多段）。先用探测步 hp 找每行的可行段，再向两端各放宽一个 hp 兜住 段界的不确定性，段内仍按原步长 h 精扫 ⇒ 采样密度与全域扫一致、解的质量不动。 */ const rowFeasible = (t4, t5) => { const ss = branch(t4, t5, A, R, ph, tool); for (let k = 0; k < ss.length; k++) if (inLimit(ss[k])) return true; return false; }; /* 可行段探测步。原为 max(h,2)：2° 的下限与用户设置的扫描步无关， 而实测本机最窄的可行段只有 0.98°，于是整条窄带被逐行判为「无可行段」， 表现为 nSol==0 的误报「姿态不可达」（典型目标 6.86,-190.94,240.46）。 降到 0.5° 后该目标由 2.55° 近似变为 0.00000° 精确，位置任务代价 +14%、 姿态任务 +54%（与引入兜底全扫相比便宜一个数量级）。 */ /* 探测步：姿态任务的可行 (θ4,θ5) 集是一条窄带（实测最窄 0.98°）， 2° 的探测步会整条漏掉 → 误报「姿态不可达」；位置任务的可行集宽， 2° 探测既准又快。故只在姿态任务下降细到 0.5°，位置任务保持逐位不变。 */ const hp = Math.max(h, POSE ? 0.5 : 2); /* 段边界外扩量。原实现用 hp 兼顾「探测步」与「边界外扩」两件事， 探测步降到 0.5° 后外扩也随之收窄，实测位置任务 2/100 组的 Σ|θ| 会变大 （最大 +0.182°，最优解正好落在段边界上）。两者职责不同，故解耦： 探测步可以细（找全薄可行段），外扩必须足够（兜住段界的不确定性）。 */ const PAD = 2; const rowSpans = t4 => { const spans = []; let lo = null; for (let t5 = L5.lo; t5 < L5.hi + 1e-9; t5 += hp) { const hi5 = Math.min(t5, L5.hi); if (rowFeasible(t4, hi5)) { if (lo === null) lo = hi5; } else if (lo !== null) { spans.push([lo - PAD, hi5 + PAD]); lo = null; } } if (lo !== null) spans.push([lo - PAD, L5.hi + PAD]); return spans; }; /* 每行可行段探测一次，粗扫与 4.1b 的根枚举各自复用。两者的行集刻意分开： 粗扫必须保持 h 的密度（根枚举目前还不保证完备，漏根时要靠粗扫的窄带候选兜底， 实测把粗扫一起放松会换来上百度级的代价劣化）；根枚举只在粗行集上跑，它是 加分项而不是替代品。 */ const spansScan = []; for (let i = 0; i < g4.length; i++) spansScan.push(rowSpans(g4[i])); const g4r = axis(L4.lo, L4.hi, hp), spansRoot = []; for (let i = 0; i < g4r.length; i++) spansRoot.push(rowSpans(g4r[i])); /* --- 4.1 全域粗扫：覆盖整个二维冗余 (J4,J5)，按行裁剪到可行段内 --- */ let all = [], samples = 0, nSol = 0, trimmed = false; /* 姿态任务另收"达标带"：oerr ≤ 1° 的候选。达标解在 (θ4,θ5) 平面上是一条窄带， 按 rcost 取种子会整体错过它（带的 Σ|θ| 常很大）；按 oerr 排序取种子才能保证 局部加密窗口罩住曲线，再由模式搜索在带上找 Σ|θ| 最小点。 */ let band = []; for (let i = 0; i < g4.length; i++) { const spans = spansScan[i]; for (let sp = 0; sp < spans.length; sp++) { const g5 = axis(Math.max(L5.lo, spans[sp][0]), Math.min(L5.hi, spans[sp][1]), h); for (let j = 0; j < g5.length; j++) { samples++; const sv = evalAt(g4[i], g5[j]); 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; trimmed = true; } } if (POSE) { // 姿态模式同样需要有界保留（带优先，再补全局最优） 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); all = keepBand.concat(rest); trimmed = band.length + rest.length < nSol; } else all.sort((x, y) => x.rcost - y.rcost); /* --- 4.1b 姿态孤立根枚举 --- 不再用细网格密度去撞那条窄带（原路径因此要背 POSETOL 硬筛、band 按 oerr 取种子、escalating ×100 重扫一整套补偿，残差也只到 ~0.02°）：逐行对残差 两个分量找变号括号，从括号中点起、只对发生变号的那一支跑阻尼高斯-牛顿 （奇异时加大阻尼而非退出）。同向判据只能收口时验——工具轴反平同样令切向 残差为 0，是这套判据最容易自证的地方。无根的刀尖构型收敛到 ‖e‖ 的真实最小。 */ if (POSE && opt.refine !== false) { const roots = [], bests = []; const seedNewton = (t4, t5, qc) => { const q0 = PT.qAt(t4, t5, qc); if (!q0) return; const r = PT.newton(q0); if (!r) return; const tgtL = r.exact ? roots : bests; let dup = false; for (const x of tgtL) { let d = 0; for (let i = 0; i < 5; i++) d += Math.abs(x.q[i] - r.q[i]); if (d < 0.02) { dup = true; break; } } if (!dup) tgtL.push(r); }; for (let ri = 0; ri < g4r.length; ri++) { const t4 = g4r[ri]; const rowBest = [null, null, null, null]; for (const sp of spansRoot[ri]) { const a5 = Math.max(L5.lo, sp[0]), b5 = Math.min(L5.hi, sp[1]); let prev = null; for (let t5 = a5; t5 <= b5 + 1e-9; t5 += hp) { const hi5 = Math.min(t5, b5); const ss = branch(t4, hi5, A, R, ph, tool); /* 按支下标对齐（不可行的支留 null 占位）：换个写法就会让 prev/cur 的 下标错开，括号判据随之失配，实测会整支漏掉一个真根、多付上百度关节角。 */ const cur = []; for (let k = 0; k < ss.length; k++) { if (!inLimit(ss[k])) { cur.push(null); continue; } const e = PT.res(ss[k]); cur.push({ q: ss[k], e: e }); if (e[2] > 0 && (!rowBest[k] || Math.hypot(e[0], e[1]) < rowBest[k].n)) rowBest[k] = { q: ss[k], t5: hi5, n: Math.hypot(e[0], e[1]) }; } if (prev) { for (let k = 0; k < Math.min(prev.length, cur.length); k++) { const pv = prev[k], cu = cur[k]; if (!pv || !cu || !pv.e[2] || !cu.e[2]) continue; if ((pv.e[0] < 0) !== (cu.e[0] < 0) || (pv.e[1] < 0) !== (cu.e[1] < 0)) seedNewton(t4, hi5 - hp / 2, pv.q); // 变号括号：中点起、沿该支 } } prev = cur; } /* 切向相切的根不产生变号，另按每支 ‖e‖ 最小的节点补种一次 */ for (const rb of rowBest) if (rb) seedNewton(t4, rb.t5, rb.q); } } for (const r of roots) { // 精确根直接进候选池 const p = fk(r.q, tool); const e2 = (p[0] - target[0]) ** 2 + (p[1] - target[1]) ** 2 + (p[2] - target[2]) ** 2; if (!(e2 <= tol * tol)) continue; const c = scoreBranch(r.q, Math.sqrt(e2)); if (c) { all.push(c); nSol++; if (c.oerr <= 1) band.push(c); } } if (!roots.length) for (const r of bests) { // 无根时的真实最小残差兜底 const p = fk(r.q, tool); const e2 = (p[0] - target[0]) ** 2 + (p[1] - target[1]) ** 2 + (p[2] - target[2]) ** 2; const c = e2 <= tol * tol ? scoreBranch(r.q, Math.sqrt(e2)) : null; if (c) { all.push(c); if (c.oerr <= 1) band.push(c); } } } /* --- 4.2 多起点局部加密 + 模式搜索 --- 姿态任务分层处理：先 escalating（poseW ×100 重扫）直到出现 oerr ≤ POSEFINE 的解， 再在达标候选里按含连续项的代价取优——避免"省几度关节角、丢 0.2° 姿态"的次优交易。 */ const refinePass = () => { if (!all.length) return; // 一个候选都没有（姿态完全不可达）：无从精修 const seeds = []; if (POSE) { /* 姿态种子：达标带按 oerr 升序、空间去同（相距 ≤ 2h 只留最优）， 保证种子贴在窄带上，局部窗口才能罩住精确解 */ for (const s of band) { if (seeds.length >= 32) break; let dup = false; for (const t of seeds) if (Math.hypot(t.q[3] - s.q[3], t.q[4] - s.q[4]) <= 2 * h) { dup = true; break; } if (!dup) seeds.push(s); } if (!seeds.length) seeds.push(all[0]); } else { for (let i = 0; i < all.length && seeds.length < 32; i++) { const s = all[i]; let dup = false; for (const t of seeds) if (Math.hypot(t.q[3] - s.q[3], t.q[4] - s.q[4]) <= h * 1.5) { dup = true; break; } if (!dup) seeds.push(s); } } const cl = (v, a, b) => v < a ? a : (v > b ? b : v); for (const s of seeds) { // 局部加密网格（h/8，±2h 窗口） const hh = POSE ? h / 8 : (h <= 0.5 ? h / 4 : h / 8); // 精修是耗时地板：常用小步长（h≤0.5°）下加密网格降为 h/4（4× 提速，窗口保持 ±2h 不变）；实测 h>0.25° 的加密步长会跳过 <0.5° 宽的低代价窄谷（60,128,150 目标 +61°），故粗步长档完全保持原行为；姿态任务种子贴窄带、保持 h/8 const w4 = axis(Math.max(L4.lo, s.q[3] - 2 * h), Math.min(L4.hi, s.q[3] + 2 * h), hh); const w5 = axis(Math.max(L5.lo, s.q[4] - 2 * h), Math.min(L5.hi, s.q[4] + 2 * h), hh); let lb = s; for (const a of w4) for (const b of w5) { samples++; const c = evalAt(a, b); if (c) { nSol++; if (c.rcost < lb.rcost) lb = c; } } all.push(lb); } all.sort((x, y) => x.rcost - y.rcost); const NB = [[1,0],[-1,0],[0,1],[0,-1],[1,1],[1,-1],[-1,1],[-1,-1]]; for (const s of all.slice(0, 24)) { // 模式搜索 let c4 = s.q[3], c5 = s.q[4], lb = s, step = h / 4, guard = 0; while (step > 2e-4 && guard++ < 400) { let moved = false; for (const d of NB) { const t4 = cl(c4 + d[0] * step, L4.lo, L4.hi), t5 = cl(c5 + d[1] * step, L5.lo, L5.hi); if (t4 === c4 && t5 === c5) continue; samples++; const c = evalAt(t4, t5); if (c && c.rcost < lb.rcost - 1e-10) { lb = c; c4 = t4; c5 = t5; moved = true; } } if (!moved) step *= 0.5; } all.push(lb); } }; if (opt.refine !== false) { refinePass(); for (let k = 0; k < 3; k++) { // escalating：尚无 POSEFINE 内解则姿态权重 ×100 重扫 if (!POSE || all.some(s => s.oerr != null && s.oerr <= POSEFINE)) break; poseW *= 100; refinePass(); } } /* --- 4.3 姿态收口：(θ4,θ5) 二维牛顿迭代 --- 达标带是 (θ4,θ5) 平面上的斜窄谷（局部灵敏度可达 ~10°/°），8 邻域模式搜索会整在 谷壁上（实测最好只收到 0.106°）。这里直接对"工具轴在目标轴法平面上的切向偏差" 做牛顿迭代：每步在新 (θ4,θ5) 上重解闭式位置支、按 θ1..θ3 最近匹配取同支， 位置天然保持精确，轴偏差压到 ~1e-12 量级。 */ if (POSE && opt.refine !== false && all.length) { const A0 = [POSE[2], POSE[5], POSE[8]]; // 目标工具轴（列 2） const uz = cross3(A0, [0, 0, 1]); const u = Math.hypot(uz[0], uz[1], uz[2]) > 0.5 ? nrm(uz) : nrm(cross3(A0, [1, 0, 0])); const v = cross3(A0, u); const err2 = q => { const w = axisCol(q); // 列 2 = 工具轴（原取 [M5[6..8]] 实为第 2 行） const d = w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2]; const tx = w[0] - d * A0[0], ty = w[1] - d * A0[1], tz = w[2] - d * A0[2]; return [tx * u[0] + ty * u[1] + tz * u[2], tx * v[0] + ty * v[1] + tz * v[2]]; }; const match = (t4, t5, qcur) => { // 同支匹配：θ1..θ3 最近 let best = null, bd = Infinity; for (const s of branch(t4, t5, A, R, ph, tool)) { if (!inLimit(s)) continue; let d = 0; for (let i = 0; i < 3; i++) d += Math.abs(wrap180(s[i] - qcur[i])); if (d < bd) { bd = d; best = s; } } return best; }; const newton = q0 => { let q = q0.slice(), dp = 0.05; for (let it = 0; it < 24; it++) { const e0 = err2(q); if (Math.hypot(e0[0], e0[1]) < 1e-12) break; const c4 = q[3], c5 = q[4]; const qa = q.slice(); qa[3] = c4 + dp; const qb = q.slice(); qb[4] = c5 + dp; const e4 = err2(qa), e5 = err2(qb); const j00 = (e4[0] - e0[0]) / dp, j01 = (e5[0] - e0[0]) / dp; const j10 = (e4[1] - e0[1]) / dp, j11 = (e5[1] - e0[1]) / dp; const det = j00 * j11 - j01 * j10; if (Math.abs(det) < 1e-12) { // 近奇异（刀尖构型）：缩差分步长重试 if (dp > 0.002) { dp *= 0.25; continue; } break; } let d4 = -(j11 * e0[0] - j01 * e0[1]) / det; let d5 = -(-j10 * e0[0] + j00 * e0[1]) / det; const mx = Math.max(Math.abs(d4), Math.abs(d5)); if (mx > 2) { d4 *= 2 / mx; d5 *= 2 / mx; } // 阻尼：单步 ≤ 2° const t4 = Math.max(L4.lo, Math.min(L4.hi, c4 + d4)); const t5 = Math.max(L5.lo, Math.min(L5.hi, c5 + d5)); if (t4 === c4 && t5 === c5) break; const nb = match(t4, t5, q); if (!nb) { if (dp > 0.002) { dp *= 0.25; continue; } break; } q = nb; } return q; }; const cands = all.slice().sort((x, y) => x.oerr - y.oerr).slice(0, 16); for (const s of cands) { if (s.oerr <= POSEFINE) continue; // 已达标，无需收口 const tries = [s.q]; for (const d of [[0.15, 0], [-0.15, 0], [0, 0.15], [0, -0.15], [0.25, 0.25], [-0.25, -0.25]]) { const qp = s.q.slice(); qp[3] = Math.max(L4.lo, Math.min(L4.hi, qp[3] + d[0])); qp[4] = Math.max(L5.lo, Math.min(L5.hi, qp[4] + d[1])); const nb = match(qp[3], qp[4], s.q); if (nb) tries.push(nb); // 微扰重启：探盆地边界（奇异刀尖构型） } for (const t of tries) { const q2 = newton(t); if (!q2) continue; const p2 = fk(q2, tool); const e2 = Math.hypot(p2[0] - target[0], p2[1] - target[1], p2[2] - target[2]); if (e2 > tol) continue; const c2 = scoreBranch(q2, e2); if (c2 && c2.oerr < s.oerr - 1e-9) { nSol++; all.push(c2); break; } } } } /* --- 4.4 终了精修：把代价最优的若干条入选候选都收敛成真根 --- 根枚举的变号括号只收「残差穿过零」的根，与某个精修候选相邻的那一支可能漏检； 漏检时若把达标门收死，就会为 0.01° 姿态多付几十度关节角。这里让入选集合自己 走到 ‖e‖=0，精确集合与最优集合随之重合。 */ if (POSE && opt.refine !== false && all.length) { const top = all.slice().sort((x, y) => x.rc0 - y.rc0).slice(0, 12); for (const s0 of top) { if (s0.oerr != null && s0.oerr < 1e-6) continue; const r = PT.newton(s0.q); if (!r || !r.exact) continue; const p = fk(r.q, tool); const e2 = (p[0] - target[0]) ** 2 + (p[1] - target[1]) ** 2 + (p[2] - target[2]) ** 2; if (!(e2 <= tol * tol)) continue; const c = scoreBranch(r.q, Math.sqrt(e2)); if (c) { all.push(c); if (c.oerr <= 1) band.push(c); } } } /* --- 4.5 位置优先的「姿态最近解」搜索（仅在 POSETOL 硬门把候选全挡下时启用）--- 位置优先的语义是 min oerr s.t. 位置精确。姿态在 POSETOL 内不可达时上面全部 阶段都会得到空池（这也是改动前回落到 DLS 的原因）。此处不丢弃已有信息： 从轴夹角最小的若干种子出发，在 (θ4,θ5) 上做模式搜索直接最小化 oerr。 每个搜索点仍走 branch() 闭式位置支 ⇒ 位置保持精确，残差如实上报、不虚报。 常态（姿态精确可达）下本段完全不执行，扫描路径与耗时不变。 */ /* 原条件 !all.length 只在「一个候选都没有」时启用，实测姿态精确解常在 粗扫与根枚举之后仍未被生成（目标 17.9,194.8,436.5 与 383.9,241.8,824.2）， 此时 all 非空但其中最好的也差 0.07°~1.33°，于是被误标为"精确到达"。 放宽为「没有任何 oerr ≤ POSEFINE 的候选」，让本段从轴夹角最小的种子 出发把残差收下去；姿态精确可达时本段仍不执行，扫描路径不变。 */ if (POSE && PRIO === 'pos' && !all.some(sv => sv.oerr != null && sv.oerr <= POSEFINE) && axisSeeds.length) { const pack = (r) => { const c = costOf(r.q, W); const rc0 = REF ? costOf(r.q, W, REF, LAM) : c; return { q: r.q, cost: c, rc0: rc0, rcost: rc0, err: r.err, oerr: r.oerr }; }; /* 在 (θ4,θ5) 上取最优 oerr（同一点的多条位置支都试） */ const oerrAt = (t4, t5) => { let best = null; const ss = branch(t4, t5, A, R, ph, tool); for (let k = 0; k < ss.length; k++) { const s0 = ss[k]; if (!inLimit(s0)) continue; const p = fk(s0, tool); const dx = p[0] - target[0], dy = p[1] - target[1], dz = p[2] - target[2]; const e2 = dx * dx + dy * dy + dz * dz; if (!(e2 <= tol2)) continue; const cl = closeRoll(s0); if (!best || cl.oerr < best.oerr) best = { q: cl.q, oerr: cl.oerr, err: Math.sqrt(e2) }; } return best; }; const NB = [[1,0],[-1,0],[0,1],[0,-1],[1,1],[1,-1],[-1,1],[-1,-1]]; const cl4 = (v, a, b) => v < a ? a : (v > b ? b : v); let bs = null; for (const sd of axisSeeds) { let lb = oerrAt(sd.q[3], sd.q[4]); if (!lb) continue; samples++; let c4 = sd.q[3], c5 = sd.q[4], step = Math.max(h, 0.5), guard = 0; while (step > 2e-4 && guard++ < 400) { // 模式搜索：步长减半直到收敛 let moved = false; for (const d of NB) { const t4 = cl4(c4 + d[0] * step, L4.lo, L4.hi), t5 = cl4(c5 + d[1] * step, L5.lo, L5.hi); if (t4 === c4 && t5 === c5) continue; samples++; const c = oerrAt(t4, t5); if (c && c.oerr < lb.oerr - 1e-12) { lb = c; c4 = t4; c5 = t5; moved = true; } } if (!moved) step *= 0.5; } if (lb && (!bs || lb.oerr < bs.oerr - 1e-12)) bs = lb; } if (bs) { nSol++; all.push(pack(bs)); } } all.sort((x, y) => x.rcost - y.rcost); /* 采用解：姿态任务优先从 oerr ≤ POSEFINE 的达标候选里按含连续项代价选；无达标解才尽力而为 */ let pool = all, poolByOerr = false; if (POSE) { /* 有真根时优先取真根，但不为姿态精度付大额关节角：精确根只有在代价不高于 达标最优 +2° 时才接管（实测两者通常差 <0.05°，个别漏检情形能差到 40°）。 */ const acc = all.filter(s => s.oerr != null && s.oerr <= POSEFINE); const ex = all.filter(s => s.oerr != null && s.oerr <= 1e-4); if (acc.length) { const ca = Math.min(...acc.map(s => s.rc0)); pool = (ex.length && Math.min(...ex.map(s => s.rc0)) <= ca + 2) ? ex : acc; } else if (PRIO === 'pos') { /* 位置优先：无姿态达标解 → 取姿态残差最小者（同量级时取 Σ|θ| 更小者） */ poolByOerr = true; pool = all.slice(); } } 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); if (best && list.indexOf(best) < 0) list.unshift(best); /* 真残差补算：热循环里延后的 fk 校验在这里对入选候选统一补一次（err 为真值） */ if (best) { const errOf = q => { const p = fk(q, tool); const dx = p[0] - target[0], dy = p[1] - target[1], dz = p[2] - target[2]; return Math.hypot(dx, dy, dz); }; best.err = errOf(best.q); const nErr = Math.min(list.length, 64); for (let i = 0; i < nErr; i++) if (list[i].err === 0) list[i].err = errOf(list[i].q); } return { ok: !!best, reachable: !!best, target: target.slice(), tool: tool, pose: !!POSE, priority: PRIO, /* 姿态分级：reachable 只表示"有解"，不足以区分"精确"与"近似"（缺陷 3）。 poseFine=姿态偏差 ≤POSEFINE(0.05°) 才算精确到达；poseTier 供上层显式区分： fine = 精确 approx = 落在 POSETOL(2°) 内的近似 poor = 连 POSETOL 都没进 none = 没有任何候选 reachablePose 是严格版（位置与姿态同时达标），需要严格语义时用它。 */ poseFine: POSE ? !!(best && best.oerr != null && best.oerr <= POSEFINE) : null, poseTier: !POSE ? null : (!best ? 'none' : (best.oerr != null && best.oerr <= POSEFINE ? 'fine' : (best.oerr != null && best.oerr <= POSETOL ? 'approx' : 'poor'))), reachablePose: POSE ? !!(best && best.oerr != null && best.oerr <= POSEFINE && best.err != null && best.err <= Math.sqrt(tol2)) : null, q: best ? best.q.slice() : null, cost: best ? best.cost : null, /* 本步行程 = 判据值：Σ|wrap(θ−ref)|（连续轴按等价角计），便于界面核对 */ moveSum: (REF && best) ? (() => { let t = 0; for (let i = 0; i < 6; i++) t += Math.abs(wrap180(best.q[i] - REF[i])); return t; })() : null, err: best ? best.err : null, oerr: best ? best.oerr : null, nSol: nSol, truncated: trimmed, samples: samples, ms: nowMs() - t0, list: list }; } /* ---------- 5. 最近可达位姿（阻尼最小二乘，兜底用） ---------- */ function closest(target, tool, tries, ref) { const dp = 0.05, lam = 4.0; let bestQ = null, bestD = Infinity; /* anchor：参考位姿锚定。准退化构型（手臂近似伸直）下 J3 等轴的雅可比列梯度≈0， DLS 伪逆仍会给它分配微小步长，160 次迭代累积成大幅无用漂移（实测 82°）。 每次迭代施加 0.03·(q−anchor) 的回拉，把退化方向的漂移压掉，任务真正需要的 方向（远大于此回拉）不受影响。 */ const run = (q, anchor) => { const J = new Float64Array(18); for (let it = 0; it < 160; it++) { const p = fk(q, tool); const d = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]); if (d < bestD) { bestD = d; bestQ = q.slice(); } if (d < 1e-9) break; for (let c = 0; c < 6; c++) { const s = q.slice(); s[c] += dp; const pp = fk(s, tool); J[c * 3] = (pp[0] - p[0]) / dp; J[c * 3 + 1] = (pp[1] - p[1]) / dp; J[c * 3 + 2] = (pp[2] - p[2]) / dp; } let m00 = lam, m01 = 0, m02 = 0, m11 = lam, m12 = 0, m22 = lam; for (let c = 0; c < 6; c++) { const ax2 = J[c * 3], ay = J[c * 3 + 1], az2 = J[c * 3 + 2]; m00 += ax2 * ax2; m01 += ax2 * ay; m02 += ax2 * az2; m11 += ay * ay; m12 += ay * az2; m22 += az2 * az2; } const det = m00 * (m11 * m22 - m12 * m12) - m01 * (m01 * m22 - m12 * m02) + m02 * (m01 * m12 - m11 * m02); if (Math.abs(det) < 1e-12) break; const i00 = (m11 * m22 - m12 * m12) / det, i01 = -(m01 * m22 - m12 * m02) / det, i02 = (m01 * m12 - m11 * m02) / det, i11 = (m00 * m22 - m02 * m02) / det, i12 = -(m00 * m12 - m01 * m02) / det, i22 = (m00 * m11 - m01 * m01) / det; const ex = p[0] - target[0], ey = p[1] - target[1], ez = p[2] - target[2]; const y0 = -(i00 * ex + i01 * ey + i02 * ez); const y1 = -(i01 * ex + i11 * ey + i12 * ez); const y2 = -(i02 * ex + i12 * ey + i22 * ez); const dq = [0, 0, 0, 0, 0, 0]; let mx = 0; for (let c = 0; c < 6; c++) { dq[c] = J[c * 3] * y0 + J[c * 3 + 1] * y1 + J[c * 3 + 2] * y2; mx = Math.max(mx, Math.abs(dq[c])); } const gain = mx > 12 ? 12 / mx : 1; for (let c = 0; c < 6; c++) { const t = LIM[ORDER[c]]; let stp = dq[c] * gain; if (anchor) stp -= 0.03 * (q[c] - anchor[c]); q[c] = Math.max(t.lo, Math.min(t.hi, q[c] + stp)); } } }; /* 零空间抛光：手臂伸直（目标在可达边界外）时 J3 轴与臂线共线，转 J3 不改变 TCP，DLS 对该方向梯度为零、停在哪里全看随机初始角。这里在"到目标距离几乎 不变"（≤slack）的前提下令姿态滑向最小总关节角，把随机角收敛回确定代表值。 同时兼顾连续性：ref 给出时从 ref 起步，邻近目标 successive 解算平滑跟进。 */ const distQ = q => Math.hypot.apply(null, fk(q, tool).map((v, i) => v - target[i])); const polish = (q, slack) => { if (!q) return; const d0 = distQ(q); let step = 8; for (let pass = 0; pass < 4; pass++) { let moved = true, guard = 0; while (moved && guard++ < 40) { moved = false; for (let i = 0; i < 5; i++) { // J6 不改 TCP 位置，抛光无意义 const t = LIM[ORDER[i]]; for (const s of [1, -1]) { const v = q[i] + s * step; if (v < t.lo || v > t.hi) continue; const old = q[i], c0 = costOf(q, null); q[i] = v; const d = distQ(q); if (d <= d0 + slack && costOf(q, null) < c0 - 1e-9) moved = true; else q[i] = old; } } } step *= 0.25; } }; let anchorQ = null, anchorD = Infinity; if (ref && ref.length === 6) { anchorQ = ref.slice(); run(anchorQ, ref); // 锚定轮：从当前姿态出发保连续 anchorD = distQ(anchorQ); } run([0, 0, 0, 0, 0, 0]); /* 确定性拟随机重启：固定种子的 LCG，取代 Math.random —— 同一目标重复解算 结果必须逐位一致，否则边界目标每次刷新姿态都不同，就是"随机倒摆"的根源 */ let seed = 0x5bd1e995; const rnd = () => { seed = (seed * 1103515245 + 12345) & 0x7fffffff; return seed / 0x7fffffff; }; const N = tries == null ? 160 : tries; for (let k = 0; k < N; k++) { const q = [0, 0, 0, 0, 0, 0]; for (let c = 0; c < 5; c++) { const t = LIM[ORDER[c]]; q[c] = t.lo + rnd() * (t.hi - t.lo); } run(q); } /* 锚定解与全局最优距离差 ≤0.5mm 时优先锚定解：刚过边界的目标（差 mm 以内） 保住原构型自然伸出，不为零点几毫米换个 Fold 姿态；差得远时仍取全局最近 */ if (anchorQ && anchorD <= bestD + 0.5) { bestQ = anchorQ; bestD = anchorD; } if (bestQ) { // 报告抛光后真实距离，不虚报 polish(bestQ, 0.05); // 距离容忍 0.05mm（远小于边界误差量级） bestD = distQ(bestQ); } return { q: bestQ, dist: bestD }; } /* ---------- 5b. 最近可达位姿（位置 + 姿态联合，阻尼最小二乘） ---------- */ /* 误差 6 维：位置 3（mm）+ 姿态 3（R_curᵀ·R_des 的旋转矢量，rad）。 姿态项按 20 mm/rad 折算进误差向量（≈0.35mm/度），阻尼项吸收量纲差。 */ function closest6(target, Rdes, tool, tries, ref) { const OW = 20.0; // mm per rad const dp = 0.05, lam = 6.0; let bestQ = null, bestE = Infinity, bestOri = 0; const errOf = q => { const p = fk(q, tool), N = mul3(T3(orient(q)), Rdes); const tr = N[0] + N[4] + N[8], ang = Math.acos(clamp1((tr - 1) / 2)); const s = Math.sin(ang); const k = s > 1e-6 ? 1 / (2 * s) : 0.5; // 小角时直接用斜对称向量/2，免奇异 const rv = [(N[7] - N[5]) * k, (N[2] - N[6]) * k, (N[3] - N[1]) * k]; return [p[0] - target[0], p[1] - target[1], p[2] - target[2], rv[0] * OW, rv[1] * OW, rv[2] * OW]; }; const norm6 = e => Math.hypot(e[0], e[1], e[2], e[3], e[4], e[5]); const oriOf = q => { // 纯姿态偏差（度） const N = mul3(T3(orient(q)), Rdes); return Math.acos(clamp1((N[0] + N[4] + N[8] - 1) / 2)) * R2D; }; /* 6×6 线性方程组：高斯消去 + 列主元（A = J·Jᵀ + λ²I，y = A⁻¹e，dq = Jᵀy） */ const solveLin = (A, b, n) => { const M = A.slice(), x = b.slice(); for (let c = 0; c < n; c++) { let p = c; for (let r = c + 1; r < n; r++) if (Math.abs(M[r * n + c]) > Math.abs(M[p * n + c])) p = r; if (Math.abs(M[p * n + c]) < 1e-14) return null; if (p !== c) { for (let j = 0; j < n; j++) { const t = M[c * n + j]; M[c * n + j] = M[p * n + j]; M[p * n + j] = t; } const t = x[c]; x[c] = x[p]; x[p] = t; } for (let r = 0; r < n; r++) { if (r === c) continue; const f = M[r * n + c] / M[c * n + c]; if (!f) continue; for (let j = c; j < n; j++) M[r * n + j] -= f * M[c * n + j]; x[r] -= f * x[c]; } } for (let c = 0; c < n; c++) x[c] /= M[c * n + c]; return x; }; const run = (q, anchor) => { const J = new Float64Array(36); // 6×6（误差 × 关节） for (let it = 0; it < 160; it++) { const e = errOf(q); const ne = norm6(e); if (ne < bestE) { bestE = ne; bestQ = q.slice(); bestOri = oriOf(q); } if (ne < 1e-9) break; for (let c = 0; c < 6; c++) { const s = q.slice(); s[c] += dp; const ep = errOf(s); for (let r = 0; r < 6; r++) J[r * 6 + c] = (ep[r] - e[r]) / dp; } const A = new Array(36).fill(0); for (let i = 0; i < 6; i++) { for (let j = 0; j < 6; j++) { let s = 0; for (let k = 0; k < 6; k++) s += J[i * 6 + k] * J[j * 6 + k]; A[i * 6 + j] = s + (i === j ? lam * lam : 0); } } const y = solveLin(A, e.map(v => -v), 6); if (!y) break; const dq = new Array(6); let mx = 0; for (let c = 0; c < 6; c++) { let s = 0; for (let r = 0; r < 6; r++) s += J[r * 6 + c] * y[r]; dq[c] = s; mx = Math.max(mx, Math.abs(s)); } const gain = mx > 12 ? 12 / mx : 1; for (let c = 0; c < 6; c++) { const t = LIM[ORDER[c]]; let stp = dq[c] * gain; if (anchor) stp -= 0.03 * (q[c] - anchor[c]); q[c] = Math.max(t.lo, Math.min(t.hi, q[c] + stp)); } } }; /* 零空间抛光：在位置与姿态偏差都不增的前提下令 Σ|θ| 最小（步进、步长逐轮衰减） */ const slackOf = q => { const e = errOf(q); return [Math.hypot(e[0], e[1], e[2]), Math.hypot(e[3], e[4], e[5]) / OW]; }; const polish = (q, slack) => { if (!q) return; const [d0, o0] = slackOf(q); let step = 8; for (let pass = 0; pass < 4; pass++) { let moved = true, guard = 0; while (moved && guard++ < 40) { moved = false; for (let i = 0; i < 5; i++) { // J6 是纯滚转，抛它只会劣化姿态偏差 const t = LIM[ORDER[i]]; for (const s of [1, -1]) { const v = q[i] + s * step; if (v < t.lo || v > t.hi) continue; const old = q[i], c0 = costOf(q, null); q[i] = v; const [d, o] = slackOf(q); if (d <= d0 + slack && o <= o0 + 0.05 && costOf(q, null) < c0 - 1e-9) moved = true; else q[i] = old; } } } step *= 0.25; } }; let anchorQ = null, anchorE = Infinity; if (ref && ref.length === 6) { anchorQ = ref.slice(); run(anchorQ, ref); anchorE = norm6(errOf(anchorQ)); } run([0, 0, 0, 0, 0, 0]); let seed = 0x5bd1e995; const rnd = () => { seed = (seed * 1103515245 + 12345) & 0x7fffffff; return seed / 0x7fffffff; }; const N = tries == null ? 160 : tries; for (let k = 0; k < N; k++) { const q = [0, 0, 0, 0, 0, 0]; for (let c = 0; c < 5; c++) { const t = LIM[ORDER[c]]; q[c] = t.lo + rnd() * (t.hi - t.lo); } run(q); } if (anchorQ && anchorE <= bestE + 0.5) { bestQ = anchorQ; bestE = anchorE; } if (bestQ) { polish(bestQ, 0.05); // 位置 0.05mm / 姿态 0.05° 内不劣化 bestE = norm6(errOf(bestQ)); bestOri = oriOf(bestQ); } return { q: bestQ, dist: Math.hypot.apply(null, bestQ ? fk(bestQ, tool).map((v, i) => v - target[i]) : [1e9, 1e9, 1e9]), ori: bestOri }; } /* ---------- 5c. 姿态优先的最近解：姿态必须精确，位置取最近 ---------- 位置精确解集是 (θ4,θ5) 平面上的 2 维族；姿态精确（工具轴对齐 + θ6 闭式补滚转）解集 是 3 维族——两者不是同一集合，姿态优先必须离开位置精确族去搜索。本机构有降阶结构 （J4∥J5 ⇒ R4·R5 = R_y(θ4+θ5)，与 axisCol 同一约定）： 工具轴 w = Rx(θ1)·Ry(θ2)·Rz(−θ3)·Ry(s)·ẑ , s = θ4 + θ5 给定 (θ3, s) 后 (θ1,θ2) 可闭式解出（至多两支，见 axisSolve），剩余自由参数只有 θ4 （θ5 = s − θ4）⇒ 在 (θ3, s, θ4) 三维上粗扫取种子、再坐标下降精修即可。轴一旦精确对齐， 由 rollClose 定出的 θ6 令 oerr 恒为 0，不再有姿态残差；位置误差由 fk 如实给出。 */ function closestOri(target, Rdes, tool, ref) { if (!SAMEAX) return null; // J4 与 J5 不同轴：降阶结构不成立，交回 DLS const t = [Rdes[2], Rdes[5], Rdes[8]]; const L4 = LIM.J4, L5 = LIM.J5, L2 = LIM.J2; const sLo = L4.lo + L5.lo, sHi = L4.hi + L5.hi; /* 解 Rx(θ1)·Ry(θ2)·v = t（v = Rz(−θ3)·Ry(s)·ẑ），返回至多两支 [θ1,θ2]（度） */ const axisSolve = (th3, s) => { const v = mv3(rotAxis(D.J3, th3 * D2R), mv3(rotAxis(D.J4, s * D2R), [0, 0, 1])); const Rv = Math.hypot(v[0], v[2]); if (Rv < 1e-12) return []; const cs = t[0] / Rv; if (cs < -1 || cs > 1) return []; const psi = Math.atan2(v[2], v[0]), d2 = Math.acos(cs), out = []; for (let k = 0; k < 2; k++) { const th2 = psi + (k ? -d2 : d2); const c2 = Math.cos(th2), s2 = Math.sin(th2); const P = s2 * v[0] - c2 * v[2], Q = v[1], det = P * P + Q * Q; if (det < 1e-14) continue; // 工具轴落在 ±X：θ1 退化，交给别的种子 if (th2 < L2.lo - 1e-9 || th2 > L2.hi + 1e-9) continue; const th1 = Math.atan2((P * t[1] + Q * t[2]) / det, (Q * t[1] - P * t[2]) / det); out.push([wrap180(th1 * R2D), wrap180(th2 * R2D)]); } return out; }; const e2Of = q => { const p = fk(q, tool); const dx = p[0] - target[0], dy = p[1] - target[1], dz = p[2] - target[2]; return dx * dx + dy * dy + dz * dz; }; /* 给定 (θ3, s, θ4) 组装关节角：θ1..θ3 闭式、θ5 = s − θ4、θ4 夹进可行区间 */ const qOf = (th3, s, th4, prev) => { const br = axisSolve(th3, s); if (!br.length) return null; let b = br[0], bd = Infinity; for (const x of br) { // 同支匹配：与上一步的 θ1/θ2 最近 const d = Math.abs(wrap180(x[0] - prev[0])) + Math.abs(wrap180(x[1] - prev[1])); if (d < bd) { bd = d; b = x; } } const a4 = Math.max(L4.lo, s - L5.hi), b4 = Math.min(L4.hi, s - L5.lo); if (a4 > b4 + 1e-9) return null; const th4c = Math.max(a4, Math.min(b4, th4)); return [b[0], b[1], wrap180(th3), th4c, s - th4c, 0]; }; /* 粗扫 (θ3, s, θ4)：只保留每格最优，再按位置误差取空间去同的若干种子 */ const cand = []; for (let th3 = -180; th3 <= 180 + 1e-9; th3 += 5) { for (let s = sLo; s <= sHi + 1e-9; s += 5) { const sf = Math.min(s, sHi); const br = axisSolve(th3, sf); if (!br.length) continue; const a4 = Math.max(L4.lo, sf - L5.hi), b4 = Math.min(L4.hi, sf - L5.lo); if (a4 > b4 + 1e-9) continue; for (const b of br) { let bc = null; for (let th4 = a4; th4 <= b4 + 1e-9; th4 += 5) { const th4c = Math.min(th4, b4); const q = [b[0], b[1], wrap180(th3), th4c, sf - th4c, 0]; const e2 = e2Of(q); if (!bc || e2 < bc.d2) bc = { q: q, d2: e2 }; } if (bc) cand.push(bc); } } } if (!cand.length) return null; cand.sort((x, y) => x.d2 - y.d2); const seeds = []; for (const c of cand) { if (seeds.length >= 32) break; let dup = false; for (const sd of seeds) if (Math.hypot(wrap180(sd.q[2] - c.q[2]), sd.q[3] - c.q[3], sd.q[4] - c.q[4]) < 15) { dup = true; break; } if (!dup) seeds.push(c); } /* 坐标下降精修（θ3 / s / θ4 三方向，步长减半直到收敛） */ const refine = q0 => { let q = q0.slice(), step = 5; for (let pass = 0; pass < 60; pass++) { let moved = false; for (let i = 0; i < 3; i++) { for (const sg of [1, -1]) { const th3 = i === 0 ? q[2] + sg * step : q[2]; const s = (i === 1 ? q[3] + q[4] + sg * step : q[3] + q[4]); if (s < sLo - 1e-9 || s > sHi + 1e-9) continue; const th4 = i === 2 ? q[3] + sg * step : q[3]; const nq = qOf(th3, s, th4, q); if (nq && e2Of(nq) < e2Of(q) - 1e-12) { q = nq; moved = true; } } } if (!moved) { step *= 0.5; if (step < 1e-4) break; } } return q; }; let bestQ = null, bestE = Infinity; const pool = []; // 精修结果集：用于带 ref 的连续性仲裁 for (const sd of seeds) { const q = refine(sd.q), e2 = e2Of(q); pool.push({ q: q, d2: e2 }); if (e2 < bestE) { bestE = e2; bestQ = q; } } if (!bestQ) return null; if (ref && ref.length === 6) { /* 与「最近可达位姿」同一约定：位置差在 0.5 mm 内时优先保连续性，避免等价支间来回跳 */ let bd = Infinity, bq = null; for (const p of pool) { if (p.d2 > bestE + 1.0) continue; // d2 差 1.0 ≈ 距离差 0.5mm 量级 let d = 0; for (let i = 0; i < 6; i++) d += Math.abs(wrap180(p.q[i] - ref[i])); if (d < bd) { bd = d; bq = p.q; } } if (bq) bestQ = bq; } const rc = rollClose(bestQ, Rdes); return { q: rc.q, dist: Math.sqrt(e2Of(bestQ)), ori: rc.oerr }; } /* ---------- 5d. 混合优先：位置与姿态按可调权重折中 ---------- 滑杆 w ∈ [0,1] 自由分配两端的比重： w = 1 与 w = 0 由 solveTo 直接交给两条既有专用算法（位置优先 / 姿态优先）， 结果逐位一致；本段只负责 0 < w < 1 的加权折中。 折中语义 =「沿折中历程的进度」，代价与归一化尺度都取两端锚点自身： J(q) = w·(e/e_ref)² + (1−w)·(o/o_ref)² e_ref = 姿态端锚点的位置差（姿态优先解的 TCP 位置差）， o_ref = 位置端锚点的姿态残差（位置优先解的姿态残差）。 两端锚点各把「本端最差方向」定标，于是滑杆读数与目标的绝对误差量级无关 （50% 即两端误差各自归一后对半）。两端锚点复用两条既有算法： 位置端 = solve(priority:'pos')（位置精确族 + 闭式滚转 + 孤立根收口；位置不可达 时退化为 closest() 的最近可达位置），姿态端 = closestOri()（工具轴闭式对齐 + θ6 补滚转 ⇒ 姿态精确、位置最近）。 中间值以两锚点及关节空间插值为种子，对 6 维残差 [√w·(p−t)/e_ref , √(1−w)·RV°/o_ref] 做阻尼高斯-牛顿 + 单轴抛光，取 J 最小者 （J 同量级带内取相对当前姿态的行程 Σ|wrap(θ−ref)| 更小者）。两端任一维已精确（≤0.05）时 折中无意义，直接返回位置端解。全程无随机数：同一输入逐位可复现。 */ let mixScale = null; // 最近一次全量折中的归一化尺度（拖动快路沿用） const mixClamp = v => Math.max(0, Math.min(1, v == null || !isFinite(+v) ? 0.5 : +v)); /* 折中共用内核：归一化代价 + 加权阻尼高斯-牛顿（§5d 全量与快路共用） */ function mixKernel(target, tool, Rtar, w, eRef, oRef) { const kp = Math.sqrt(w) / eRef, ko = Math.sqrt(1 - w) / oRef; const RS = 100; // 残差整体放大：量级与 §5b 的 mm 制对齐，阻尼比例可沿用 const lam = 6.0, dp = 0.05; let samples = 0; /* 闭式滚转（θ6）后的真实 (e mm, o 度) 与归一化代价 J */ const canon = q => { const rc = rollClose(q, Rtar), p = fk(rc.q, tool); const e = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]); samples++; return { q: rc.q, e: e, o: rc.oerr, J: w * (e / eRef) ** 2 + (1 - w) * (rc.oerr / oRef) ** 2 }; }; /* 6 维残差（RS 倍放大）：位置 3 + 姿态旋转矢量 3（度制，范数 = 姿态夹角） */ const resid6 = q => { const p = fk(q, tool), N = mul3(T3(orient(q)), Rtar); const ang = Math.acos(clamp1((N[0] + N[4] + N[8] - 1) / 2)); const s = Math.sin(ang), k = s > 1e-8 ? ang / (2 * s) : 0.5; // 旋转矢量 = vee·k（弧度） samples++; return [(p[0] - target[0]) * kp * RS, (p[1] - target[1]) * kp * RS, (p[2] - target[2]) * kp * RS, (N[7] - N[5]) * k * R2D * ko * RS, (N[2] - N[6]) * k * R2D * ko * RS, (N[3] - N[1]) * k * R2D * ko * RS]; }; /* 阻尼高斯-牛顿（与 §5b 同构：A = J·Jᵀ + λ²I，y = A⁻¹(−e)，dq = Jᵀy，步长 ≤12°） */ /* 6×6 线性方程组：高斯消去 + 列主元（A = J·Jᵀ + λ²I，y = A⁻¹e，dq = Jᵀy） */ const solveLin = (A, b, n) => { const M = A.slice(), x = b.slice(); for (let c = 0; c < n; c++) { let p = c; for (let r = c + 1; r < n; r++) if (Math.abs(M[r * n + c]) > Math.abs(M[p * n + c])) p = r; if (Math.abs(M[p * n + c]) < 1e-14) return null; if (p !== c) { for (let j = 0; j < n; j++) { const t = M[c * n + j]; M[c * n + j] = M[p * n + j]; M[p * n + j] = t; } const t = x[c]; x[c] = x[p]; x[p] = t; } for (let r = 0; r < n; r++) { if (r === c) continue; const f = M[r * n + c] / M[c * n + c]; if (!f) continue; for (let j = c; j < n; j++) M[r * n + j] -= f * M[c * n + j]; x[r] -= f * x[c]; } } for (let c = 0; c < n; c++) x[c] /= M[c * n + c]; return x; }; const dls = (seed, anchor) => { const q = seed.slice(), Jm = new Float64Array(36); let best = null; for (let it = 0; it < 160; it++) { const e = resid6(q); let n2 = 0; for (let i = 0; i < 6; i++) n2 += e[i] * e[i]; if (!best || n2 < best.n2) best = { q: q.slice(), n2: n2 }; if (n2 < 1e-10) break; for (let c = 0; c < 6; c++) { const s = q.slice(); s[c] += dp; const ep = resid6(s); for (let r = 0; r < 6; r++) Jm[r * 6 + c] = (ep[r] - e[r]) / dp; } const A = new Array(36).fill(0); for (let i = 0; i < 6; i++) { for (let j = 0; j < 6; j++) { let s = 0; for (let k = 0; k < 6; k++) s += Jm[i * 6 + k] * Jm[j * 6 + k]; A[i * 6 + j] = s + (i === j ? lam * lam : 0); } } const y = solveLin(A, e.map(v => -v), 6); if (!y) break; const dq = new Array(6); let mx = 0, moved = 0; for (let c = 0; c < 6; c++) { let s = 0; for (let r = 0; r < 6; r++) s += Jm[r * 6 + c] * y[r]; dq[c] = s; mx = Math.max(mx, Math.abs(s)); } const gain = mx > 12 ? 12 / mx : 1; for (let c = 0; c < 6; c++) { const t = LIM[ORDER[c]]; let stp = dq[c] * gain; if (anchor) stp -= 0.03 * (q[c] - anchor[c]); // 参考位姿锚定：退化方向不漂移 const v = Math.max(t.lo, Math.min(t.hi, q[c] + stp)); moved += Math.abs(v - q[c]); q[c] = v; } if (moved < 1e-9) break; } return best ? best.q : null; }; /* 单轴抛光：J 不再下降为止，步长逐轮减半（θ6 由 rollClose 闭式定，不参与抛光） */ const polish = q0 => { let cur = canon(q0), q = cur.q, step = 1.5; for (let pass = 0; pass < 24 && step > 1e-3; pass++) { let moved = false; for (let c = 0; c < 5; c++) { const t = LIM[ORDER[c]]; for (const sg of [1, -1]) { const v = q[c] + sg * step; if (v < t.lo || v > t.hi) continue; const qq = q.slice(); qq[c] = v; const cc = canon(qq); if (cc.J < cur.J - 1e-12) { cur = cc; q = cc.q; moved = true; } } } if (!moved) step *= 0.5; } return cur; }; return { canon: canon, dls: dls, polish: polish, nSamples: () => samples }; } function mixSolve(target, opt) { opt = opt || {}; if (!opt.pose || !opt.pose.R || opt.pose.R.length !== 9) return solve(target, opt); // 无姿态任务：退回纯位置求解 const t0 = nowMs(); const tool = +opt.tool || 0; const W = opt.weights || [1, 1, 1, 1, 1, 1]; const REF = (opt.ref && opt.ref.length === 6) ? opt.ref.slice() : null; const LAM = +opt.refW || 0; const Rtar = opt.pose.R; const w = mixClamp(opt.mixW); const EXACT = 0.05; // 两端任一维已 ≤0.05 ⇒ 视为已精确到达（同 POSEFINE） const scaleOf = (e, o) => { mixScale = { e: Math.max(e, 0.05), o: Math.max(o, 0.05) }; }; /* 位置端锚点：既有位置优先解（位置精确族内姿态残差最小者，含 §4.5 最近姿态搜索） */ const pa = solve(target, Object.assign({}, opt, { priority: 'pos' })); let pEnd = null; if (pa.ok && pa.q) pEnd = { q: pa.q.slice(), e: pa.err || 0, o: pa.oerr || 0 }; /* 姿态端锚点：姿态精确族的最小位置差（工具轴闭式对齐 + θ6 补滚转） */ const oa = closestOri(target, Rtar, tool, REF); let oEnd = null; if (oa && oa.q) { const rc = rollClose(oa.q, Rtar), p = fk(rc.q, tool); oEnd = { q: rc.q, e: Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]), o: rc.oerr }; } const stamp = (res, pn, on) => Object.assign({}, res, { priority: 'mix', mixW: w, mixP: pn ? { e: pn.e, o: pn.o } : null, mixO: on ? { e: on.e, o: on.o } : null }); /* 两端中任一端在另一维也达标 ⇒ 已同时精确，折中无意义：返回该端（标准解）。 先判位置端：位置优先解本身就最优；位置端不达标而姿态端在位置上也达标时， 姿态端在「位置 + 姿态」两维同时占优（位置优先族搜不到的到达解，姿态端找到了）， 此时必须返回姿态端，不能返回被占优的位置端解。 */ if (pEnd && pEnd.o <= EXACT) { scaleOf(oEnd ? oEnd.e : 1, pEnd.o); return stamp(pa, pEnd, oEnd); } if (oEnd && oEnd.e <= EXACT) { scaleOf(oEnd.e, pEnd ? pEnd.o : 1); const cost = costOf(oEnd.q, W); return stamp({ ok: true, reachable: true, target: target.slice(), tool: tool, pose: true, q: oEnd.q.slice(), cost: cost, err: oEnd.e, oerr: oEnd.o, nSol: 1, samples: 0, ms: nowMs() - t0, list: [{ q: oEnd.q.slice(), cost: cost, rc0: cost, rcost: cost, err: oEnd.e, oerr: oEnd.o }] }, pEnd, oEnd); } /* 位置端：位置不可达时退化为最近可达位置（closest），姿态残差如实取 */ if (!pEnd) { const cp = closest(target, tool, opt.tries, REF); if (!cp.q) return pa; // 连最近可达位置都求不出：交回原结果（异常路径） const rc = rollClose(cp.q, Rtar), p = fk(rc.q, tool); pEnd = { q: rc.q, e: Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]), o: rc.oerr }; } if (!oEnd) { // 姿态端锚点缺失（结构前提不满足，或限位内无姿态精确解）： /* 按纯姿态优先的同一兜底取 6 维联合 DLS 最近位姿当姿态端，保证兜底语义一致 */ const c6 = closest6(target, Rtar, tool, opt.tries, REF); if (c6 && c6.q) { const rc = rollClose(c6.q, Rtar), p = fk(rc.q, tool); oEnd = { q: rc.q, e: Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]), o: rc.oerr }; } else { scaleOf(pEnd.e, pEnd.o); return stamp(pa, pEnd, null); } } const eRef = Math.max(oEnd.e, 0.01), oRef = Math.max(pEnd.o, 0.01); scaleOf(eRef, oRef); const K = mixKernel(target, tool, Rtar, w, eRef, oRef); /* 种子：两端锚点 + 关节空间插值（落在两端之间的盆地里）+ 参考位姿 */ const seeds = [pEnd.q, oEnd.q]; for (const t of [0.25, 0.5, 0.75]) { const q = pEnd.q.slice(); for (let i = 0; i < 6; i++) { const tl = LIM[ORDER[i]]; q[i] = Math.max(tl.lo, Math.min(tl.hi, q[i] + wrap180(oEnd.q[i] - q[i]) * t)); } seeds.push(q); } if (REF) seeds.push(REF); const cands = [K.canon(pEnd.q), K.canon(oEnd.q)]; for (const sd of seeds) { const r = K.dls(sd, REF); if (r) cands.push(K.polish(r)); } /* 选优：J 最小；J 同量级（2% 带内）取行程（相对当前姿态的 Σ|wrap(θ−ref)|）更小者，防等价支横跳 */ cands.sort((x, y) => x.J - y.J); const j0 = cands[0].J, jBand = j0 + 0.02 * j0 + 1e-6; let best = cands[0], bc = Infinity; for (const c of cands) { if (c.J > jBand) break; const cst = REF ? costOf(c.q, W, REF, LAM) : costOf(c.q, W); if (cst < bc - 1e-12) { bc = cst; best = c; } } const cost = costOf(best.q, W); /* 候选表（供动态限位/碰撞改选）：与最优 J 同量级者，按行程（有 ref）升序 */ const list = []; for (const c of cands) { if (c !== best && c.J > j0 * 1.5 + 0.05) continue; const cst = costOf(c.q, W); const rc = REF ? costOf(c.q, W, REF, LAM) : cst; list.push({ q: c.q.slice(), cost: cst, rc0: rc, rcost: rc, err: c.e, oerr: c.o }); } list.sort((x, y) => x.rcost - y.rcost); return { ok: true, reachable: !!pa.ok, target: target.slice(), tool: tool, pose: true, priority: 'mix', mixW: w, mixP: { e: pEnd.e, o: pEnd.o }, mixO: { e: oEnd.e, o: oEnd.o }, q: best.q.slice(), cost: cost, err: best.e, oerr: best.o, resid: best.e > EXACT ? best.e : null, nSol: list.length, truncated: false, samples: K.nSamples(), ms: nowMs() - t0, list: list, note: pa.ok ? null : '混合优先（位置 ' + Math.round(w * 100) + '%）：目标位姿超可达空间 → 两端锚点折中最优（位置差 ' + best.e.toFixed(2) + ' mm · 姿态差 ' + best.o.toFixed(2) + '°）' }; } /* 混合优先的拖动快路：沿上一帧折中解做一次加权 DLS + 抛光，归一化尺度沿用最近一次 全量折中的缓存（目标微动下两端误差量级不变）；位移超限（换盆地）返回 null 交回全量。 与 §6a 同一哲学：快路只负责「能快的时候快」，松手后的全量求解才是最终结果。 */ function mixWarm(target, opt) { const q0 = opt.warm; if (!q0 || q0.length !== 6 || !mixScale) return null; const t0 = nowMs(); const tool = +opt.tool || 0; const W = opt.weights || [1, 1, 1, 1, 1, 1]; const REF = (opt.ref && opt.ref.length === 6) ? opt.ref : null; const w = mixClamp(opt.mixW); const K = mixKernel(target, tool, opt.pose.R, w, mixScale.e, mixScale.o); const q1 = K.polish(K.dls(q0, REF) || q0); let dsh = 0; for (let i = 0; i < 5; i++) dsh += Math.abs(wrap180(q1.q[i] - q0[i])); if (dsh > 25) return null; // 同支位移过大：可能已换盆地，交回全量重算 const cost = costOf(q1.q, W); return { ok: true, reachable: true, warm: true, target: target.slice(), tool: tool, pose: true, priority: 'mix', mixW: w, q: q1.q.slice(), cost: cost, err: q1.e, oerr: q1.o, nSol: 1, truncated: false, samples: K.nSamples(), ms: nowMs() - t0, list: [{ q: q1.q.slice(), cost: cost, rc0: cost, rcost: cost, err: q1.e, oerr: q1.o }] }; } /* ---------- 6. 一键求解（不可达时返回最近可达位姿/位姿） ---------- */ function solveTo(target, opt) { opt = opt || {}; /* 混合优先（§5d）：两端（0% / 100%）直接交给两条既有专用算法——结果与「姿态优先 / 位置优先」逐位一致；中间值走加权折中搜索（拖动优先快路，校验不过回落全量）。 */ if (opt.pose && opt.pose.R && opt.pose.R.length === 9 && opt.priority === 'mix') { const w = mixClamp(opt.mixW); if (w >= 1 - 1e-9 || w <= 1e-9) return solveTo(target, Object.assign({}, opt, { priority: w >= 0.5 ? 'pos' : 'ori' })); const m = (opt.warm ? mixWarm(target, opt) : null) || mixSolve(target, opt); if (m) return m; } if (opt.warm) { // 快路优先，校验不过自动回落全量 const w = solveWarm(target, opt); if (w) return w; } const res = solve(target, opt); if (res.ok) return res; const pose = opt.pose && opt.pose.R && opt.pose.R.length === 9; /* 姿态优先：目标点够不到指定姿态时，不用 6 维 DLS 折中位置与姿态，而是在 「姿态精确解集」上最小化位置误差——姿态保持精确、位置差如实上报。 */ if (pose && opt.priority === 'ori') { const ro = closestOri(target, opt.pose.R, +opt.tool || 0, opt.ref); if (ro && ro.q) return { ok: false, reachable: false, target: target.slice(), tool: +opt.tool || 0, pose: true, priority: 'ori', q: ro.q.slice(), cost: costOf(ro.q, opt.weights), err: ro.dist, resid: ro.dist, oerr: ro.ori, nSol: res.nSol, samples: res.samples, ms: res.ms, list: [], note: '姿态优先：姿态精确到达，TCP 取最近可达点' }; } const cp = pose ? closest6(target, opt.pose.R, +opt.tool || 0, opt.tries, opt.ref) : closest(target, +opt.tool || 0, opt.tries, opt.ref); if (!cp.q) return res; return { ok: false, reachable: false, target: target.slice(), tool: +opt.tool || 0, pose: pose, priority: res.priority, q: cp.q.slice(), cost: costOf(cp.q, opt.weights), err: cp.dist, resid: cp.dist, oerr: pose ? cp.ori : null, nSol: res.nSol, samples: res.samples, ms: res.ms, list: [], note: pose ? '目标位姿超出可达空间：返回最近位姿（阻尼最小二乘，位置 mm + 姿态联合）' : '目标超出可达工作空间：返回最近可达位姿（阻尼最小二乘）' }; } /* ---------- 6a. 实时续解：跟随上一帧的解走，而不是每次重扫全冗余维 ---------- 拖动时目标每帧只挪几毫米、姿态只转一两度，真解在 (θ4,θ5) 平面上连续移动； 全域粗扫（0.25° 下 6~30 万点）为这点位移付全价，是 100 ms 级延迟的唯一来源。 这里只在上一帧解附近取同一闭式支、走一小步同支搜索，姿态任务再用 poseTools 的牛顿把工具轴收口到精确。任一校验不过（换支、越限、位置残差超差、姿态没收口、 关节跳太大）一律返回 null 交回全量求解——快路只负责"能快的时候快"。 */ function solveWarm(target, opt) { opt = opt || {}; const q0 = opt.warm; if (!q0 || q0.length !== 6) return null; const tool = +opt.tool || 0, tol = opt.tol || 1e-6, t0 = nowMs(); const W = opt.weights || [1, 1, 1, 1, 1, 1]; const REF = (opt.ref && opt.ref.length === 6) ? opt.ref : null; const LAM = +opt.refW || 0; const POSE = (opt.pose && opt.pose.R && opt.pose.R.length === 9) ? opt.pose.R.slice() : null; const PRIO = (opt.priority === 'pos' || opt.priority === 'ori') ? opt.priority : null; const A = target[0] - K.X0, BY = target[1] - K.Y0, BZ = target[2] - K.Z1; const R = Math.hypot(BY, BZ), ph = Math.atan2(BZ, BY); const PT = POSE ? poseTools(target, tool, POSE) : null; const L4 = LIM.J4, L5 = LIM.J5; const cl = (v, a, b) => v < a ? a : (v > b ? b : v); let best = null, samples = 0; /* 递进窗口：目标挪得远时上一帧附近可能已经没有同支解，逐档放宽搜索半径， 仍找不到才交回全量粗扫（姿态任务的命中率先受这一档影响最大）。 */ const radii = [Math.max(+(opt.warmRad || 4), 0.5), 14, 40]; for (const rad of radii) { if (best) break; for (const d4 of [0, 1, -1, 2, -2, 3, -3, 4, -4].map(v => v * rad / 4)) { for (const d5 of [0, 1, -1, 2, -2, 3, -3, 4, -4].map(v => v * rad / 4)) { const t4 = cl(q0[3] + d4, L4.lo, L4.hi), t5 = cl(q0[4] + d5, L5.lo, L5.hi); const dshCap = rad > 20 ? 60 : 25; // 放宽窗口时允许同支稍大的移动 const ss = branch(t4, t5, A, R, ph, tool); samples++; for (const s0 of ss) { let dsh = 0; for (let i = 0; i < 5; i++) dsh += Math.abs(wrap180(s0[i] - q0[i])); if (dsh > dshCap) continue; // 换了支/大幅倒摆：不是续解 let p = fk(s0, tool); let e = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]); if (!(e <= tol)) continue; // 否定式：NaN 目标在此落选 let q = s0, oerr = 0, exact = true; if (POSE) { /* 精确优先但不强制：拖动中目标姿态每帧只转一两度，同支跟随的姿态残差 通常在 0.05° 内（松手的全量求解会纠偏），要求必须收到根会让快路频繁 失效、反而退回 100 ms 级全扫。 */ const r = PT.newton(s0); let qq = null; if (r && r.exact) qq = r.q; else { const e0 = PT.res(s0); if (e0 && e0[2] > 0.99999 && Math.hypot(e0[0], e0[1]) < 1e-3) qq = s0; /* 位置优先：姿态收不到根也照跟同支位置解（残差如实上报）， 否则拖进「姿态不可达区」后每帧都要回落全量扫描。 */ else if (PRIO === 'pos') qq = s0; } if (!qq) continue; p = fk(qq, tool); e = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]); if (!(e <= tol)) continue; const c2 = rollClose(qq, POSE); q = c2.q; oerr = c2.oerr; exact = !!(r && r.exact && qq === r.q); if (PRIO !== 'pos' && !(oerr <= 0.05)) continue; } const cost = costOf(q, W); const rc = REF ? costOf(q, W, REF, LAM) : cost; /* 位置优先：主键是姿态残差（轴精确的根 oerr≈0 自然胜出），次键才是 Σ|θ| */ const better = PRIO === 'pos' ? (!best || oerr < best.oerr - 1e-9 || (Math.abs(oerr - best.oerr) <= 1e-9 && rc < best.rcost - 1e-12)) : (!best || (exact && !best.exact) || (exact === best.exact && rc < best.rcost - 1e-12)); if (better) best = { q: q, cost: cost, rc0: REF ? costOf(q, W, REF, LAM) : cost, rcost: rc, err: e, oerr: oerr, exact: exact }; } } } } if (!best) return null; return { ok: true, reachable: true, warm: true, target: target.slice(), tool: tool, pose: !!POSE, priority: PRIO, q: best.q.slice(), cost: best.cost, err: best.err, oerr: POSE ? best.oerr : null, nSol: samples, truncated: false, samples: samples, ms: nowMs() - t0, list: [best] }; } /* ---------- 6a5. 路径安全路由：端点安全 ≠ 过渡安全 ---------- 查看器的动画是逐轴线性插值（ik-glue.js animTo，连续轴走短弧），所以"移动路径" 就是关节空间里 from→to 的一条直线段。实测把目标球从臂的一侧横拖到另一侧： 1300 帧里 870 帧（66.9%）处于穿模状态，主要是 立柱上段×J5 电机模组(189 帧)、 立柱中段×J6 电机模组(186 帧)——端点解本身也撞，而 live 拖拽走的 solveTo 一个碰撞判定都没做。这里把"这条直线能不能走"当一等判据： 1) 候选表按代价升序，取第一个"端点安全 + 直线过渡安全"的解； 2) 一个都没有 → 沿最优候选方向二分退回最近的可安全抵达位形（跟手但停在障碍前），标 blocked； 3) 当前位形本身已干涉（滑块被拖越界）→ 退回旧的"只看端点"口径，至少把臂解出来。 开销：首个候选通常一次就过（1 次端点 + PATH_N-1 次过渡 ≈ 8 次 collideFree）； 最坏 CAND_MAX 个候选全查 ≈ 60 次，实测在 Worker 里跑，不冻结主线程。 */ const PATH_N = 9, CAND_MAX = 24, SHORT_DEG = 6, FINE_DEG = 3; /* 关节空间步长（连续轴按短弧折算后的最大单轴变化量） */ function stepDeg(from, to) { let m = 0; for (let i = 0; i < 6; i++) { const d = LIM[ORDER[i]] && LIM[ORDER[i]].cont ? wrap180(to[i] - from[i]) : to[i] - from[i]; if (Math.abs(d) > m) m = Math.abs(d); } return m; } function transitPt(from, to, k) { return from.map((v, i) => { const d = LIM[ORDER[i]] && LIM[ORDER[i]].cont ? wrap180(to[i] - v) : to[i] - v; return v + d * k; }); } /* 直线段过渡是否全程无干涉（不含端点：起点由调用侧保证，终点已单独查过）。 步长 ≤SHORT_DEG° 的跟随帧只查中点：实测拖拽中相邻解的步长多在几度以内， 9 点全扫会把 live 从 1~44ms 放大到 441ms/步（≈10×），而这么短的线段 两端安全 + 中点安全已经覆盖了实际风险（穿模体是厘米级，不是度级）。 */ function pathFree(from, to, n) { n = n || PATH_N; for (let i = 1; i < n; i++) if (!collideFree(transitPt(from, to, i / n))) return false; return true; } function findSafeTarget(cur, from) { if (cur.q && collideFree(cur.q)) return { q: cur.q, res: cur }; let best = null, bestCost = Infinity; const n = Math.min((cur.list || []).length, 120); for (let i = 0; i < n; i++) { const s = cur.list[i]; if (s && s.q && collideFree(s.q)) { 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; const len = (cur.list || []).length; if (len > 120) { const stride = Math.max(1, Math.floor((len - 120) / 40)); 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 }) }; } } } return null; } function routeFrom(from, cur, lazy) { if (!COL || !cur || !cur.q || !from) return cur; if (!collideFree(from)) return cur; // 起点已在干涉里：先把自己拔出来再说 const seen = new Set(), cand = []; const push = q => { if (!q || q.length !== 6) return; const key = q.map(v => v.toFixed(2)).join(','); if (seen.has(key)) return; seen.add(key); cand.push(q.slice()); }; push(cur.q); for (const s of (cur.list || [])) push(s.q); // 优先验证与起点 from 关节距离最近的候选解 cand.sort((a, b) => costOf(a, null, from) - costOf(b, null, from)); const cands = cand.slice(0, CAND_MAX); let fineLeft = 4; // 细验预算：全通过的候选才会走满 n 次探测 for (const q of cands) { if (!collideFree(q)) continue; // 快速二分初筛：若中点干涉直接短路（1 次判定否决碰撞射线），节省大半无谓细验开销 if (!collideFree(transitPt(from, q, 0.5))) continue; /* 粗筛（≤13 点）先廉价否掉明显穿模的候选，再用与审计同分辨率的 3° 细验定案 */ if (!pathFree(from, q, Math.min(13, pathN(from, q, SHORT_DEG)))) continue; if (fineLeft <= 0) break; // 预算用尽就不接受没细验过的候选（宁可贴边停，不赌） fineLeft--; if (pathFree(from, q, pathN(from, q, FINE_DEG))) { const same = q.every((v, i) => Math.abs(v - cur.q[i]) < 1e-9); return Object.assign({}, cur, { q: q, routed: true, blocked: false, kept: same }); } } /* 没有一条直线能干净抵达：沿最优候选（优先端点安全的那个）方向二分退回安全边界， 让臂贴着障碍停住，而不是穿过去。 */ /* 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) { for (const q of cands) if (collideFree(q)) { bestQ = q; break; } } if (!bestQ) bestQ = cands[0] || cur.q; let lo = 0, hi = 1; // lo=起点（已确认安全），hi=整跳（上面已证明不干净） /* 先按端点二分找到"射线上的第一个撞点"（便宜：每次 1 个探测）， 再对 from→q 整段做 3° 细验；细验不过就按 0.6 回缩重验。 早先直接用 9 点粗扫当谓词，等于把 19° 的采样间隔当成安全边界，实测漏过一帧。 */ while (hi - lo > 0.02) { const mid = 0.5 * (lo + hi); if (collideFree(transitPt(from, bestQ, mid))) lo = mid; else hi = mid; } let q = transitPt(from, bestQ, lo); for (let t = 0; t < 5 && lo > 0.02 && !pathFree(from, q, pathN(from, q, FINE_DEG)); t++) { lo *= 0.6; q = transitPt(from, bestQ, lo); } return Object.assign({}, cur, { q: q, safeT: safeT, routed: true, blocked: lo < 0.98, kept: false }); } /* 拖拽/松手统一入口：solveTo + 路径路由（opt.from = 当前位形） */ const FULLIK = { key: null, cur: null, T: null, at: 0 }; function solveRoute(target, opt) { opt = opt || {}; if (opt.from && !opt.ref) opt.ref = opt.from; const rt = opt.quick && RM && !opt.noAround; const t0 = nowMs(); let cur = solveTo(target, opt); /* 实时管线：热跟随只给一个"同支续解"，它若撞了（目标那一支被立柱挡住），就得看别的构型—— 补一次全量 IK（同一目标缓存复用：松手后目标不动，不必每帧重解） */ if (rt && cur && (!cur.q || !collideFree(cur.q))) { const key = target.map(v => v.toFixed(2)).join(',') + '|' + (opt.tool || 0) + '|' + (opt.pose ? JSON.stringify(opt.pose) : ''); const near = FULLIK.T && FULLIK.key && Math.hypot(target[0] - FULLIK.T[0], target[1] - FULLIK.T[1], target[2] - FULLIK.T[2]) < 40 && nowMs() - FULLIK.at < 400; if (FULLIK.key === key) cur = FULLIK.cur; else if (near) cur = Object.assign({}, FULLIK.cur, { approx: true }); // 拖动中：先朝附近目标的安全解走，目标停下再精解（避免每帧全量 IK 尖峰） else { const o2 = Object.assign({}, opt); delete o2.warm; o2.quick = false; /* 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(); } } } const t1 = nowMs(); const r = rt ? routeRT(opt.from, cur) : routeOrVia(opt.from, target, cur, opt); if (r && typeof r === 'object') { r.tIk = t1 - t0; r.tRoute = nowMs() - t1; } return r; } /* 两段式绕行：直线被立柱挡住、但目标本身有安全解时，先解一个"抬高版"的中间位形 （目标正上方 VIA_DZ），走直线过去，再从那里落回目标。实测 100 个拖拽步里 14 步属于这一类（有安全端点、直线到不了）。只在松手全量求解时用： 每多一个候选要多解一次 IK + 两段直线检查，实时拖拽扛不住。 */ const VIA_DZ = [260, 520]; function routeVia(from, target, opt) { for (const dz of VIA_DZ) { const Tw = [target[0], target[1], target[2] + dz]; const rw = solveRoute(Tw, Object.assign({}, opt, { noVia: true, noAround: true })); if (!rw || !rw.q || rw.blocked || rw.unsafe || !collideFree(rw.q)) continue; const r2 = solveRoute(target, Object.assign({}, opt, { from: rw.q.slice(), noVia: true, noAround: true, ref: rw.q })); if (!r2 || !r2.q || r2.blocked || r2.unsafe || !collideFree(r2.q)) continue; /* 中间位形表必须把两段各自的子绕行都接上：from→(rw.via)→rw.q→(r2.via)→q。 早先只放 [rw.q] 会把 rw/r2 自己的 via 覆盖掉，于是动画走的那条线并不是校验过的那条， 实测松手口径因此在离起点 3° 处穿模 6 帧（立柱中段×末端工具法兰）。 */ return Object.assign({}, r2, { via: (rw.via || []).concat([rw.q.slice()], r2.via || []), detourDz: dz, detour: 'via' }); } return null; } /* 绕行使者：收臂 → 转 J1 → 再伸展。 CRUISE 表由 scratch/find_cruise_poses.js 搜出并校验：那组 J2/J4/J5 组合在 J1 整周每 10° 采样下 collideFree 全通过（280 组枚举里 243 组满足，取半径最贴身 且收法不同的 4 组）。几何一改必须重跑该脚本，否则巡航位形可能已经会撞。 三段分别是：当前位形→原地收臂（J1 不变）、收臂态把 J1 转到目标解的肩角、 再伸展到目标解。每段都过 pathFree，段内 J1 按 ≤10° 采样（与搜索口径一致）。 */ /* 巡航参考位形：按收缩幅度从浅到深排序。 [0, -15, ...] 为轻微避让，[0, -75, ...] 为深折叠。优先选用微调姿态，避免激进蜷缩。 */ const CRUISE = [ [0, -15, 0, -95, -60, 0], [0, -45, 0, -95, -60, 0], [0, -75, 0, -95, -60, 0], [0, -90, 0, -65, -60, 0] ]; /* 转 J1 那一段按 5° 网格查表（懒算 + 记忆化） */ const CRUISE_OK = CRUISE.map(() => new Map()); function cruiseFree(ci, j1) { const key = Math.round(j1 / 5) * 5; const m = CRUISE_OK[ci]; let v = m.get(key); if (v === undefined) { const q = CRUISE[ci].slice(); q[0] = key; v = collideFree(q); m.set(key, v); } return v; } function turnFree(ci, j1a, j1b) { const n = Math.max(1, Math.ceil(Math.abs(wrap180(j1b - j1a)) / 5)); for (let i = 0; i <= n; i++) if (!cruiseFree(ci, j1a + (wrap180(j1b - j1a)) * i / n)) return false; return true; } /* 采样点数按线段在关节空间的步长自适应：每 SHORT_DEG(6°) 一个点，最少 3 点、最多 40 点 */ function pathN(from, to, deg) { return Math.max(3, Math.min(40, Math.round(stepDeg(from, to) / (deg || SHORT_DEG)) + 1)); } /* 轻量偏移量集合：用于避开障碍物边缘（抬肩/微收肘），避免整臂蜷缩 */ const GENTLE_OFFSETS = [ [-10, 0], [-20, 0], [-15, -10], [0, -15], [-20, -15], [10, 0], [20, 0], [-30, 0], [-10, 10] ]; /* 智能平滑协同避障规划（模拟人类避障直觉）： 1) 微调避让：针对贴边擦碰，在直线弦中点微调 J2/J4，各轴保持平滑协同，绝不缩臂； 2) 协同单中间点 + 二分松弛：J1 全程平滑旋转，中间位形只做必要的外展/微收，并向直线插值松弛； 3) 模拟人类自然避障（回退一点 -> 绕行过去 -> 伸展到位）： 当手臂靠近立柱或跨度大时，在起点方位先原位/微转收臂回退，跨过立柱后再展开到位； 4) 保底安全回退：极端奇异位形下确保无碰撞。 */ function routeAroundPlan(from, cur, quick, safeT) { let qT = safeT ? safeT.q : null, resT = safeT ? safeT.res : cur; if (!qT) { const found = findSafeTarget(cur, from); if (found) { qT = found.q; resT = found.res; } } if (!qT) return null; const chordMid = transitPt(from, qT, 0.5); const dj1 = wrap180(qT[0] - from[0]); const j1Mid = from[0] + dj1 * 0.5; // 阶段 1：微调避障（针对贴边穿模：直接在弦中点微调 J2/J4，完全不蜷缩手臂） for (const [dj2, dj4] of GENTLE_OFFSETS) { const testQ = chordMid.slice(); testQ[1] += dj2; testQ[3] += dj4; if (!collideFree(testQ)) continue; const nA = Math.min(8, pathN(from, testQ, FINE_DEG)); const nB = Math.min(8, pathN(testQ, qT, FINE_DEG)); if (pathFree(from, testQ, nA) && pathFree(testQ, qT, nB)) { return { res: resT, via: [testQ], detour: 'gentle' }; } } // 阶段 2：连续协同单中间点避障（J1 保持平滑转动，避障姿态向自然弦二分松弛） const use = quick ? [0, 1, 2] : [0, 1, 2, 3]; for (const ci of use) { const c = CRUISE[ci]; const qMidSafe = c.slice(); qMidSafe[0] = j1Mid; if (!collideFree(qMidSafe)) continue; const nA = Math.min(8, pathN(from, qMidSafe, FINE_DEG)); const nB = Math.min(8, pathN(qMidSafe, qT, FINE_DEG)); if (pathFree(from, qMidSafe, nA) && pathFree(qMidSafe, qT, nB)) { // 二分松弛：寻找能安全通过的最小偏离量（靠近弦中点） let lo = 0, hi = 1; for (let it = 0; it < 4; it++) { const midA = (lo + hi) * 0.5; const testQ = transitPt(chordMid, qMidSafe, midA); if (collideFree(testQ) && pathFree(from, testQ, 6) && pathFree(testQ, qT, 6)) { hi = midA; } else { lo = midA; } } const optMid = transitPt(chordMid, qMidSafe, hi); return { res: resT, via: [optMid], detour: 'coordinated' }; } } // 阶段 3：模拟人类自然避障（回退一点 -> 绕行过去 -> 伸展到位） // 用户需求："模拟人类，哪怕中间有东西挡了，应该回退一点然后再移动过去，而不是说挡着了就不计算了" // 第一中间点在起点方位（微转 frac）处收臂回退，第二中间点在避开立柱后跨过，再伸展入目标 for (const ci of use) { const c = CRUISE[ci]; for (const frac of [0.08, 0.0, 0.15, 0.25]) { const w1 = c.slice(); w1[0] = from[0] + dj1 * frac; const w2 = c.slice(); w2[0] = qT[0] - dj1 * frac; if (!collideFree(w1) || !collideFree(w2)) continue; const n1 = Math.min(8, pathN(from, w1, FINE_DEG)); const n2 = Math.min(8, pathN(w1, w2, FINE_DEG)); const n3 = Math.min(8, pathN(w2, qT, FINE_DEG)); if (pathFree(from, w1, n1) && pathFree(w1, w2, n2) && pathFree(w2, qT, n3)) { return { res: resT, via: [w1, w2], detour: 'retract_pass' }; } } } // 阶段 4：保底三段式（仅在所有平滑协同均被阻挡的极端奇异位形启用） for (const ci of use) { const c = CRUISE[ci]; const fold = c.slice(); fold[0] = from[0]; const turn = c.slice(); turn[0] = qT[0]; const ok = pathFree(from, fold, Math.min(8, pathN(from, fold, FINE_DEG))) && turnFree(ci, fold[0], turn[0]) && pathFree(turn, qT, Math.min(8, pathN(turn, qT, FINE_DEG))); if (ok) return { res: resT, via: [fold, turn], detour: 'around' }; } 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, kMax, listMax, cfMax) { kMax = kMax || GOAL_K; listMax = listMax || 160; cfMax = cfMax || 1e9; 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, listMax); i++) add(L[i] && L[i].q); out.sort((a, b) => wdist(from, a) - wdist(from, b)); const goals = [], tried = []; for (const g of out) { if (goals.length >= kMax || cfMax <= 0) break; if (goals.some(h => wdist(g, h) < 8)) continue; // 同一盆地只留一个 if (cfMax < 1e8) { // 限量模式：撞过的盆地也不再重复判定 if (tried.some(h => wdist(g, h) < 8)) continue; tried.push(g); } cfMax--; 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; } /* ---------- 6d. 离线路网（PRM）+ 在线 Lazy A*（2026-10-15，实时版） ---------- 障碍全是静态的（立柱 + 自身零件），关节空间的自由区不随时间变化，所以把"安全路网"离线建好： webapp/tools/gen_roadmap.cjs 均匀采样无干涉节点，连 k 近邻并按 EDGE_RES 细验每条边， 随 collide.json 的 roadmap 字段发布（指纹校验，碰撞模型变了自动弃用）。 在线：起点 / 各目标接到路网的 k 个近邻（这些边"待验"），A* 求最短路，只细验路径上的待验边， 失败就删边重搜（Lazy PRM）。路网内部的边离线已验，在线零成本。最后限时贪心捷径拉直。 */ function wd(a, b) { // 展开无关的加权距离（连续轴短弧） let s = 0; for (let i = 0; i < 6; i++) { const d = LIM[ORDER[i]].cont ? wrap180(b[i] - a[i]) : b[i] - a[i]; const x = d * PW[i]; s += x * x; } return Math.sqrt(s); } function modelFingerprint() { let h = 2166136261 >>> 0; const mix = v => { h = Math.imul(h ^ (Math.round(v * 1000) | 0), 16777619) >>> 0; }; mix(CAPM); for (const c of CAPG) { mix(c.g); mix(c.r); for (const v of c.a) mix(v); for (const v of c.b) mix(v); } for (const id of ORDER) { const t = LIM[id]; mix(t.lo); mix(t.hi); mix(t.cont ? 1 : 0); } return h.toString(16); } const RM = (() => { if (!COL || !COL.roadmap) return null; const R = COL.roadmap; if (R.fp !== modelFingerprint()) { if (typeof console !== 'undefined') console.warn('[IK] roadmap 指纹不符，弃用（重跑 webapp/tools/gen_roadmap.cjs）'); return null; } const dec = (b64, Ctor) => { if (typeof atob === 'function') { const bin = atob(b64), u8 = new Uint8Array(bin.length); for (let i = 0; i < bin.length; i++) u8[i] = bin.charCodeAt(i); return new Ctor(u8.buffer); } const b = Buffer.from(b64, 'base64'); const u8 = new Uint8Array(b.length); u8.set(b); return new Ctor(u8.buffer); }; const qi = dec(R.q, Int16Array), off = dec(R.off, Uint32Array), adj = dec(R.adj, Uint16Array); const N = R.n, Q = []; for (let i = 0; i < N; i++) { const q = new Array(6); for (let k = 0; k < 6; k++) q[k] = qi[i * 6 + k] / 100; Q.push(q); } return { N: N, Q: Q, off: off, adj: adj }; })(); /* 会话内"终身"增广：每次在线细验通过的接入边连同起点 / 目标位形一起并入路网（只存在内存里）。 拖拽时起点和目标集中在一小片区域，几次之后那里路网变密、接入边变短且多半已验， 后续查询基本只剩 A*（亚毫秒）。上限 RMX_MAX 个节点，满了就不再并入。 */ const RMX = { Q: [], adj: [] }, RMX_MAX = 4000; function rmKnn(q, k) { const best = []; const M = RM.N + RMX.Q.length; for (let i = 0; i < M; i++) { const d = wd(q, i < RM.N ? RM.Q[i] : RMX.Q[i - RM.N]); if (best.length < k || d < best[best.length - 1][1]) { let j = best.length; best.push(null); while (j > 0 && best[j - 1][1] > d) { best[j] = best[j - 1]; j--; } best[j] = [i, d]; if (best.length > k) best.pop(); } } return best; } const RM_K = 12; /* 返回 { path:[from, ..., goal] (展开坐标) } 或 null */ function rmPlan(from, goals, budgetMs) { if (!RM) return null; const t0 = nowMs(), deadline = t0 + budgetMs, N = RM.N, NX = N + RMX.Q.length, S = NX, G0 = NX + 1; const pos = i => i < N ? RM.Q[i] : i < NX ? RMX.Q[i - N] : i === S ? from : goals[i - G0]; const ekey = (a, b) => a < b ? a * 65536 + b : b * 65536 + a; const bad = new Set(), good = new Set(); const extra = new Map(); // 本次查询的待验边：节点 → [[邻, 代价], ...] const link = (a, b, c) => { if (!extra.has(a)) extra.set(a, []); if (!extra.has(b)) extra.set(b, []); extra.get(a).push([b, c]); extra.get(b).push([a, c]); }; for (const [i, d] of rmKnn(from, RM_K)) link(S, i, d); goals.forEach((g, j) => { for (const [i, d] of rmKnn(g, RM_K)) link(G0 + j, i, d); link(S, G0 + j, wd(from, g)); // 直连也是候选：能直走 A* 自然选它 }); 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 edgeOk = (a, b) => { // 只对本次查询的待验边做在线细验 const k = ekey(a, b); if (good.has(k)) return true; if (bad.has(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); return ok; }; const augment = () => { /* 增广（成功或超时都做：已验的接入边是下一帧的资产，跨帧"随时可停"地累积进度）：起点 / 用到的目标并入，带上所有已验通过的接入边 */ if (RMX.Q.length + 2 <= RMX_MAX) { const newId = new Map(); const addNode = id => { if (newId.has(id)) return newId.get(id); const nid = N + RMX.Q.length; RMX.Q.push(pos(id).map((v, k) => LIM[ORDER[k]].cont ? wrap180(v) : v)); RMX.adj.push([]); newId.set(id, nid); return nid; }; const mapId = id => id >= NX ? addNode(id) : id; RMX.adjR = RMX.adjR || new Map(); const addEdge = (a, b, c) => { for (const [x, y] of [[a, b], [b, a]]) { if (x >= N) RMX.adj[x - N].push([y, c]); else { if (!RMX.adjR.has(x)) RMX.adjR.set(x, []); RMX.adjR.get(x).push([y, c]); } } }; for (const k of good) { const a = Math.floor(k / 65536), b = k % 65536; addEdge(mapId(a), mapId(b), wd(pos(a), pos(b))); } } }; let rounds = 0; while (nowMs() < deadline && rounds < 60) { rounds++; const gsc = new Map(), par = new Map(), heap = []; const push = (f, n) => { heap.push([f, n]); let i = heap.length - 1; while (i > 0) { const p = (i - 1) >> 1; if (heap[p][0] <= heap[i][0]) break; [heap[p], heap[i]] = [heap[i], heap[p]]; i = p; } }; const pop = () => { const top = heap[0], last = heap.pop(); if (heap.length) { heap[0] = last; let i = 0; for (;;) { const l = 2 * i + 1, r = l + 1; let m = i; if (l < heap.length && heap[l][0] < heap[m][0]) m = l; if (r < heap.length && heap[r][0] < heap[m][0]) m = r; if (m === i) break; [heap[m], heap[i]] = [heap[i], heap[m]]; i = m; } } return top; }; gsc.set(S, 0); push(h(S), S); let found = -1; while (heap.length) { const [f, u] = pop(); const gu = gsc.get(u); if (f > gu + h(u) + 1e-9) continue; if (isGoal(u)) { found = u; break; } const relax = (v, c, lazy) => { if (lazy && bad.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); } }; if (u < N) for (let e = RM.off[u]; e < RM.off[u + 1]; e++) { const v = RM.adj[e]; relax(v, wd(RM.Q[u], RM.Q[v]), false); } if (u < NX) { const ax = u < N ? null : RMX.adj[u - N]; if (ax) for (const [v, c] of ax) relax(v, c, false); } if (u < N) { const ax = RMX.adjR && RMX.adjR.get(u); if (ax) for (const [v, c] of ax) relax(v, c, false); } const ex = extra.get(u); if (ex) for (const [v, c] of ex) relax(v, c, true); } if (found < 0) { augment(); return null; } const ids = [], lz = []; for (let v = found; v !== S; ) { const [u, lazy] = par.get(v); ids.push(v); lz.push(lazy); v = u; } ids.push(S); ids.reverse(); lz.reverse(); let ok = true; for (let i = 0; i + 1 < ids.length; i++) if (lz[i] && !edgeOk(ids[i], ids[i + 1])) { ok = false; break; } if (timedOut) { augment(); return null; } if (!ok) continue; const path = [from.slice()]; for (let i = 1; i < ids.length; i++) path.push(unwrapTo(path[i - 1], pos(ids[i]))); augment(); return { path: path, ms: nowMs() - t0, rounds: rounds }; } augment(); return null; } /* 限时贪心拉直：从当前点往后找最远可直达的点（由远到近试） */ function rmSmooth(path, deadline) { if (path.length <= 2) return path; const out = [path[0]]; let i = 0; while (i < path.length - 1) { let j = path.length - 1; if (nowMs() < deadline) { for (; j > i + 1; j--) { if (edgeFree(out[out.length - 1], unwrapTo(out[out.length - 1], path[j]))) break; if (nowMs() > deadline) { j = i + 1; break; } } } else j = i + 1; out.push(unwrapTo(out[out.length - 1], path[j])); i = j; } return out; } /* ---------- 6e. 实时热启动（拖拽连续重规划） ---------- 拖拽时每次重规划的起点 = 手臂此刻在上一条路径上的位置。上一条路径每段都已细验， "起点 → 所在段终点"是已验段的子段，免检；目标每帧只挪一点，新目标接到旧目标只需验一条短边。 于是一次重规划通常只验 1~2 条短边（几毫秒），而不是重新接入路网。 路径尾巴会累积小折线：每次先试"倒数第二点 → 新目标"直连，能连就替换掉旧目标。 */ let RT = null; // { path:[展开位形...] } 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]); const qa = unwrapTo(a, q); let num = 0, den = 0; for (let k = 0; k < 6; k++) { const d = b[k] - a[k]; num += (qa[k] - a[k]) * d; den += d * d; } const t = den > 1e-12 ? Math.min(1, Math.max(0, num / den)) : 0; let err = 0; for (let k = 0; k < 6; k++) err = Math.max(err, Math.abs(a[k] + (b[k] - a[k]) * t - qa[k])); if (err <= tol) return [i, t]; } return null; } function rtWarm(from, goals, deadline) { if (!RT || RT.path.length < 2) return null; const hit = onPath(RT.path, from, 0.05); if (!hit) return null; const rest = [from.slice()]; for (let i = hit[0] + 1; i < RT.path.length; i++) rest.push(unwrapTo(rest[rest.length - 1], RT.path[i])); if (rest.length >= 2 && wd(rest[0], rest[1]) < 1e-6) rest.splice(1, 1); const last = rest[rest.length - 1]; for (const g0 of goals) { if (nowMs() > deadline) break; const g = unwrapTo(last, g0); /* 1) 直连（整段，能走就最好） 2) 倒数第二点 → 新目标 3) 旧目标 → 新目标（短边） */ if (edgeFree(rest[0], unwrapTo(rest[0], g))) return [rest[0], unwrapTo(rest[0], g)]; if (rest.length >= 2) { const pre = rest[rest.length - 2]; if (edgeFree(pre, unwrapTo(pre, g))) return rest.slice(0, -1).concat([unwrapTo(pre, g)]); } if (rest.length >= 2 && edgeFree(last, g)) return rest.concat([g]); } return null; } /* 实时管线（拖拽 quick 模式且路网可用时）：只走"每步都细验过"的分支，并且每一步都有硬时限。 直连 → 热启动 → 路网 Lazy A*（40 ms）→ 都不行就沿直线走到最后一个安全点停住。 不再进入旧的启发式阶梯（慢，且其段只抽查少量点，拖拽仿真里实测会穿模）。 */ function routeRT(from, cur) { if (!COL || !cur || !cur.q || !from) return cur; if (!collideFree(from)) return cur; // 起点已在干涉里：交给脱困逻辑 CFMEMO = new Map(); try { const t0 = nowMs(); const goals = gatherGoals(cur, from, 3, 4000, 30); const done = (path, detour, blocked) => { 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); 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'); /* 本帧没规划完：沿上一条（已验）路径继续走，下一帧接着规划——不急停、不乱走 */ if (RT && RT.path.length >= 2) { const hit = onPath(RT.path, from, 0.05); if (hit) { const rest = [from.slice()]; for (let i = hit[0] + 1; i < RT.path.length; i++) rest.push(unwrapTo(rest[rest.length - 1], RT.path[i])); if (rest.length >= 2) { const q = rest[rest.length - 1]; return Object.assign({}, cur, { q: q.map((v, i) => LIM[ORDER[i]].cont ? wrap180(v) : v), via: rest.slice(1, -1), routed: true, blocked: false, kept: false, detour: 'continue', pending: true, planMs: nowMs() - t0 }); } } } } /* 规划不到：沿"朝最近目标的直线"按 EDGE_RES 前进，停在最后一个无干涉采样点 */ const g = goals.length ? goals[0] : unwrapTo(from, cur.q); let m = 0; for (let i = 0; i < 6; i++) m = Math.max(m, Math.abs(g[i] - from[i])); const n = Math.max(1, Math.ceil(m / EDGE_RES)); let last = from.slice(); for (let k = 1; k <= n; k++) { const q = from.map((v, i) => v + (g[i] - v) * k / n); if (!collideFree(q)) break; last = q; } return done([from.slice(), last], null, true); } finally { CFMEMO = null; } } function planAround(from, cur, quick) { const goals = gatherGoals(cur, from); if (!goals.length) return null; const budget = quick ? 150 : 1500; /* 实时版：有离线路网就直接 Lazy A*（典型几十毫秒），直连边也在图里（A* 自然优先），失败再走下面的旧流程 */ if (RM) { const t0 = nowMs(); if (quick) { const w = rtWarm(from, goals, t0 + 40); if (w) { RT = { path: w }; return { goal: w[w.length - 1], via: w.slice(1, -1), detour: w.length > 2 ? 'planned' : null, ms: nowMs() - t0, rm: 'warm' }; } } const rp = rmPlan(from, goals, quick ? 60 : 300); if (rp) { let sm = rmSmooth(rp.path, nowMs() + (quick ? 25 : 80)); /* 松手精修：再做一轮随机捷径 + 逐轴拉直（路网路径节点稀疏，有可观的多余行程） */ if (!quick && sm.length > 2) { let seed = 2166136261; for (const v of from) seed = Math.imul(seed ^ Math.round(v * 10), 16777619); let st = seed >>> 0 || 1; const rnd = () => { st = (st * 1664525 + 1013904223) >>> 0; return st / 4294967296; }; sm = shortcut(sm, nowMs() + 120, rnd); } RT = { path: sm }; return { goal: sm[sm.length - 1], via: sm.slice(1, -1), detour: 'planned', ms: nowMs() - t0, rm: true }; } } /* 先试直线（多目标、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) { 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) { RT = from && r && r.q ? { path: [from.slice(), unwrapTo(from, r.q)] } : null; return r; } /* 首选：采样规划（多目标 + 平滑），给出的路径已按 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); /* 旧阶梯的段只抽查少量点（实测拖拽仿真里 coordinated 段会穿模）：执行前按 EDGE_RES 整条细验， 不过就宁可贴边停下（blocked），绝不把没验透的折线交给动画 */ if (around && polyFree(from, around.via, around.res.q)) return Object.assign({}, around.res, { via: around.via, routed: true, blocked: false, detour: around.detour || 'around' }); } if (opt.noVia) return r; const rv = routeVia(from, target, opt); if (rv && rv.q && polyFree(from, rv.via || [], rv.q)) return rv; return r; } function polyFree(from, via, q) { // 整条折线（展开后）按 EDGE_RES 细验 let p = from; for (const raw of (via || []).concat([q])) { const t = unwrapTo(p, raw); if (!edgeFree(p, t)) return false; p = t; } return true; } /* ---------- 6b. 动态限位求解：静态解 + 贴边轴放宽重解 ---------- 静态限位来自"其它轴全零"的单轴扫描，对非相邻碰撞对（典型：腕/法兰撞 固定立柱）系统性过紧——实测 J2 正端随 J1 从 +9.75°（垂臂零位）放到 +108.75°（肩侧抬 90°）。若静态解的某轴贴在边界上，就按当前姿态二分出 该轴的动态边界，临时放宽该轴限位后重解；新解必须通过全姿态碰撞判定、 且总关节角不升才采用，否则原样返回（最坏即静态行为，无回退风险）。 */ function solveDyn(target, opt) { opt = opt || {}; if (opt.from && !opt.ref) opt.ref = opt.from; let cur = solveTo(target, opt); if (!COL || opt.noDyn || !cur.q) return cur; /* 静态限位盒不保证组合姿态无干涉（单轴扫描各轴独立取限位，组合折叠会撞—— sweep_v2 的组合扫描只记录不收紧滑块）。这里对最终解做全姿态碰撞校验： 撞了就沿候选表找安全的解，优先采用最贴近起点的安全位形。 */ if (!collideFree(cur.q)) { const safeT = findSafeTarget(cur, opt.from); if (safeT) cur = Object.assign({}, safeT.res, { swaped: true }); else cur = Object.assign({}, cur, { unsafe: true }); } for (let pass = 0; pass < 2; pass++) { const q = cur.q; let axis = null, dir = 0; for (let i = 0; i < 5; i++) { const t = LIM[ORDER[i]]; if (t.cont) continue; if (q[i] >= t.hi - 0.5) { axis = ORDER[i]; dir = +1; break; } if (q[i] <= t.lo + 0.5) { axis = ORDER[i]; dir = -1; break; } } if (!axis) break; const bound = dynLimit(axis, q, dir, 0.5); if (Math.abs(bound - q[ORDER.indexOf(axis)]) < 1.0) break; // 动态边界没放宽 const save = { lo: LIM[axis].lo, hi: LIM[axis].hi }; let r1 = null; try { if (dir > 0) LIM[axis].hi = bound; else LIM[axis].lo = bound; /* 增益 <5° 不值得一次全量重解（实测重解与首解同量级 ~190ms），直接跳过； 大幅放宽（典型：J1=90° 时 J2 从 +9 放到 +106）才值得。 */ if (bound - q[ORDER.indexOf(axis)] < (dir > 0 ? 5 : -5)) break; /* 重解步长粗一档：只需要"比静态更优"的解（接受判据就是 cost 不升）， 不需要全局最优——全量精扫贵在与首解同量级，粗档约省 3~4 倍。 */ const step1 = Math.max((+opt.step || 0.25) * 2, 0.5); r1 = solveTo(target, Object.assign({}, opt, { noDyn: true, step: step1 })); } finally { LIM[axis].lo = save.lo; LIM[axis].hi = save.hi; } if (!r1 || !r1.q || r1.cost >= cur.cost - 1e-9) break; // 没有更优 → 收工 let picked = null; if (collideFree(r1.q)) picked = r1; else for (const s of r1.list) { // 热循环不查碰撞：候选表里找第一个安全的 if (s.cost < cur.cost - 1e-9 && collideFree(s.q)) { picked = Object.assign({}, r1, { q: s.q.slice(), cost: s.cost }); break; } } if (!picked) break; cur = Object.assign({}, picked, { dynAxis: axis, dynBound: bound }); } return opt.from ? routeOrVia(opt.from, target, cur, opt) : cur; } /* ---------- 7. 自检：闭式 FK ↔ 矩阵链；逆解重构；姿态重构 ---------- */ function selfTest(n, nPose) { n = n || 400; nPose = nPose == null ? 8 : nPose; let maxFk = 0; for (let k = 0; k < n; k++) { const q = []; for (let i = 0; i < 6; i++) { const t = LIM[ORDER[i]]; q.push(t.cont ? (Math.random() * 720 - 360) : t.lo + Math.random() * (t.hi - t.lo)); } const a = fk(q, 0), b = fkChain(q, 0); maxFk = Math.max(maxFk, Math.hypot(a[0] - b[0], a[1] - b[1], a[2] - b[2])); } let maxIk = 0, hit = 0, m = 0; for (let k = 0; k < Math.min(n, 200); k++) { const q = []; for (let i = 0; i < 6; i++) { const t = LIM[ORDER[i]]; q.push(t.lo + Math.random() * (t.hi - t.lo)); } const T = fk(q, 0); const A = T[0] - K.X0, BY = T[1] - K.Y0, BZ = T[2] - K.Z1; const sols = branch(q[3], q[4], A, Math.hypot(BY, BZ), Math.atan2(BZ, BY), 0); if (!sols.length) continue; m++; let best = Infinity; for (const s of sols) best = Math.min(best, Math.hypot.apply(null, fk(s, 0).map((v, i) => v - T[i]))); maxIk = Math.max(maxIk, best); if (best < 1e-6) hit++; } const ps = nPose > 0 ? poseSelfTest(nPose) : { poseHit: 0, poseTried: 0, poseErr: 0, poseMs: 0 }; return { maxFkErr: maxFk, maxIkErr: maxIk, ikHit: hit, ikTried: m, structWarn: WARN, K: K, LIM: LIM, poseHit: ps.poseHit, poseTried: ps.poseTried, poseErr: ps.poseErr, poseMs: ps.poseMs }; } /* 姿态重构自检：取随机可行位形的真实工具姿态为任务，闭式扫描应精确命中。 单次约 70 ms（step=1 全量扫描），200 组同步跑 ≈ 14 s——放在页面启动路径里会把 首屏从 ~1.4 s 拖到 ~15 s。故单独成函数，供界面分片调用。 */ function poseSelfTest(nPose) { let pHit = 0, pTried = 0, pErr = 0, pMs = 0; for (let k = 0; k < nPose; k++) { const q = []; for (let i = 0; i < 6; i++) { const t = LIM[ORDER[i]]; q.push(t.lo + Math.random() * (t.hi - t.lo)); } const T = fk(q, 0), R = orient(q); pTried++; const r = solve(T, { pose: { R: R }, step: 1.0, tol: 1e-6 }); pMs += r.ms; if (r.ok && r.oerr != null) { pErr = Math.max(pErr, r.oerr); if (r.oerr < 0.05 && r.err < 1e-3) pHit++; } } return { poseHit: pHit, poseTried: pTried, poseErr: pErr, poseMs: pTried ? pMs / pTried : 0 }; } return { fk: fk, fkChain: fkChain, solve: solve, solveTo: solveTo, solveWarm: solveWarm, solveDyn: solveDyn, solveRoute: solveRoute, routeFrom: routeFrom, transitPt: transitPt, planAround: planAround, edgeFree: edgeFree, PST: PST, unwrapTo: unwrapTo, rtWarm: rtWarm, rtReset: () => { RT = null; }, rmPlan: rmPlan, rmSmooth: rmSmooth, modelFingerprint: modelFingerprint, RM: RM, wd: wd, 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, dynLimit: dynLimit, dynLimits: dynLimits, K: K, LIM: LIM, P: P, D: D, ORDER: ORDER, wrap180: wrap180, costOf: costOf }; } return { make: make, ORDER: ORDER, poseMat: poseMat, poseAng: poseAng, rotErrDeg: rotErrDeg, segSegDist: segSegDist }; }));