cbb
4fdc37da9f
- 4.3牛顿收口 err2 原取 [M5[6..8]]（行主序第2行）当"列2工具轴"，正确应为列2 [M[2],M[5],M[8]]；该节因此从未起作用。修复后 selfTest 姿态命中 5/6→6/6、 最差 oerr 0.602°→0.024° - scoreBranch 轴夹角预筛：θ6 只滚转不改工具轴 ⇒ oerr≥轴夹角（20万样本零违反）， 轴夹角就超 POSETOL 的候选免 orient 全矩阵+acos 直接拒 - keepBand.indexOf → Set.has（原 O(n²) 约6000×4000恒等比较/次）；evalAt 误差范数 hypot→平方和 - A/B 40 组随机位形输出与合入前逐位一致（Δθ=0.000°、nSol 一致、oerr 零变差）， node 六项回归全绿，浏览器冒烟已到达/残差6.4e-14mm/零报错
811 lines
39 KiB
JavaScript
811 lines
39 KiB
JavaScript
/* ============================================================================
|
||
* 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;
|
||
}
|
||
/* 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) {
|
||
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) 的闭式分支 ---------- */
|
||
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, tol2 = tol * tol;
|
||
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 内解
|
||
/* 轴夹角预筛：θ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) => { // 给定位置解，解 θ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 w = axisCol(q);
|
||
if (1 - (w[0] * A0[0] + w[1] * A0[1] + w[2] * A0[2]) > 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 p = fk(q, 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 c = scoreBranch(q, Math.sqrt(e2));
|
||
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 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.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 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; }
|
||
}
|
||
}
|
||
}
|
||
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 };
|
||
})); |