198 lines
7.4 KiB
JavaScript
198 lines
7.4 KiB
JavaScript
/**
|
||
* homingOrchestrator.js
|
||
* Vollständiger Homing-Ablauf: Board-Pipeline (1→2→3b) + 4b-Schleife.
|
||
*
|
||
* Abhängigkeiten werden von server.js per Parameter übergeben
|
||
* (kein Circular-Import-Problem).
|
||
*/
|
||
|
||
import path from 'path';
|
||
import fs from 'fs';
|
||
import fsPromises from 'fs/promises';
|
||
|
||
/**
|
||
* Modell-Welt-x eines Markers bei slider=0, durch Aufsummieren der origin.x
|
||
* entlang der Kette Link→…→Base. Das Ergebnis ist winkel-unabhängig (und damit
|
||
* exakt) genau dann, wenn alle revolute-Gelenke der Kette um die x-Achse drehen
|
||
* (Rotation um x erhält die x-Koordinate). Andernfalls xSafe=false.
|
||
*
|
||
* @param {object} links robot.json links
|
||
* @param {string} linkName
|
||
* @param {number[]} localPos Marker-Position im lokalen Link-Frame [x,y,z]
|
||
* @returns {{ worldX: number, xSafe: boolean }}
|
||
*/
|
||
function modelWorldXAtSliderZero(links, linkName, localPos) {
|
||
let xOffset = 0;
|
||
let xSafe = true;
|
||
let cur = linkName;
|
||
const seen = new Set();
|
||
|
||
while (cur && links[cur]?.jointToParent && !seen.has(cur)) {
|
||
seen.add(cur);
|
||
const jtp = links[cur].jointToParent;
|
||
xOffset += jtp.origin?.[0] ?? 0;
|
||
|
||
const axis = jtp.axis ?? [0, 0, 0];
|
||
const isXAxis = Math.abs(axis[0]) === 1 && axis[1] === 0 && axis[2] === 0;
|
||
if (jtp.type === 'revolute' && !isXAxis) xSafe = false;
|
||
|
||
cur = links[cur].parent;
|
||
}
|
||
return { worldX: xOffset + (localPos?.[0] ?? 0), xSafe };
|
||
}
|
||
|
||
/**
|
||
* Schätzt die Slider-X-Position aus den triangulierten Marker-Positionen
|
||
* (aruco_marker_poses.json).
|
||
*
|
||
* Für jeden beobachteten Arm-Marker wird der implizierte Slider-Wert berechnet:
|
||
* slider_i = beobachtetes_world_x − Modell_world_x(slider=0)
|
||
* und über alle x-zuverlässigen Marker gemittelt. So wird der Gelenk-Offset
|
||
* (z.B. Arm1.origin.x = 110 mm) korrekt herausgerechnet.
|
||
*
|
||
* Fallback (kein robot.json oder keine zuverlässigen Marker): alter Mittelwert
|
||
* der rohen world-x – nur als Notlösung.
|
||
*
|
||
* @param {string} arucoJsonPath
|
||
* @param {string} [robotJsonPath]
|
||
* @returns {number} x_mm
|
||
*/
|
||
export function estimateXFromMarkers(arucoJsonPath, robotJsonPath) {
|
||
try {
|
||
const data = JSON.parse(fs.readFileSync(arucoJsonPath, 'utf8'));
|
||
const links = robotJsonPath
|
||
? (JSON.parse(fs.readFileSync(robotJsonPath, 'utf8')).links ?? {})
|
||
: {};
|
||
|
||
const samples = [];
|
||
for (const obs of (data.markers ?? [])) {
|
||
if (!obs.link || obs.link === 'Board') continue;
|
||
const modelMarker = links[obs.link]?.markers?.find(m => m.id === obs.marker_id);
|
||
if (!modelMarker?.position) continue;
|
||
const { worldX, xSafe } = modelWorldXAtSliderZero(links, obs.link, modelMarker.position);
|
||
if (!xSafe) continue;
|
||
const obsX = obs.position_mm?.[0];
|
||
if (obsX == null) continue;
|
||
samples.push(obsX - worldX);
|
||
}
|
||
|
||
if (samples.length > 0) {
|
||
return samples.reduce((a, b) => a + b, 0) / samples.length;
|
||
}
|
||
|
||
// ── Fallback: alter, geometrisch ungenauer Mittelwert ──
|
||
const armMarkers = (data.markers ?? []).filter(m => m.link && m.link !== 'Board');
|
||
if (armMarkers.length === 0) return 0.0;
|
||
return armMarkers.reduce((s, m) => s + (m.position_mm?.[0] ?? 0), 0) / armMarkers.length;
|
||
} catch {
|
||
return 0.0;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* Führt den vollständigen Homing-Ablauf als SSE-Stream aus.
|
||
*
|
||
* @param {{
|
||
* robotJsonPath: string,
|
||
* homingDir: string,
|
||
* send: (obj: object) => void,
|
||
* runScript: (args: string[], send: Function) => Promise<number>,
|
||
* runBoardPipeline: (runDir: string, send: Function) => Promise<void>,
|
||
* SCRIPT_4B: string,
|
||
* }} opts
|
||
*/
|
||
export async function runHoming({
|
||
robotJsonPath,
|
||
homingDir,
|
||
send,
|
||
runScript,
|
||
runBoardPipeline,
|
||
SCRIPT_4B,
|
||
}) {
|
||
// Lauf-Verzeichnis anlegen
|
||
const ts = makeTimestamp();
|
||
const runDir = path.join(homingDir, ts);
|
||
await fsPromises.mkdir(runDir, { recursive: true });
|
||
|
||
send({ type: 'log', text: `▶ Homing-Run: ${ts}` });
|
||
send({ type: 'log', text: `▶ Ordner: ${runDir}` });
|
||
send({ type: 'log', text: `▶ Robot-JSON: ${robotJsonPath}` });
|
||
send({ type: 'log', text: '' });
|
||
|
||
// ── Schritt 1–3b: Board-Pipeline ─────────────────────────────────────────
|
||
send({ type: 'step', step: 1, total: 6, text: 'Foto + Marker-Triangulierung …' });
|
||
await runBoardPipeline(runDir, send);
|
||
|
||
// Prüfen ob aruco_marker_poses.json erzeugt wurde
|
||
const arucoJson = path.join(runDir, 'aruco_marker_poses.json');
|
||
try {
|
||
await fsPromises.access(arucoJson);
|
||
} catch {
|
||
send({ type: 'error', text: '❌ aruco_marker_poses.json fehlt – Script 3b hat nicht funktioniert.' });
|
||
send({ type: 'done', exitCode: -1, runDir: ts });
|
||
return;
|
||
}
|
||
|
||
// ── Schritt 2: X-Position bestimmen ─────────────────────────────────────
|
||
send({ type: 'step', step: 2, total: 6, text: 'X-Position bestimmen …' });
|
||
const xMm = estimateXFromMarkers(arucoJson, robotJsonPath);
|
||
send({ type: 'log', text: `▶ Geschätzte X-Position: ${xMm.toFixed(1)} mm` });
|
||
send({ type: 'analysis', key: 'x_mm', value: xMm });
|
||
|
||
// ── Schritt 3–6: 4b-Kette (Arm1 → Ellbow → Arm2 → Hand) ─────────────
|
||
const links = ['Arm1', 'Ellbow', 'Arm2', 'Hand'];
|
||
let fromState = null;
|
||
|
||
for (let i = 0; i < links.length; i++) {
|
||
const link = links[i];
|
||
send({ type: 'step', step: 3 + i, total: 6, text: `Gelenkwinkel ${link} …` });
|
||
send({ type: 'log', text: `\n─── 4b: ${link} ${'─'.repeat(35 - link.length)}` });
|
||
|
||
const outputPath = path.join(runDir, `state_${link}.json`);
|
||
const args = [
|
||
SCRIPT_4B,
|
||
'--robot', robotJsonPath,
|
||
'--aruco', arucoJson,
|
||
'--link', link,
|
||
'--output', outputPath,
|
||
];
|
||
if (fromState) args.push('--from-state', fromState);
|
||
else args.push('--x-mm', String(xMm));
|
||
|
||
const exit = await runScript(args, send);
|
||
if (exit !== 0) {
|
||
send({ type: 'error', text: `❌ 4b ${link} Exit ${exit}` });
|
||
send({ type: 'done', exitCode: exit, runDir: ts });
|
||
return;
|
||
}
|
||
fromState = outputPath;
|
||
|
||
// Zwischenergebnis an Analysis-Sektion
|
||
try {
|
||
const stateData = JSON.parse(await fsPromises.readFile(outputPath, 'utf8'));
|
||
const acc = stateData.accumulated_state ?? stateData;
|
||
send({ type: 'analysis', key: `state_${link}`, value: acc });
|
||
} catch { /* ignorieren */ }
|
||
}
|
||
|
||
// ── Endergebnis ──────────────────────────────────────────────────────────
|
||
try {
|
||
const finalData = JSON.parse(await fsPromises.readFile(fromState, 'utf8'));
|
||
const finalState = finalData.accumulated_state ?? finalData;
|
||
send({ type: 'log', text: '' });
|
||
send({ type: 'log', text: `✅ Homing abgeschlossen: ${ts}` });
|
||
send({ type: 'done', exitCode: 0, state: finalState, runDir: ts });
|
||
} catch (err) {
|
||
send({ type: 'error', text: `❌ Endzustand konnte nicht gelesen werden: ${err.message}` });
|
||
send({ type: 'done', exitCode: -1, runDir: ts });
|
||
}
|
||
}
|
||
|
||
/** Timestamp-String YYYYMMDD_HHmmss */
|
||
function makeTimestamp() {
|
||
const now = new Date();
|
||
const p = (n, l = 2) => String(n).padStart(l, '0');
|
||
return `${now.getFullYear()}${p(now.getMonth() + 1)}${p(now.getDate())}`
|
||
+ `_${p(now.getHours())}${p(now.getMinutes())}${p(now.getSeconds())}`;
|
||
}
|