Refactor Claude

2026-06-14 22:35:44 +02:00
parent 42f042c9d0
commit 375ee4cf69
4 changed files with 243 additions and 71 deletions
--- 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
--- 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;
  }
--- a/server/homingXEstimate.cjs
+++ 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 };
--- a/test/homingXEstimate.test.js
+++ 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
+  });
+});