Refactor Claude

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
+  });
+});