feat: 新增IK实时求解与运动模拟功能,集成正逆运动学查看器,补充定位图截图

This commit is contained in:
cbb
2026-09-28 12:08:41 +08:00
parent 79c1265202
commit c0c78251b5
11 changed files with 3955 additions and 25 deletions
+404
View File
@@ -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 };
})();