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
+23
View File
@@ -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 };
};
};