diff --git a/.gitignore b/.gitignore index 90546d9..1aed878 100644 --- a/.gitignore +++ b/.gitignore @@ -44,3 +44,4 @@ docs/shots/ # 定位图/ 只是"给用户看的核验图落点"，一次性、会反复清空； # 关卡比对的权威物是 tests/baselines/（md5 名单 + flaky 参考像素），不在这里。 定位图/ +ml/data/ diff --git a/ml/gen/cert.cjs b/ml/gen/cert.cjs new file mode 100644 index 0000000..eec3012 --- /dev/null +++ b/ml/gen/cert.cjs @@ -0,0 +1,43 @@ +/* 距离下界认证（2026-10-17，方案 §8.3 / §4.4） + * 关节直线段 a→b 安全 ⇔ 对每个组对 (gA,gB)（gB ≥ gA+2）： + * gap_ab(a) + gap_ab(b) > Σ_{k=gA}^{gB-1} r_k·|Δθ_k| + * gap 取 S.gapMatrix（距离 − need，含 margin），r_k 为 S.leverBounds 的每轴杠杆上界（mm/°）。 + * 不满足就二分，子段 < MIN_DEG 仍不过则判不安全。连续轴按 unwrapTo 口径展开后再插值。 */ +'use strict'; +module.exports = function makeCert(S, opt) { + opt = opt || {}; + const CAP = opt.cap || 40, MIN_DEG = opt.minDeg || 0.002; + const R = opt.lever || S.leverBounds(4000); + const L = S.LIM, O = S.ORDER, w = S.wrap180; + const memo = new Map(); + const key = q => q.map(v => v.toFixed(5)).join(','); + let queries = 0; + const gm = q => { const k = key(q); let M = memo.get(k); if (!M) { M = S.gapMatrix(q, CAP); queries++; if (memo.size > 50000) memo.clear(); memo.set(k, M); } return M; }; + const minGap = M => { let m = Infinity; for (let a = 0; a < 7; a++) for (let b = a + 2; b < 7; b++) m = Math.min(m, M[a * 7 + b]); return m; }; + const inBox = q => O.every((id, i) => L[id].cont || (q[i] >= L[id].lo - 1e-6 && q[i] <= L[id].hi + 1e-6)); + function seg(a, b, depth) { + const Ma = gm(a), Mb = gm(b); + const dq = a.map((v, i) => Math.abs(b[i] - v)); + let ok = true; + for (let ga = 0; ga < 7 && ok; ga++) for (let gb = ga + 2; gb < 7; gb++) { + const lhs = Ma[ga * 7 + gb] + Mb[ga * 7 + gb]; + let rhs = 0; for (let k = ga; k < gb; k++) rhs += R[k] * dq[k]; + if (!(lhs > rhs)) { ok = false; break; } + } + if (ok) return true; + if (minGap(Ma) <= 0 || minGap(Mb) <= 0) return false; + if (Math.max.apply(null, dq) < MIN_DEG || depth > 40) return false; + const m = a.map((v, i) => 0.5 * (v + b[i])); + return seg(a, m, depth + 1) && seg(m, b, depth + 1); + } + /* 整条折线：返回 {ok, queries, minGap, failAt} */ + function path(pts) { + const q0 = queries; let mg = Infinity; + const P = [pts[0].slice()]; + for (let i = 1; i < pts.length; i++) P.push(P[i - 1].map((v, k) => L[O[k]].cont ? v + w(pts[i][k] - v) : pts[i][k])); + for (let i = 0; i < P.length; i++) { if (!inBox(P[i])) return { ok: false, queries: queries - q0, minGap: mg, failAt: i, why: 'box' }; mg = Math.min(mg, minGap(gm(P[i]))); } + for (let i = 1; i < P.length; i++) if (!seg(P[i - 1], P[i], 0)) return { ok: false, queries: queries - q0, minGap: mg, failAt: i, why: 'cert' }; + return { ok: true, queries: queries - q0, minGap: mg, unwrapped: P }; + } + return { seg: (a, b) => seg(a, b, 0), path, lever: R, gapMin: q => minGap(gm(q)) }; +}; diff --git a/ml/gen/consistency.cjs b/ml/gen/consistency.cjs new file mode 100644 index 0000000..58c991b --- /dev/null +++ b/ml/gen/consistency.cjs @@ -0,0 +1,45 @@ +/* M1 门禁：老师一致性（方案 §4.7 / R1） + * node ml/gen/consistency.cjs [N=100] [seed=4242] + * 取 B/C 类（需要绕行）任务，起点不变、目标位置扰动 ±5 mm 造 4 个邻居，与原任务老师路径比较： + * 两条路径按巡航时间 τ 各重采样 32 点，逐点取各轴最大差；最大偏差 < 30° 视为同一绕行同伦类。 + * 门禁：一致率 ≥ 90%。另报告同一任务重复求解的确定性。 */ +'use strict'; +const N = +process.argv[2] || 100, SEED = +process.argv[3] || 4242; +const { S, ArmIK } = require('../../tests/route_bench_setup.cjs')(); +const sm = require('./sampler.cjs')(S, SEED); +const teach = require('./teacher.cjs')(S, ArmIK); +const { VMAX } = require('./timelaw.cjs'); +function resample(P, n) { + const seg = []; for (let i = 1; i < P.length; i++) { let m = 0; for (let j = 0; j < 6; j++) m = Math.max(m, Math.abs(P[i][j] - P[i - 1][j]) / VMAX[j]); seg.push(m); } + const tot = seg.reduce((a, b) => a + b, 0) || 1; const out = []; + for (let k = 0; k < n; k++) { let tau = tot * k / (n - 1), i = 0; while (i < seg.length - 1 && tau > seg[i]) { tau -= seg[i]; i++; } + const u = seg[i] > 0 ? Math.min(1, tau / seg[i]) : 1; out.push(P[i].map((v, j) => v + (P[i + 1][j] - v) * u)); } + return out; +} +const devAx = (A, B) => { const a = resample(A, 32), b = resample(B, 32); const m = [0, 0, 0, 0, 0, 0]; for (let k = 0; k < 32; k++) for (let j = 0; j < 6; j++) m[j] = Math.max(m[j], Math.abs(a[k][j] - b[k][j])); return m; }; +const dev = (A, B) => Math.max(...devAx(A, B)); +const axisHist = [0, 0, 0, 0, 0, 0]; let sameNoJ6 = 0; +let tasks = 0, pairs = 0, same = 0, detSame = 0, detN = 0; const devs = [], byCat = {}; +while (tasks < N) { + const t = sm.sample(); if (t.cat !== 'B' && t.cat !== 'C') continue; + const base = teach(t); if (!base.ok || !base.reached) continue; + tasks++; + const again = teach(t); if (again.ok) { detN++; if (dev(base.pts, again.pts) < 1e-6) detSame++; } + for (let k = 0; k < 4; k++) { + const u = [sm.rnd() - 0.5, sm.rnd() - 0.5, sm.rnd() - 0.5], n = Math.hypot(...u); + const T2 = t.T.map((v, i) => v + 5 * u[i] / n); + const r = teach(Object.assign({}, t, { T: T2 })); + if (!r.ok || !r.reached) continue; + const da = devAx(base.pts, r.pts), d = Math.max(...da); devs.push(d); pairs++; + if (Math.max(...da.slice(0, 5)) < 30) sameNoJ6++; + if (d >= 30) axisHist[da.indexOf(d)]++; + const c = byCat[t.cat] = byCat[t.cat] || { pairs: 0, same: 0 }; c.pairs++; + if (d < 30) { same++; c.same++; } + } + if (tasks % 10 === 0) process.stderr.write(`${tasks}/${N}\n`); +} +devs.sort((a, b) => a - b); +console.log(JSON.stringify({ tasks, pairs, consistentRate: +(same / pairs).toFixed(3), gate: same / pairs >= 0.9 ? 'PASS' : 'FAIL', + devP50: +devs[devs.length >> 1].toFixed(1), devP90: +devs[Math.floor(devs.length * 0.9)].toFixed(1), + byCat: Object.fromEntries(Object.entries(byCat).map(([k, v]) => [k, +(v.same / v.pairs).toFixed(3)])), + consistentIgnoringJ6: +(sameNoJ6 / pairs).toFixed(3), worstAxisOfInconsistent: axisHist, deterministic: detSame + '/' + detN })); diff --git a/ml/gen/gen_dataset.cjs b/ml/gen/gen_dataset.cjs new file mode 100644 index 0000000..e29c525 --- /dev/null +++ b/ml/gen/gen_dataset.cjs @@ -0,0 +1,39 @@ +/* M1 数据生成器（2026-10-17，方案 §4） + * node ml/gen/gen_dataset.cjs --n 500 --seed 1 --out ml/data/shard_0001.jsonl + * 每行一条任务：采样 → 老师 → 距离下界认证过滤 → 局部时间律（50 Hz）→ 写出。 + * 认证不过的样本也写出（keep=false + 原因），供 stats 统计丢弃率；训练只用 keep=true。 */ +'use strict'; +const fs = require('fs'), path = require('path'); +const args = {}; for (let i = 2; i < process.argv.length; i += 2) args[process.argv[i].replace(/^--/, '')] = process.argv[i + 1]; +const N = +args.n || 100, SEED = +args.seed || 1, OUT = args.out || 'ml/data/shard_' + String(SEED).padStart(4, '0') + '.jsonl'; +const { S, ArmIK } = require('../../tests/route_bench_setup.cjs')(); +const sampler = require('./sampler.cjs')(S, SEED * 7919 + 13); +const teach = require('./teacher.cjs')(S, ArmIK); +const LEVER = S.leverBounds(4000); +const cert = require('./cert.cjs')(S, { lever: LEVER }); +const { timeParam } = require('./timelaw.cjs'); +const fp = S.modelFingerprint ? S.modelFingerprint() : null; +fs.mkdirSync(path.dirname(OUT), { recursive: true }); +const fd = fs.openSync(OUT, 'w'); +const r3 = v => Math.round(v * 1000) / 1000; +const t00 = performance.now(); +for (let i = 0; i < N; i++) { + const task = sampler.sample(); + const t = teach(task); + const rec = { id: 's' + String(SEED).padStart(4, '0') + '_' + String(i).padStart(6, '0'), cat: task.cat, mode: task.mode, + q0: task.q0.map(r3), T: task.T.map(r3), R: task.R, fp }; + if (!t.ok) { rec.keep = false; rec.why = t.why; fs.writeSync(fd, JSON.stringify(rec) + '\n'); continue; } + const c0 = performance.now(); + const c = cert.path(t.pts); + Object.assign(rec, { path: t.pts.map(q => q.map(r3)), reached: t.reached, err: r3(t.err), oerr: r3(t.oerr), travel_deg: r3(t.travel), + plan_ms: Math.round(t.ms), blocked: t.blocked, poseRelaxed: t.poseRelaxed, detour: t.detour, + cert: { ok: c.ok, queries: c.queries, minGap: r3(c.minGap), ms: Math.round(performance.now() - c0), why: c.why || null, failAt: c.failAt == null ? null : c.failAt } }); + /* 训练可用：认证通过；D 类允许未到达（停靠），其余类必须到达；poseRelaxed 的 pose 样本降为 pos */ + rec.keep = c.ok && (t.reached || task.cat === 'D'); + if (t.poseRelaxed && task.mode === 'pose') { rec.mode = 'pos'; rec.R = null; rec.demoted = true; rec.reached = t.err < 1; } + if (rec.keep) { const tp = timeParam(t.pts); rec.traj = tp.traj.map(q => q.map(r3)); rec.exec_s = r3(tp.steps * 0.02); } + fs.writeSync(fd, JSON.stringify(rec) + '\n'); + if ((i + 1) % 25 === 0) process.stderr.write(`[seed ${SEED}] ${i + 1}/${N} ${((performance.now() - t00) / 1000 / (i + 1)).toFixed(2)} s/条\n`); +} +fs.closeSync(fd); +process.stderr.write(`[seed ${SEED}] done ${N} in ${((performance.now() - t00) / 1000).toFixed(0)} s\n`); diff --git a/ml/gen/run_parallel.sh b/ml/gen/run_parallel.sh new file mode 100755 index 0000000..907ed8f --- /dev/null +++ b/ml/gen/run_parallel.sh @@ -0,0 +1,11 @@ +#!/usr/bin/env bash +# 并行生成：bash ml/gen/run_parallel.sh <每进程条数> <进程数> [起始种子=1] +# 例：8 核机器 5 万条 → bash ml/gen/run_parallel.sh 6250 8 +set -e +N=${1:-500}; P=${2:-2}; S0=${3:-1} +cd "$(dirname "$0")/../.." +mkdir -p ml/data +for ((i=0;i ml/data/log_$s.txt & done +wait +node ml/gen/stats.cjs ml/data/shard_*.jsonl > ml/data/stats.json +cat ml/data/stats.json diff --git a/ml/gen/sampler.cjs b/ml/gen/sampler.cjs new file mode 100644 index 0000000..0dbc753 --- /dev/null +++ b/ml/gen/sampler.cjs @@ -0,0 +1,32 @@ +/* 任务采样（方案 §4.3）：A 均匀 45% / B 直线被挡 30% / C 贴立柱 15% / D 目标不可达 10%；pos/pose 各半（D 只做 pos） */ +'use strict'; +module.exports = function makeSampler(S, seed) { + const L = S.LIM, O = S.ORDER, w = S.wrap180; + let st = (seed >>> 0) || 1; const rnd = () => { st = (st * 1664525 + 1013904223) >>> 0; return st / 4294967296; }; + const rq = () => O.map(id => { const t = L[id]; return t.lo + rnd() * (t.hi - t.lo); }); + const d = (a, b) => a.map((v, i) => L[O[i]].cont ? w(b[i] - v) : b[i] - v); + const segOK = (a, b) => { const dd = d(a, b); const n = Math.max(1, Math.ceil(Math.max(...dd.map(Math.abs)))); for (let i = 1; i <= n; i++) if (!S.collideFree(a.map((v, k) => v + dd[k] * i / n))) return false; return true; }; + const hitsColumn = q => (S.collideDetail(q) || []).some(p => /立柱/.test(p.cnA + p.cnB)); + const nearColumn = q => { for (const k of [0, 1, 2]) for (const sg of [-4, 4]) { const p = q.slice(); p[k] += sg; if (hitsColumn(p)) return true; } return false; }; + const freeQ = () => { for (;;) { const q = rq(); if (S.collideFree(q)) return q; } }; + function sample() { + const u = rnd(); + const cat = u < 0.45 ? 'A' : u < 0.75 ? 'B' : u < 0.90 ? 'C' : 'D'; + let a, b, T, R = null; + if (cat === 'A') { a = freeQ(); b = freeQ(); } + else if (cat === 'B') { for (;;) { a = freeQ(); b = freeQ(); if (!segOK(a, b)) break; } } + else if (cat === 'C') { + for (;;) { b = rq(); if (!S.collideFree(b) || !nearColumn(b)) continue; a = null; + for (let t = 0; t < 200; t++) { const c = rq(); if (S.collideFree(c) && !segOK(c, b)) { a = c; break; } } if (a) break; } + } else { + a = freeQ(); + if (rnd() < 0.6) { for (;;) { b = rq(); if (hitsColumn(b)) break; } T = S.fk(b, 0); } // 目标位置落在立柱附近/内部 + else { b = rq(); const p = S.fk(b, 0), o = S.fk(O.map(() => 0), 0); T = p.map((v, i) => o[i] + (v - o[i]) * 1.6); } // 工作空间外 + } + const mode = cat === 'D' ? 'pos' : (rnd() < 0.5 ? 'pos' : 'pose'); + if (!T) T = S.fk(b, 0); + if (mode === 'pose') R = S.orient(b); + return { cat, mode, q0: a, T, R, qb: b }; + } + return { sample, rnd, segOK }; +}; diff --git a/ml/gen/stats.cjs b/ml/gen/stats.cjs new file mode 100644 index 0000000..dcf4f21 --- /dev/null +++ b/ml/gen/stats.cjs @@ -0,0 +1,21 @@ +/* M1 统计：node ml/gen/stats.cjs ml/data/shard_*.jsonl > ml/data/stats.json */ +'use strict'; +const fs = require('fs'); +const files = process.argv.slice(2); +const by = {}; let total = 0, keep = 0, steps = 0, samples = 0, padTail = 0; +const q = a => { a.sort((x, y) => x - y); return a.length ? { p50: +a[a.length >> 1].toFixed(1), p90: +a[Math.floor(a.length * 0.9)].toFixed(1), max: +a[a.length - 1].toFixed(1) } : null; }; +const plan = [], certMs = [], travel = [], exec = [], gaps = []; +for (const f of files) for (const line of fs.readFileSync(f, 'utf8').split('\n')) { + if (!line) continue; const r = JSON.parse(line); total++; + const k = r.cat + '/' + r.mode; const b = by[k] = by[k] || { n: 0, reached: 0, certFail: 0, keep: 0, demoted: 0 }; + b.n++; if (r.reached) b.reached++; if (r.cert && !r.cert.ok) b.certFail++; if (r.demoted) b.demoted++; + if (r.plan_ms != null) plan.push(r.plan_ms); if (r.cert) certMs.push(r.cert.ms); if (r.cert) gaps.push(r.cert.minGap); + if (r.keep) { b.keep++; keep++; travel.push(r.travel_deg); exec.push(r.exec_s); + const n = r.traj.length - 1; steps += n; const s = Math.floor(n / 3) + 1; samples += s; + for (let t = 0; t <= n; t += 3) if (t + 16 > n) padTail++; } +} +const out = { tasks: total, keep, keepRate: +(keep / total).toFixed(3), byCatMode: by, + planMs: q(plan), certMs: q(certMs), vertexMinGapMm: q(gaps), travelDeg: q(travel), execS: q(exec), + samplesEvery3: samples, samplesPerTask: +(samples / keep).toFixed(1), paddedSampleShare: +(padTail / samples).toFixed(3), + projectedSamplesFor50k: Math.round(samples / total * 50000) }; +console.log(JSON.stringify(out, null, 1)); diff --git a/ml/gen/teacher.cjs b/ml/gen/teacher.cjs new file mode 100644 index 0000000..6db8729 --- /dev/null +++ b/ml/gen/teacher.cjs @@ -0,0 +1,23 @@ +/* 老师（方案 §4.3）：solveDyn 松手口径——有离线路网时内部走 PRM 路网 Lazy A* + 捷径平滑（种子由起点决定，确定性）， + * 路网失败再落到 RRT-Connect。返回展开后的折线与到达判据。 */ +'use strict'; +module.exports = function makeTeacher(S, ArmIK) { + const L = S.LIM, O = S.ORDER, w = S.wrap180; + return function teach(task) { + const opt = { tool: 0, step: 0.5, ref: task.q0, from: task.q0.slice() }; + if (task.mode === 'pose') { opt.pose = { R: task.R }; opt.priority = 'pos'; } + if (S.rtReset) S.rtReset(); + const t0 = performance.now(); + const r = S.solveDyn(task.T, opt); + const ms = performance.now() - t0; + if (!r || !r.q) return { ok: false, ms, why: 'noq' }; + const pts = [task.q0.slice()].concat(r.via || [], [r.q]); + for (let i = 1; i < pts.length; i++) pts[i] = pts[i - 1].map((v, k) => L[O[k]].cont ? v + w(pts[i][k] - v) : pts[i][k]); + const qf = pts[pts.length - 1], tcp = S.fk(qf, 0); + const err = Math.hypot(tcp[0] - task.T[0], tcp[1] - task.T[1], tcp[2] - task.T[2]); + const oerr = task.mode === 'pose' ? ArmIK.rotErrDeg(S.orient(qf), task.R) : 0; + const reached = err < 1 && oerr < 0.5 && !r.blocked; + let travel = 0; for (let i = 1; i < pts.length; i++) for (let k = 0; k < 6; k++) travel += Math.abs(pts[i][k] - pts[i - 1][k]); + return { ok: true, pts, reached, err, oerr, travel, ms, blocked: !!r.blocked, poseRelaxed: !!r.poseRelaxed, unsafe: !!r.unsafe, detour: r.detour || null }; + }; +}; diff --git a/ml/gen/timelaw.cjs b/ml/gen/timelaw.cjs new file mode 100644 index 0000000..37c7ffb --- /dev/null +++ b/ml/gen/timelaw.cjs @@ -0,0 +1,33 @@ +/* 局部可观测时间律（2026-10-17，方案 §4.5） + * 折线按"巡航时间" τ 参数化：每段 τ = max_j |Δq_j| / VMAX_j（各轴同步、主导轴满速）。 + * 归一化速度 v∈[0,1]（1 = 巡航）：v ← min(1, v + A·dt, sqrt(2·A·剩余 τ))，只依赖当前速度与剩余路程。 + * 输出 50 Hz（dt = 20 ms）状态序列，末点精确落在终点。 */ +'use strict'; +const VMAX = [60, 60, 90, 90, 120, 180]; // °/s，与 tests/route_bench.cjs 同（轴速比 0.6/0.6/0.9/0.9/1.2/1.8） +const A = 5.0; // 1/s²：0→巡航约 0.2 s（a_max 待真机参数） +const DT = 0.02; +function timeParam(P, opt) { + opt = opt || {}; + const vmax = opt.vmax || VMAX, a = opt.acc || A, dt = opt.dt || DT; + const segT = []; + for (let i = 1; i < P.length; i++) { let m = 0; for (let j = 0; j < 6; j++) m = Math.max(m, Math.abs(P[i][j] - P[i - 1][j]) / vmax[j]); segT.push(m); } + const cum = [0]; for (const s of segT) cum.push(cum[cum.length - 1] + s); + const total = cum[cum.length - 1]; + const at = tau => { + if (tau >= total) return P[P.length - 1].slice(); + let i = 0; while (i < segT.length - 1 && cum[i + 1] < tau) i++; + const u = segT[i] > 0 ? (tau - cum[i]) / segT[i] : 1; + return P[i].map((v, j) => v + (P[i + 1][j] - v) * u); + }; + const out = [P[0].slice()]; let tau = 0, v = 0, guard = 0; + while (tau < total - 1e-9 && guard++ < 100000) { + const rem = total - tau; + const v1 = Math.min(1, v + a * dt, Math.sqrt(2 * a * Math.max(0, rem - 0.5 * v * dt)) + 1e-9); + let step = 0.5 * (v + v1) * dt; + if (step <= 1e-9) step = Math.min(rem, a * dt * dt); // 防止末端速度掉到 0 卡住 + tau = Math.min(total, tau + step); v = v1; + out.push(at(tau)); + } + return { traj: out, total, steps: out.length - 1 }; +} +module.exports = { timeParam, VMAX, A, DT }; diff --git a/webapp/js/ik-solver.js b/webapp/js/ik-solver.js index ac7e7e0..f35c973 100644 --- a/webapp/js/ik-solver.js +++ b/webapp/js/ik-solver.js @@ -368,11 +368,12 @@ function make(cfg) { } /* 逐对扫描（簇级宽相位 → 胶囊线段距离）。visit(cnA,cnB,d,need) 对每个距离 < need 的胶囊对回调；无回调（collideFree）时首对即短路返回。 */ + let GAPCAP = 20; function scanPairs(q, visit, onlyCls, gapCb) { if (!COL) return false; const { R: Rs, T: Ts } = cumXf(q); /* 两关节类查表（只在纯判定时用；明细/生成表走精算） */ - const INF = gapCb ? 20 : 0; // 生成表：宽相位外扩 20 mm，表内 <20 mm 的间隙为精确值 + const INF = gapCb ? GAPCAP : 0; // 生成表：宽相位外扩 GAPCAP（默认 20 mm），表内 (bl + INF) * (bl + INF)) continue; const d = segSegDistRaw(e1[0], e1[1], e1[2], e1[3], e1[4], e1[5], e2[0], e2[1], e2[2], e2[3], e2[4], e2[5]); - if (gapCb) { gapCb(d - need); continue; } + if (gapCb) { gapCb(d - need, pr[n][0], pr[n][1]); continue; } if (d < need) { hit = true; if (visit) visit(e1[6], e2[6], d, need); @@ -462,6 +463,59 @@ function make(cfg) { } return hit; } + /* 全胶囊对最小间隙（mm，= 距离 − need，负值为干涉；≥cap 一律记 cap，下界、保守）（2026-10-17， + ml/gen 距离下界认证用）：不走 pair2d 查表、不提前退出，比 collideFree 慢，只给离线/安全层用。 */ + function clearance(q, cap) { + if (!COL) return Infinity; + cap = cap || 20; + let best = cap; const save = GAPCAP; GAPCAP = cap; + try { scanPairs(q, () => {}, undefined, g => { if (g < best) best = g; }); } finally { GAPCAP = save; } + return best; + } + /* 按组对的最小间隙表（7×7 扁平，[gA*7+gB]，gA {}, undefined, (g, i, j) => { + let a = CAPG[i].g, b = CAPG[j].g; if (a > b) { const t = a; a = b; b = t; } + if (g < M[a * 7 + b]) M[a * 7 + b] = g; + }); + } finally { GAPCAP = save; } + return M; + } + /* 每轴杠杆上界 r_j（mm/°）：轴 j 转 1° 时，j 之后任一胶囊端点相对 j 之前部件的最大位移。 + 随机采样 n 个位形取最大值再放大 (1+pad)。胶囊间距对端点位移 1-Lipschitz，故 + Σ r_j|Δθ_j| 是关节直线段上任意胶囊对间隙变化量的上界（用于 d(a)+d(b) > Σ r_j|Δθ_j| 认证）。 */ + function leverBounds(n, pad, seed) { + n = n || 4000; pad = pad === undefined ? 0.05 : pad; + let st = (seed || 7) >>> 0; const rnd = () => { st = (st * 1664525 + 1013904223) >>> 0; return st / 4294967296; }; + const r = [0, 0, 0, 0, 0, 0]; + for (let t = 0; t < n; t++) { + const q = ORDER.map(id => { const L = LIM[id]; return L.cont ? -180 + 360 * rnd() : L.lo + (L.hi - L.lo) * rnd(); }); + const { R: Rs, T: Ts } = cumXf(q); + for (let j = 0; j < 6; j++) { + const Rj = Rs[j], tj = Ts[j], Pj = P[ORDER[j]], Dj = D[ORDER[j]]; + const o = [Rj[0] * Pj[0] + Rj[1] * Pj[1] + Rj[2] * Pj[2] + tj[0], Rj[3] * Pj[0] + Rj[4] * Pj[1] + Rj[5] * Pj[2] + tj[1], Rj[6] * Pj[0] + Rj[7] * Pj[1] + Rj[8] * Pj[2] + tj[2]]; + const u = [Rj[0] * Dj[0] + Rj[1] * Dj[1] + Rj[2] * Dj[2], Rj[3] * Dj[0] + Rj[4] * Dj[1] + Rj[5] * Dj[2], Rj[6] * Dj[0] + Rj[7] * Dj[1] + Rj[8] * Dj[2]]; + const un = Math.hypot(u[0], u[1], u[2]); + for (const c of CAPG) { + if (c.g < j + 1) continue; + const R = Rs[c.g], T = Ts[c.g]; + for (const e of [c.a, c.b]) { + const w = [R[0] * e[0] + R[1] * e[1] + R[2] * e[2] + T[0] - o[0], R[3] * e[0] + R[4] * e[1] + R[5] * e[2] + T[1] - o[1], R[6] * e[0] + R[7] * e[1] + R[8] * e[2] + T[2] - o[2]]; + const dp = (w[0] * u[0] + w[1] * u[1] + w[2] * u[2]) / un; + const dd = Math.sqrt(Math.max(0, w[0] * w[0] + w[1] * w[1] + w[2] * w[2] - dp * dp)); + if (dd > r[j]) r[j] = dd; + } + } + } + } + return r.map(v => v * D2R * (1 + pad)); + } /* 全姿态动态碰撞判定：True = 无动态干涉（模型近似，含 margin 外扩） */ const PST = { n: 0 }; /* 单次求解内的判定记忆：routeFrom / gatherGoals / 规划器会对同一批 IK 候选反复判定 */ @@ -3109,7 +3163,7 @@ function make(cfg) { solveRoute: solveRoute, routeFrom: routeFrom, transitPt: transitPt, planAround: planAround, edgeFree: edgeFree, PST: PST, unwrapTo: unwrapTo, rtWarm: rtWarm, rtReset: () => { RT = null; }, rmPlan: rmPlan, rmSmooth: rmSmooth, modelFingerprint: modelFingerprint, RM: RM, wd: wd, classGap: classGap, classFingerprint: classFingerprint, P2D: P2D, mixSolve: mixSolve, closest: closest, closest6: closest6, closestOri: closestOri, orient: orient, branch: branch, selfTest: selfTest, poseSelfTest: poseSelfTest, - collideFree: collideFree, collideDetail: collideDetail, + collideFree: collideFree, collideDetail: collideDetail, clearance: clearance, gapMatrix: gapMatrix, leverBounds: leverBounds, dynLimit: dynLimit, dynLimits: dynLimits, K: K, LIM: LIM, P: P, D: D, ORDER: ORDER, wrap180: wrap180, costOf: costOf }; }