cbb/Robot_Arm
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: 查看器新增姿态控制算法与箭头拖拽交互（TCP位置+工具姿态6自由度联合IK），IK控制条改两行，补充定位图截图

Browse Source 
- arm-ik-solver.js：位置闭式支附加 θ6 滚转解算（N=R5^T·R_des，工具轴与 θ6 无关的结构）；达标带（e≤1°）按 e 升序取种子 + escalating（惩罚×100 重扫）+ (θ4,θ5) 二维牛顿收口，分层保证"姿态达标再谈省关节角"；超可达空间走 6 维阻尼最小二乘（位置 mm+姿态 deg 联合）兜底；自检补姿态重构
- ik-glue.js：姿态控制开关 + 方位/仰角/滚转输入 + 取当前/垂直向下；目标点处方向箭头（弧球法调指向）与粉色侧球/箭杆（调滚转），就近仲裁不抢黄球拖拽；面板显示目标/实际姿态与残差（闭式/场景图双口径）
- 界面语义统一为工具朝外法向，入栈绕工具 x 翻 180° 换算求解器内部约定
This commit is contained in:
cbb
2026-09-28 22:09:09 +08:00
parent c0c78251b5
commit a70724e8a9
10 changed files with 3260 additions and 304 deletions
Download Patch File Download Diff File Expand all files Collapse all files
web/arm-ik-solver.js
+444 -40
View File
@@ -38,6 +38,45 @@ const mv3 = (A, v) => [
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) {
@@ -109,6 +148,17 @@ function make(cfg) {
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);
@@ -171,8 +221,13 @@ function make(cfg) {
}
return true;
};
const costOf = (q, W) => {
let c = 0; for (let i = 0; i < 6; i++) c += Math.abs(q[i]) * (W ? W[i] : 1);
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());
@@ -189,7 +244,43 @@ function make(cfg) {
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);
@@ -199,8 +290,8 @@ function make(cfg) {
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 = costOf(q, W);
if (!best || c < best.cost) best = { q: q, cost: c, err: e };
const c = scoreBranch(q, e);
if (c && (!best || c.rcost < best.rcost - 1e-12)) best = c;
}
return best;
};
@@ -218,46 +309,70 @@ function make(cfg) {
/* --- 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 (s) { all.push(s); nSol++; if (POSE && s.oerr <= 1) band.push(s); }
}
if (all.length > KEEP * 12) { // 有界保留：只留代价最优者
all.sort((x, y) => x.cost - y.cost);
if (!POSE && all.length > KEEP * 12) { // 有界保留：只留代价最优者
all.sort((x, y) => x.rcost - y.rcost);
all.length = KEEP;
trimmed = true;
}
}
all.sort((x, y) => x.cost - y.cost);
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 多起点局部加密 + 模式搜索 --- */
const seeds = [];
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);
}
if (opt.refine !== false) {
/* --- 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 best = s;
let lb = s;
for (const a of w4) for (const b of w5) {
samples++;
const c = evalAt(a, b);
if (c) { nSol++; if (c.cost < best.cost) best = c; }
if (c) { nSol++; if (c.rcost < lb.rcost) lb = c; }
}
all.push(best);
all.push(lb);
}
all.sort((x, y) => x.cost - y.cost);
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], best = s, step = h / 4, guard = 0;
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) {
@@ -265,28 +380,128 @@ function make(cfg) {
if (t4 === c4 && t5 === c5) continue;
samples++;
const c = evalAt(t4, t5);
if (c && c.cost < best.cost - 1e-10) { best = c; c4 = t4; c5 = t5; moved = true; }
if (c && c.rcost < lb.rcost - 1e-10) { lb = c; c4 = t4; c5 = t5; moved = true; }
}
if (!moved) step *= 0.5;
}
all.push(best);
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();
}
}
all.sort((x, y) => x.cost - y.cost);
const best = all.length ? all[0] : null;
/* --- 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, nSol: nSol, truncated: trimmed,
samples: samples, ms: nowMs() - t0, list: all.slice(0, KEEP)
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) {
function closest(target, tool, tries, ref) {
const dp = 0.05, lam = 4.0;
let bestQ = null, bestD = Infinity;
const run = q => {
/* 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);
@@ -321,38 +536,211 @@ function make(cfg) {
const gain = mx > 12 ? 12 / mx : 1;
for (let c = 0; c < 6; c++) {
const t = LIM[ORDER[c]];
q[c] = Math.max(t.lo, Math.min(t.hi, q[c] + dq[c] * gain));
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 + Math.random() * (t.hi - t.lo); }
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 };
}
/* ---------- 6. 一键求解（不可达时返回最近可达位姿） ---------- */
/* ---------- 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 cp = closest(target, +opt.tool || 0, opt.tries);
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: '目标超出可达工作空间：返回最近可达位姿（阻尼最小二乘）'
note: pose ? '目标位姿超出可达空间：返回最近位姿（阻尼最小二乘，位置 mm + 姿态联合）'
: '目标超出可达工作空间：返回最近可达位姿（阻尼最小二乘）'
};
}
/* ---------- 7. 自检：闭式 FK ↔ 矩阵链；逆解重构 ---------- */
function selfTest(n) {
/* ---------- 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 = [];
@@ -374,13 +762,29 @@ function make(cfg) {
maxIk = Math.max(maxIk, best);
if (best < 1e-6) hit++;
}
return { maxFkErr: maxFk, maxIkErr: maxIk, ikHit: hit, ikTried: m, structWarn: WARN, K: K, LIM: LIM };
/* 姿态重构：取随机可行位形的真实工具姿态为任务，闭式扫描应精确命中 */
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, branch: branch, selfTest: selfTest,
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 };
return { make: make, ORDER: ORDER, poseMat: poseMat, poseAng: poseAng, rotErrDeg: rotErrDeg };
}));