ChK/appRobotHoming
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Refactor Claude

Browse Source
This commit is contained in:
chk
2026-06-14 22:35:44 +02:00
parent 42f042c9d0
commit 375ee4cf69
4 changed files with 243 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
doc/Homing_ROADMAP.md
View File

@@ -64,7 +64,7 @@ Foto alle Kameras
│ → aruco_marker_poses.json │ → aruco_marker_poses.json
▼ ▼
X-Position aus Marker-Positionen schätzen 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} 4b_revolute_angle.py --link Arm1 --x-mm {x_mm}
│ → state_Arm1.json (accumulated_state) │ → state_Arm1.json (accumulated_state)
@@ -95,12 +95,13 @@ X-Slider-Position über `--x-mm`.
| Komponente | Datei | Beschreibung | | Komponente | Datei | Beschreibung |
|-----------|-------|--------------| |-----------|-------|--------------|
| Board-Pipeline | `server/server.js` → `runBoardPipeline(runDir, send)` | Foto + Scripts 1, 2, 3b; von Board-Run und Homing genutzt | | 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 | | Homing-Orchestrator | `server/homingOrchestrator.js` → `runHoming()` | Kompletter Ablauf als SSE-Stream |
| Backend-Route | `POST /api/homing/run` | SSE-Stream, startet `runHoming()` | | Backend-Route | `POST /api/homing/run` | SSE-Stream, startet `runHoming()` |
| State senden | `POST /api/homing/send-state` | Weiterleitung an `ROBOT_URL/api/state` | | 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 | | 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}/` **Lauf-Verzeichnisse:** `data/homing/{timestamp}/`
@@ -144,7 +145,9 @@ die aktuelle Konfiguration.
## Offene Punkte ## Offene Punkte
- [ ] **Arm-Marker eintragen** (Nutzer): `links.Arm1/Ellbow/Arm2/Hand.markers` in `robot.json` - [~] **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 und eingetragenen Markern durchführen - [~] **Erstmals testen**: Homing-Run mit echtem Roboter — Arm1 erkannt (x, y); Ellbow scheitert noch an fehlenden Markern
- [ ] **X-Schätzung verfeinern** (optional): `estimateXFromMarkers()` könnte gelenk-spezifischere Logik nutzen statt einfachem Mittelwert - [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 - [ ] **robot.json via Driver-API** (optional): wenn Driver `GET ROBOT_URL/api/robot/config` bereitstellt

73
server/homingOrchestrator.js
View File

@@ -9,81 +9,24 @@
import path from 'path'; import path from 'path';
import fs from 'fs'; import fs from 'fs';
import fsPromises from 'fs/promises'; import fsPromises from 'fs/promises';
import { estimateXFromParsed } from './homingXEstimate.cjs';
/** /**
* Modell-Welt-x eines Markers bei slider=0, durch Aufsummieren der origin.x * Schätzt die Slider-X-Position aus den triangulierten Marker-Positionen.
* entlang der Kette Link→…→Base. Das Ergebnis ist winkel-unabhängig (und damit * Dünner I/O-Wrapper: liest die Dateien und delegiert die reine Geometrie an
* exakt) genau dann, wenn alle revolute-Gelenke der Kette um die x-Achse drehen * `estimateXFromParsed()` (`server/homingXEstimate.cjs`, unit-getestet).
* (Rotation um x erhält die x-Koordinate). Andernfalls xSafe=false.
* *
* @param {object} links robot.json links * @param {string} arucoJsonPath Pfad zu aruco_marker_poses.json
* @param {string} linkName * @param {string} [robotJsonPath] Pfad zu robot.json (für den Gelenk-Offset)
* @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 * @returns {number} x_mm
*/ */
export function estimateXFromMarkers(arucoJsonPath, robotJsonPath) { export function estimateXFromMarkers(arucoJsonPath, robotJsonPath) {
try { try {
const data = JSON.parse(fs.readFileSync(arucoJsonPath, 'utf8')); const arucoData = JSON.parse(fs.readFileSync(arucoJsonPath, 'utf8'));
const links = robotJsonPath const links = robotJsonPath
? (JSON.parse(fs.readFileSync(robotJsonPath, 'utf8')).links ?? {}) ? (JSON.parse(fs.readFileSync(robotJsonPath, 'utf8')).links ?? {})
: {}; : {};
return estimateXFromParsed(arucoData, 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 { } catch {
return 0.0; return 0.0;
} }

90
server/homingXEstimate.cjs Normal file
View File

@@ -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 };

136
test/homingXEstimate.test.js Normal file
View File

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