Robot_Arm/ml/gen/gen_dataset.cjs

/* M1 数据生成器（2026-10-17，方案 §4）
 *   node ml/gen/gen_dataset.cjs --n 500 --seed 1 --out ml/data/shard_0001.jsonl [--consensus 4]
 * --consensus k：邻域共识过滤。起点不变、目标位置各扰动 5 mm 造 k 个邻居跑老师，原任务路径若与多数邻居
 *   不在同一绕行类（pathdev.sameClass），整条样本 keep=false（why=consensus）。成本约 (1+k) 倍。
 * 每行一条任务：采样 → 老师 → 距离下界认证过滤 → 局部时间律（50 Hz）→ 写出。
 * 认证不过的样本也写出（keep=false + 原因），供 stats 统计丢弃率；训练只用 keep=true。
 * 2026-10-18 断点续写：第 i 条任务的随机数只由 (seed, i) 决定（sampler.reseed）；输出文件已存在时保留其中完整的行、
 *   截掉写了一半的最后一行，从下一条接着写 —— 生成几小时被中断，重跑只补剩下的，不重来、不重复。 */
'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 { sameClass } = require('./pathdev.cjs');
const K_CONS = +args.consensus || 0;
/* --tasks file.jsonl：按给定任务（q0/T/R/mode/cat）请老师，不采样（挖错循环 E2 补路径用） */
const GIVEN = args.tasks ? fs.readFileSync(args.tasks, 'utf8').split('\n').filter(Boolean).map(l => JSON.parse(l)) : null;
const NT = GIVEN ? GIVEN.length : N;
const cert = require('./cert.cjs')(S, {});
const { timeParam } = require('./timelaw.cjs');
const fp = S.modelFingerprint ? S.modelFingerprint() : null;
fs.mkdirSync(path.dirname(OUT), { recursive: true });
let done = 0;
if (fs.existsSync(OUT)) {
  const keep = [];
  for (const l of fs.readFileSync(OUT, 'utf8').split('\n')) { if (!l) continue; try { JSON.parse(l); keep.push(l); } catch (e) { break; } }
  done = Math.min(keep.length, NT);
  fs.writeFileSync(OUT, keep.slice(0, done).map(l => l + '\n').join(''));
  if (done) process.stderr.write(`[seed ${SEED}] 续写：已有 ${done}/${NT} 条，从第 ${done + 1} 条继续\n`);
}
const fd = fs.openSync(OUT, 'a');
const taskSeed = i => (Math.imul(SEED, 2654435761) ^ Math.imul(i + 1, 40503) ^ 0x9e3779b9) >>> 0;
const r3 = v => Math.round(v * 1000) / 1000;
const t00 = performance.now();
for (let i = done; i < NT; i++) {
  sampler.reseed(taskSeed(i));
  const task = GIVEN ? Object.assign({ cat: 'E2' }, GIVEN[i], { mode: GIVEN[i].R ? 'pose' : 'pos' }) : 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();
  let c = null;
  if (t.ptsJ6Frozen) { const cf = cert.path(t.ptsJ6Frozen); if (cf.ok) { t.pts = t.ptsJ6Frozen; t.travel = t.travelOf(t.pts); c = cf; rec.j6Frozen = true; } }
  if (!c) c = cert.path(t.pts);
  if (task.cat === 'D' && t.reached) rec.cat = 'Dr';                  // "不可达"采样其实可达：单独记类，不当停靠样本
  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 (rec.keep && K_CONS > 0 && t.reached) {
    let agree = 0, valid = 0;
    for (let k = 0; k < K_CONS; k++) {
      const u = [sampler.rnd() - 0.5, sampler.rnd() - 0.5, sampler.rnd() - 0.5], n = Math.hypot(u[0], u[1], u[2]);
      const nb = teach(Object.assign({}, task, { T: task.T.map((v, j) => v + 5 * u[j] / n) }));
      if (!nb.ok || !nb.reached) continue;
      valid++; if (sameClass(t.pts, nb.ptsJ6Frozen && rec.j6Frozen ? nb.ptsJ6Frozen : nb.pts, task.mode)) agree++;
    }
    rec.consensus = { agree, valid };
    if (valid >= 2 && agree * 2 < valid) { rec.keep = false; rec.why = 'consensus'; }
  }
  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}/${NT}  ${((performance.now() - t00) / 1000 / (i + 1 - done)).toFixed(2)} s/条\n`);
}
fs.closeSync(fd);
process.stderr.write(`[seed ${SEED}] done ${NT}（本次 ${NT - done}） in ${((performance.now() - t00) / 1000).toFixed(0)} s\n`);