Robot_Arm/web/arm-ik-solver.js

/* ============================================================================
 *  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 x = d[0], y = d[1], z = d[2], c = Math.cos(a), s = Math.sin(a), 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);

/* 两旋转矩阵间的夹角（度）：θ = 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: -180, hi: 180, continuous: true };
    LIM[id] = { lo: +t.lo, hi: +t.hi, cont: !!t.continuous };
  }
  const TCP0 = (cfg.tcp || [18.5, 128, 93]).slice();
  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;
  }

  /* ---------- 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) 的闭式分支 ---------- */
  function branch(th4, th5, A, R, ph, tool) {
    const L = K.L5 + (+tool || 0);
    const s5 = Math.sin(th5 * D2R), c5 = Math.cos(th5 * D2R);
    const s4 = Math.sin(th4 * D2R), c4 = Math.cos(th4 * 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 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-7) return out;
      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))));
    const TH = [sig, -sig, Math.PI - sig, -(Math.PI - sig)];
    for (let k = 0; k < 4; k++) {
      const th3 = TH[k];
      const kap = Vx * Math.cos(th3), 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 * Math.sin(th3);
      const th1 = (Math.abs(b) + Math.abs(cc) > 1e-12) ? (ph - Math.atan2(cc, b)) * R2D : 0;
      out.push([wrap180(th1), wrap180(th2), wrap180(th3 * R2D), th4, th5, 0]);
    }
    return out;
  }

  const inLimit = q => {
    for (let i = 0; i < 5; i++) {
      const t = LIM[ORDER[i]];
      if (q[i] < t.lo - 1e-9 || q[i] > t.hi + 1e-9) return false;
    }
    return true;
  };
  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 && lam) { const d = wrap180(q[i] - ref[i]); c += lam * w * d * d; }
    }
    return c;
  };
  const nowMs = () => (typeof performance !== 'undefined' ? performance.now() : Date.now());

  /* ---------- 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;
    const KEEP = 4000;
    /* 参考位姿连续项：交互求解时以当前姿态为 ref，代价加入 lam·Σwrap(θᵢ-refᵢ)²。
       等价多分支（θ3 的 ±σ/±(π−σ)、肘上/肘下）之间只按 Σ|θ| 选优会随目标微动
       反复横跳，导致某轴大幅倒摆；连续项把同族分支锁住，仅当近支越限/误差超差才换支。
       ref 缺省或 refW=0 时行为与原来完全一致（纯 Σ|θ| 全局最优）。 */
    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 ∈ ±180° 连续，总在限位内。 */
    const POSE = (opt.pose && opt.pose.R && opt.pose.R.length === 9) ? opt.pose.R.slice() : null;
    const POSETOL = 2.0;          // 粗扫接受的最大姿态偏差（度）
    const POSEFINE = 0.05;        // 达标线：偏差 ≤ 此值视为姿态精确到达（机械上已无意义之差）
    let poseW = 200.0;            // 姿态偏差惩罚权重（对 deg²）：0.3° 偏差即折合 Σ|θ| 量级
                                  // （数十度）的代价； escalating 时 ×100 直至出现 POSEFINE 内解
    const closeRoll = (q) => {    // 给定位置解，解 θ6 并返回姿态偏差（度）
      const M5 = orient(q, 5);
      const N = mul3(T3(M5), POSE);                 // 需满足 N = Rz(−θ6)
      const th6 = -Math.atan2(N[3], N[0]) * R2D;    // Rz(α): [1][0]=sinα, [0][0]=cosα ⇒ α=−θ6
      const q2 = q.slice(); q2[5] = th6;
      const oerr = rotErrDeg(mul3(M5, rotAxis(D.J6, th6 * D2R)), POSE);
      return { q: q2, oerr: oerr };
    };

    const scoreBranch = (q, e) => {              // 对一条位置精确支打分（位置误差 e 已由闭式保证）
      let oerr = 0;
      if (POSE) {
        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 p = fk(q, tool);
        const e = Math.hypot(p[0] - target[0], p[1] - target[1], p[2] - target[2]);
        if (e > tol) continue;
        const c = scoreBranch(q, e);
        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), g5 = axis(L5.lo, L5.hi, h);

    /* --- 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++) {
      for (let j = 0; j < g5.length; j++) {
        samples++;
        const s = evalAt(g4[i], g5[j]);
        if (s) { all.push(s); nSol++; if (POSE && s.oerr <= 1) band.push(s); }
      }
      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 rest = all.filter(s => keepBand.indexOf(s) < 0).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.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 = 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 M5 = orient(q, 5), w = [M5[6], M5[7], M5[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; }
        }
      }
    }
    all.sort((x, y) => x.rcost - y.rcost);
    /* 采用解：姿态任务优先从 oerr ≤ POSEFINE 的达标候选里按含连续项代价选；无达标解才尽力而为 */
    let pool = all;
    if (POSE) {
      const acc = all.filter(s => s.oerr != null && s.oerr <= POSEFINE);
      if (acc.length) pool = acc;
    }
    pool.sort((x, y) => 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.cost - y.cost).slice(0, KEEP);
    if (best && list.indexOf(best) < 0) list.unshift(best);
    return {
      ok: !!best, reachable: !!best, target: target.slice(), tool: tool,
      pose: !!POSE,
      q: best ? best.q.slice() : null, cost: best ? best.cost : 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 };
  }

  /* ---------- 6. 一键求解（不可达时返回最近可达位姿/位姿） ---------- */
  function solveTo(target, opt) {
    opt = opt || {};
    const res = solve(target, opt);
    if (res.ok) return res;
    const pose = opt.pose && opt.pose.R && opt.pose.R.length === 9;
    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,
      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 + 姿态联合）'
                 : '目标超出可达工作空间：返回最近可达位姿（阻尼最小二乘）'
    };
  }

  /* ---------- 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++;
    }
    /* 姿态重构：取随机可行位形的真实工具姿态为任务，闭式扫描应精确命中 */
    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 { maxFkErr: maxFk, maxIkErr: maxIk, ikHit: hit, ikTried: m, structWarn: WARN, K: K, LIM: LIM,
             poseHit: pHit, poseTried: pTried, poseErr: pErr, poseMs: pTried ? pMs / pTried : 0 };
  }

  return { fk: fk, fkChain: fkChain, solve: solve, solveTo: solveTo,
           closest: closest, closest6: closest6, orient: orient,
           branch: branch, selfTest: selfTest,
           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 };
}));