feat: 新增IK实时求解与运动模拟功能，集成正逆运动学查看器，补充定位图截图
This commit is contained in:
@@ -0,0 +1,386 @@
|
||||
/* ============================================================================
|
||||
* 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]
|
||||
];
|
||||
|
||||
/* ==========================================================================*/
|
||||
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]];
|
||||
}
|
||||
|
||||
/* ---------- 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) => {
|
||||
let c = 0; for (let i = 0; i < 6; i++) c += Math.abs(q[i]) * (W ? W[i] : 1);
|
||||
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;
|
||||
|
||||
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 = costOf(q, W);
|
||||
if (!best || c < best.cost) best = { q: q, cost: c, err: e };
|
||||
}
|
||||
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;
|
||||
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 (all.length > KEEP * 12) { // 有界保留：只留代价最优者
|
||||
all.sort((x, y) => x.cost - y.cost);
|
||||
all.length = KEEP;
|
||||
trimmed = true;
|
||||
}
|
||||
}
|
||||
all.sort((x, y) => x.cost - y.cost);
|
||||
|
||||
/* --- 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) {
|
||||
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;
|
||||
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; }
|
||||
}
|
||||
all.push(best);
|
||||
}
|
||||
all.sort((x, y) => x.cost - y.cost);
|
||||
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;
|
||||
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.cost < best.cost - 1e-10) { best = c; c4 = t4; c5 = t5; moved = true; }
|
||||
}
|
||||
if (!moved) step *= 0.5;
|
||||
}
|
||||
all.push(best);
|
||||
}
|
||||
}
|
||||
all.sort((x, y) => x.cost - y.cost);
|
||||
const best = all.length ? all[0] : null;
|
||||
return {
|
||||
ok: !!best, reachable: !!best, target: target.slice(), tool: tool,
|
||||
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)
|
||||
};
|
||||
}
|
||||
|
||||
/* ---------- 5. 最近可达位姿（阻尼最小二乘，兜底用） ---------- */
|
||||
function closest(target, tool, tries) {
|
||||
const dp = 0.05, lam = 4.0;
|
||||
let bestQ = null, bestD = Infinity;
|
||||
const run = q => {
|
||||
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]];
|
||||
q[c] = Math.max(t.lo, Math.min(t.hi, q[c] + dq[c] * gain));
|
||||
}
|
||||
}
|
||||
};
|
||||
run([0, 0, 0, 0, 0, 0]);
|
||||
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); }
|
||||
run(q);
|
||||
}
|
||||
return { q: bestQ, dist: bestD };
|
||||
}
|
||||
|
||||
/* ---------- 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);
|
||||
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,
|
||||
nSol: res.nSol, samples: res.samples, ms: res.ms, list: [],
|
||||
note: '目标超出可达工作空间：返回最近可达位姿（阻尼最小二乘）'
|
||||
};
|
||||
}
|
||||
|
||||
/* ---------- 7. 自检：闭式 FK ↔ 矩阵链；逆解重构 ---------- */
|
||||
function selfTest(n) {
|
||||
n = n || 400;
|
||||
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++;
|
||||
}
|
||||
return { maxFkErr: maxFk, maxIkErr: maxIk, ikHit: hit, ikTried: m, structWarn: WARN, K: K, LIM: LIM };
|
||||
}
|
||||
|
||||
return { fk: fk, fkChain: fkChain, solve: solve, solveTo: solveTo,
|
||||
closest: closest, branch: branch, selfTest: selfTest,
|
||||
K: K, LIM: LIM, P: P, D: D, ORDER: ORDER, wrap180: wrap180, costOf: costOf };
|
||||
}
|
||||
|
||||
return { make: make, ORDER: ORDER };
|
||||
}));
|
||||
+404
@@ -0,0 +1,404 @@
|
||||
/* ============================================================================
|
||||
* ik-glue.js —— 把 ArmIK 求解器接到「机器人手臂3D查看器」的场景与 UI 上
|
||||
* 依赖查看器全局：JO / TRAVEL / meshes / JG / ang / applyJoint / scene / cam /
|
||||
* cv / VC / sel / requestRender / az,po,rd,tx,ty,tz
|
||||
* ==========================================================================*/
|
||||
(function () {
|
||||
'use strict';
|
||||
if (typeof ArmIK === 'undefined' || typeof JO === 'undefined') return;
|
||||
|
||||
const D2R = Math.PI / 180;
|
||||
const ORDER = ArmIK.ORDER;
|
||||
const $ = id => document.getElementById(id);
|
||||
const JJ = {}; for (const j of JO) JJ[j.id] = j;
|
||||
const CN = {}; for (const id of ORDER) CN[id] = (JJ[id] && JJ[id].cn) || id;
|
||||
|
||||
/* ---------- 0. TCP：末端工具法兰安装面中心（由 J6 组内零件几何自动取得）---------- */
|
||||
let tcpZ = 93;
|
||||
(function () {
|
||||
let lo = Infinity;
|
||||
for (const m of meshes) {
|
||||
const u = m.userData;
|
||||
if (!u || u.grp !== 'J6' || !u.printed || u.name.slice(0, 4) === 'ENV_') continue;
|
||||
const b = m.geometry.boundingBox; if (!b) continue;
|
||||
lo = Math.min(lo, b.min.y); // 查看器系 y = CAD 系 z
|
||||
}
|
||||
if (isFinite(lo)) tcpZ = lo;
|
||||
})();
|
||||
const TCP0 = [JJ.J6.rear[0], JJ.J6.rear[1], tcpZ];
|
||||
|
||||
/* ---------- 1. 建立求解器 ---------- */
|
||||
const JMap = {}; for (const j of JO) JMap[j.id] = { rear: j.rear, d: j.d };
|
||||
const TR = {}; for (const t of TRAVEL) TR[t.axis] = { lo: t.lo, hi: t.hi, continuous: !!t.continuous };
|
||||
const S = ArmIK.make({ joints: JMap, travel: TR, tcp: TCP0 });
|
||||
const LIM = S.LIM;
|
||||
let lastTool = 0;
|
||||
let TGT = [150, 128, 260];
|
||||
|
||||
/* ---------- 2. 场景标注 ---------- */
|
||||
const gTgt = new THREE.Group(); scene.add(gTgt);
|
||||
gTgt.add(new THREE.Mesh(new THREE.SphereGeometry(8.5, 22, 16),
|
||||
new THREE.MeshStandardMaterial({ color: 0xffb454, emissive: 0x7a4d05, metalness: .25, roughness: .4 })));
|
||||
const AXCOL = [0xff6b6b, 0x3ddc97, 0x4c9aff];
|
||||
const AXDIR = [[1, 0, 0], [0, 0, -1], [0, 1, 0]]; // CAD 的 X / Y / Z 在查看器系中的方向
|
||||
for (let i = 0; i < 3; i++) {
|
||||
const L = 55, d = AXDIR[i], bg = new THREE.BufferGeometry();
|
||||
bg.setAttribute('position', new THREE.BufferAttribute(new Float32Array([
|
||||
-d[0] * L, -d[1] * L, -d[2] * L, d[0] * L, d[1] * L, d[2] * L]), 3));
|
||||
gTgt.add(new THREE.Line(bg, new THREE.LineBasicMaterial({ color: AXCOL[i] })));
|
||||
}
|
||||
const RING = new THREE.Mesh(new THREE.RingGeometry(9.5, 12.5, 40),
|
||||
new THREE.MeshBasicMaterial({ color: 0xffb454, side: THREE.DoubleSide, transparent: true, opacity: .45 }));
|
||||
RING.rotation.x = -Math.PI / 2; scene.add(RING);
|
||||
|
||||
const ballTcp = new THREE.Mesh(new THREE.SphereGeometry(6.5, 20, 14),
|
||||
new THREE.MeshStandardMaterial({ color: 0x3ddc97, emissive: 0x14603f, metalness: .3, roughness: .35 }));
|
||||
JG.J6.add(ballTcp);
|
||||
|
||||
const linkGeo = new THREE.BufferGeometry();
|
||||
linkGeo.setAttribute('position', new THREE.BufferAttribute(new Float32Array(6), 3));
|
||||
const linkLine = new THREE.Line(linkGeo,
|
||||
new THREE.LineBasicMaterial({ color: 0xffb454, transparent: true, opacity: .9 }));
|
||||
scene.add(linkLine);
|
||||
|
||||
const tcpCad = () => [TCP0[0], TCP0[1], TCP0[2] - lastTool];
|
||||
function refreshTarget() {
|
||||
const v = VC(TGT);
|
||||
gTgt.position.set(v[0], v[1], v[2]);
|
||||
RING.position.set(-v[0], -v[1], -v[2]);
|
||||
RING.visible = VIEW === 'motion' && Math.abs(v[1]) > 14;
|
||||
ballTcp.position.set(...VC(tcpCad()));
|
||||
scene.updateMatrixWorld(true); // 必须先刷新 6 级矩阵链再读 TCP
|
||||
const w = new THREE.Vector3(...VC(tcpCad())).applyMatrix4(JG.J6.matrixWorld);
|
||||
const a = gTgt.position, pa = linkGeo.attributes.position;
|
||||
pa.array[0] = a.x; pa.array[1] = a.y; pa.array[2] = a.z;
|
||||
pa.array[3] = w.x; pa.array[4] = w.y; pa.array[5] = w.z;
|
||||
pa.needsUpdate = true;
|
||||
const d = Math.hypot(w.x - a.x, w.y - a.y, w.z - a.z);
|
||||
ballTcp.material.color.setHex(d < 1.2 ? 0x3ddc97 : 0xffb454);
|
||||
requestRender();
|
||||
}
|
||||
|
||||
/* ---------- 3. 关节写入（同步滑块与标签）---------- */
|
||||
let anim = null;
|
||||
function setJoints(q, doAnim) {
|
||||
if (doAnim) { animTo(q, 620); return; }
|
||||
for (let i = 0; i < 6; i++) {
|
||||
const id = ORDER[i]; ang[id] = q[i];
|
||||
const s = $('s' + id), v = $('v' + id);
|
||||
if (s) s.value = q[i];
|
||||
if (v) v.textContent = (+q[i]).toFixed(2) + '\u00B0';
|
||||
applyJoint(id);
|
||||
}
|
||||
refreshTarget();
|
||||
}
|
||||
function animTo(q, ms) {
|
||||
if (anim) { anim = null; }
|
||||
const from = ORDER.map(id => ang[id]);
|
||||
anim = { from: from, to: q.slice(), t0: performance.now(), ms: ms };
|
||||
const tick = () => {
|
||||
if (!anim) return;
|
||||
const k = Math.min(1, (performance.now() - anim.t0) / anim.ms);
|
||||
const e = k * k * (3 - 2 * k);
|
||||
for (let i = 0; i < 6; i++) {
|
||||
const id = ORDER[i], a = anim.from[i] + (anim.to[i] - anim.from[i]) * e;
|
||||
ang[id] = a;
|
||||
const sl = $('s' + id), v = $('v' + id);
|
||||
if (sl) sl.value = a;
|
||||
if (v) v.textContent = a.toFixed(2) + '\u00B0';
|
||||
applyJoint(id);
|
||||
}
|
||||
refreshTarget();
|
||||
if (k < 1) requestAnimationFrame(tick);
|
||||
else { anim = null; for (let i = 0; i < 6; i++) applyJoint(ORDER[i]); requestRender(); }
|
||||
};
|
||||
requestAnimationFrame(tick);
|
||||
}
|
||||
|
||||
/* ---------- 4. 结果渲染 ---------- */
|
||||
const badge = $('ikBadge');
|
||||
function renderResult(res, src) {
|
||||
const q = res.q || [0, 0, 0, 0, 0, 0];
|
||||
const tb = $('ikTab').tBodies[0]; tb.innerHTML = '';
|
||||
for (let i = 0; i < 6; i++) {
|
||||
const id = ORDER[i], t = LIM[id];
|
||||
const tr = document.createElement('tr');
|
||||
tr.innerHTML = '<td>' + id + '</td><td>' + CN[id] + '</td>' +
|
||||
'<td class="num">' + q[i].toFixed(2) + '\u00B0</td>' +
|
||||
'<td class="num dim">' + Math.abs(q[i]).toFixed(1) + '</td>' +
|
||||
'<td class="dim">' + (t.cont ? '\u00B1180' : t.lo + '..' + t.hi) + '</td>';
|
||||
if (i === 5 && !res.tool) tr.title = 'TCP 位于 J6 轴线上，转动 J6 不改变 TCP 位置，故取 0 使总角最小';
|
||||
tb.appendChild(tr);
|
||||
}
|
||||
lastTool = res.tool || 0;
|
||||
const dcl = res.err != null ? res.err : 0;
|
||||
const sc = (function () { // 用场景图真实矩阵链复算
|
||||
const save = ORDER.map(id => ang[id]);
|
||||
for (let i = 0; i < 6; i++) { ang[ORDER[i]] = q[i]; applyJoint(ORDER[i]); }
|
||||
scene.updateMatrixWorld(true);
|
||||
const w = new THREE.Vector3(...VC(tcpCad())).applyMatrix4(JG.J6.matrixWorld);
|
||||
for (let i = 0; i < 6; i++) { ang[ORDER[i]] = save[i]; applyJoint(ORDER[i]); }
|
||||
return [w.x, -w.z, w.y];
|
||||
})();
|
||||
const dsc = Math.hypot(sc[0] - res.target[0], sc[1] - res.target[1], sc[2] - res.target[2]);
|
||||
const sum = q.reduce((a, b) => a + b, 0);
|
||||
$('ikSum').innerHTML =
|
||||
'<i>总关节角 \u03A3|\u03B8|</i><b class="hi">' + S.costOf(q, null).toFixed(2) + '\u00B0</b>' +
|
||||
'<i>\u03A3\u03B8 代数和</i><span>' + sum.toFixed(1) + '\u00B0</span>' +
|
||||
'<i>残差 闭式/场景图</i><span class="' + (dcl < 1e-4 && dsc < .05 ? 'good' : 'bad') + '">' +
|
||||
dcl.toExponential(1) + ' / ' + dsc.toExponential(1) + ' mm</span>' +
|
||||
'<i>可行采样点</i><span>' + res.nSol + '</span>' +
|
||||
'<i>耗时</i><span>' + res.ms.toFixed(0) + ' ms</span>' +
|
||||
'<i>目标</i><span>(' + res.target.map(v => v.toFixed(1)).join(', ') + ')</span>' +
|
||||
(res.resid != null ? '<i>距目标</i><span class="bad">' + res.resid.toFixed(2) + ' mm</span>' : '');
|
||||
const ok = res.reachable && dcl < 1e-3;
|
||||
badge.textContent = ok ? '已到达' : (res.reachable ? '校验偏差' : '不可达·最近位姿');
|
||||
badge.className = 'badge ' + (ok ? 'goodb' : (res.reachable ? 'warnb' : 'badb'));
|
||||
$('ikSrc').textContent = src;
|
||||
$('ikBar').innerHTML = '<b class="hi">\u03A3|\u03B8| ' + S.costOf(q, null).toFixed(2) + '\u00B0</b>' +
|
||||
'<span class="sep">|</span>\u6B8B\u5DEE ' +
|
||||
'<span class="' + (dsc < .05 ? 'good' : 'bad') + '">' + dsc.toExponential(1) + ' mm</span>' +
|
||||
'<span class="sep">|</span>\u53EF\u884C\u91C7\u6837\u70B9 ' + res.nSol +
|
||||
'<span class="sep">|</span>' + res.ms.toFixed(0) + ' ms';
|
||||
/* 备选互异构型 */
|
||||
const alt = [];
|
||||
for (let i = 0; i < res.list.length && alt.length < 6; i++) {
|
||||
const s = res.list[i];
|
||||
let dup = false;
|
||||
for (const a of alt) {
|
||||
let dd = 0; for (let c = 0; c < 6; c++) dd += Math.abs(a.q[c] - s.q[c]);
|
||||
if (dd < 12) { dup = true; break; }
|
||||
}
|
||||
if (!dup) alt.push(s);
|
||||
}
|
||||
$('ikAltN').textContent = '\u4E92\u5F02\u6784\u578B ' + alt.length + ' \u4E2A\uFF08\u89E3\u65CF\u5171 ' +
|
||||
res.nSol + ' \u91C7\u6837\u70B9\uFF09\u00B7 \u70B9\u884C\u5E94\u7528';
|
||||
const box = $('ikAlts'); box.innerHTML = '';
|
||||
alt.forEach((s, i) => {
|
||||
const d = document.createElement('div');
|
||||
d.className = 'altrow';
|
||||
d.innerHTML = '<span class="ai">' + (i + 1) + '</span><b>' + s.cost.toFixed(2) + '\u00B0</b>' +
|
||||
'<span class="aq">' + s.q.map(v => v.toFixed(1)).join(' / ').replace(/ /g, '') + '</span>';
|
||||
d.title = ORDER.map((id, k) => id + ' = ' + s.q[k].toFixed(2) + '\u00B0').join(' ');
|
||||
d.onclick = () => setJoints(s.q, true);
|
||||
box.appendChild(d);
|
||||
});
|
||||
}
|
||||
|
||||
/* ---------- 5. 求解 ---------- */
|
||||
function runSolve() {
|
||||
const X = +$('ikX').value, Y = +$('ikY').value, Z = +$('ikZ').value;
|
||||
if (!isFinite(X) || !isFinite(Y) || !isFinite(Z)) {
|
||||
badge.textContent = '坐标非法'; badge.className = 'badge badb'; return;
|
||||
}
|
||||
TGT = [X, Y, Z];
|
||||
lastTool = +$('ikTool').value || 0;
|
||||
refreshTarget();
|
||||
badge.textContent = '计算中…'; badge.className = 'badge wait';
|
||||
setTimeout(() => {
|
||||
const opt = { tool: lastTool, step: +$('ikStep').value || 0.25 };
|
||||
const res = S.solveTo(TGT, opt);
|
||||
renderResult(res, res.reachable ? '闭式全域扫描 · 冗余维精确覆盖'
|
||||
: '超工作空间 → 阻尼最小二乘最近位姿');
|
||||
if (res.q) setJoints(res.q, true);
|
||||
}, 0);
|
||||
}
|
||||
|
||||
/* 实时追踪：拖动过程中同步求解、立即应用（无动画，跟在手后面）。
|
||||
求解是同步的，同一时刻只跑一个；拖动中的后续 pointermove 只记待处理标志，
|
||||
当前求解结束后用最新 TGT 再解一次，避免事件排队越拖越卡。 */
|
||||
let liveBusy = false, livePending = false;
|
||||
function runSolveLive() {
|
||||
if (liveBusy) { livePending = true; return; }
|
||||
liveBusy = true;
|
||||
try {
|
||||
lastTool = +$('ikTool').value || 0;
|
||||
const res = S.solveTo(TGT, { tool: lastTool, step: +$('ikStep').value || 0.25 });
|
||||
renderResult(res, res.reachable ? '实时追踪 · 闭式全域扫描 · 冗余维精确覆盖'
|
||||
: '实时追踪 · 超工作空间（阻尼最小二乘最近位姿）');
|
||||
if (res.q) setJoints(res.q, false);
|
||||
} finally { liveBusy = false; }
|
||||
if (livePending) { livePending = false; runSolveLive(); }
|
||||
}
|
||||
|
||||
/* ---------- 6. 目标拖动 / 拾取 ---------- */
|
||||
let grab = null, camSnap = null, dragged = false, justPicked = false;
|
||||
const ray2 = new THREE.Raycaster();
|
||||
const snapCam = () => ({ az: az, po: po, tx: tx, ty: ty, tz: tz, rd: rd });
|
||||
const putCam = s => { az = s.az; po = s.po; tx = s.tx; ty = s.ty; tz = s.tz; rd = s.rd; };
|
||||
let mode = 'drag';
|
||||
function setMode(m) {
|
||||
mode = m;
|
||||
$('ikPick').classList.toggle('on', m === 'pick');
|
||||
$('ikRt').classList.toggle('on', m === 'rt');
|
||||
$('ikHint').textContent = m === 'pick' ? '视图单击：目标投到当前高度的水平面'
|
||||
: (m === 'rt' ? '实时追踪中：拖动黄球即算即动，手臂跟着目标走（松手精修）'
|
||||
: '拖动黄球 = 移动目标（Shift 仅上下）；求解在松手后自动执行');
|
||||
}
|
||||
function projTgt() {
|
||||
const p = gTgt.position.clone().project(cam);
|
||||
const r = cv.getBoundingClientRect();
|
||||
return { x: (p.x * .5 + .5) * r.width, y: (-p.y * .5 + .5) * r.height };
|
||||
}
|
||||
cv.addEventListener('pointerdown', e => {
|
||||
if (e.button !== 0 || VIEW !== 'motion') return; // 仅运动模拟模式可操作目标点
|
||||
justPicked = false;
|
||||
if (mode === 'pick') { grab = { pick: true }; dragged = false; return; }
|
||||
const pt = projTgt();
|
||||
if (!(Math.hypot(e.offsetX - pt.x, e.offsetY - pt.y) < 26 || e.shiftKey)) return;
|
||||
grab = { sx: e.offsetX, sy: e.offsetY };
|
||||
camSnap = snapCam(); dragged = false;
|
||||
});
|
||||
cv.addEventListener('pointermove', e => {
|
||||
if (!grab || grab.pick) return;
|
||||
const dx = e.offsetX - grab.sx, dy = e.offsetY - grab.sy;
|
||||
if (Math.abs(dx) + Math.abs(dy) > 2) dragged = true;
|
||||
if (camSnap) putCam(camSnap); // 撤销查看器自身的轨道 / 平移
|
||||
const k = (2 * rd * Math.tan(cam.fov * Math.PI / 360)) / Math.max(1, cv.clientHeight);
|
||||
cam.updateMatrixWorld();
|
||||
const right = new THREE.Vector3().setFromMatrixColumn(cam.matrixWorld, 0);
|
||||
const up = new THREE.Vector3().setFromMatrixColumn(cam.matrixWorld, 1);
|
||||
const cur = VC(TGT), v = new THREE.Vector3(cur[0], cur[1], cur[2]);
|
||||
if (e.shiftKey || e.altKey) { // 约束到世界竖直（CAD +Z）
|
||||
const dot = Math.max(0.25, Math.abs(up.dot(new THREE.Vector3(0, 1, 0))));
|
||||
v.addScaledVector(new THREE.Vector3(0, 1, 0), -dy * k / dot);
|
||||
} else v.addScaledVector(right, dx * k).addScaledVector(up, -dy * k);
|
||||
TGT = [v.x, -v.z, Math.max(0, v.y)];
|
||||
grab.sx = e.offsetX; grab.sy = e.offsetY;
|
||||
syncInputs(); refreshTarget();
|
||||
if (mode === 'rt' && dragged) runSolveLive(); // 实时追踪：拖动过程中即算即动
|
||||
});
|
||||
addEventListener('pointerup', () => { camSnap = null; });
|
||||
cv.addEventListener('pointerup', e => {
|
||||
if (!grab) return;
|
||||
if (grab.pick) { // 视图拾取：射线打到当前高度的水平面
|
||||
grab = null; justPicked = true;
|
||||
const r = cv.getBoundingClientRect();
|
||||
ray2.setFromCamera(new THREE.Vector2(((e.clientX - r.left) / r.width) * 2 - 1,
|
||||
-((e.clientY - r.top) / r.height) * 2 + 1), cam);
|
||||
const plane = new THREE.Plane(new THREE.Vector3(0, 1, 0), -TGT[2]); // 查看器 y = CAD z
|
||||
const hit = new THREE.Vector3();
|
||||
if (ray2.ray.intersectPlane(plane, hit)) {
|
||||
TGT = [hit.x, -hit.z, Math.max(0, TGT[2])];
|
||||
syncInputs(); refreshTarget(); runSolve();
|
||||
}
|
||||
return;
|
||||
}
|
||||
const moved = dragged; grab = null;
|
||||
if (moved) runSolve();
|
||||
});
|
||||
cv.addEventListener('click', () => { // 撤销交互对查看器选中态 / 高亮的副作用
|
||||
if (justPicked) justPicked = false;
|
||||
else if (!dragged) return;
|
||||
dragged = false;
|
||||
if (sel) { sel.material.emissive.setHex(0); sel.material.emissiveIntensity = 0; sel = null; }
|
||||
const hud = $('hud'); if (hud) hud.textContent = '';
|
||||
requestRender();
|
||||
}, true);
|
||||
|
||||
/* ---------- 7. 控件 ---------- */
|
||||
function syncInputs() {
|
||||
$('ikX').value = TGT[0].toFixed(1);
|
||||
$('ikY').value = TGT[1].toFixed(1);
|
||||
$('ikZ').value = TGT[2].toFixed(1);
|
||||
}
|
||||
['ikX', 'ikY', 'ikZ', 'ikTool'].forEach(id => {
|
||||
const el = $(id); if (!el) return;
|
||||
el.addEventListener('change', () => runSolve());
|
||||
});
|
||||
$('ikStep').onchange = () => runSolve();
|
||||
$('ikSolve').onclick = () => runSolve();
|
||||
$('ikPick').onclick = () => setMode(mode === 'pick' ? 'drag' : 'pick');
|
||||
$('ikRt').onclick = () => setMode(mode === 'rt' ? 'drag' : 'rt');
|
||||
$('ikHome').onclick = () => {
|
||||
setJoints([0, 0, 0, 0, 0, 0], true);
|
||||
badge.textContent = '已复位'; badge.className = 'badge wait';
|
||||
$('ikSrc').textContent = '已复零位（下表为上一次求解结果，滑块与模型为当前真实姿态）';
|
||||
};
|
||||
$('ikZero').onclick = () => { TGT = [150, 128, 260]; syncInputs(); refreshTarget(); runSolve(); };
|
||||
|
||||
/* ---------- 8. 视图模式：三维模型（静态） / 装配图 / 运动模拟 ---------- */
|
||||
let VIEW = 'model';
|
||||
const MOTION_UI = ['ikbar', 'ikpanel', 'joints', 'jointsHead', 'footIk'];
|
||||
function setView(v, keepPose) {
|
||||
VIEW = v;
|
||||
const isMotion = v === 'motion';
|
||||
document.body.classList.toggle('motion', isMotion);
|
||||
for (const id of MOTION_UI) {
|
||||
const e = $(id);
|
||||
if (e) e.style.display = isMotion ? '' : 'none';
|
||||
}
|
||||
gTgt.visible = isMotion; RING.visible = isMotion;
|
||||
ballTcp.visible = isMotion; linkLine.visible = isMotion;
|
||||
if (isMotion) {
|
||||
if (+exp.value) document.getElementById('bAsm').click(); // 运动模拟须在装配状态下进行
|
||||
runSolve(); // 进入即求解并动画到目标点
|
||||
} else {
|
||||
if (mode !== 'drag') setMode('drag'); // 离开运动模拟：交互模式复位
|
||||
if (v === 'model' && !keepPose) setJoints([0, 0, 0, 0, 0, 0], true); // 静态模型：关节锁定零位
|
||||
}
|
||||
requestRender();
|
||||
}
|
||||
document.querySelectorAll('.tab[data-view]').forEach(b =>
|
||||
b.addEventListener('click', () => setView(b.dataset.view)));
|
||||
|
||||
/* ---------- 9. 启动：自检 + 工作空间采样 ---------- */
|
||||
function boot() {
|
||||
const st = S.selfTest(600);
|
||||
const ws = (function () {
|
||||
let lo = [1e9, 1e9, 1e9], hi = [-1e9, -1e9, -1e9], rmax = 0;
|
||||
for (let k = 0; k < 120000; k++) {
|
||||
const q = [];
|
||||
for (let i = 0; i < 5; i++) { const t = LIM[ORDER[i]]; q.push(t.lo + Math.random() * (t.hi - t.lo)); }
|
||||
q.push(0);
|
||||
const p = S.fk(q, 0);
|
||||
for (let i = 0; i < 3; i++) { lo[i] = Math.min(lo[i], p[i]); hi[i] = Math.max(hi[i], p[i]); }
|
||||
rmax = Math.max(rmax, Math.hypot(p[1] - S.K.Y0, p[2] - S.K.Z1));
|
||||
}
|
||||
return { lo: lo, hi: hi, rmax: rmax };
|
||||
})();
|
||||
$('ikChk').innerHTML =
|
||||
'<i>结构前提</i><span class="' + (st.structWarn.length ? 'bad' : 'good') + '">' +
|
||||
(st.structWarn.length ? st.structWarn.join('；') : '自洽 ✓') + '</span>' +
|
||||
'<i>FK 校验</i><span class="good">' + st.maxFkErr.toExponential(1) + ' mm</span>' +
|
||||
'<i>IK 校验</i><span class="good">' + st.ikHit + '/' + st.ikTried + ' · ' + st.maxIkErr.toExponential(1) + ' mm</span>' +
|
||||
'<i>TCP 法兰面</i><span>(' + TCP0.map(v => v.toFixed(1)).join(', ') + ')</span>' +
|
||||
'<i>*运动学可达</i><span>限位内 12 万随机位形，未计自碰撞</span>' +
|
||||
'<i>可达 x*</i><span>' + ws.lo[0].toFixed(0) + '…' + ws.hi[0].toFixed(0) +
|
||||
' mm（前伸 ' + ws.hi[0].toFixed(0) + '）</span>' +
|
||||
'<i>可达 y*</i><span>' + ws.lo[1].toFixed(0) + '…' + ws.hi[1].toFixed(0) + ' mm</span>' +
|
||||
'<i>可达 z*</i><span>' + ws.lo[2].toFixed(0) + '…' + ws.hi[2].toFixed(0) + ' mm</span>' +
|
||||
'<i>离 J1 轴*</i><span class="good">最远 ' + ws.rmax.toFixed(0) + ' mm</span>';
|
||||
/* 打包自检：确认所有依赖都在本文件内（便于单文件分发，防止被拆开后才出问题） */
|
||||
const ext = document.querySelectorAll(
|
||||
'script[src],link[href],img[src],iframe[src],source[src],embed[src],object[data],use[href]').length;
|
||||
const nSheet = (typeof SHEETS !== 'undefined' && SHEETS) ? String(SHEETS).split('<svg').length - 1 : 0;
|
||||
const ok = (t) => '<span class="' + (t ? 'good' : 'bad') + '">';
|
||||
const nScript = document.querySelectorAll('script:not([src])').length;
|
||||
$('ikDep').innerHTML =
|
||||
'<i>打包方式</i><span class="good">单文件自包含，可直接拷走</span>' +
|
||||
'<i>three.js</i>' + ok(typeof THREE !== 'undefined') + '已内联 · r' +
|
||||
((typeof THREE !== 'undefined' && THREE.REVISION) || '?') + '</span>' +
|
||||
'<i>几何数据</i>' + ok(typeof DATA !== 'undefined') + '已内联 · ' + DATA.parts.length + ' 件零件</span>' +
|
||||
'<i>关节 / 限位</i>' + ok(typeof JO !== 'undefined' && typeof TRAVEL !== 'undefined') +
|
||||
'已内联 · ' + JO.length + ' 轴 / ' + TRAVEL.length + ' 条限位</span>' +
|
||||
'<i>二维图纸</i>' + ok(nSheet > 0) + '已内联 · ' + nSheet + ' 张 SVG</span>' +
|
||||
'<i>IK 求解器</i>' + ok(typeof ArmIK !== 'undefined') + '已内联（纯 JS，无依赖）</span>' +
|
||||
'<i>内联脚本</i><span>' + nScript + ' 个（三维引擎 / 模型数据 / 求解器 / 界面）</span>' +
|
||||
'<i>外部引用</i>' + ok(ext === 0) +
|
||||
(ext === 0 ? '0 个（script/link/img/iframe 等全类型已查）' : ext + ' 个 —— 已破坏自包含！') + '</span>';
|
||||
|
||||
syncInputs(); refreshTarget();
|
||||
const q = new URLSearchParams(location.search);
|
||||
const hasJ = ['j1', 'j2', 'j3', 'j4', 'j5', 'j6'].some(k => q.has(k));
|
||||
const v0 = q.get('view');
|
||||
if ((v0 === 'motion' || v0 === 'sheet') && document.querySelector('.tab[data-view="' + v0 + '"]')) {
|
||||
document.querySelector('.tab[data-view="' + v0 + '"]').click();
|
||||
} else setView('model', hasJ);
|
||||
}
|
||||
boot();
|
||||
|
||||
window.IK = { solver: S, setTarget: p => { TGT = p.slice(); syncInputs(); refreshTarget(); },
|
||||
getTarget: () => TGT.slice(), run: runSolve, TCP0: TCP0, LIM: LIM,
|
||||
setView: setView, getView: () => VIEW };
|
||||
})();
|
||||
+985
-6
File diff suppressed because it is too large
Load Diff
+985
-6
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user