feat(arm): 建模代码拆成每件一模块,查看器新增工程图与装配步骤页
- src/parts/p01~p09 + src/part_tools.py:几何、件号/配色/说明与装配帧归属随各自件文件走,arm_model.py 只留装配流水线并对件级符号做只读转发;P04_CH / P07_CH / P06_WIN_XZ / FENCE_OPEN_CUT 这类开关必须写所在件文件,写 arm_model.X 不生效,A/B 判据为翻转后体积变化
- docs/sheet_{exploded,front,side,joint}.svg + draw_sheet.py:四张工程图进查看器页签,支持图层开关、件号高亮与 BOM/3D 联动跳转
- travel/travel_detail/collide/bom/screws 随最新几何重生成;补线束走线、拆卸定位点与 P04 承压面剖切验证图
- .gitignore 排除 .bak/ 与 *.bak_* 源码快照(旧版本 git 历史里已有)
This commit is contained in:
+306
-14
@@ -388,22 +388,45 @@ function make(cfg) {
|
||||
const side = dir > 0 ? 'hi' : 'lo';
|
||||
const hard = HARD[id] && HARD[id][side];
|
||||
const hi0 = dir > 0 ? LIM[id].hi : LIM[id].lo;
|
||||
if (hard) return hi0; // 硬边:配置无关,静态值即真值
|
||||
let hi = sign * (LIM[id].cont ? 360 : Math.min(MOTOR || 180, 180)); // 连续轴(J1/J3/J6)无机械限位:放到 ±360°,不受 MOTOR 封顶
|
||||
/* 连续轴的搜索端点取 ±180:转过的角度以 360 为周期,360° 与 0° 是同一个位姿,
|
||||
拿它当端点探一次必然"安全",二分会把区间内真实存在的撞点整段跳过(实测 J3 在
|
||||
J4=-17.875 的折肘位形下首撞 148°,而旧口径直接返回 360)。 */
|
||||
let hi = sign * (LIM[id].cont ? 180 : Math.min(MOTOR || 180, 180));
|
||||
/* 相邻打印件对的 1D 自由区间(adj 表,精确 CAD 实测、与姿态无关)封顶:
|
||||
典型漏判场景——J4=-100 时 J5+ 被 P07×P08 卡在 96°,而该对落在静态扫描的
|
||||
视野外(零位时立柱先把 J5 卡在 34°),动态胶囊模型也看不见相邻对。 */
|
||||
const adj = ADJ[id];
|
||||
if (adj && dir > 0) hi = Math.min(hi, adj[1]);
|
||||
if (adj && dir < 0) hi = Math.max(hi, adj[0]);
|
||||
/* 硬边侧(静态限位由相邻打印件对决定、与姿态无关,胶囊模型看不见那一对):
|
||||
静态值只当区间上限,区间内仍要按当前姿态二分——否则折肘位形下动态检查被整个
|
||||
跳过,滑块放到静态 -93°,而实测 J5 在 J3=-180° 时 -28° 末端法兰已撞立柱下段。 */
|
||||
if (hard && Math.abs(hi0) < Math.abs(hi)) hi = hi0;
|
||||
if (hi === q[k]) return q[k];
|
||||
const at = v => { const qq = q.slice(); qq[k] = v; return collideFree(qq); };
|
||||
if (!at(q[k])) return q[k]; // 当前姿已干涉:不放大
|
||||
if (at(hi)) return hi;
|
||||
let lo = q[k];
|
||||
while (Math.abs(hi - lo) > tol) {
|
||||
const mid = 0.5 * (lo + hi);
|
||||
if (at(mid)) lo = mid; else hi = mid;
|
||||
if (!LIM[id].cont) { // 限位轴:折过去就是折过去,区间内单峰,二分即可
|
||||
if (at(hi)) return hi;
|
||||
while (Math.abs(hi - lo) > tol) {
|
||||
const mid = 0.5 * (lo + hi);
|
||||
if (at(mid)) lo = mid; else hi = mid;
|
||||
}
|
||||
return lo;
|
||||
}
|
||||
/* 连续轴(J1/J3/J6):整周内可行集不连续——末段绕过立柱后会重新变安全,
|
||||
"端点安全"根本不代表中间没撞(实测 J1=-81 位形向 + 方向 180° 端点安全,
|
||||
但 -26° 起撞立柱中段)。所以先按固定步长粗扫出首个干涉角,再在该段内二分。
|
||||
步长 4° 起(区间 360° 时约 40 次 collideFree ≈ 150 ms,只在滑块缓存未命中时发生)。 */
|
||||
const step = (hi > q[k] ? 1 : -1) * Math.max(4, Math.abs(hi - q[k]) / 40);
|
||||
let jam = null;
|
||||
for (let v = lo + step; (hi > q[k] ? v < hi : v > hi); v += step) {
|
||||
if (at(v)) lo = v; else { jam = v; break; }
|
||||
}
|
||||
if (jam === null) { if (at(hi)) return hi; jam = hi; }
|
||||
while (Math.abs(jam - lo) > tol) {
|
||||
const mid = 0.5 * (lo + jam);
|
||||
if (at(mid)) lo = mid; else jam = mid;
|
||||
}
|
||||
return lo;
|
||||
}
|
||||
@@ -1908,6 +1931,277 @@ function make(cfg) {
|
||||
};
|
||||
}
|
||||
|
||||
/* ---------- 6a5. 路径安全路由:端点安全 ≠ 过渡安全 ----------
|
||||
查看器的动画是逐轴线性插值(ik-glue.js animTo,连续轴走短弧),所以"移动路径"
|
||||
就是关节空间里 from→to 的一条直线段。实测把目标球从臂的一侧横拖到另一侧:
|
||||
1300 帧里 870 帧(66.9%)处于穿模状态,主要是 立柱上段×J5 电机模组(189 帧)、
|
||||
立柱中段×J6 电机模组(186 帧)——端点解本身也撞,而 live 拖拽走的 solveTo
|
||||
一个碰撞判定都没做。这里把"这条直线能不能走"当一等判据:
|
||||
1) 候选表按代价升序,取第一个"端点安全 + 直线过渡安全"的解;
|
||||
2) 一个都没有 → 沿最优候选方向二分退回最近的可安全抵达位形(跟手但停在障碍前),标 blocked;
|
||||
3) 当前位形本身已干涉(滑块被拖越界)→ 退回旧的"只看端点"口径,至少把臂解出来。
|
||||
开销:首个候选通常一次就过(1 次端点 + PATH_N-1 次过渡 ≈ 8 次 collideFree);
|
||||
最坏 CAND_MAX 个候选全查 ≈ 60 次,实测在 Worker 里跑,不冻结主线程。 */
|
||||
const PATH_N = 9, CAND_MAX = 24, SHORT_DEG = 6, FINE_DEG = 3;
|
||||
/* 关节空间步长(连续轴按短弧折算后的最大单轴变化量) */
|
||||
function stepDeg(from, to) {
|
||||
let m = 0;
|
||||
for (let i = 0; i < 6; i++) {
|
||||
const d = LIM[ORDER[i]] && LIM[ORDER[i]].cont ? wrap180(to[i] - from[i]) : to[i] - from[i];
|
||||
if (Math.abs(d) > m) m = Math.abs(d);
|
||||
}
|
||||
return m;
|
||||
}
|
||||
function transitPt(from, to, k) {
|
||||
return from.map((v, i) => {
|
||||
const d = LIM[ORDER[i]] && LIM[ORDER[i]].cont ? wrap180(to[i] - v) : to[i] - v;
|
||||
return v + d * k;
|
||||
});
|
||||
}
|
||||
/* 直线段过渡是否全程无干涉(不含端点:起点由调用侧保证,终点已单独查过)。
|
||||
步长 ≤SHORT_DEG° 的跟随帧只查中点:实测拖拽中相邻解的步长多在几度以内,
|
||||
9 点全扫会把 live 从 1~44ms 放大到 441ms/步(≈10×),而这么短的线段
|
||||
两端安全 + 中点安全已经覆盖了实际风险(穿模体是厘米级,不是度级)。 */
|
||||
function pathFree(from, to, n) {
|
||||
n = n || PATH_N;
|
||||
for (let i = 1; i < n; i++) if (!collideFree(transitPt(from, to, i / n))) return false;
|
||||
return true;
|
||||
}
|
||||
function findSafeTarget(cur, from) {
|
||||
if (cur.q && collideFree(cur.q)) return { q: cur.q, res: cur };
|
||||
let best = null, bestCost = Infinity;
|
||||
const n = Math.min((cur.list || []).length, 120);
|
||||
for (let i = 0; i < n; i++) {
|
||||
const s = cur.list[i];
|
||||
if (s && s.q && collideFree(s.q)) {
|
||||
const cst = costOf(s.q, null, from);
|
||||
if (cst < bestCost) {
|
||||
bestCost = cst;
|
||||
best = { q: s.q, res: Object.assign({}, cur, { q: s.q.slice(), cost: s.cost, err: s.err, oerr: s.oerr }) };
|
||||
}
|
||||
}
|
||||
}
|
||||
if (best) return best;
|
||||
const len = (cur.list || []).length;
|
||||
if (len > 120) {
|
||||
const stride = Math.max(1, Math.floor((len - 120) / 40));
|
||||
for (let i = 120; i < len; i += stride) {
|
||||
const s = cur.list[i];
|
||||
if (s && s.q && collideFree(s.q)) {
|
||||
return { q: s.q, res: Object.assign({}, cur, { q: s.q.slice(), cost: s.cost, err: s.err, oerr: s.oerr }) };
|
||||
}
|
||||
}
|
||||
}
|
||||
return null;
|
||||
}
|
||||
function routeFrom(from, cur) {
|
||||
if (!COL || !cur || !cur.q || !from) return cur;
|
||||
if (!collideFree(from)) return cur; // 起点已在干涉里:先把自己拔出来再说
|
||||
const seen = new Set(), cand = [];
|
||||
const push = q => {
|
||||
if (!q || q.length !== 6) return;
|
||||
const key = q.map(v => v.toFixed(2)).join(',');
|
||||
if (seen.has(key)) return;
|
||||
seen.add(key); cand.push(q.slice());
|
||||
};
|
||||
push(cur.q);
|
||||
for (const s of (cur.list || [])) push(s.q);
|
||||
// 优先验证与起点 from 关节距离最近的候选解
|
||||
cand.sort((a, b) => costOf(a, null, from) - costOf(b, null, from));
|
||||
const cands = cand.slice(0, CAND_MAX);
|
||||
let fineLeft = 4; // 细验预算:全通过的候选才会走满 n 次探测
|
||||
for (const q of cands) {
|
||||
if (!collideFree(q)) continue;
|
||||
// 快速二分初筛:若中点干涉直接短路(1 次判定否决碰撞射线),节省大半无谓细验开销
|
||||
if (!collideFree(transitPt(from, q, 0.5))) continue;
|
||||
/* 粗筛(≤13 点)先廉价否掉明显穿模的候选,再用与审计同分辨率的 3° 细验定案 */
|
||||
if (!pathFree(from, q, Math.min(13, pathN(from, q, SHORT_DEG)))) continue;
|
||||
if (fineLeft <= 0) break; // 预算用尽就不接受没细验过的候选(宁可贴边停,不赌)
|
||||
fineLeft--;
|
||||
if (pathFree(from, q, pathN(from, q, FINE_DEG))) {
|
||||
const same = q.every((v, i) => Math.abs(v - cur.q[i]) < 1e-9);
|
||||
return Object.assign({}, cur, { q: q, routed: true, blocked: false, kept: same });
|
||||
}
|
||||
}
|
||||
/* 没有一条直线能干净抵达:沿最优候选(优先端点安全的那个)方向二分退回安全边界,
|
||||
让臂贴着障碍停住,而不是穿过去。 */
|
||||
const safeT = findSafeTarget(cur, from);
|
||||
let bestQ = safeT ? safeT.q : null;
|
||||
if (!bestQ) {
|
||||
for (const q of cands) if (collideFree(q)) { bestQ = q; break; }
|
||||
}
|
||||
if (!bestQ) bestQ = cands[0] || cur.q;
|
||||
let lo = 0, hi = 1; // lo=起点(已确认安全),hi=整跳(上面已证明不干净)
|
||||
/* 先按端点二分找到"射线上的第一个撞点"(便宜:每次 1 个探测),
|
||||
再对 from→q 整段做 3° 细验;细验不过就按 0.6 回缩重验。
|
||||
早先直接用 9 点粗扫当谓词,等于把 19° 的采样间隔当成安全边界,实测漏过一帧。 */
|
||||
while (hi - lo > 0.02) {
|
||||
const mid = 0.5 * (lo + hi);
|
||||
if (collideFree(transitPt(from, bestQ, mid))) lo = mid; else hi = mid;
|
||||
}
|
||||
let q = transitPt(from, bestQ, lo);
|
||||
for (let t = 0; t < 5 && lo > 0.02 && !pathFree(from, q, pathN(from, q, FINE_DEG)); t++) {
|
||||
lo *= 0.6; q = transitPt(from, bestQ, lo);
|
||||
}
|
||||
return Object.assign({}, cur, { q: q, safeT: safeT, routed: true, blocked: lo < 0.98, kept: false });
|
||||
}
|
||||
/* 拖拽/松手统一入口:solveTo + 路径路由(opt.from = 当前位形) */
|
||||
function solveRoute(target, opt) {
|
||||
opt = opt || {};
|
||||
if (opt.from && !opt.ref) opt.ref = opt.from;
|
||||
return routeOrVia(opt.from, target, solveTo(target, opt), opt);
|
||||
}
|
||||
/* 两段式绕行:直线被立柱挡住、但目标本身有安全解时,先解一个"抬高版"的中间位形
|
||||
(目标正上方 VIA_DZ),走直线过去,再从那里落回目标。实测 100 个拖拽步里
|
||||
14 步属于这一类(有安全端点、直线到不了)。只在松手全量求解时用:
|
||||
每多一个候选要多解一次 IK + 两段直线检查,实时拖拽扛不住。 */
|
||||
const VIA_DZ = [260, 520];
|
||||
function routeVia(from, target, opt) {
|
||||
for (const dz of VIA_DZ) {
|
||||
const Tw = [target[0], target[1], target[2] + dz];
|
||||
const rw = solveRoute(Tw, Object.assign({}, opt, { noVia: true, noAround: true }));
|
||||
if (!rw || !rw.q || rw.blocked || rw.unsafe || !collideFree(rw.q)) continue;
|
||||
const r2 = solveRoute(target, Object.assign({}, opt, { from: rw.q.slice(), noVia: true, noAround: true, ref: rw.q }));
|
||||
if (!r2 || !r2.q || r2.blocked || r2.unsafe || !collideFree(r2.q)) continue;
|
||||
/* 中间位形表必须把两段各自的子绕行都接上:from→(rw.via)→rw.q→(r2.via)→q。
|
||||
早先只放 [rw.q] 会把 rw/r2 自己的 via 覆盖掉,于是动画走的那条线并不是校验过的那条,
|
||||
实测松手口径因此在离起点 3° 处穿模 6 帧(立柱中段×末端工具法兰)。 */
|
||||
return Object.assign({}, r2, {
|
||||
via: (rw.via || []).concat([rw.q.slice()], r2.via || []), detourDz: dz, detour: 'via'
|
||||
});
|
||||
}
|
||||
return null;
|
||||
}
|
||||
/* 绕行使者:收臂 → 转 J1 → 再伸展。
|
||||
CRUISE 表由 scratch/find_cruise_poses.js 搜出并校验:那组 J2/J4/J5 组合在
|
||||
J1 整周每 10° 采样下 collideFree 全通过(280 组枚举里 243 组满足,取半径最贴身
|
||||
且收法不同的 4 组)。几何一改必须重跑该脚本,否则巡航位形可能已经会撞。
|
||||
三段分别是:当前位形→原地收臂(J1 不变)、收臂态把 J1 转到目标解的肩角、
|
||||
再伸展到目标解。每段都过 pathFree,段内 J1 按 ≤10° 采样(与搜索口径一致)。 */
|
||||
/* 巡航参考位形:按收缩幅度从浅到深排序。
|
||||
[0, -15, ...] 为轻微避让,[0, -75, ...] 为深折叠。优先选用微调姿态,避免激进蜷缩。 */
|
||||
const CRUISE = [
|
||||
[0, -15, 0, -95, -60, 0], [0, -45, 0, -95, -60, 0],
|
||||
[0, -75, 0, -95, -60, 0], [0, -90, 0, -65, -60, 0]
|
||||
];
|
||||
/* 转 J1 那一段按 5° 网格查表(懒算 + 记忆化) */
|
||||
const CRUISE_OK = CRUISE.map(() => new Map());
|
||||
function cruiseFree(ci, j1) {
|
||||
const key = Math.round(j1 / 5) * 5;
|
||||
const m = CRUISE_OK[ci];
|
||||
let v = m.get(key);
|
||||
if (v === undefined) { const q = CRUISE[ci].slice(); q[0] = key; v = collideFree(q); m.set(key, v); }
|
||||
return v;
|
||||
}
|
||||
function turnFree(ci, j1a, j1b) {
|
||||
const n = Math.max(1, Math.ceil(Math.abs(wrap180(j1b - j1a)) / 5));
|
||||
for (let i = 0; i <= n; i++) if (!cruiseFree(ci, j1a + (wrap180(j1b - j1a)) * i / n)) return false;
|
||||
return true;
|
||||
}
|
||||
/* 采样点数按线段在关节空间的步长自适应:每 SHORT_DEG(6°) 一个点,最少 3 点、最多 40 点 */
|
||||
function pathN(from, to, deg) {
|
||||
return Math.max(3, Math.min(40, Math.round(stepDeg(from, to) / (deg || SHORT_DEG)) + 1));
|
||||
}
|
||||
/* 轻量偏移量集合:用于避开障碍物边缘(抬肩/微收肘),避免整臂蜷缩 */
|
||||
const GENTLE_OFFSETS = [
|
||||
[-10, 0], [-20, 0], [-15, -10], [0, -15], [-20, -15],
|
||||
[10, 0], [20, 0], [-30, 0], [-10, 10]
|
||||
];
|
||||
/* 智能平滑协同避障规划(模拟人类避障直觉):
|
||||
1) 微调避让:针对贴边擦碰,在直线弦中点微调 J2/J4,各轴保持平滑协同,绝不缩臂;
|
||||
2) 协同单中间点 + 二分松弛:J1 全程平滑旋转,中间位形只做必要的外展/微收,并向直线插值松弛;
|
||||
3) 模拟人类自然避障(回退一点 -> 绕行过去 -> 伸展到位):
|
||||
当手臂靠近立柱或跨度大时,在起点方位先原位/微转收臂回退,跨过立柱后再展开到位;
|
||||
4) 保底安全回退:极端奇异位形下确保无碰撞。 */
|
||||
function routeAroundPlan(from, cur, quick, safeT) {
|
||||
let qT = safeT ? safeT.q : null, resT = safeT ? safeT.res : cur;
|
||||
if (!qT) {
|
||||
const found = findSafeTarget(cur, from);
|
||||
if (found) { qT = found.q; resT = found.res; }
|
||||
}
|
||||
if (!qT) return null;
|
||||
|
||||
const chordMid = transitPt(from, qT, 0.5);
|
||||
const dj1 = wrap180(qT[0] - from[0]);
|
||||
const j1Mid = from[0] + dj1 * 0.5;
|
||||
|
||||
// 阶段 1:微调避障(针对贴边穿模:直接在弦中点微调 J2/J4,完全不蜷缩手臂)
|
||||
for (const [dj2, dj4] of GENTLE_OFFSETS) {
|
||||
const testQ = chordMid.slice();
|
||||
testQ[1] += dj2; testQ[3] += dj4;
|
||||
if (!collideFree(testQ)) continue;
|
||||
const nA = Math.min(8, pathN(from, testQ, FINE_DEG));
|
||||
const nB = Math.min(8, pathN(testQ, qT, FINE_DEG));
|
||||
if (pathFree(from, testQ, nA) && pathFree(testQ, qT, nB)) {
|
||||
return { res: resT, via: [testQ], detour: 'gentle' };
|
||||
}
|
||||
}
|
||||
|
||||
// 阶段 2:连续协同单中间点避障(J1 保持平滑转动,避障姿态向自然弦二分松弛)
|
||||
const use = quick ? [0, 1, 2] : [0, 1, 2, 3];
|
||||
for (const ci of use) {
|
||||
const c = CRUISE[ci];
|
||||
const qMidSafe = c.slice();
|
||||
qMidSafe[0] = j1Mid;
|
||||
if (!collideFree(qMidSafe)) continue;
|
||||
const nA = Math.min(8, pathN(from, qMidSafe, FINE_DEG));
|
||||
const nB = Math.min(8, pathN(qMidSafe, qT, FINE_DEG));
|
||||
if (pathFree(from, qMidSafe, nA) && pathFree(qMidSafe, qT, nB)) {
|
||||
// 二分松弛:寻找能安全通过的最小偏离量(靠近弦中点)
|
||||
let lo = 0, hi = 1;
|
||||
for (let it = 0; it < 4; it++) {
|
||||
const midA = (lo + hi) * 0.5;
|
||||
const testQ = transitPt(chordMid, qMidSafe, midA);
|
||||
if (collideFree(testQ) && pathFree(from, testQ, 6) && pathFree(testQ, qT, 6)) {
|
||||
hi = midA;
|
||||
} else {
|
||||
lo = midA;
|
||||
}
|
||||
}
|
||||
const optMid = transitPt(chordMid, qMidSafe, hi);
|
||||
return { res: resT, via: [optMid], detour: 'coordinated' };
|
||||
}
|
||||
}
|
||||
|
||||
// 阶段 3:模拟人类自然避障(回退一点 -> 绕行过去 -> 伸展到位)
|
||||
// 用户需求:"模拟人类,哪怕中间有东西挡了,应该回退一点然后再移动过去,而不是说挡着了就不计算了"
|
||||
// 第一中间点在起点方位(微转 frac)处收臂回退,第二中间点在避开立柱后跨过,再伸展入目标
|
||||
for (const ci of use) {
|
||||
const c = CRUISE[ci];
|
||||
for (const frac of [0.08, 0.0, 0.15, 0.25]) {
|
||||
const w1 = c.slice(); w1[0] = from[0] + dj1 * frac;
|
||||
const w2 = c.slice(); w2[0] = qT[0] - dj1 * frac;
|
||||
if (!collideFree(w1) || !collideFree(w2)) continue;
|
||||
const n1 = Math.min(8, pathN(from, w1, FINE_DEG));
|
||||
const n2 = Math.min(8, pathN(w1, w2, FINE_DEG));
|
||||
const n3 = Math.min(8, pathN(w2, qT, FINE_DEG));
|
||||
if (pathFree(from, w1, n1) && pathFree(w1, w2, n2) && pathFree(w2, qT, n3)) {
|
||||
return { res: resT, via: [w1, w2], detour: 'retract_pass' };
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 阶段 4:保底三段式(仅在所有平滑协同均被阻挡的极端奇异位形启用)
|
||||
for (const ci of use) {
|
||||
const c = CRUISE[ci];
|
||||
const fold = c.slice(); fold[0] = from[0];
|
||||
const turn = c.slice(); turn[0] = qT[0];
|
||||
const ok = pathFree(from, fold, Math.min(8, pathN(from, fold, FINE_DEG))) &&
|
||||
turnFree(ci, fold[0], turn[0]) &&
|
||||
pathFree(turn, qT, Math.min(8, pathN(turn, qT, FINE_DEG)));
|
||||
if (ok) return { res: resT, via: [fold, turn], detour: 'around' };
|
||||
}
|
||||
return null;
|
||||
}
|
||||
function routeOrVia(from, target, cur, opt) {
|
||||
const r = routeFrom(from, cur);
|
||||
if (!r.blocked || !from) return r;
|
||||
const around = opt.noAround ? null : routeAroundPlan(from, cur, opt.quick, r.safeT);
|
||||
if (around) return Object.assign({}, around.res, { via: around.via, routed: true, blocked: false, detour: around.detour || 'around' });
|
||||
if (opt.noVia) return r;
|
||||
return routeVia(from, target, opt) || r;
|
||||
}
|
||||
|
||||
/* ---------- 6b. 动态限位求解:静态解 + 贴边轴放宽重解 ----------
|
||||
静态限位来自"其它轴全零"的单轴扫描,对非相邻碰撞对(典型:腕/法兰撞
|
||||
固定立柱)系统性过紧——实测 J2 正端随 J1 从 +9.75°(垂臂零位)放到
|
||||
@@ -1916,18 +2210,15 @@ function make(cfg) {
|
||||
且总关节角不升才采用,否则原样返回(最坏即静态行为,无回退风险)。 */
|
||||
function solveDyn(target, opt) {
|
||||
opt = opt || {};
|
||||
if (opt.from && !opt.ref) opt.ref = opt.from;
|
||||
let cur = solveTo(target, opt);
|
||||
if (!COL || opt.noDyn || !cur.q) return cur;
|
||||
/* 静态限位盒不保证组合姿态无干涉(单轴扫描各轴独立取限位,组合折叠会撞——
|
||||
sweep_v2 的组合扫描只记录不收紧滑块)。这里对最终解做全姿态碰撞校验:
|
||||
撞了就沿候选表(按代价升序)找第一个安全的,代价容忍 1.5 倍 + 20°。 */
|
||||
撞了就沿候选表找安全的解,优先采用最贴近起点的安全位形。 */
|
||||
if (!collideFree(cur.q)) {
|
||||
let pick = null;
|
||||
for (let i = 0; i < cur.list.length && i < 40; i++) {
|
||||
const s = cur.list[i];
|
||||
if (s.cost <= cur.cost * 1.5 + 20 && collideFree(s.q)) { pick = s; break; }
|
||||
}
|
||||
if (pick) cur = Object.assign({}, cur, { q: pick.q.slice(), cost: pick.cost, swaped: true });
|
||||
const safeT = findSafeTarget(cur, opt.from);
|
||||
if (safeT) cur = Object.assign({}, safeT.res, { swaped: true });
|
||||
else cur = Object.assign({}, cur, { unsafe: true });
|
||||
}
|
||||
for (let pass = 0; pass < 2; pass++) {
|
||||
@@ -1968,7 +2259,7 @@ function make(cfg) {
|
||||
if (!picked) break;
|
||||
cur = Object.assign({}, picked, { dynAxis: axis, dynBound: bound });
|
||||
}
|
||||
return cur;
|
||||
return opt.from ? routeOrVia(opt.from, target, cur, opt) : cur;
|
||||
}
|
||||
|
||||
/* ---------- 7. 自检:闭式 FK ↔ 矩阵链;逆解重构;姿态重构 ---------- */
|
||||
@@ -2022,6 +2313,7 @@ function make(cfg) {
|
||||
}
|
||||
|
||||
return { fk: fk, fkChain: fkChain, solve: solve, solveTo: solveTo, solveWarm: solveWarm, solveDyn: solveDyn,
|
||||
solveRoute: solveRoute, routeFrom: routeFrom, transitPt: transitPt,
|
||||
mixSolve: mixSolve, closest: closest, closest6: closest6, closestOri: closestOri, orient: orient,
|
||||
branch: branch, selfTest: selfTest, poseSelfTest: poseSelfTest,
|
||||
collideFree: collideFree, collideDetail: collideDetail,
|
||||
|
||||
+76
-29
@@ -269,24 +269,43 @@ function refreshJointLabelsLazy(q) {
|
||||
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 && !t.cont) {
|
||||
if (COL && D[id]) {
|
||||
/* 边界由几何说话:机械区间(adj 实测)之内,再按当前姿态二分胶囊碰撞。
|
||||
lo 侧一律收紧;hi 侧只有"硬边"(相邻件自碰撞,配置无关)才受静态值封顶,
|
||||
可放宽侧(撞固定立柱那类)由模型说话——动态边界本就可以远超静态 hi。 */
|
||||
const lo = Math.max(t.lo, S.dynLimit(id, q, -1, 1.0));
|
||||
可放宽侧(撞固定立柱那类)由模型说话——动态边界本就可以远超静态 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, S.dynLimit(id, q, 1, 1.0))
|
||||
: S.dynLimit(id, q, 1, 1.0);
|
||||
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; // 与静态不同(宽或窄)都标记
|
||||
el.innerHTML = (dyn ? '<b style="color:var(--warn)">' : '') +
|
||||
lo.toFixed(0) + '\u00B0\u2026' + hi.toFixed(0) + '\u00B0' + (dyn ? ' \u25B2</b>' : '');
|
||||
/* 连续轴没收紧时显示"整周可用",收紧了就换成区间:同一个 <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.lo + '\u2026' + t.hi +
|
||||
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' : '');
|
||||
@@ -298,8 +317,8 @@ function refreshJointLabels(q) {
|
||||
|
||||
/* ---------- 3. 关节写入(同步滑块与标签)---------- */
|
||||
let anim = null;
|
||||
function setJoints(q, doAnim) {
|
||||
if (doAnim) { animTo(q, 620); return; }
|
||||
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];
|
||||
@@ -311,7 +330,7 @@ function setJoints(q, doAnim) {
|
||||
refreshTarget();
|
||||
if (COL) refreshJointLabelsLazy(ORDER.map(id => ang[id]));
|
||||
}
|
||||
function animTo(q, ms) {
|
||||
function animTo(q, ms, via) {
|
||||
if (anim) { anim = null; }
|
||||
const from = ORDER.map(id => ang[id]);
|
||||
/* 连续轴(J1 肩抬举 / J3 大臂自转 / J6 腕自转)走「短弧」。
|
||||
@@ -319,20 +338,31 @@ function animTo(q, ms) {
|
||||
若按数值直接线性插值,+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]);
|
||||
同一物理角,视觉零跳变,同时保证滑块/关节表数值与求解结果一致。
|
||||
via:绕行中间位形(solveRoute 判直线被挡时给的两段式路径),逐段同样按短弧折算,
|
||||
每段各 ms 毫秒——渲染走的段与碰撞校验走的段是同一批,不存在"校验过了但动画穿模"。 */
|
||||
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();
|
||||
anim = { from: from, to: to, t0: performance.now(), ms: ms };
|
||||
const total = ms * segs.length;
|
||||
anim = { from: from, to: segs[segs.length - 1], t0: performance.now(), ms: total };
|
||||
const tick = () => {
|
||||
if (!anim) return;
|
||||
const k = Math.min(1, (performance.now() - anim.t0) / anim.ms);
|
||||
const el = performance.now() - anim.t0;
|
||||
const si = Math.min(segs.length - 1, Math.floor(el / ms));
|
||||
const k = Math.min(1, (el - si * ms) / ms);
|
||||
const e = k * k * (3 - 2 * k);
|
||||
const a0 = si === 0 ? from : segs[si - 1], a1 = segs[si];
|
||||
for (let i = 0; i < 6; i++) {
|
||||
const id = ORDER[i], a = anim.from[i] + (anim.to[i] - anim.from[i]) * e;
|
||||
const id = ORDER[i], a = a0[i] + (a1[i] - a0[i]) * e;
|
||||
ang[id] = a;
|
||||
const sl = $('s' + id), v = $('v' + id);
|
||||
if (sl) sl.value = a;
|
||||
@@ -459,6 +489,13 @@ function renderResult(res, src) {
|
||||
const mixMid = poseOn && res.priority === 'mix' && !posOk && !oriOk;
|
||||
badge.textContent = res.unsafe
|
||||
? '位姿与底座/立柱近似干涉(目标可能不可达或位于立柱区内)'
|
||||
: res.blocked
|
||||
? '路径被立柱挡住 · 已停在边界(目标差 ' + dsc.toFixed(1) + ' mm)'
|
||||
: res.detour
|
||||
? (res.detour === 'retract_pass' ? '避障到达 · 模拟人类回退绕过立柱'
|
||||
: (res.detour === 'gentle' ? '避障到达 · 微调协同避开立柱'
|
||||
: (res.detour === 'coordinated' ? '避障到达 · 平滑协同跨越立柱'
|
||||
: '绕行立柱后到达')))
|
||||
: (posOk && oriOk) ? '已到达'
|
||||
: oriPrio ? '姿态到达·位置最近 ' + dsc.toFixed(1) + ' mm'
|
||||
: mixMid ? '混合折中 · 位置差 ' + dsc.toFixed(1) + ' mm · 姿态残差 ' + dori.toFixed(2) + '\u00B0'
|
||||
@@ -569,7 +606,8 @@ function runSolve() {
|
||||
refreshTarget();
|
||||
badge.textContent = '计算中…'; badge.className = 'badge wait';
|
||||
setTimeout(() => {
|
||||
const opt = { tool: lastTool, step: +$('ikStep').value || 0.25, ref: curRef() };
|
||||
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; // 已被更新的一发取代:交给那一发渲染
|
||||
@@ -582,7 +620,7 @@ function runSolve() {
|
||||
: (res.note ? res.note
|
||||
: (poseOn ? '超可达位姿空间 → 阻尼最小二乘最近位姿(位置 mm + 姿态联合)'
|
||||
: '超工作空间 → 阻尼最小二乘最近位置')));
|
||||
if (res.q) setJoints(res.q, true);
|
||||
if (res.q) setJoints(res.q, true, res.via);
|
||||
});
|
||||
}, 0);
|
||||
}
|
||||
@@ -604,9 +642,13 @@ function runSolveLive() {
|
||||
};
|
||||
try {
|
||||
lastTool = +$('ikTool').value || 0;
|
||||
const opt = { tool: lastTool, step: +$('ikStep').value || 0.25, ref: curRef(), warm: curRef() };
|
||||
/* 实时追踪原来走 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('solveTo', [TGT.slice(), opt], (res, fresh) => { // Worker 后台:全量回落也不冻结主线程
|
||||
solveAsync('solveRoute', [TGT.slice(), opt], (res, fresh) => { // Worker 后台:全量回落也不冻结主线程
|
||||
try {
|
||||
if (!res || !fresh) return;
|
||||
renderResult(res, res.warm
|
||||
@@ -623,7 +665,9 @@ function runSolveLive() {
|
||||
: (res.note ? '实时追踪 · ' + res.note
|
||||
: (poseOn ? '实时追踪 · 超可达位姿空间(阻尼最小二乘最近位姿)'
|
||||
: '实时追踪 · 超工作空间(阻尼最小二乘最近位姿)'))));
|
||||
if (res.q) setJoints(res.q, false);
|
||||
/* 无绕行时立即落位(跟手);一旦给出绕行中间位形,就必须分段动画——
|
||||
直接跳到 res.q 等于把"收臂→转 J1→伸展"三段压成一条直线,正是要避免的穿模。 */
|
||||
if (res.q) setJoints(res.q, !!(res.via && res.via.length), res.via, 300);
|
||||
} finally { settle(fresh); }
|
||||
});
|
||||
} catch (err) { settle(false); }
|
||||
@@ -835,7 +879,9 @@ 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';
|
||||
@@ -849,6 +895,7 @@ function setView(v, keepPose) {
|
||||
} 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();
|
||||
}
|
||||
@@ -861,11 +908,11 @@ document.querySelectorAll('.tab[data-view]').forEach(b =>
|
||||
与当前代码不符时会标红提示重算。
|
||||
重新生成: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 }
|
||||
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) */
|
||||
|
||||
+23010
-22203
File diff suppressed because one or more lines are too long
+1
-1
File diff suppressed because one or more lines are too long
+1
-1
File diff suppressed because one or more lines are too long
+23010
-22203
File diff suppressed because one or more lines are too long
Reference in New Issue
Block a user