diff --git a/doc/Homing_ROADMAP.md b/doc/Homing_ROADMAP.md index 0efd70b..8c9ade4 100644 --- a/doc/Homing_ROADMAP.md +++ b/doc/Homing_ROADMAP.md @@ -64,7 +64,7 @@ Foto alle Kameras │ → aruco_marker_poses.json ▼ X-Position aus Marker-Positionen schätzen - │ → x_mm (Durchschnitt x der Nicht-Board-Marker) + │ → x_mm (pro Arm-Marker: beobachtetes_x − Modell_x(slider=0), gemittelt) ▼ 4b_revolute_angle.py --link Arm1 --x-mm {x_mm} │ → state_Arm1.json (accumulated_state) @@ -95,12 +95,13 @@ X-Slider-Position über `--x-mm`. | Komponente | Datei | Beschreibung | |-----------|-------|--------------| | Board-Pipeline | `server/server.js` → `runBoardPipeline(runDir, send)` | Foto + Scripts 1, 2, 3b; von Board-Run und Homing genutzt | -| X-Schätzung | `server/homingOrchestrator.js` → `estimateXFromMarkers()` | Mittelwert x der Nicht-Board-Marker aus `aruco_marker_poses.json` | +| X-Schätzung | `server/homingOrchestrator.js` → `estimateXFromMarkers()` | Pro Arm-Marker `beobachtetes_x − Modell_x(slider=0)`, gemittelt — rechnet den kinematischen Gelenk-Offset (z.B. Arm1.origin.x=110) heraus. Nur x-zuverlässige Ketten (x-Rotation: Arm1/Ellbow). Fallback: roher Mittelwert | | Homing-Orchestrator | `server/homingOrchestrator.js` → `runHoming()` | Kompletter Ablauf als SSE-Stream | | Backend-Route | `POST /api/homing/run` | SSE-Stream, startet `runHoming()` | | State senden | `POST /api/homing/send-state` | Weiterleitung an `ROBOT_URL/api/state` | | Run-Daten | `GET /api/homing/run-data?run=ts` | Debug-Bilder (base64) + finalState | -| Frontend | `public/index.html` + `public/client.js` | Homing-Buttons, Fortschrittsbalken, Tree View | +| Frontend | `public/index.html` + `public/client.js` | Homing-Buttons, Fortschrittsbalken, Tree View; schreibt Teil-Pose als `G92`-GCode ins Eingabefeld | +| Board-Viewer (Homing) | `public/boardViewer.html?mode=homing` | Skelett + Arm-Marker per FK (Three.js), gemessene Marker als Kugeln + Fehlerlinien; progressiver Update je erkanntem Gelenk | **Lauf-Verzeichnisse:** `data/homing/{timestamp}/` @@ -144,7 +145,9 @@ die aktuelle Konfiguration. ## Offene Punkte -- [ ] **Arm-Marker eintragen** (Nutzer): `links.Arm1/Ellbow/Arm2/Hand.markers` in `robot.json` -- [ ] **Erstmals testen**: Homing-Run mit echtem Roboter und eingetragenen Markern durchführen -- [ ] **X-Schätzung verfeinern** (optional): `estimateXFromMarkers()` könnte gelenk-spezifischere Logik nutzen statt einfachem Mittelwert +- [~] **Arm-Marker eintragen** (Nutzer): `links.Arm1/Ellbow/Arm2/Hand.markers` in `robot.json` — Arm1 + Ellbow eingetragen, Arm2/Hand offen +- [~] **Erstmals testen**: Homing-Run mit echtem Roboter — Arm1 erkannt (x, y); Ellbow scheitert noch an fehlenden Markern +- [x] **X-Schätzung verfeinern** (2026-06-14): `estimateXFromMarkers()` rechnet den kinematischen Gelenk-Offset heraus statt rohem Mittelwert — behebt den ~110 mm Versatz der Modell-Marker +- [x] **Unit-Test für X-Schätzung** (2026-06-14): reine Geometrie nach `server/homingXEstimate.cjs` ausgelöst, `test/homingXEstimate.test.js` (9 Tests, inkl. Regression gegen den Offset-Bug) +- [ ] **y-Restfehler** (~2°): erkannt 30° → ausgegeben 28°; vermutlich X-Rest-Rauschen + 4b-Fit-Residuum, noch zu untersuchen - [ ] **robot.json via Driver-API** (optional): wenn Driver `GET ROBOT_URL/api/robot/config` bereitstellt diff --git a/server/homingOrchestrator.js b/server/homingOrchestrator.js index b528e9e..6fe9577 100644 --- a/server/homingOrchestrator.js +++ b/server/homingOrchestrator.js @@ -9,81 +9,24 @@ import path from 'path'; import fs from 'fs'; import fsPromises from 'fs/promises'; +import { estimateXFromParsed } from './homingXEstimate.cjs'; /** - * 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. + * Schätzt die Slider-X-Position aus den triangulierten Marker-Positionen. + * Dünner I/O-Wrapper: liest die Dateien und delegiert die reine Geometrie an + * `estimateXFromParsed()` (`server/homingXEstimate.cjs`, unit-getestet). * - * @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] + * @param {string} arucoJsonPath Pfad zu aruco_marker_poses.json + * @param {string} [robotJsonPath] Pfad zu robot.json (für den Gelenk-Offset) * @returns {number} x_mm */ export function estimateXFromMarkers(arucoJsonPath, robotJsonPath) { try { - const data = JSON.parse(fs.readFileSync(arucoJsonPath, 'utf8')); + const arucoData = 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; + return estimateXFromParsed(arucoData, links); } catch { return 0.0; } diff --git a/server/homingXEstimate.cjs b/server/homingXEstimate.cjs new file mode 100644 index 0000000..890171f --- /dev/null +++ b/server/homingXEstimate.cjs @@ -0,0 +1,90 @@ +/** + * homingXEstimate.cjs + * =================== + * Reine Geometrie-Logik zur Schätzung der Slider-X-Position (kein I/O, kein fs). + * + * Bewusst CommonJS (`.cjs`): so kann sowohl der ESM-Server + * (`server/homingOrchestrator.js` via Node-Interop `import { … } from './…cjs'`) + * als auch Jest (`require('../server/homingXEstimate.cjs')`) dieselbe Logik nutzen. + * Folgt damit dem Repo-Muster „pure Logik herauslösen" (vgl. `public/yAxisCompute.js`), + * nur als `.cjs`, weil der Importeur ein ESM-Modul unter `"type":"module"` ist. + */ + +/** + * 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. Sobald ein revolute-Gelenk um eine andere Achse dreht, hängt + * das Welt-x vom (hier unbekannten) Winkel ab → xSafe=false. + * + * @param {object} links robot.json `links` + * @param {string} linkName Link des Markers + * @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 bereits geparsten Daten. + * + * 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 kinematische + * Gelenk-Offset (z.B. Arm1.origin.x = 110 mm) korrekt herausgerechnet. + * + * Übersprungen werden: Board-Marker, Marker ohne Modell-Eintrag (unbekannte ID), + * Marker nicht-x-zuverlässiger Ketten und Marker ohne beobachtetes x. + * + * Fallback (keine zuverlässigen Marker, z.B. ohne robot.json): roher Mittelwert + * der world-x aller Nicht-Board-Marker – nur Notlösung. + * + * @param {{ markers?: Array<{ marker_id:number, link?:string, position_mm?:number[] }> }} arucoData + * @param {object} links robot.json `links` (oder {} falls nicht vorhanden) + * @returns {number} x_mm + */ +function estimateXFromParsed(arucoData, links) { + const markers = arucoData?.markers ?? []; + const samples = []; + + for (const obs of 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 = 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; +} + +module.exports = { modelWorldXAtSliderZero, estimateXFromParsed }; diff --git a/test/homingXEstimate.test.js b/test/homingXEstimate.test.js new file mode 100644 index 0000000..d07d004 --- /dev/null +++ b/test/homingXEstimate.test.js @@ -0,0 +1,136 @@ +/** + * homingXEstimate.test.js + * ======================= + * Unit-Test für server/homingXEstimate.cjs (reine X-Schätzungs-Geometrie). + * + * Sichert insbesondere den Bug vom 2026-06-14 ab: Vorher setzte die Schätzung + * x_slider = Mittelwert der beobachteten world-x und vergaß den kinematischen + * Gelenk-Offset (Arm1.origin.x = 110). Dadurch lagen die Modell-Marker ~110 mm + * zu weit in +x. Korrekt: slider_i = beobachtetes_x − Modell_x(slider=0). + */ + +const fs = require('fs'); +const path = require('path'); + +const { modelWorldXAtSliderZero, estimateXFromParsed } = + require('../server/homingXEstimate.cjs'); + +// ── Synthetische Kinematik (Struktur wie robot.json, minimal) ────────────────── +function makeLinks() { + return { + Board: {}, // Root, kein jointToParent + Base: { parent: 'Board', jointToParent: { type: 'linear', axis: [1, 0, 0], origin: [0, 0, 16] }, markers: [] }, + Arm1: { + parent: 'Base', + jointToParent: { type: 'revolute', axis: [-1, 0, 0], origin: [110, 108, 45] }, + markers: [ + { id: 198, position: [0, -160, 35] }, + { id: 229, position: [0, -250, 35] }, + { id: 197, position: [-35, -250, 0] }, + ], + }, + Ellbow: { + parent: 'Arm1', + jointToParent: { type: 'revolute', axis: [-1, 0, 0], origin: [0, -250, 0] }, + markers: [ { id: 244, position: [125, 0, 0] } ], + }, + Arm2: { + parent: 'Ellbow', + jointToParent: { type: 'revolute', axis: [0, -1, 0], origin: [90, 0, 0] }, // dreht um y → NICHT x-sicher + markers: [ { id: 300, position: [0, -100, 0] } ], + }, + }; +} + +describe('modelWorldXAtSliderZero', () => { + const links = makeLinks(); + + test('summiert origin.x entlang x-Rotations-Kette (Arm1)', () => { + // Arm1.origin.x (110) + local x (0) + expect(modelWorldXAtSliderZero(links, 'Arm1', [0, -160, 35])) + .toEqual({ worldX: 110, xSafe: true }); + // local x (-35) wird mitgenommen + expect(modelWorldXAtSliderZero(links, 'Arm1', [-35, -250, 0])) + .toEqual({ worldX: 75, xSafe: true }); + }); + + test('Ellbow bleibt x-sicher (gesamte Kette dreht um x)', () => { + // Ellbow.origin.x (0) + Arm1.origin.x (110) + local x (125) + expect(modelWorldXAtSliderZero(links, 'Ellbow', [125, 0, 0])) + .toEqual({ worldX: 235, xSafe: true }); + }); + + test('Arm2 ist NICHT x-sicher (dreht um y)', () => { + const r = modelWorldXAtSliderZero(links, 'Arm2', [0, -100, 0]); + expect(r.xSafe).toBe(false); + }); + + test('unbekannter Link → worldX = local x, xSafe bleibt true', () => { + expect(modelWorldXAtSliderZero(links, 'DoesNotExist', [7, 0, 0])) + .toEqual({ worldX: 7, xSafe: true }); + }); +}); + +describe('estimateXFromParsed', () => { + const links = makeLinks(); + + test('rechnet den Gelenk-Offset heraus (Arm1 + Ellbow), nicht roher Mittelwert', () => { + // Alle Marker sind so gewählt, dass der implizierte Slider exakt 40 ist. + const aruco = { markers: [ + { marker_id: 198, link: 'Arm1', position_mm: [150, 0, 0] }, // 150 - 110 = 40 + { marker_id: 229, link: 'Arm1', position_mm: [150, 0, 0] }, // 150 - 110 = 40 + { marker_id: 197, link: 'Arm1', position_mm: [115, 0, 0] }, // 115 - 75 = 40 + { marker_id: 244, link: 'Ellbow', position_mm: [275, 0, 0] }, // 275 - 235 = 40 + ]}; + expect(estimateXFromParsed(aruco, links)).toBeCloseTo(40, 6); + // Roher Mittelwert (alter Bug) wäre (150+150+115+275)/4 = 172.5 → NICHT das. + expect(estimateXFromParsed(aruco, links)).not.toBeCloseTo(172.5, 1); + }); + + test('überspringt Board, unbekannte IDs, x-unsichere Ketten und fehlende Positionen', () => { + const aruco = { markers: [ + { marker_id: 198, link: 'Arm1', position_mm: [150, 0, 0] }, // zählt → 40 + { marker_id: 229, link: 'Arm1', position_mm: [150, 0, 0] }, // zählt → 40 + { marker_id: 5, link: 'Board', position_mm: [999, 0, 0] }, // Board → skip + { marker_id: 999, link: 'Arm1', position_mm: [500, 0, 0] }, // unbekannte ID → skip + { marker_id: 300, link: 'Arm2', position_mm: [500, 0, 0] }, // x-unsicher → skip + { marker_id: 197, link: 'Arm1' }, // kein position_mm → skip + ]}; + expect(estimateXFromParsed(aruco, links)).toBeCloseTo(40, 6); + }); + + test('Fallback ohne robot.json: roher Mittelwert der Nicht-Board-Marker', () => { + const aruco = { markers: [ + { marker_id: 198, link: 'Arm1', position_mm: [150, 0, 0] }, + { marker_id: 229, link: 'Arm1', position_mm: [150, 0, 0] }, + { marker_id: 197, link: 'Arm1', position_mm: [115, 0, 0] }, + { marker_id: 5, link: 'Board', position_mm: [999, 0, 0] }, // Board zählt nicht + ]}; + // (150 + 150 + 115) / 3 = 138.333… + expect(estimateXFromParsed(aruco, {})).toBeCloseTo(138.3333, 3); + }); + + test('keine Marker → 0.0', () => { + expect(estimateXFromParsed({ markers: [] }, links)).toBe(0.0); + expect(estimateXFromParsed({}, links)).toBe(0.0); + }); +}); + +describe('Regression gegen echte robot.json', () => { + test('Arm1-Marker bei beobachtetem x=150/150/115 → Slider ≈ 40 (nicht ~138)', () => { + const robotPath = path.resolve(__dirname, '../scripts/robot_1781069752019.json'); + const links = JSON.parse(fs.readFileSync(robotPath, 'utf8')).links; + + // Marker-IDs/locals stammen aus robot.json (Arm1: 198 [0,..], 229 [0,..], 197 [-35,..]); + // Arm1.origin.x = 110, Base.origin.x = 0. + const aruco = { markers: [ + { marker_id: 198, link: 'Arm1', position_mm: [150, 12, 60] }, + { marker_id: 229, link: 'Arm1', position_mm: [150, -8, 60] }, + { marker_id: 197, link: 'Arm1', position_mm: [115, 3, 25] }, + ]}; + + const x = estimateXFromParsed(aruco, links); + expect(x).toBeCloseTo(40, 6); // korrekt: Offset herausgerechnet + expect(x).not.toBeCloseTo(138.3, 1); // alter Bug (roher Mittelwert) ausgeschlossen + }); +});