Hark
2a0a2e55ad
- M1 数据生成器：采样/老师/距离下界认证/时间律/统计/一致性门禁；确定性老师（碰撞次数计时 + resetSession）、pos 任务冻结 J6、--consensus 邻域共识过滤、Dr 类。 - 全流程代码与运行手册：数据打包/可微运动学与对拍/MeanFlow+WTA 模型/闭环仿真/APG/挖错循环/ONNX 导出；webapp 策略运行时 ik-policy.js、?policy=1 接入、tests/policy_bench.cjs；ik-solver 增 pairList/clusterExport/criticalPairs/pairGap/leverTable。
1202 lines
66 KiB
JavaScript
1202 lines
66 KiB
JavaScript
/* ============================================================================
|
||
* 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 COL = (typeof COLLIDE !== 'undefined' && COLLIDE.caps && COLLIDE.caps.length) ? COLLIDE : null;
|
||
/* 可行域不再取 docs/travel.json 的静态扫描值——那是"其它轴全零"时对固定立柱的一次
|
||
性结果，随姿态大变（电机行程现为 ±360°，见 collide.motor_limit_deg）。改由两处
|
||
真实几何决定：adj = 相邻打印件 1D 自由区间（CAD 实测、与姿态无关，胶囊模型故意
|
||
不检相邻对）；跨 ≥2 组的零件对由 dynLimit 按当前姿态实时二分。 */
|
||
const ADJ = (COL && COL.adj) || {};
|
||
const MOTOR = +((COL && COL.motor_limit_deg) || 360); // 电机行程（滑块范围）
|
||
const STATIC_TL = {}; for (const t of TRAVEL) STATIC_TL[t.axis] = { lo: t.lo, hi: t.hi };
|
||
const TR = {};
|
||
const noAdj = [];
|
||
for (const t of TRAVEL) {
|
||
const a = ADJ[t.axis];
|
||
if (!a && !t.continuous) noAdj.push(t.axis);
|
||
TR[t.axis] = { lo: a ? a[0] : t.lo, hi: a ? a[1] : t.hi, continuous: !!t.continuous };
|
||
}
|
||
/* 限位轴丢了 adj 就会退回 travel.json 的零位扫描值（表现为"又被锁回 −106"）：
|
||
fit_capsules.py 重写 collide.json 时已沿用 adj，若仍缺说明只跑了拟合没跑实测。 */
|
||
if (COL && noAdj.length)
|
||
console.warn('[IK] collide.json 缺 ' + noAdj.join('/') + ' 的 adj 实测区间，这几轴退回零位扫描限位；' +
|
||
'重跑 python scratch/probe_adj.py --workers 10');
|
||
const S = ArmIK.make({ joints: JMap, travel: TR, tcp: TCP0, collide: COL });
|
||
const LIM = S.LIM;
|
||
let lastTool = 0;
|
||
let TGT = [150, 128, 260];
|
||
/* 选解判据（用户指定）：所有电机需要转过的角度之和最小 —— 以当前手臂姿态为参考。
|
||
连续轴 θ 与 θ±360 等价，角差取 wrap180；不给 ref 时退化为 Σ|θ| 最小（离线批算）。
|
||
（旧实现是 Σ|θ| + λ·ΣΔ² 的二次连续项，已被本判据取代。） */
|
||
const curRef = () => ORDER.map(id => ang[id]);
|
||
|
||
/* ---------- 1b. 姿态控制状态 ---------- */
|
||
let poseOn = false;
|
||
const POSE = { az: 0, el: 90, roll: 0 }; // 目标姿态：工具轴方位角 / 仰角 + 绕轴滚转（度）
|
||
/* 双优先级：位置与姿态在远目标上不可兼得（实测某目标位置 0mm 时姿态差 44°，而姿态 0° 时位置差 116mm）。
|
||
'pos' 位置优先 —— 保 TCP 点位精确，姿态取最接近可达值；
|
||
'ori' 姿态优先 —— 保指定姿态精确，TCP 取最接近可达点；
|
||
'mix' 混合优先 —— 滑杆自由分配两端比重：0% = 姿态优先、100% = 位置优先（两端与纯算法逐位一致），
|
||
中间值按两端锚点误差归一的折中代价 J = w·(e/e_ref)² + (1−w)·(o/o_ref)² 搜索。 */
|
||
const prioVal = () => { const e = $('ikPrio'); const v = e && e.value; return v === 'ori' || v === 'mix' ? v : 'pos'; };
|
||
const mixWVal = () => { const e = $('ikMixW'); return e ? Math.max(0, Math.min(100, +e.value || 0)) / 100 : 0.5; };
|
||
const prioTxt = () => prioVal() === 'ori' ? '姿态优先'
|
||
: (prioVal() === 'mix' ? '混合优先 ' + Math.round(mixWVal() * 100) + '% 位置' : '位置优先');
|
||
const Rdes = () => ArmIK.poseMat(POSE.az, POSE.el, POSE.roll);
|
||
const syncPoseInputs = () => {
|
||
$('ikAz').value = (+POSE.az).toFixed(1);
|
||
$('ikEl').value = (+POSE.el).toFixed(1);
|
||
$('ikRoll').value = (+POSE.roll).toFixed(1);
|
||
};
|
||
const fillPoseFromCurrent = () => { // 用当前真实工具姿态回填（开启时避免姿态跳变）
|
||
const a = ArmIK.poseAng(Uof(ORDER.map(id => ang[id])));
|
||
POSE.az = a.az; POSE.el = a.el; POSE.roll = a.roll;
|
||
syncPoseInputs();
|
||
};
|
||
const CAD2V = v => [v[0], v[2], -v[1]]; // CAD 向量 → 查看器坐标（与查看器 VC 一致）
|
||
const V2CAD = v => [v[0], -v[2], v[1]]; // 查看器坐标 → CAD 向量（CAD2V 的逆）
|
||
/* 用户姿态 ↔ 求解器姿态的换算：求解器内部工具坐标系 z 轴取法兰面法向（朝臂内，
|
||
即 TCP 外伸的反向），而界面上的方位/仰角/滚转与箭头都以「工具朝外方向」描述。
|
||
两者相差绕工具 x 轴 180°（x 不变、y/z 取反），入栈/出栈各翻一次即可。 */
|
||
const RX180 = [1, 0, 0, 0, -1, 0, 0, 0, -1];
|
||
const mul3g = (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 Rsolver = () => mul3g(Rdes(), RX180); // 交给求解器/场景校验（列 2 = 朝臂内）
|
||
const Uof = q => mul3g(S.orient(q), RX180); // 手臂当前姿态 → 用户系（列 2 = 朝臂外）
|
||
|
||
/* ---------- 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);
|
||
|
||
/* 当前工具轴线：TCP 球沿真实工具轴伸出（绿），与目标箭头对照看姿态差 */
|
||
const curGeo = new THREE.BufferGeometry();
|
||
curGeo.setAttribute('position', new THREE.BufferAttribute(new Float32Array(6), 3));
|
||
const curLine = new THREE.Line(curGeo,
|
||
new THREE.LineBasicMaterial({ color: 0x3ddc97, transparent: true, opacity: .95 }));
|
||
scene.add(curLine);
|
||
|
||
/* 姿态拖动箭头（挂目标球组，随目标移动）：
|
||
拖箭头头部 = 调工具轴指向（方位/俯仰，弧球式）；拖箭杆/捏环/粉色侧球 = 绕轴滚转。
|
||
轴向为本地 +Z：杆/锥几何预旋到 +Z，gArrow 再用四元数对准目标工具轴。
|
||
侧球挂在目标点上、沿工具 x 轴侧向偏移——箭头正对镜头时杆身投影缩成一团，
|
||
侧球始终在侧向，任何视角都抓得到。 */
|
||
const AXLEN = 150, HEADL = 26, HEADZ = AXLEN - HEADL / 2, GRIPZ = (AXLEN - HEADL) / 2, KNOBOFF = 0.72 * AXLEN;
|
||
const gArrow = new THREE.Group(); gTgt.add(gArrow);
|
||
const knob = new THREE.Mesh(new THREE.SphereGeometry(9, 18, 12),
|
||
new THREE.MeshStandardMaterial({ color: 0xff8fb0, emissive: 0x5c1f38, metalness: .2, roughness: .4,
|
||
transparent: true, opacity: .8 }));
|
||
knob.userData.role = 'roll';
|
||
gTgt.add(knob);
|
||
const arrowShaft = (function () {
|
||
const g = new THREE.CylinderGeometry(3.0, 3.0, AXLEN - HEADL, 12);
|
||
g.rotateX(Math.PI / 2); // 圆柱默认轴向 Y → +Z
|
||
const m = new THREE.Mesh(g,
|
||
new THREE.MeshStandardMaterial({ color: 0xffb454, emissive: 0x6b4a10, metalness: .2, roughness: .4 }));
|
||
m.position.z = (AXLEN - HEADL) / 2;
|
||
m.userData.role = 'roll'; // 抓杆身 = 滚转
|
||
return m;
|
||
})();
|
||
const arrowRing = (function () {
|
||
const m = new THREE.Mesh(new THREE.TorusGeometry(10, 2.8, 10, 40),
|
||
new THREE.MeshStandardMaterial({ color: 0xff8fb0, emissive: 0x5c1f38, metalness: .2, roughness: .4 }));
|
||
m.rotation.x = Math.PI / 2; // 环面垂直于轴
|
||
m.position.z = GRIPZ;
|
||
m.userData.role = 'roll';
|
||
return m;
|
||
})();
|
||
const arrowHead = (function () {
|
||
const g = new THREE.ConeGeometry(7.5, HEADL, 18);
|
||
g.rotateX(Math.PI / 2);
|
||
const m = new THREE.Mesh(g,
|
||
new THREE.MeshStandardMaterial({ color: 0xffd166, emissive: 0x7a5a10, metalness: .2, roughness: .35 }));
|
||
m.position.z = HEADZ;
|
||
m.userData.role = 'head';
|
||
return m;
|
||
})();
|
||
gArrow.add(arrowShaft, arrowRing, arrowHead);
|
||
const axisViewer = () => { // 目标工具轴在查看器系的方向（单位）
|
||
const R = Rdes(), v = CAD2V([R[2], R[5], R[8]]); // 行主序：列 2 = 工具轴（朝臂外）
|
||
return new THREE.Vector3(v[0], v[1], v[2]).normalize();
|
||
};
|
||
/* 姿态把手拾取返回 'axis' | 'roll' | null（同时不抢黄球的位置拖动）：
|
||
· 箭头头与侧球都做「屏幕就近」检测，谁近听谁的（箭头正对镜头时两者投影会靠近）；
|
||
· 都够近时黄球优先交回位置拖动；再往后才是箭杆/捏环/箭头锥体的几何射线。 */
|
||
const knobScreen = () => {
|
||
const p = knob.getWorldPosition(new THREE.Vector3()).project(cam);
|
||
const r = cv.getBoundingClientRect();
|
||
return { x: (p.x * .5 + .5) * r.width, y: (-p.y * .5 + .5) * r.height };
|
||
};
|
||
function pickPoseHandle(e) {
|
||
const pt = projTgt();
|
||
const db = Math.hypot(e.offsetX - pt.x, e.offsetY - pt.y);
|
||
const hp = projArrow(HEADZ);
|
||
const dh = Math.hypot(e.offsetX - hp.x, e.offsetY - hp.y);
|
||
const kp = knobScreen();
|
||
const dk = Math.hypot(e.offsetX - kp.x, e.offsetY - kp.y);
|
||
if (dh < 22 && (dk >= 26 || dh <= dk) && dh <= db) return 'axis';
|
||
if (dk < 26 && dk <= db) return 'roll';
|
||
if (db < 26) return null; // 黄球抓取区：交回位置拖动
|
||
ray2.setFromCamera(ndc2(e), cam);
|
||
for (const h of ray2.intersectObjects([arrowShaft, arrowRing, arrowHead, knob], false)) {
|
||
const r = h.object.userData.role;
|
||
if (r === 'head') return 'axis';
|
||
if (r === 'roll') return 'roll';
|
||
}
|
||
return null;
|
||
}
|
||
|
||
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);
|
||
/* 目标箭头（指向工具轴）与当前工具轴线（绿，对照看姿态差） */
|
||
gArrow.visible = poseOn && VIEW === 'motion';
|
||
if (poseOn) {
|
||
const R = Rdes();
|
||
gArrow.quaternion.setFromUnitVectors(new THREE.Vector3(0, 0, 1), axisViewer());
|
||
const xv = CAD2V([R[0], R[3], R[6]]); // 行主序：列 0 = 工具 x 轴；侧球沿它侧偏，始终离开箭头
|
||
knob.position.set(xv[0] * KNOBOFF, xv[1] * KNOBOFF, xv[2] * KNOBOFF);
|
||
}
|
||
knob.visible = poseOn && VIEW === 'motion';
|
||
const Rcur = S.orient(ORDER.map(id => ang[id]));
|
||
const avc = CAD2V([Rcur[2], Rcur[5], Rcur[8]]); // 求解器列 2 = 法兰法向（朝臂内），取反为外伸方向
|
||
const av = [-avc[0], -avc[1], -avc[2]];
|
||
const ca = curGeo.attributes.position;
|
||
ca.array[0] = w.x; ca.array[1] = w.y; ca.array[2] = w.z;
|
||
ca.array[3] = w.x + av[0] * 60; ca.array[4] = w.y + av[1] * 60; ca.array[5] = w.z + av[2] * 60;
|
||
ca.needsUpdate = true;
|
||
curLine.visible = VIEW === 'motion';
|
||
requestRender();
|
||
}
|
||
|
||
/* ---------- 2b. 滑块动态限位夹取（边界缓存）----------
|
||
collideFree 一票 ~5ms（1995 胶囊、零位最坏），不允许每次 input 都二分。
|
||
缓存 [轴, 其余轴 1° 量化姿态] → {lo, hi}：拖同一轴时其余轴不动，直接取缓存；
|
||
只有拖过缓存边界（或换了姿态）才重算一次二分（~40ms，一次卡顿换正确边界）。 */
|
||
const bnd = { key: '', lo: 0, hi: 0 };
|
||
function clampAxis(id, v) {
|
||
if (!COL) return v;
|
||
const others = ORDER.map(x => x === id ? '-' : Math.round(ang[x])).join(',');
|
||
const key = id + '|' + others;
|
||
if (bnd.key !== key) {
|
||
bnd.key = key;
|
||
const q = ORDER.map(x => x === id ? ang[x] : Math.round(ang[x]));
|
||
if (!S.collideFree(q)) {
|
||
/* 当前姿本身已干涉（上次被拖越界后卡在界外）：dynLimit 会把"当前值"当边界，
|
||
滑块就再也回不来了。此时退回该轴机械区间（相邻件实测自由区间，必然无自碰撞）。 */
|
||
bnd.lo = LIM[id].lo; bnd.hi = LIM[id].hi;
|
||
} else {
|
||
bnd.lo = S.dynLimit(id, q, -1);
|
||
bnd.hi = S.dynLimit(id, q, 1);
|
||
}
|
||
}
|
||
return Math.max(bnd.lo, Math.min(bnd.hi, v));
|
||
}
|
||
/* 刷新滑块标签里的动态范围（异步执行，避免阻塞拖拽） */
|
||
/* 节流版：拖动/实时追踪中每次 setJoints 都会触发 3 次 dynLimit（每次 ~9 次 collideFree 二分 ≈ 28 ms），
|
||
高频下把 0.1 ms 的快路放大 280×。250 ms 节流 + 尾随补一次；动画结束/松手仍走立即版。 */
|
||
let _rjlLast = -1e9, _rjlTimer = 0;
|
||
function refreshJointLabelsLazy(q) {
|
||
const t = performance.now();
|
||
if (t - _rjlLast > 250) { _rjlLast = t; refreshJointLabels(q); return; }
|
||
if (!_rjlTimer) _rjlTimer = setTimeout(() => {
|
||
_rjlTimer = 0; _rjlLast = performance.now();
|
||
refreshJointLabels(ORDER.map(id => ang[id]));
|
||
}, 260);
|
||
}
|
||
/* 六轴动态边界改到 Worker 里算：连续轴现在必须整周粗扫（实测 6 轴×2 向一次
|
||
175ms → 1131ms），留在主线程会把拖拽冻住。dynAsync 只记账、不写 IKWlast，
|
||
否则会把在飞的 solveTo/solveDyn 结果误标成过期（历史上丢过回调导致实时追踪停摆）。 */
|
||
function dynAsync(q, tol, job) {
|
||
const w = ikWorker();
|
||
if (!w) { job(S.dynLimits(q, tol)); return; } // Worker 不可用：退回主线程同步算
|
||
const id = ++IKWseq;
|
||
IKWjobs.set(id, r => { job(r && !r.__err ? r : S.dynLimits(q, tol)); });
|
||
w.postMessage({ id: id, op: 'dynLimits', args: [q, tol] });
|
||
}
|
||
function refreshJointLabels(q) {
|
||
dynAsync(q.slice(), 1.0, D => paintJointLabels(D));
|
||
}
|
||
function paintJointLabels(D) {
|
||
D = D || {};
|
||
for (const id of ORDER) {
|
||
const el = $('e' + id);
|
||
if (!el) continue;
|
||
const t = LIM[id];
|
||
if (COL && D[id]) {
|
||
/* 边界由几何说话：机械区间（adj 实测）之内，再按当前姿态二分胶囊碰撞。
|
||
lo 侧一律收紧；hi 侧只有"硬边"（相邻件自碰撞，配置无关）才受静态值封顶，
|
||
可放宽侧（撞固定立柱那类）由模型说话——动态边界本就可以远超静态 hi。
|
||
连续轴（J1/J3/J6）同样要查：折肘/折腕位形下末段会扫到固定立柱，
|
||
旧口径因为跳过连续轴而把它显示成整周可用。 */
|
||
const lo = Math.max(t.cont ? -180 : t.lo, D[id].lo);
|
||
const hardHi = COL.hard && COL.hard[id] && COL.hard[id].hi;
|
||
const hi = hardHi ? Math.min(t.hi, D[id].hi) : D[id].hi;
|
||
const dyn = Math.abs(lo - t.lo) > 0.5 || Math.abs(hi - t.hi) > 0.5; // 与静态不同（宽或窄）都标记
|
||
/* 连续轴没收紧时显示"整周可用"，收紧了就换成区间：同一个 <em> 是唯一写手，
|
||
别让静态的 360 度连续徽标和实时夹取区间并存（状态灯与数字会打架）。 */
|
||
el.className = dyn ? '' : (t.cont ? 'ok' : '');
|
||
el.innerHTML = (t.cont && !dyn) ? '整周可用' :
|
||
((dyn ? '<b style="color:var(--warn)">' : '') +
|
||
lo.toFixed(0) + '\u00B0\u2026' + hi.toFixed(0) + '\u00B0' + (dyn ? ' \u25B2</b>' : ''));
|
||
const st = STATIC_TL[id] || {};
|
||
el.title = (t.cont ? '连续轴（整周）' : '机械区间（相邻件实测）') + t.lo + '\u2026' + t.hi +
|
||
'\u00B0；电机行程 ±' + MOTOR + '\u00B0；当前姿态动态边界 ' +
|
||
lo.toFixed(1) + '\u2026' + hi.toFixed(1) + '\u00B0（胶囊碰撞模型实时算）' +
|
||
(st.lo != null ? '\n零位静态扫描参考 ' + st.lo + '\u2026' + st.hi + '\u00B0' : '');
|
||
} else {
|
||
el.textContent = t.lo + '\u00B0\u2026' + t.hi + '\u00B0';
|
||
}
|
||
}
|
||
}
|
||
|
||
/* ---------- 3. 关节写入（同步滑块与标签）---------- */
|
||
let anim = null;
|
||
function setJoints(q, doAnim, via, ms) {
|
||
if (doAnim) { animTo(q, ms || 620, via); return; }
|
||
anim = null; // 直接写入必须取消在飞动画，否则入场动画会覆写实时解
|
||
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();
|
||
if (COL) refreshJointLabelsLazy(ORDER.map(id => ang[id]));
|
||
}
|
||
/* 实时碰撞守护（2026-10-15）：动画每帧对当前显示位形做一次碰撞判定（~1.2 ms/帧），
|
||
命中立即停住并亮红标。规划给的折线本已按 1.5° 细验，这里是最后一道独立防线，
|
||
也让"此刻是否碰撞"实时可见。统计供测试/控制台：window.__RT_GUARD()。 */
|
||
const RTG = { frames: 0, hits: 0, ms: 0, last: null };
|
||
window.__RT_GUARD = () => Object.assign({}, RTG, { avgMs: RTG.frames ? RTG.ms / RTG.frames : 0 });
|
||
function rtGuard(qNow) {
|
||
if (!S || !S.collideFree) return true;
|
||
const t = performance.now();
|
||
const ok = S.collideFree(qNow);
|
||
RTG.ms += performance.now() - t; RTG.frames++;
|
||
if (!ok) {
|
||
RTG.hits++; RTG.last = qNow.slice();
|
||
const b = document.getElementById('ikBadge');
|
||
if (b) { b.textContent = '实时碰撞守护：检测到干涉，已停住'; b.className = 'badge badb'; }
|
||
}
|
||
return ok;
|
||
}
|
||
function animTo(q, ms, via) {
|
||
/* 实时跟随：上一段动画还在走时被新规划打断，新动画从"正在运动"接续——
|
||
不再从零速缓入（否则每次重规划都顿一下），只在终点缓出。 */
|
||
const moving = !!anim;
|
||
if (anim) { anim = null; }
|
||
const from = ORDER.map(id => ang[id]);
|
||
const guardOn = !S.collideFree || S.collideFree(from); // 起点已在干涉里（脱困动作）不设守护
|
||
/* 连续轴（J1 肩抬举 / J3 大臂自转 / J6 腕自转）走「短弧」。
|
||
三者物理上无机械限位，θ 与 θ±360 是同一物理位形（applyJoint 走 cos/sin）；
|
||
若按数值直接线性插值，+151.9° → −167.0° 会反向扫 318.9° 绕一整圈（实测）。
|
||
这里先求「离起始角最近的等价角」作为插值终点，运动变成 41.1° 的短弧；
|
||
end 保存求解器给的代表角，末帧落回它——与插值终点恰好相差 360° 的整数倍，
|
||
同一物理角，视觉零跳变，同时保证滑块/关节表数值与求解结果一致。
|
||
via：绕行中间位形（solveRoute 判直线被挡时给的两段式路径），逐段同样按短弧折算，
|
||
渲染走的段与碰撞校验走的段是同一批，不存在"校验过了但动画穿模"。 */
|
||
const segs = [];
|
||
let p = from;
|
||
for (const raw of (via || []).concat([q])) {
|
||
const t = raw.slice();
|
||
for (let i = 0; i < 6; i++) {
|
||
const id = ORDER[i];
|
||
if (LIM[id].cont) t[i] = p[i] + S.wrap180(raw[i] - p[i]);
|
||
}
|
||
segs.push(t); p = t;
|
||
}
|
||
const end = q.slice();
|
||
/* 时间参数化（2026-10-15）：旧版每段固定 ms 且每段各自 smoothstep —— 每个中间位形都
|
||
减速到 0 再起步（顿挫），且 5° 的小段和 150° 的大段用同样时间（小段慢吞吞、大段猛甩）。
|
||
现在：各段时长 ∝ 该段"最慢轴所需时间"（按轴速比 VMAX 折算，近端轴更慢），
|
||
整条折线只做一次全局 smoothstep：只在起点/终点加减速，途经中间位形不停顿。
|
||
走的仍是校验过的那条折线（中间位形经捷径平滑后通常只剩 1~2 个，折角很少）。 */
|
||
const VREL = [0.6, 0.6, 0.9, 0.9, 1.2, 1.8];
|
||
const pts = [from].concat(segs), dur = [];
|
||
for (let i = 1; i < pts.length; i++) {
|
||
let t = 0;
|
||
for (let j = 0; j < 6; j++) t = Math.max(t, Math.abs(pts[i][j] - pts[i - 1][j]) / VREL[j]);
|
||
dur.push(Math.max(t, 1e-3));
|
||
}
|
||
const sum = dur.reduce((a, b) => a + b, 0);
|
||
/* 总时长：ms 视为"标准 90° 移动"的时长，按路径实际长度伸缩，夹在 [0.6ms, 3.5ms] */
|
||
const total = Math.max(ms * 0.6, Math.min(ms * 3.5, ms * sum / 150));
|
||
const cum = [0]; for (const d of dur) cum.push(cum[cum.length - 1] + d / sum);
|
||
anim = { from: from, to: segs[segs.length - 1], t0: performance.now(), ms: total };
|
||
const tick = () => {
|
||
if (!anim) return;
|
||
const el = performance.now() - anim.t0;
|
||
const u = Math.min(1, el / total);
|
||
const k = u;
|
||
const e = moving ? 1 - (1 - u) * (1 - u) // 接续运动：只缓出
|
||
: u * u * (3 - 2 * u); // 静止起步：全局一次缓入缓出
|
||
let si = 0; while (si < dur.length - 1 && e > cum[si + 1]) si++;
|
||
const f = Math.min(1, Math.max(0, (e - cum[si]) / (cum[si + 1] - cum[si])));
|
||
const a0 = pts[si], a1 = pts[si + 1];
|
||
const qNow = a0.map((v, i) => v + (a1[i] - v) * f);
|
||
if (guardOn && !rtGuard(qNow)) { anim = null; return; } // 停在上一帧（已判无干涉）的位形
|
||
for (let i = 0; i < 6; i++) {
|
||
const id = ORDER[i], a = qNow[i];
|
||
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;
|
||
/* 落回代表角（与插值终点等价，视觉无变化），使 ang 与 res.q 对齐 */
|
||
for (let i = 0; i < 6; i++) {
|
||
const id = ORDER[i];
|
||
ang[id] = end[i];
|
||
const sl = $('s' + id), v = $('v' + id);
|
||
if (sl) sl.value = end[i];
|
||
if (v) v.textContent = end[i].toFixed(2) + '\u00B0';
|
||
applyJoint(id);
|
||
}
|
||
if (COL) refreshJointLabels(ORDER.map(id => ang[id]));
|
||
requestRender();
|
||
}
|
||
};
|
||
requestAnimationFrame(tick);
|
||
}
|
||
|
||
/* ---------- 4. 结果渲染 ---------- */
|
||
const badge = $('ikBadge');
|
||
/* 场景图矩阵链复算姿态偏差（度）：
|
||
· 场景侧：直接取 J6 组世界旋转作用在工具 x/z 轴上的方向（transformDirection，
|
||
免去列主序手工换算），正交化后组装成查看器系旋转 Rs；
|
||
· 目标侧：CAD 系旋转 Rtar 经坐标旋转 Rv（CAD→查看器）共轭到查看器系
|
||
R_viewer = Rv·R_cad·Rvᵀ（单边变换列向量不够——基变了整个旋转都要转）。 */
|
||
const RV3 = [1, 0, 0, 0, 0, 1, 0, -1, 0]; // CAD→查看器 3×3（行主序）
|
||
const RV3T = [1, 0, 0, 0, 0, -1, 0, 1, 0]; // 转置
|
||
function sceneOrientErr(q, Rtar) {
|
||
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 M = JG.J6.matrixWorld;
|
||
const zs = new THREE.Vector3(0, 0, 1).transformDirection(M);
|
||
let xs = new THREE.Vector3(1, 0, 0).transformDirection(M);
|
||
xs.sub(zs.clone().multiplyScalar(xs.dot(zs))).normalize(); // 正交化（防场景缩放引入的分量）
|
||
const ys = new THREE.Vector3().crossVectors(zs, xs);
|
||
const Rs = [xs.x, ys.x, zs.x, xs.y, ys.y, zs.y, xs.z, ys.z, zs.z]; // 列 = 工具 x/y/z（查看器系）
|
||
const Rt = []; // 目标：共轭到查看器系
|
||
for (let i = 0; i < 3; i++) for (let j = 0; j < 3; j++) {
|
||
let s = 0;
|
||
for (let k = 0; k < 3; k++) s += RV3[i * 3 + k] * Rtar[k * 3 + j];
|
||
Rt.push(s);
|
||
}
|
||
const Rt2 = [];
|
||
for (let i = 0; i < 3; i++) for (let j = 0; j < 3; j++) {
|
||
let s = 0;
|
||
for (let k = 0; k < 3; k++) s += Rt[i * 3 + k] * RV3T[k * 3 + j];
|
||
Rt2.push(s);
|
||
}
|
||
for (let i = 0; i < 6; i++) { ang[ORDER[i]] = save[i]; applyJoint(ORDER[i]); }
|
||
return ArmIK.rotErrDeg(Rs, Rt2);
|
||
}
|
||
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 ? '\u00B1' + MOTOR
|
||
: (res.dynAxis === id ? t.lo + '..' + '<b style="color:var(--warn)">' + res.dynBound.toFixed(1) + '*</b>'
|
||
: t.lo + '..' + t.hi)) + '</td>';
|
||
if (i === 5 && !res.tool) tr.title = poseOn
|
||
? '姿态控制开启：θ6 为绕工具轴的滚转角，由目标姿态（方位/仰角/滚转）解算'
|
||
: 'TCP 位于 J6 轴线上，转动 J6 不改变 TCP 位置，故取 0 使总角最小';
|
||
tb.appendChild(tr);
|
||
}
|
||
lastTool = res.tool || 0;
|
||
const dcl = res.err != null ? res.err : 0;
|
||
const dori = res.oerr != null ? res.oerr : 0;
|
||
let doriSc = 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);
|
||
if (poseOn) doriSc = sceneOrientErr(q, Rsolver());
|
||
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);
|
||
const pa = poseOn ? ArmIK.poseAng(Uof(q)) : null;
|
||
$('ikSum').innerHTML =
|
||
(res.moveSum != null ? '<i>本步行程 \u03A3|\u0394\u03B8|（判据）</i><b class="hi">' +
|
||
res.moveSum.toFixed(2) + '\u00B0</b>' : '') +
|
||
'<i>总关节角 \u03A3|\u03B8|</i><span>' + S.costOf(q, null).toFixed(2) + '\u00B0</span>' +
|
||
'<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>' +
|
||
(poseOn ? '<i>目标姿态</i><span>方位 ' + POSE.az.toFixed(1) + '\u00B0 · 仰角 ' + POSE.el.toFixed(1) +
|
||
'\u00B0 · 滚转 ' + POSE.roll.toFixed(1) + '\u00B0</span>' : '') +
|
||
(poseOn ? '<i>实际姿态</i><span>' + pa.az.toFixed(1) + '\u00B0 · ' + pa.el.toFixed(1) +
|
||
'\u00B0 · ' + pa.roll.toFixed(1) + '\u00B0</span>' : '') +
|
||
(poseOn ? '<i>姿态残差 闭式/场景图</i><span class="' + (dori < .05 && doriSc < .1 ? 'good' : 'bad') + '">' +
|
||
dori.toFixed(3) + '\u00B0 / ' + doriSc.toFixed(3) + '\u00B0</span>' : '') +
|
||
(poseOn && res.priority === 'mix' && res.mixP && res.mixO
|
||
? '<i>折中权重</i><span>位置 ' + Math.round((res.mixW == null ? 0.5 : res.mixW) * 100) + '% · 姿态 ' +
|
||
(100 - Math.round((res.mixW == null ? 0.5 : res.mixW) * 100)) + '%（按两端锚点误差归一）</span>' +
|
||
'<i>两端锚点</i><span>位置端 ' + res.mixP.e.toFixed(2) + ' mm / ' + res.mixP.o.toFixed(2) +
|
||
'\u00B0 · 姿态端 ' + res.mixO.e.toFixed(1) + ' mm / ' + res.mixO.o.toFixed(2) + '\u00B0</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>' : '') +
|
||
(poseOn && res.resid != null && res.oerr != null
|
||
? '<i>姿态偏差</i><span class="bad">' + res.oerr.toFixed(2) + '\u00B0</span>' : '');
|
||
const oriOk = !poseOn || (dori < 0.05 && doriSc < 0.1);
|
||
/* 以残差判到达：阻尼最小二乘兜底也可能精确到达（扫描漏掉的奇异刀尖解），不能只看 reachable */
|
||
const posOk = dsc < .05;
|
||
/* 姿态优先/混合姿态端：姿态精确、TCP 落在最近可达点——与「不可达」语义不同，单独标出并给出距离 */
|
||
const oriPrio = poseOn && (res.priority === 'ori' || res.priority === 'mix') && oriOk && !posOk;
|
||
/* 混合折中：位置与姿态都有残差（两端都不可兼得时的加权折中点） */
|
||
const mixMid = poseOn && res.priority === 'mix' && !posOk && !oriOk;
|
||
badge.textContent = res.unsafe
|
||
? '位姿与底座/立柱近似干涉（目标可能不可达或位于立柱区内）'
|
||
: res.blocked
|
||
? '路径被立柱挡住 · 已停在边界（目标差 ' + dsc.toFixed(1) + ' mm）'
|
||
: res.detour
|
||
? (res.detour === 'planned' ? '避障到达 · 规划绕行（' + ((res.via || []).length) + ' 个中间位形）'
|
||
: res.detour === 'retract_pass' ? '避障到达 · 模拟人类回退绕过立柱'
|
||
: (res.detour === 'gentle' ? '避障到达 · 微调协同避开立柱'
|
||
: (res.detour === 'coordinated' ? '避障到达 · 平滑协同跨越立柱'
|
||
: '绕行立柱后到达')))
|
||
: (res.poseRelaxed && posOk) ? '位置到达 · 精确姿态会撞立柱，已取最接近的无碰撞姿态（偏差 ' + dori.toFixed(1) + '\u00B0）'
|
||
: (posOk && oriOk) ? '已到达'
|
||
: oriPrio ? '姿态到达·位置最近 ' + dsc.toFixed(1) + ' mm'
|
||
: mixMid ? '混合折中 · 位置差 ' + dsc.toFixed(1) + ' mm · 姿态残差 ' + dori.toFixed(2) + '\u00B0'
|
||
: (posOk && poseOn && dori > 0.05 ? '位置到达·姿态残差 ' + dori.toFixed(2) + '\u00B0' : '不可达·最近位姿');
|
||
badge.className = 'badge ' + (res.unsafe || (!posOk && !oriPrio && !mixMid) ? 'badb'
|
||
: (posOk && oriOk) ? 'goodb' : 'warnb');
|
||
$('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>' +
|
||
(poseOn ? '<span class="sep">|</span>\u59FF\u6001\u6B8B\u5DEE <span class="' +
|
||
(dori < .05 && doriSc < .1 ? 'good' : 'bad') + '">' + dori.toFixed(3) + '\u00B0</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. 求解（Worker 化）----------
|
||
solveDyn/solveTo 的全量扫描（40~430 ms）挪进后台线程，主线程只收结果渲染，
|
||
松手/按钮不再冻结 UI。单文件自包含：ArmIK 源码取自页面内嵌 <script>，
|
||
连同关节/限位/碰撞配置一次性注入 Blob Worker；拖动实时追踪同样走 Worker，
|
||
全量回落也不再卡顿。Worker 不可用（老浏览器/CSP/file 限制）时自动回落
|
||
主线程同步求解，行为与旧版逐位一致。碰撞 dynLimit/clampAxis 仍留主线程
|
||
（滑块交互需要同步边界），主线程的 S 实例保留给 FK/渲染/控制台 API。 */
|
||
let IKW = null, IKWseq = 0, IKWlast = 0, IKWready = false;
|
||
/* 后端状态是给测试和人一起看的：worker=真后台线程，main=同步兜底。
|
||
旧实现失败时只把 IKW 置 false，页面照常跑但拖动会卡，谁也发现不了。 */
|
||
let IK_BACKEND = 'init';
|
||
const IK_BACKEND_LOG = [];
|
||
function ikBackend(kind, msg) {
|
||
IK_BACKEND = kind;
|
||
IK_BACKEND_LOG.push([kind, msg || '']);
|
||
if (kind === 'worker') {
|
||
console.log('[IK] 求解后端 = worker');
|
||
} else {
|
||
console.error('[IK] 求解后端 = ' + kind + (msg ? '：' + msg : ''));
|
||
const b = document.getElementById('ikBadge');
|
||
if (b) { b.textContent = '后台线程未运行 → 主线程同步求解（拖动会卡顿）'; b.className = 'badge badb'; }
|
||
}
|
||
}
|
||
const ikBootCfg = () => ({ joints: JMap, travel: TR, tcp: TCP0, collide: COL });
|
||
/* 给自动化测试与控制台用：后端状态与失败原因，不依赖面板文字 */
|
||
window.__IK_BACKEND = () => IK_BACKEND;
|
||
window.__IK_BACKEND_LOG = () => IK_BACKEND_LOG.slice();
|
||
|
||
/* 首选真 Worker 文件：需要 http（file:// 下浏览器拒绝跨源脚本）。 */
|
||
function ikWorkerFromFile() {
|
||
const w = new Worker('workers/ik.worker.js');
|
||
w.postMessage({ op: 'init', cfg: ikBootCfg() });
|
||
return w;
|
||
}
|
||
/* 兜底：单文件分发版是 file:// 打开的，那里没有可加载的 worker 文件，
|
||
只能把页面里内联的求解器源码做成 Blob Worker。 */
|
||
function ikWorkerFromBlob() {
|
||
let src = null;
|
||
for (const sc of document.querySelectorAll('script')) {
|
||
if (sc.textContent.indexOf('六轴手臂逆运动学求解器') >= 0) { src = sc.textContent; break; }
|
||
}
|
||
if (!src) throw new Error('页面内没有内联求解器源码，Blob 兜底也不可用');
|
||
const boot = String.raw`
|
||
let S = null;
|
||
onmessage = e => {
|
||
const d = e.data || {};
|
||
if (d.op === 'init') { S = ArmIK.make(d.cfg); postMessage({ op: 'ready' }); return; }
|
||
let r;
|
||
try { r = S[d.op].apply(S, d.args); } catch (err) { r = { __err: String(err && err.message || err) }; }
|
||
postMessage({ id: d.id, r: r });
|
||
};
|
||
`;
|
||
const u = URL.createObjectURL(new Blob([src + boot], { type: 'application/javascript' }));
|
||
try {
|
||
const w = new Worker(u);
|
||
w.postMessage({ op: 'init', cfg: ikBootCfg() });
|
||
return w;
|
||
} finally {
|
||
URL.revokeObjectURL(u); // Blob 只在构造时被读一次，失败路径也不留常驻对象
|
||
}
|
||
}
|
||
/* 每个请求按 id 记账，回包必定派发一次；「是否还是最新一帧」用第二参交给调用方判断。
|
||
旧写法只留一个 IKWjob 槽：live 在飞时被 runSolve 插队，那一发的回调就彻底蒸发，
|
||
而它是唯一给 liveBusy 复位的地方——一次竞态即让实时追踪永久停摆。 */
|
||
const IKWjobs = new Map();
|
||
function ikWorker() {
|
||
if (IKW) return IKW;
|
||
if (IKW === false) return null;
|
||
/* file:// 下不能用 URL 构造 Worker（错误还是异步 onerror 才到，构造阶段不抛），
|
||
所以按协议选路；选错时再互相兜底。 */
|
||
const tries = location.protocol === 'file:' ? [ikWorkerFromBlob, ikWorkerFromFile]
|
||
: [ikWorkerFromFile, ikWorkerFromBlob];
|
||
let w = null, why = null;
|
||
for (const mk of tries) {
|
||
try { w = mk(); break; } catch (e) { why = ((why ? why + ' / ' : '') + (e && e.message || e)); }
|
||
}
|
||
if (!w) { IKW = false; ikBackend('main', why); return null; }
|
||
w.onmessage = e => {
|
||
const d = e.data || {};
|
||
if (d.op === 'ready') {
|
||
IKWready = true;
|
||
clearTimeout(IKWtimer);
|
||
ikBackend('worker');
|
||
return;
|
||
}
|
||
const job = IKWjobs.get(d.id);
|
||
if (!job) return; // 已结清过（正常不会发生）
|
||
IKWjobs.delete(d.id);
|
||
job(d.r, d.id === IKWlast); // 过期请求也派发，只是标为不新鲜
|
||
};
|
||
w.onerror = ev => { // Worker 挂了：把在飞的账全部结清并说清楚
|
||
const pend = Array.from(IKWjobs.values()); IKWjobs.clear();
|
||
for (const j of pend) j(null, false);
|
||
IKW = false; IKWready = false;
|
||
ikBackend('main', 'worker onerror: ' + ((ev && ev.message) || 'unknown'));
|
||
};
|
||
/* 握手超时同样是失败：不报就是"看起来在跑、其实一直同步" */
|
||
const IKWtimer = setTimeout(() => {
|
||
if (!IKWready) {
|
||
try { w.terminate(); } catch (e) {}
|
||
IKW = false; IKWready = false;
|
||
ikBackend('main', '3s 内没收到 worker ready');
|
||
}
|
||
}, 10000);
|
||
IKW = w;
|
||
return IKW;
|
||
}
|
||
function solveAsync(op, args, job) {
|
||
const w = ikWorker();
|
||
if (!w) { job(S[op].apply(S, args), true); return; } // 同步兜底（结果结构完全一致）
|
||
const id = ++IKWseq; IKWlast = id;
|
||
IKWjobs.set(id, (r, fresh) => { // Worker 内异常 → 主线程重算一次
|
||
if (r && r.__err) {
|
||
if (fresh) { try { r = S[op].apply(S, args); } catch (e2) { r = null; } }
|
||
else r = null;
|
||
}
|
||
job(r, fresh);
|
||
});
|
||
w.postMessage({ id: id, op: op, args: args });
|
||
}
|
||
|
||
/* ---------- 5a. 求解入口 ---------- */
|
||
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();
|
||
/* 策略模式（?policy=1，ik-policy-glue.js）：目标交给闭环策略执行，不调用规划器（2026-10-17） */
|
||
if (window.__POLICY && window.__POLICY.active() && window.__POLICY.setGoal(TGT.slice(), poseOn ? Rsolver() : null, lastTool)) {
|
||
badge.textContent = '策略执行中…'; badge.className = 'badge wait'; return;
|
||
}
|
||
badge.textContent = '计算中…'; badge.className = 'badge wait';
|
||
setTimeout(() => {
|
||
const opt = { tool: lastTool, step: +$('ikStep').value || 0.25, ref: curRef(),
|
||
from: ORDER.map(id => ang[id]) }; // 松手求解同样按"直线能不能走"挑解
|
||
if (poseOn) { opt.pose = { R: Rsolver() }; opt.priority = prioVal(); if (opt.priority === 'mix') opt.mixW = mixWVal(); }
|
||
solveAsync('solveDyn', [TGT.slice(), opt], (res, fresh) => {
|
||
if (!res || !fresh) return; // 已被更新的一发取代：交给那一发渲染
|
||
renderResult(res, res.reachable
|
||
? (res.dynAxis ? '动态限位放宽重解 · ' + res.dynAxis + ' 边界 ' + res.dynBound.toFixed(1) + '°'
|
||
: (poseOn ? (prioVal() === 'mix'
|
||
? prioTxt() + ' · 两端锚点加权折中（位置精确族 + 姿态精确族）'
|
||
: prioTxt() + ' · 闭式全域扫描 + 孤立根枚举 · 位置+姿态联合')
|
||
: '闭式全域扫描 · 按行裁剪冗余维'))
|
||
: (res.note ? res.note
|
||
: (poseOn ? '超可达位姿空间 → 阻尼最小二乘最近位姿（位置 mm + 姿态联合）'
|
||
: '超工作空间 → 阻尼最小二乘最近位置')));
|
||
if (res.q) setJoints(res.q, true, res.via);
|
||
});
|
||
}, 0);
|
||
}
|
||
|
||
/* 实时追踪：拖动过程中即时求解、立即应用（无动画，跟在手后面）。
|
||
求解在 Worker 后台跑（全量回落也不冻结 UI）；同一时刻只跑一个，
|
||
在飞期间后续 pointermove 只记待处理标志，结果回来后用最新 TGT 再解一次。
|
||
门闩由「派发」而非「渲染」复位（settle 放在 finally）：被 runSolve 插队而作废的
|
||
那一发同样必须放行，否则一次竞态就永久停摆。作废时待处理标志一并交回——
|
||
插队者用的已是最新 TGT，此时重发只会把它再挤掉、连松手的全量精修一起丢掉。 */
|
||
let liveBusy = false, livePending = false;
|
||
function runSolveLive() {
|
||
if (liveBusy) { livePending = true; return; }
|
||
liveBusy = true;
|
||
const settle = (fresh) => {
|
||
liveBusy = false;
|
||
if (!fresh) { livePending = false; return; }
|
||
if (livePending) { livePending = false; runSolveLive(); }
|
||
};
|
||
try {
|
||
lastTool = +$('ikTool').value || 0;
|
||
if (window.__POLICY && window.__POLICY.active() && window.__POLICY.setGoal(TGT.slice(), poseOn ? Rsolver() : null, lastTool)) { settle(true); return; }
|
||
/* 实时追踪原来走 solveTo —— 一个碰撞判定都不做：实测把目标球横拖过立柱，
|
||
1300 帧里 870 帧处于穿模状态。改用 solveRoute：按"从当前位形这条直线能不能走"
|
||
挑解，走不通就贴着障碍停（blocked），不再把手势直接映射进立柱里。 */
|
||
const opt = { tool: lastTool, step: +$('ikStep').value || 0.25, ref: curRef(), warm: curRef(),
|
||
from: ORDER.map(id => ang[id]), noVia: true, quick: true }; // 拖拽中不做抬高版绕行（要再解两次全量 IK）
|
||
if (poseOn) { opt.pose = { R: Rsolver() }; opt.priority = prioVal(); if (opt.priority === 'mix') opt.mixW = mixWVal(); }
|
||
solveAsync('solveRoute', [TGT.slice(), opt], (res, fresh) => { // Worker 后台：全量回落也不冻结主线程
|
||
try {
|
||
if (!res || !fresh) return;
|
||
renderResult(res, res.warm
|
||
? (res.dynAxis ? '实时追踪 · 动态限位 ' + res.dynAxis + '=' + res.dynBound.toFixed(1) + '°'
|
||
: (poseOn ? (res.priority === 'mix'
|
||
? '实时追踪 · 混合优先沿上一帧同盆地续解（加权折中局部跟随 · 松手全量精修）'
|
||
: '实时追踪 · 沿上一帧构型同支续解（位置精确 · 姿态牛顿收口 · 松手全量精修）')
|
||
: '实时追踪 · 沿上一帧构型同支续解（冗余维局部跟随 · 松手全量精修）'))
|
||
: (res.reachable
|
||
? (poseOn ? (prioVal() === 'mix'
|
||
? '实时追踪 · ' + prioTxt() + ' · 两端锚点加权折中'
|
||
: '实时追踪 · ' + prioTxt() + ' · 闭式全域扫描 + 孤立根枚举 · 位置+姿态联合')
|
||
: '实时追踪 · 闭式全域扫描 · 按行裁剪冗余维')
|
||
: (res.note ? '实时追踪 · ' + res.note
|
||
: (poseOn ? '实时追踪 · 超可达位姿空间（阻尼最小二乘最近位姿）'
|
||
: '实时追踪 · 超工作空间（阻尼最小二乘最近位姿）'))));
|
||
/* 无绕行时立即落位（跟手）；一旦给出绕行中间位形，就必须分段动画——
|
||
直接跳到 res.q 等于把"收臂→转 J1→伸展"三段压成一条直线，正是要避免的穿模。 */
|
||
if (res.q) setJoints(res.q, !!(res.via && res.via.length), res.via, 300);
|
||
} finally { settle(fresh); }
|
||
});
|
||
} catch (err) { settle(false); }
|
||
}
|
||
|
||
/* ---------- 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 仅上下）；拖箭头 = 调工具轴指向；拖箭杆或粉球 = 滚转；松手自动求解'))
|
||
+ (poseOn ? ' · 姿态控制中：箭头/粉球已激活，数字框可精确微调' : ' · 开「姿态控制」后出现可拖箭头');
|
||
}
|
||
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 };
|
||
}
|
||
/* 箭头头部 / 滚转圆环的世界坐标 → 屏幕（用于命中判定） */
|
||
function projArrow(localZ) {
|
||
const p = new THREE.Vector3(0, 0, localZ).applyMatrix4(gArrow.matrixWorld).project(cam);
|
||
const r = cv.getBoundingClientRect();
|
||
return { x: (p.x * .5 + .5) * r.width, y: (-p.y * .5 + .5) * r.height };
|
||
}
|
||
const ndc2 = e => {
|
||
const r = cv.getBoundingClientRect();
|
||
return new THREE.Vector2(((e.clientX - r.left) / r.width) * 2 - 1,
|
||
-((e.clientY - r.top) / r.height) * 2 + 1);
|
||
};
|
||
/* 指针在「目标处、垂直于工具轴的平面」上绕轴的转角（度）：滚转把手拖动用 */
|
||
function rollAngleAt(e) {
|
||
const n = axisViewer();
|
||
const pl = new THREE.Plane().setFromNormalAndCoplanarPoint(n, gTgt.position);
|
||
const hit = new THREE.Vector3();
|
||
ray2.setFromCamera(ndc2(e), cam); // 该版本 setFromCamera 不返回 this，勿链式
|
||
if (!ray2.ray.intersectPlane(pl, hit)) return null; // 错过平面：该帧不参与滚转（返回 0 会按 -a0 突跳）
|
||
const v = hit.sub(gTgt.position);
|
||
const up = Math.abs(n.y) > 0.9 ? new THREE.Vector3(1, 0, 0) : new THREE.Vector3(0, 1, 0);
|
||
const u = new THREE.Vector3().crossVectors(n, up).normalize();
|
||
const w = new THREE.Vector3().crossVectors(n, u);
|
||
return Math.atan2(v.dot(w), v.dot(u)) * 180 / Math.PI;
|
||
}
|
||
/* 弧球式瞄准：指针射线与「以目标为球心、半径 = 箭长」的球求交，方向 = 交点 − 球心 */
|
||
function aimAxis(e) {
|
||
ray2.setFromCamera(ndc2(e), cam);
|
||
const o = ray2.ray.origin, d = ray2.ray.direction, C = gTgt.position;
|
||
const ox = o.x - C.x, oy = o.y - C.y, oz = o.z - C.z;
|
||
const b = ox * d.x + oy * d.y + oz * d.z;
|
||
const c = ox * ox + oy * oy + oz * oz - AXLEN * AXLEN;
|
||
const disc = b * b - c;
|
||
const t = Math.max(0, disc >= 0 ? -b - Math.sqrt(disc) : -b); // 未命中取射线上距球心最近点
|
||
const v = new THREE.Vector3(o.x + d.x * t - C.x, o.y + d.y * t - C.y, o.z + d.z * t - C.z);
|
||
if (v.lengthSq() < 1e-6) return;
|
||
v.normalize();
|
||
const vc = V2CAD([v.x, v.y, v.z]);
|
||
POSE.az = Math.atan2(vc[1], vc[0]) * 180 / Math.PI;
|
||
POSE.el = Math.acos(Math.max(-1, Math.min(1, vc[2]))) * 180 / Math.PI;
|
||
syncPoseInputs(); refreshTarget();
|
||
}
|
||
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();
|
||
const db = Math.hypot(e.offsetX - pt.x, e.offsetY - pt.y);
|
||
if (poseOn) { // 箭头头 / 滚转把手（与黄球就近仲裁，见 pickPoseHandle）
|
||
const part = pickPoseHandle(e);
|
||
if (part === 'axis') { grab = { pose: 'axis' }; dragged = false; camSnap = snapCam(); return; }
|
||
if (part === 'roll') { grab = { pose: 'roll', a0: rollAngleAt(e) }; dragged = false; camSnap = snapCam(); return; }
|
||
}
|
||
if (!(db < 26 || e.shiftKey)) return;
|
||
grab = { sx: e.offsetX, sy: e.offsetY };
|
||
camSnap = snapCam(); dragged = false;
|
||
});
|
||
cv.addEventListener('pointermove', e => {
|
||
if (!grab || grab.pick) return;
|
||
if (camSnap) putCam(camSnap); // 撤销查看器自身的轨道 / 平移
|
||
cam.updateMatrixWorld(); // 轨道参数已还原，射线/列向量要用新矩阵
|
||
if (grab.pose === 'axis') { // 拖箭头头：工具轴指向
|
||
dragged = true;
|
||
aimAxis(e);
|
||
if (mode === 'rt') runSolveLive();
|
||
return;
|
||
}
|
||
if (grab.pose === 'roll') { // 拖圆环：绕轴滚转
|
||
dragged = true;
|
||
const a1 = rollAngleAt(e);
|
||
if (a1 == null) return; // 射线错过平面：跳过该帧，角度不变
|
||
POSE.roll = S.wrap180(POSE.roll + S.wrap180(a1 - grab.a0));
|
||
grab.a0 = a1;
|
||
syncPoseInputs(); refreshTarget();
|
||
if (mode === 'rt') runSolveLive();
|
||
return;
|
||
}
|
||
const dx = e.offsetX - grab.sx, dy = e.offsetY - grab.sy;
|
||
if (Math.abs(dx) + Math.abs(dy) > 2) dragged = true;
|
||
const k = (2 * rd * Math.tan(cam.fov * Math.PI / 360)) / Math.max(1, cv.clientHeight);
|
||
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; });
|
||
/* 挂 window 而非画布：在画布外松手也要结束抓取，否则 grab 残留 → 光挪鼠标就拖着目标走 */
|
||
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();
|
||
/* 混合优先：比重滑杆（仅混合模式显示）。滑动即算即动（走混合快路，松手全量折中），
|
||
与拖动黄球同一套 live 机制；两端 0% / 100% 直接交回姿态优先 / 位置优先。 */
|
||
function syncMixUI() {
|
||
const box = $('ikMixBox'), v = $('ikMixV');
|
||
if (box) box.style.display = prioVal() === 'mix' ? 'flex' : 'none';
|
||
if (v) v.textContent = Math.round(mixWVal() * 100) + '% 位置';
|
||
}
|
||
if ($('ikPrio')) $('ikPrio').onchange = () => { syncMixUI(); if (poseOn) runSolve(); };
|
||
if ($('ikMixW')) {
|
||
$('ikMixW').addEventListener('input', () => { syncMixUI(); if (poseOn) runSolveLive(); });
|
||
$('ikMixW').addEventListener('change', () => { syncMixUI(); if (poseOn) runSolve(); });
|
||
}
|
||
syncMixUI();
|
||
$('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(); };
|
||
|
||
/* --- 姿态控制：开关 / 参数 / 快捷 --- */
|
||
const poseEls = ['ikAz', 'ikEl', 'ikRoll'].map($);
|
||
if ($('ikPose')) $('ikPose').onclick = () => {
|
||
poseOn = !poseOn;
|
||
$('ikPose').classList.toggle('on', poseOn);
|
||
$('ikPoseBox').style.display = poseOn ? 'flex' : 'none';
|
||
if (poseOn) fillPoseFromCurrent(); // 开启即用当前工具姿态，避免求解跳变
|
||
refreshTarget();
|
||
setMode(mode);
|
||
runSolve();
|
||
};
|
||
poseEls.forEach((el, i) => {
|
||
if (!el) return;
|
||
el.addEventListener('change', () => {
|
||
POSE.az = +$('ikAz').value || 0;
|
||
POSE.el = Math.max(0, Math.min(180, +$('ikEl').value || 0));
|
||
POSE.roll = +$('ikRoll').value || 0;
|
||
syncPoseInputs();
|
||
if (poseOn) runSolve();
|
||
});
|
||
});
|
||
if ($('ikPoseCur')) $('ikPoseCur').onclick = () => { fillPoseFromCurrent(); if (poseOn) runSolve(); };
|
||
if ($('ikPoseDown')) $('ikPoseDown').onclick = () => {
|
||
POSE.az = 0; POSE.el = 180; POSE.roll = 0; syncPoseInputs(); if (poseOn) runSolve();
|
||
};
|
||
|
||
/* ---------- 8. 视图模式：三维模型（静态） / 装配图 / 运动模拟 ---------- */
|
||
let VIEW = 'model';
|
||
const MOTION_UI = ['ikbar', 'ikpanel', 'joints', 'jointsHead', 'footIk'];
|
||
function setView(v, keepPose) {
|
||
VIEW = v;
|
||
const isMotion = v === 'motion';
|
||
const isSheet = v === 'sheet';
|
||
document.body.classList.toggle('motion', isMotion);
|
||
document.body.classList.toggle('sheet-mode', isSheet);
|
||
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;
|
||
curLine.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); // 静态模型：关节锁定零位
|
||
if (isSheet && typeof openSheetView === 'function') openSheetView();
|
||
}
|
||
requestRender();
|
||
}
|
||
document.querySelectorAll('.tab[data-view]').forEach(b =>
|
||
b.addEventListener('click', () => setView(b.dataset.view)));
|
||
|
||
/* 预置自检结果（离线用同一份求解器代码算出，样本量比在线默认更大）。
|
||
启动时不再重算：原先姿态自检 200 组要 ~14 s（400 组 ~23 s），是本页唯一的启动瓶颈。
|
||
指纹 = FNV-1a over（限位表 + K + TCP + 胶囊数 + 行程表 + 求解器块源码），
|
||
与当前代码不符时会标红提示重算。
|
||
重新生成：node scratch/compute_precomputed.cjs（模型或求解器改动后重跑一条命令） */
|
||
const PRECOMPUTED_CHK = {
|
||
stamp: '2026-10-14 04:02', sig: 'fd1f8bff',
|
||
fk: { n: 5000, err: 5.684341886080801e-13 },
|
||
ik: { n: 200, hit: 200, tried: 200, err: 8.320331044333469e-13 },
|
||
pose: { n: 400, hit: 398, err: 1.5596676084670544, ms: 86.40111575000003 },
|
||
ws: { n: 400000, x: [-377, 416], y: [-388, 643], z: [96, 1126], rmax: 517 }
|
||
};
|
||
/* 紧凑指纹：与离线脚本 compute_precomputed.cjs 里的算法逐字一致
|
||
（注意取 script.textContent——HTML 解析会把行尾归一化成 LF） */
|
||
function chkSig() {
|
||
const p = [];
|
||
for (const id of ORDER) { const t = LIM[id]; p.push(id + t.lo + ',' + t.hi + ',' + t.cont); }
|
||
p.push(TCP0.join(','));
|
||
p.push([S.K.X0, S.K.Y0, S.K.Z1, S.K.Z2, S.K.Z3, S.K.Z4, S.K.Z5].join(','));
|
||
p.push(String(typeof COLLIDE !== 'undefined' && COLLIDE.caps ? COLLIDE.caps.length : -1));
|
||
p.push(typeof TRAVEL !== 'undefined'
|
||
? TRAVEL.map(t => t.axis + t.lo + t.hi + (t.continuous ? 1 : 0)).join('') : '');
|
||
let str = p.join('|');
|
||
const scr = document.scripts; // 4 个内联脚本：0 three / 1 数据 / 2 求解器 / 3 界面
|
||
if (scr.length > 3 && scr[2]) str += '|' + scr[2].textContent;
|
||
let hh = 0x811c9dc5;
|
||
for (let i = 0; i < str.length; i++) { hh ^= str.charCodeAt(i); hh = Math.imul(hh, 0x01000193) >>> 0; }
|
||
return (hh >>> 0).toString(16).padStart(8, '0');
|
||
}
|
||
|
||
/* ---------- 9. 启动：自检 + 工作空间采样 ---------- */
|
||
function boot() {
|
||
/* 启动不再做任何重计算：自检数值全部取自预置结果，只用指纹复核是否仍适用。
|
||
「结构前提」仍取运行时 WARN（make() 时已算好，零额外代价）。 */
|
||
const SIG_NOW = chkSig();
|
||
const CHK = {
|
||
/* 结构前提是 make() 时定下的常量（求解器内部 WARN），与样本量无关：
|
||
用一次 1 样本的 selfTest 取出即可（~0 ms），其余字段一律用预置值。 */
|
||
st: { structWarn: S.selfTest(1, 0).structWarn, maxFkErr: PRECOMPUTED_CHK.fk.err,
|
||
ikHit: PRECOMPUTED_CHK.ik.hit, ikTried: PRECOMPUTED_CHK.ik.tried, maxIkErr: PRECOMPUTED_CHK.ik.err },
|
||
ws: { lo: [PRECOMPUTED_CHK.ws.x[0], PRECOMPUTED_CHK.ws.y[0], PRECOMPUTED_CHK.ws.z[0]],
|
||
hi: [PRECOMPUTED_CHK.ws.x[1], PRECOMPUTED_CHK.ws.y[1], PRECOMPUTED_CHK.ws.z[1]],
|
||
rmax: PRECOMPUTED_CHK.ws.rmax },
|
||
mode: 'pre', // 'pre' 用预置值 | 'live' 本机实测
|
||
pose: null, // null 用预置 | {running,done,total} | {hit,tried,err,ms}
|
||
sigOk: SIG_NOW === PRECOMPUTED_CHK.sig
|
||
};
|
||
let reRunning = false;
|
||
function poseLine() {
|
||
const P = PRECOMPUTED_CHK.pose, p = CHK.pose;
|
||
if (!p) {
|
||
if (!CHK.sigOk) return '<i>姿态 IK 校验</i><span class="bad">预置结果已失效，请点下方「重新计算」</span>';
|
||
return '<i>姿态 IK 校验</i><span class="' + (P.hit >= P.n * 0.9 ? 'good' : 'bad') + '">' +
|
||
P.hit + '/' + P.n + ' 命中 · 最大 ' + P.err.toFixed(2) + '° · ' + P.ms.toFixed(0) + ' ms/次</span>';
|
||
}
|
||
if (p.running) return '<i>姿态 IK 校验</i><span>本机实测中 ' + p.done + '/' + p.total + '…</span>';
|
||
return '<i>姿态 IK 校验</i><span class="' + (p.hit >= p.tried * 0.9 ? 'good' : 'bad') + '">' +
|
||
p.hit + '/' + p.tried + ' 命中 · 最大 ' + p.err.toFixed(2) + '° · ' + p.ms.toFixed(0) + ' ms/次</span>';
|
||
}
|
||
function renderChk() {
|
||
const st = CHK.st, ws = CHK.ws, P = PRECOMPUTED_CHK;
|
||
/* 指纹不符时把"为什么变慢/为什么值是新的"讲清楚：即使已自动转为实测，
|
||
也在来源行保留失效提示，避免用户不明所以。 */
|
||
const src = (CHK.mode === 'live')
|
||
? (CHK.sigOk
|
||
? '<span class="good">本机测试（FK ' + P.fk.n + ' · IK ' + P.ik.n + ' · 姿态 ' + P.pose.n + ' · 工作空间 ' + P.ws.n + ' 位形）</span>'
|
||
: '<span class="bad">⚠ 预置结果已失效（预置指纹 ' + P.sig + ' ≠ 当前 ' + SIG_NOW +
|
||
'，模型或求解器已变更）· 以下为本次本机测试</span>')
|
||
: (CHK.sigOk
|
||
? '<span>预置 · 生成于 ' + P.stamp + '（FK ' + P.fk.n + ' · IK ' + P.ik.n + ' · 姿态 ' + P.pose.n +
|
||
' · 工作空间 ' + P.ws.n + '）· 指纹 ' + SIG_NOW + ' 匹配</span>'
|
||
: '<span class="bad">预置结果已失效：预置指纹 ' + P.sig + ' ≠ 当前 ' + SIG_NOW + '（模型或求解器已变更）</span>');
|
||
$('ikChk').innerHTML =
|
||
'<i>数据来源</i>' + src +
|
||
'<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>限位内随机位形，未计自碰撞</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>' +
|
||
poseLine() +
|
||
'<i>姿态结构</i><span>J5∩J6 垂直相交于腕心；θ6 为绕工具轴滚转，不影响 TCP 位置</span>' +
|
||
'<i>操作</i><span><b id="ikChkRe" style="cursor:pointer;text-decoration:underline;color:var(--ac)">' +
|
||
(reRunning ? '实测进行中…' : '重新计算（本机测试，姿态 ' + P.pose.n + ' 组分片执行 · 约 20–40 s，期间可继续操作）') +
|
||
'</b></span>';
|
||
const re = $('ikChkRe');
|
||
if (re) re.onclick = () => runRecompute();
|
||
}
|
||
/* 「重新计算」：便宜部分同步跑（FK/IK ~30 ms、工作空间 ~120 ms），
|
||
姿态部分 4 组一片 + 让出主线程，避免一次长任务冻住界面。 */
|
||
function runRecompute() {
|
||
if (reRunning) return;
|
||
reRunning = true;
|
||
const P = PRECOMPUTED_CHK;
|
||
CHK.st = S.selfTest(P.fk.n, 0);
|
||
CHK.ws = (function () {
|
||
let lo = [1e9, 1e9, 1e9], hi = [-1e9, -1e9, -1e9], rmax = 0;
|
||
for (let k = 0; k < P.ws.n; 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 };
|
||
})();
|
||
CHK.mode = 'live';
|
||
CHK.pose = { running: true, done: 0, total: P.pose.n };
|
||
const acc = { hit: 0, tried: 0, err: 0, msSum: 0 };
|
||
renderChk();
|
||
const step = () => {
|
||
const r = S.poseSelfTest(4);
|
||
acc.hit += r.poseHit; acc.tried += r.poseTried;
|
||
acc.err = Math.max(acc.err, r.poseErr);
|
||
acc.msSum += r.poseMs * r.poseTried;
|
||
CHK.pose.done += r.poseTried;
|
||
if (CHK.pose.done < CHK.pose.total) { renderChk(); setTimeout(step, 16); return; }
|
||
CHK.pose = { hit: acc.hit, tried: acc.tried, err: acc.err, ms: acc.msSum / Math.max(1, acc.tried) };
|
||
reRunning = false;
|
||
renderChk();
|
||
};
|
||
setTimeout(step, 16);
|
||
}
|
||
renderChk();
|
||
/* 展开自检栏本身不触发计算；只有"预置结果已失效"时才自动补算 */
|
||
{
|
||
const cb = $('ikChkBox');
|
||
if (cb) cb.addEventListener('toggle', () => { if (cb.open && !CHK.sigOk && !CHK.pose) runRecompute(); });
|
||
}
|
||
/* 依赖加载自检：单文件时代这里断言"外部引用必须为 0"，拆分后那条不变量正好反过来。
|
||
现在报的是"每样东西到没到、从哪来、求解跑在哪个后端"，右下角面板与测试都读它。 */
|
||
const nSheet = (typeof SHEETS !== 'undefined' && SHEETS) ? String(SHEETS).split('<svg').length - 1 : 0;
|
||
const ok = (t) => '<span class="' + (t ? 'good' : 'bad') + '">';
|
||
const mode = location.protocol === 'file:' ? 'file://（单文件分发版）' : 'http（webapp/ 多文件）';
|
||
$('ikDep').innerHTML =
|
||
'<i>加载方式</i><span class="good">' + mode + '</span>' +
|
||
'<i>three.js</i>' + ok(typeof THREE !== 'undefined') +
|
||
'r' + ((typeof THREE !== 'undefined' && THREE.REVISION) || '?') + '</span>' +
|
||
'<i>几何数据</i>' + ok(typeof DATA !== 'undefined') +
|
||
((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') + 'ArmIK</span>' +
|
||
'<i>求解后端</i>' + ok(IK_BACKEND === 'worker') + IK_BACKEND + '</span>';
|
||
|
||
const q = new URLSearchParams(location.search);
|
||
const tgt = q.get('tgt'); // tgt=x,y,z：直接以该目标进入运动模拟（复现用）
|
||
if (tgt && /^-?[\d.]+,-?[\d.]+,-?[\d.]+$/.test(tgt)) TGT = tgt.split(',').map(Number);
|
||
syncInputs(); refreshTarget();
|
||
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(); },
|
||
setJoints: setJoints, curQ: () => ORDER.map(id => ang[id]),
|
||
clampAxis: clampAxis, refreshJointLabels: refreshJointLabelsLazy,
|
||
getTarget: () => TGT.slice(), run: runSolve, TCP0: TCP0, LIM: LIM,
|
||
setView: setView, getView: () => VIEW,
|
||
setPose: (az, el, roll, solve) => {
|
||
POSE.az = +az || 0; POSE.el = Math.max(0, Math.min(180, +el || 0)); POSE.roll = +roll || 0;
|
||
syncPoseInputs(); refreshTarget(); if (solve !== false) runSolve();
|
||
},
|
||
getPose: () => ({ on: poseOn, az: POSE.az, el: POSE.el, roll: POSE.roll }),
|
||
/* 优先级 / 混合比重（调试与复现用）：setPrio('mix', 0.3) */
|
||
setPrio: (p, w) => {
|
||
const e = $('ikPrio');
|
||
if (e && (p === 'pos' || p === 'ori' || p === 'mix')) e.value = p;
|
||
if (w != null && $('ikMixW')) $('ikMixW').value = Math.round(Math.max(0, Math.min(1, +w)) * 100);
|
||
syncMixUI();
|
||
if (poseOn) runSolve();
|
||
return { prio: prioVal(), mixW: mixWVal() };
|
||
},
|
||
/* 可拖控点的屏幕坐标（画布内像素）：黄球 / 箭头头 / 滚转侧球 */
|
||
pickPts: () => poseOn ? { ball: projTgt(), head: projArrow(HEADZ), knob: knobScreen() } : null,
|
||
/* 调试：某视口坐标下的把手拾取结果（'axis'/'roll'/null） */
|
||
_pickHandle: (cx, cy) => poseOn ? pickPoseHandle({ clientX: cx, clientY: cy,
|
||
offsetX: cx - cv.getBoundingClientRect().left, offsetY: cy - cv.getBoundingClientRect().top }) : null };
|
||
})(); |