appRobotHoming/test/homingXEstimate.test.js

/**
 * 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
  });
});