Files
Robot_Arm/web/ik-glue.js
T

404 lines
20 KiB
JavaScript
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
/* ============================================================================
* 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 };
})();