/* ============================================================================ * 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 = '' + id + '' + CN[id] + '' + '' + q[i].toFixed(2) + '\u00B0' + '' + Math.abs(q[i]).toFixed(1) + '' + '' + (t.cont ? '\u00B1180' : t.lo + '..' + t.hi) + ''; 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 = '总关节角 \u03A3|\u03B8|' + S.costOf(q, null).toFixed(2) + '\u00B0' + '\u03A3\u03B8 代数和' + sum.toFixed(1) + '\u00B0' + '残差 闭式/场景图' + dcl.toExponential(1) + ' / ' + dsc.toExponential(1) + ' mm' + '可行采样点' + res.nSol + '' + '耗时' + res.ms.toFixed(0) + ' ms' + '目标(' + res.target.map(v => v.toFixed(1)).join(', ') + ')' + (res.resid != null ? '距目标' + res.resid.toFixed(2) + ' mm' : ''); 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 = '\u03A3|\u03B8| ' + S.costOf(q, null).toFixed(2) + '\u00B0' + '|\u6B8B\u5DEE ' + '' + dsc.toExponential(1) + ' mm' + '|\u53EF\u884C\u91C7\u6837\u70B9 ' + res.nSol + '|' + 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 = '' + (i + 1) + '' + s.cost.toFixed(2) + '\u00B0' + '' + s.q.map(v => v.toFixed(1)).join(' / ').replace(/ /g, '') + ''; 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 = '结构前提' + (st.structWarn.length ? st.structWarn.join(';') : '自洽 ✓') + '' + 'FK 校验' + st.maxFkErr.toExponential(1) + ' mm' + 'IK 校验' + st.ikHit + '/' + st.ikTried + ' · ' + st.maxIkErr.toExponential(1) + ' mm' + 'TCP 法兰面(' + TCP0.map(v => v.toFixed(1)).join(', ') + ')' + '*运动学可达限位内 12 万随机位形,未计自碰撞' + '可达 x*' + ws.lo[0].toFixed(0) + '…' + ws.hi[0].toFixed(0) + ' mm(前伸 ' + ws.hi[0].toFixed(0) + ')' + '可达 y*' + ws.lo[1].toFixed(0) + '…' + ws.hi[1].toFixed(0) + ' mm' + '可达 z*' + ws.lo[2].toFixed(0) + '…' + ws.hi[2].toFixed(0) + ' mm' + '离 J1 轴*最远 ' + ws.rmax.toFixed(0) + ' mm'; /* 打包自检:确认所有依赖都在本文件内(便于单文件分发,防止被拆开后才出问题) */ 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(' ''; const nScript = document.querySelectorAll('script:not([src])').length; $('ikDep').innerHTML = '打包方式单文件自包含,可直接拷走' + 'three.js' + ok(typeof THREE !== 'undefined') + '已内联 · r' + ((typeof THREE !== 'undefined' && THREE.REVISION) || '?') + '' + '几何数据' + ok(typeof DATA !== 'undefined') + '已内联 · ' + DATA.parts.length + ' 件零件' + '关节 / 限位' + ok(typeof JO !== 'undefined' && typeof TRAVEL !== 'undefined') + '已内联 · ' + JO.length + ' 轴 / ' + TRAVEL.length + ' 条限位' + '二维图纸' + ok(nSheet > 0) + '已内联 · ' + nSheet + ' 张 SVG' + 'IK 求解器' + ok(typeof ArmIK !== 'undefined') + '已内联(纯 JS,无依赖)' + '内联脚本' + nScript + ' 个(三维引擎 / 模型数据 / 求解器 / 界面)' + '外部引用' + ok(ext === 0) + (ext === 0 ? '0 个(script/link/img/iframe 等全类型已查)' : ext + ' 个 —— 已破坏自包含!') + ''; 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 }; })();