refactor(model): 重构P03为轻量化板式桁架并统一M4×16自攻螺钉连接方案

This commit is contained in:
cbb
2026-10-02 04:39:12 +08:00
parent 208b37048b
commit 6fe97fc643
29 changed files with 139069 additions and 1296 deletions
+1280 -57
View File
File diff suppressed because it is too large Load Diff
+413 -63
View File
@@ -29,19 +29,47 @@ 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 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];
/* 参考位姿连续项权重:以当前手臂姿态为参考解算,目标微动时锁定同族分支,
避免某轴在等价分支间大幅倒摆(实测 4mm 步长过边界最多跳 81.9° -> 1.2°) */
const REF_W = 0.02;
/* 选解判据(用户指定):所有电机需要转过的角度之和最小 —— 以当前手臂姿态为参考。
连续轴 θ 与 θ±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);
@@ -206,6 +234,68 @@ function refreshTarget() {
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);
}
function refreshJointLabels(q) {
for (const id of ORDER) {
const el = $('e' + id);
if (!el) continue;
const t = LIM[id];
if (COL && !t.cont) {
/* 边界由几何说话:机械区间(adj 实测)之内,再按当前姿态二分胶囊碰撞。
lo 侧一律收紧;hi 侧只有"硬边"(相邻件自碰撞,配置无关)才受静态值封顶,
可放宽侧(撞固定立柱那类)由模型说话——动态边界本就可以远超静态 hi。 */
const lo = Math.max(t.lo, S.dynLimit(id, q, -1, 1.0));
const hardHi = COL.hard && COL.hard[id] && COL.hard[id].hi;
const hi = hardHi ? Math.min(t.hi, S.dynLimit(id, q, 1, 1.0))
: S.dynLimit(id, q, 1, 1.0);
const dyn = Math.abs(lo - t.lo) > 0.5 || Math.abs(hi - t.hi) > 0.5; // 与静态不同(宽或窄)都标记
el.innerHTML = (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.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) {
@@ -219,11 +309,24 @@ function setJoints(q, doAnim) {
applyJoint(id);
}
refreshTarget();
if (COL) refreshJointLabelsLazy(ORDER.map(id => ang[id]));
}
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 };
/* 连续轴(J1 肩抬举 / J3 大臂自转 / J6 腕自转)走「短弧」。
三者物理上无机械限位,θ 与 θ±360 是同一物理位形(applyJoint 走 cos/sin);
若按数值直接线性插值,+151.9° → −167.0° 会反向扫 318.9° 绕一整圈(实测)。
这里先求「离起始角最近的等价角」作为插值终点,运动变成 41.1° 的短弧;
end 保存求解器给的代表角,末帧落回它——与插值终点恰好相差 360° 的整数倍,
同一物理角,视觉零跳变,同时保证滑块/关节表数值与求解结果一致。 */
const to = q.slice();
for (let i = 0; i < 6; i++) {
const id = ORDER[i];
if (LIM[id].cont) to[i] = from[i] + S.wrap180(q[i] - from[i]);
}
const end = q.slice();
anim = { from: from, to: to, t0: performance.now(), ms: ms };
const tick = () => {
if (!anim) return;
const k = Math.min(1, (performance.now() - anim.t0) / anim.ms);
@@ -238,7 +341,20 @@ function animTo(q, ms) {
}
refreshTarget();
if (k < 1) requestAnimationFrame(tick);
else { anim = null; for (let i = 0; i < 6; i++) applyJoint(ORDER[i]); requestRender(); }
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);
}
@@ -286,7 +402,9 @@ function renderResult(res, src) {
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>';
'<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 使总角最小';
@@ -309,7 +427,9 @@ function renderResult(res, src) {
const sum = q.reduce((a, b) => a + b, 0);
const pa = poseOn ? ArmIK.poseAng(Uof(q)) : null;
$('ikSum').innerHTML =
'<i>总关节角 \u03A3|\u03B8|</i><b class="hi">' + S.costOf(q, null).toFixed(2) + '\u00B0</b>' +
(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>' +
@@ -319,6 +439,11 @@ function renderResult(res, src) {
'\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>' +
@@ -328,10 +453,18 @@ function renderResult(res, src) {
const oriOk = !poseOn || (dori < 0.05 && doriSc < 0.1);
/* 以残差判到达:阻尼最小二乘兜底也可能精确到达(扫描漏掉的奇异刀尖解),不能只看 reachable */
const posOk = dsc < .05;
badge.textContent = (posOk && oriOk) ? '已到达'
/* 姿态优先/混合姿态端:姿态精确、TCP 落在最近可达点——与「不可达」语义不同,单独标出并给出距离 */
const oriPrio = poseOn && (res.priority === 'ori' || res.priority === 'mix') && oriOk && !posOk;
/* 混合折中:位置与姿态都有残差(两端都不可兼得时的加权折中点) */
const mixMid = poseOn && res.priority === 'mix' && !posOk && !oriOk;
badge.textContent = res.unsafe
? '位姿与底座/立柱近似干涉(目标可能不可达或位于立柱区内)'
: (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 ' + ((posOk && oriOk) ? 'goodb'
: (posOk ? 'warnb' : 'badb'));
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 ' +
@@ -365,7 +498,67 @@ function renderResult(res, src) {
});
}
/* ---------- 5. 求解 ---------- */
/* ---------- 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;
/* 每个请求按 id 记账,回包必定派发一次;「是否还是最新一帧」用第二参交给调用方判断。
旧写法只留一个 IKWjob 槽:live 在飞时被 runSolve 插队,那一发的回调就彻底蒸发,
而它是唯一给 liveBusy 复位的地方——一次竞态即让实时追踪永久停摆。 */
const IKWjobs = new Map();
function ikWorker() {
if (IKW) return IKW;
if (IKW === false) return null;
try {
let src = null;
for (const sc of document.querySelectorAll('script')) {
if (sc.textContent.indexOf('六轴手臂逆运动学求解器') >= 0) { src = sc.textContent; break; }
}
if (!src) throw new Error('ArmIK script 未找到');
const boot = '\nconst CFGW = ' + JSON.stringify({ joints: JMap, travel: TR, tcp: TCP0 }) + ';\n' +
'const COLW = ' + JSON.stringify(COL) + ';\n' +
'const SW = ArmIK.make({ joints: CFGW.joints, travel: CFGW.travel, tcp: CFGW.tcp, collide: COLW });\n' +
'onmessage = e => { const d = e.data; let r;\n' +
' try { r = SW[d.op].apply(SW, d.args); } catch (err) { r = { __err: String(err && err.message || err) }; }\n' +
' postMessage({ id: d.id, r: r }); };\n';
/* Blob URL 只在构造 Worker 时被读取,构造后立即回收(失败路径同样不留常驻 Blob) */
const u = URL.createObjectURL(new Blob([src + boot], { type: 'application/javascript' }));
let w;
try { w = new Worker(u); } finally { URL.revokeObjectURL(u); }
w.onmessage = e => {
const job = IKWjobs.get(e.data.id);
if (!job) return; // 已结清过(正常不会发生)
IKWjobs.delete(e.data.id);
job(e.data.r, e.data.id === IKWlast); // 过期请求也派发,只是标为不新鲜
};
w.onerror = () => { // Worker 挂了:把在飞的账全部结清
IKW = false;
const pend = Array.from(IKWjobs.values()); IKWjobs.clear();
for (const j of pend) j(null, false);
};
IKW = w;
} catch (err) { IKW = false; }
return IKW || null;
}
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)) {
@@ -376,37 +569,64 @@ function runSolve() {
refreshTarget();
badge.textContent = '计算中…'; badge.className = 'badge wait';
setTimeout(() => {
const opt = { tool: lastTool, step: +$('ikStep').value || 0.25, ref: curRef(), refW: REF_W };
if (poseOn) opt.pose = { R: Rsolver() };
const res = S.solveTo(TGT, opt);
const opt = { tool: lastTool, step: +$('ikStep').value || 0.25, ref: curRef() };
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
? (poseOn ? '闭式全域扫描 · 位置+姿态联合 · 冗余维精确覆盖' : '闭式全域扫描 · 冗余维精确覆盖')
: (poseOn ? '超可达位姿空间 → 阻尼最小二乘最近位姿(位置 mm + 姿态联合)'
: '超工作空间 → 阻尼最小二乘最近位姿'));
? (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);
});
}, 0);
}
/* 实时追踪:拖动过程中同步求解、立即应用(无动画,跟在手后面)。
求解是同步的,同一时刻只跑一个;拖动中的后续 pointermove 只记待处理标志,
当前求解结束后用最新 TGT 再解一次,避免事件排队越拖越卡。 */
/* 实时追踪:拖动过程中即时求解、立即应用(无动画,跟在手后面)。
求解在 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;
const opt = { tool: lastTool, step: +$('ikStep').value || 0.25, ref: curRef(), refW: REF_W };
if (poseOn) opt.pose = { R: Rsolver() };
const res = S.solveTo(TGT, opt);
renderResult(res, res.reachable
? (poseOn ? '实时追踪 · 闭式全域扫描 · 位置+姿态联合 · 冗余维精确覆盖'
: '实时追踪 · 闭式全域扫描 · 冗余维精确覆盖')
: (poseOn ? '实时追踪 · 超可达位姿空间(阻尼最小二乘最近位姿)'
: '实时追踪 · 超工作空间(阻尼最小二乘最近位姿)'));
if (res.q) setJoints(res.q, false);
} finally { liveBusy = false; }
if (livePending) { livePending = false; runSolveLive(); }
const opt = { tool: lastTool, step: +$('ikStep').value || 0.25, ref: curRef(), warm: curRef() };
if (poseOn) { opt.pose = { R: Rsolver() }; opt.priority = prioVal(); if (opt.priority === 'mix') opt.mixW = mixWVal(); }
solveAsync('solveTo', [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 ? '实时追踪 · 超可达位姿空间(阻尼最小二乘最近位姿)'
: '实时追踪 · 超工作空间(阻尼最小二乘最近位姿)'))));
if (res.q) setJoints(res.q, false);
} finally { settle(fresh); }
});
} catch (err) { settle(false); }
}
/* ---------- 6. 目标拖动 / 拾取 ---------- */
@@ -521,7 +741,8 @@ cv.addEventListener('pointermove', e => {
if (mode === 'rt' && dragged) runSolveLive(); // 实时追踪:拖动过程中即算即动
});
addEventListener('pointerup', () => { camSnap = null; });
cv.addEventListener('pointerup', e => {
/* 挂 window 而非画布:在画布外松手也要结束抓取,否则 grab 残留 → 光挪鼠标就拖着目标走 */
addEventListener('pointerup', e => {
if (!grab) return;
if (grab.pick) { // 视图拾取:射线打到当前高度的水平面
grab = null; justPicked = true;
@@ -559,6 +780,19 @@ function syncInputs() {
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');
@@ -621,37 +855,141 @@ function setView(v, keepPose) {
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-09-30 00:35', sig: '5153f522',
fk: { n: 5000, err: 5.749691446267182e-13 },
ik: { n: 200, hit: 200, tried: 200, err: 9.94353722747741e-13 },
pose: { n: 400, hit: 398, err: 1.7922893504241644, ms: 77.80773925 },
ws: { n: 400000, x: [-378, 415], y: [-388, 642], z: [94, 1125], 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() {
const st = S.selfTest(600, 6);
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));
/* 启动不再做任何重计算:自检数值全部取自预置结果,只用指纹复核是否仍适用。
「结构前提」仍取运行时 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>';
}
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>' +
'<i>姿态 IK 校验</i><span class="' + (st.poseHit >= st.poseTried * 0.9 ? 'good' : 'bad') + '">' +
st.poseHit + '/' + st.poseTried + ' 命中 · 最大 ' + st.poseErr.toExponential(1) +
'° · ' + st.poseMs.toFixed(0) + ' ms/次</span>' +
'<i>姿态结构</i><span>J5∩J6 垂直相交于腕心;θ6 为绕工具轴滚转,不影响 TCP 位置</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(); });
}
/* 打包自检:确认所有依赖都在本文件内(便于单文件分发,防止被拆开后才出问题) */
const ext = document.querySelectorAll(
'script[src],link[href],img[src],iframe[src],source[src],embed[src],object[data],use[href]').length;
@@ -671,8 +1009,10 @@ function boot() {
'<i>外部引用</i>' + ok(ext === 0) +
(ext === 0 ? '0 个(script/link/img/iframe 等全类型已查)' : ext + ' 个 —— 已破坏自包含!') + '</span>';
syncInputs(); refreshTarget();
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 + '"]')) {
@@ -682,6 +1022,7 @@ function boot() {
boot();
window.IK = { solver: S, setTarget: p => { TGT = p.slice(); syncInputs(); refreshTarget(); },
clampAxis: clampAxis, refreshJointLabels: refreshJointLabelsLazy,
getTarget: () => TGT.slice(), run: runSolve, TCP0: TCP0, LIM: LIM,
setView: setView, getView: () => VIEW,
setPose: (az, el, roll, solve) => {
@@ -689,6 +1030,15 @@ window.IK = { solver: S, setTarget: p => { TGT = p.slice(); syncInputs(); refres
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) */
+33975 -284
View File
File diff suppressed because one or more lines are too long
+1 -1
View File
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long