ml/gen: M1 数据生成器(采样/老师/距离下界认证/时间律/统计/一致性门禁);ik-solver 增 clearance/gapMatrix/leverBounds

This commit is contained in:
Hark
2026-10-17 15:48:19 +00:00
parent 5c74c152e9
commit b02b8291c4
10 changed files with 305 additions and 3 deletions
+57 -3
View File
@@ -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,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<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| 认证)。 */
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 };
}