ml: 数据生成 + 可微运动学对拍 + MeanFlow/WTA 策略与闭环仿真 + APG/挖错/ONNX 导出与运行手册
- 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。
This commit is contained in:
Hark
+132
-4
@@ -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），表内 <GAPCAP 的间隙为精确值
|
||||
if (!visit) for (const k in P2D) {
|
||||
const T = P2D[k], st = p2dStatus(T, q);
|
||||
if (st < 0) return true;
|
||||
@@ -445,7 +446,7 @@ function make(cfg) {
|
||||
if (mx * mx + my * my + mz * mz > (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,129 @@ function make(cfg) {
|
||||
}
|
||||
return hit;
|
||||
}
|
||||
/* 胶囊对清单 [[i, j, need]]（与 scanPairs 判定的对完全一致；ml/ 导出给 PyTorch 可微版用） */
|
||||
function pairList() {
|
||||
const out = [];
|
||||
for (const [, , cl] of PARTPAIR) for (const c of cl) for (const pr of c[2]) out.push([pr[0], pr[1], pr[2]]);
|
||||
return out;
|
||||
}
|
||||
/* 宽相位结构导出（ml/ PyTorch 版复刻同一套簇球筛选，保证 min(gap, cap) 与 JS 逐位同口径）：
|
||||
clusters[k] = {g, c(局部簇心), rad, rmax, caps}；cpairs = [[ka, kb, rsum, rmaxS]]，簇对内胶囊对取全组合，need = r_i + r_j + CAPM */
|
||||
function clusterExport() {
|
||||
const clusters = [], base = new Map();
|
||||
for (const pt of PART) { base.set(pt, clusters.length); for (const c of pt.clus) clusters.push({ g: pt.g, c: c.c.slice(), rad: c.rad, rmax: c.rmax, caps: c.caps.slice() }); }
|
||||
const cpairs = [];
|
||||
for (const [A, B, cl] of PARTPAIR) for (const c of cl) cpairs.push([base.get(A) + c[0], base.get(B) + c[1], c[3], c[4]]);
|
||||
return { clusters: clusters, cpairs: cpairs, capm: CAPM };
|
||||
}
|
||||
/* 间隙 < thr 的胶囊对 [[i, j, gap]]，按 gap 升序（安全层投影用：只对这几对做有限差分） */
|
||||
function criticalPairs(q, thr) {
|
||||
const out = [];
|
||||
if (!COL) return out;
|
||||
const save = GAPCAP; GAPCAP = thr;
|
||||
try { scanPairs(q, () => {}, undefined, (g, i, j) => { if (g < thr) out.push([i, j, g]); }); } finally { GAPCAP = save; }
|
||||
out.sort((x, y) => x[2] - y[2]);
|
||||
return out;
|
||||
}
|
||||
/* 单个胶囊对的间隙（距离 − need），只变换两根胶囊，微秒级 */
|
||||
function pairGap(q, i, j) {
|
||||
const { R: Rs, T: Ts } = cumXf(q);
|
||||
const tf = (c, e) => { const R = Rs[c.g], T = Ts[c.g]; return [R[0] * e[0] + R[1] * e[1] + R[2] * e[2] + T[0], R[3] * e[0] + R[4] * e[1] + R[5] * e[2] + T[1], R[6] * e[0] + R[7] * e[1] + R[8] * e[2] + T[2]]; };
|
||||
const A = CAPG[i], B = CAPG[j];
|
||||
const a0 = tf(A, A.a), a1 = tf(A, A.b), b0 = tf(B, B.a), b1 = tf(B, B.b);
|
||||
return segSegDistRaw(a0[0], a0[1], a0[2], a1[0], a1[1], a1[2], b0[0], b0[1], b0[2], b1[0], b1[1], b1[2]) - (A.r + B.r + CAPM);
|
||||
}
|
||||
function setDeterministic(on, msPerCheck) { VCLK = on ? { base: 0, per: msPerCheck || 0.5, tick: 0 } : null; }
|
||||
/* 清空所有跨调用状态（路网增广、坏边/死边/已验段缓存、实时路径、全量 IK 缓存），使下一次求解与历史无关 */
|
||||
function resetSession() {
|
||||
RMX.Q.length = 0; RMX.adj.length = 0; RMX.adjR = null;
|
||||
BADE.clear(); DEAD.clear(); FINE.clear();
|
||||
RT = null; RTSTALL.key = null; RTSTALL.n = 0;
|
||||
FULLIK.key = null; FULLIK.cur = null; FULLIK.T = null; FULLIK.at = 0;
|
||||
}
|
||||
/* 全胶囊对最小间隙（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<gB；未进入窄相位的对记 cap）。
|
||||
组对 (gA,gB) 的相对位姿只受关节 gA..gB−1 影响，认证时只用这几轴的杠杆和。 */
|
||||
function gapMatrix(q, cap) {
|
||||
cap = cap || 20;
|
||||
const M = new Float64Array(49).fill(cap);
|
||||
if (!COL) return M;
|
||||
const save = GAPCAP; GAPCAP = cap;
|
||||
try {
|
||||
scanPairs(q, () => {}, 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| 认证）。 */
|
||||
/* 分组杠杆表 T[k][g]（mm/°）：组 g 的胶囊端点到轴 k 的最大距离（g > k），比 leverBounds 的"全部远端"上界紧：
|
||||
组对 (gA,gB) 的相对运动只由关节 gA..gB−1 带动组 gB 产生，故 Σ_k T[k][gB]·|Δθ_k| 是该组对间隙变化上界。 */
|
||||
function leverTable(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 T = [0, 1, 2, 3, 4, 5].map(() => [0, 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], Tt = 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] + Tt[0] - o[0], R[3] * e[0] + R[4] * e[1] + R[5] * e[2] + Tt[1] - o[1], R[6] * e[0] + R[7] * e[1] + R[8] * e[2] + Tt[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 > T[j][c.g]) T[j][c.g] = dd;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
/* 组 g 的端点距离是该组上界；但组对 (gA,gB) 里远端组 gB 之外，gB 与 gA 之间的组不参与该对，
|
||||
而 gB 自身被 j<gB 的所有关节带动——直接取 T[j][gB] 即可 */
|
||||
return T.map(row => row.map(v => v * D2R * (1 + pad)));
|
||||
}
|
||||
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 候选反复判定 */
|
||||
@@ -715,7 +839,11 @@ function make(cfg) {
|
||||
for (let i = 0; i < 6; i++) { const w = W ? W[i] : 1; t += Math.abs(wrap180(q[i] - ref[i])) * w; }
|
||||
return t + 1e-6 * c;
|
||||
};
|
||||
const nowMs = () => (typeof performance !== 'undefined' ? performance.now() : Date.now());
|
||||
/* 确定性时钟（2026-10-17，ml/gen 离线老师用）：开启后各处毫秒预算改按"碰撞判定次数 × msPerCheck"计，
|
||||
另每次读时钟 +1 µs 防纯计算循环不前进。规划结果只取决于输入，不随机器快慢/负载变化。默认关闭。 */
|
||||
let VCLK = null;
|
||||
const nowMs = () => VCLK ? VCLK.base + PST.n * VCLK.per + (VCLK.tick += 0.001)
|
||||
: (typeof performance !== 'undefined' ? performance.now() : Date.now());
|
||||
|
||||
/* 姿态求解原语工厂：位置闭式支把 θ1..θ3 吸收掉之后，姿态任务只剩「工具轴指向 =
|
||||
目标轴」这 2 个方程、2 个未知数 (θ4,θ5)，解是孤立点，滚转再由 θ6 闭式补上。
|
||||
@@ -3109,7 +3237,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, leverTable: leverTable, setDeterministic: setDeterministic, resetSession: resetSession, pairList: pairList, clusterExport: clusterExport, criticalPairs: criticalPairs, pairGap: pairGap, TCP0: TCP0, CAPM: CAPM,
|
||||
dynLimit: dynLimit, dynLimits: dynLimits,
|
||||
K: K, LIM: LIM, P: P, D: D, ORDER: ORDER, wrap180: wrap180, costOf: costOf };
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user