From c1f29bc1ee56d12baa5812614b825a5abd932e40 Mon Sep 17 00:00:00 2001 From: chk <79915315+ChKendel@users.noreply.github.com> Date: Fri, 26 Jun 2026 07:18:30 +0200 Subject: [PATCH] parallel foto --- doc/Homing_8_appRobotDriver.md | 191 +++++++++++++++++++++++++++++++++ server/server.js | 35 +++--- 2 files changed, 212 insertions(+), 14 deletions(-) create mode 100644 doc/Homing_8_appRobotDriver.md diff --git a/doc/Homing_8_appRobotDriver.md b/doc/Homing_8_appRobotDriver.md new file mode 100644 index 0000000..1f97e66 --- /dev/null +++ b/doc/Homing_8_appRobotDriver.md @@ -0,0 +1,191 @@ +# Homing 8 – Übergabe an appRobotDriver (G92-Konvention) + +> Technische Doku zur Konvertierung des Homing-Ergebnisses +> (FK-State aus [`4b_revolute_angle.py`](../scripts/4b_revolute_angle.py) / +> [`5_pose_estimation.py`](../scripts/5_pose_estimation.py)) +> in die G92-Konvention von **appRobotDriver**. +> +> Umgesetzt in [`server/server.js`](../server/server.js) → Funktion `fkStateToDriverG92()`, +> aufgerufen im `POST /api/homing/send-state`-Handler. +> +> Vollständige Driver-Konvention: [`appRobotDriver/doc/Info_G92.md`](../../appRobotDriver/doc/Info_G92.md). + +--- + +## Das Problem: zwei verschiedene Winkel-Konventionen + +Die Homing-Pipeline (4b / 5_pose_estimation) liefert Gelenkwinkel im +**FK-Koordinatensystem von `robot.json`** (forward kinematics, Blender-Hierarchie). +Der appRobotDriver erwartet in `G92` eine **eigene Konvention**, die sich an +physischen Konfigurationen und Motor-Nullpunkten orientiert — nicht an der FK-Nulllage. + +Für X (Schiene), Y (Oberarm), A (Unterarm-Dreher) stimmen die Konventionen überein. +Für **B, C und Z** gibt es definierte Verschiebungen, die vor dem Senden umgerechnet +werden müssen. + +--- + +## Befehlsformat G92 + +``` +G92 X Y<°> Z<°> A<°> B<°> C<°> E +``` + +Der Befehl setzt am Driver die Motorposition **ohne Bewegung** (intern M92 = Homing). +Fehlende Achsen werden weggelassen; der Driver lässt sie unverändert. + +--- + +## Umrechnungstabelle FK → Driver + +| Achse | FK-State (Homing) | Driver G92 erwartet | Umrechnung | +|-------|--------------------------------------------|--------------------------------------------|--------------------------| +| X | xMotor in mm | xMotor in mm | — (identisch) | +| Y | Oberarm-Winkel α, absolut (0=waagerecht) | α absolut (0=waagerecht, 90=hoch) | — (identisch) | +| Z | Ellbogen-Knick **relativ** zu Arm1 | Unterarm-Winkel β **absolut** (wie Y) | `Z = Y + z_relativ` | +| A | Unterarm-Dreher (Arm2-Rotation um -y) | Unterarm-Dreher (Roll um Unterarm-Achse) | — (identisch) | +| B | FK `b=0` = gerade Hand (kein Knick) | `B=180°` = gerade Hand, `B=0°` = zurück | `B = 180 − b` | +| C | FK `c=0` = neutraler Palm-Roll | `C=90°` = neutral (ψ=0°) | `C = c + 90` | +| E | Finger-Öffnung in mm (rein geometrisch) | Finger-Öffnung in mm (Sehnen-Kopplung im Driver) | — (identisch) | + +--- + +## Detailerklärung je Achse + +### Z — Ellbogen: relativ vs. absolut + +`4b_revolute_angle.py` misst den Ellbogen-Winkel **relativ zu Arm1** (FK-Variable z = +Rotation des Ellbow-Gelenks gegenüber der Arm1-Nulllage). Der Driver interpretiert Z +dagegen als **absoluten** Unterarm-Winkel zur Horizontalen — genauso wie Y den +Oberarm beschreibt. + +``` +Z_Driver = Y_FK + z_relativ_FK +``` + +Intern rechnet der Driver für die FluidNC-Weiterleitung wieder zurück: +`FluidNC-z = (β − α) × D`, d.h. relativer Motor-Winkel. + +> **Typische Falle:** z_relativ direkt als Z senden. Bei kleinen Y-Winkeln (Y ≈ 0) ist +> der Fehler kaum merklich; bei Y = 86° beträgt er 86°. + +--- + +### B — Hand-Knick: 180°-Dreher + +Im `robot.json`: +```json +"Hand": { + "jointToParent": { "axis": [1, 0, 0], "rotation": [0, 0, 0], "variable": "b" } +} +``` + +Die FK-Nulllage (`b = 0`) bedeutet: Hand verlängert Arm2 geradeaus — kein Knick. +Der Driver definiert dagegen: + +| B (G92) | physischer Knick Unterarm↔Hand | +|---------|-------------------------------| +| 0° | 180° (Hand voll zurückgeklappt) | +| 90° | 90° (Hand ⊥ Unterarm) | +| 180° | 0° (Hand gerade) | + +Ohne Umrechnung wird `b ≈ 0` (gerade Hand) als `B = 0°` gesendet → Driver klappt +die Hand voll zurück → „rückwärts haltende Hand-Stellung". + +``` +B_Driver = 180 − b_FK +``` + +--- + +### C — Palm-Roll: 90°-Offset + +Im `robot.json`: +```json +"Palm": { + "jointToParent": { "axis": [0, -1, 0], "rotation": [0, 0, 0], "variable": "c" } +} +``` + +Die FK-Nulllage (`c = 0`) entspricht keiner Rotation des Palm-Gelenks = neutraler Roll. +Der Driver definiert `C = 90°` als neutral (ψ = 0°): + +| C (G92) | Hand-Roll ψ | +|---------|-------------| +| 0° | −90° | +| 90° | 0° (neutral) | +| 180° | +90° | + +``` +C_Driver = c_FK + 90 +``` + +> **Vorzeichen prüfen:** Falls nach dem Fix die Palm-Rotation spiegelverkehrt erscheint, +> lautet die Formel `C = 90 − c_FK` (Vorzeichen des physischen Motors umgekehrt). +> Hängt von der Einbaurichtung des Palm-Servos ab. + +--- + +### E — Greifer: Sehnen-Kopplung + +E wird als reine Finger-Öffnung in mm übergeben (FK und Driver identisch — keine +Winkelumrechnung). Der Driver berechnet den tatsächlichen Motor-Wert intern: + +``` +eMotor = E − b − c (b, c in Radiant!) +``` + +Die Sehnen-Kopplung kompensiert, dass Handgelenk-Knick (B) und Palm-Roll (C) +an der Greifer-Sehne ziehen. Sind B und C korrekt gesetzt, stimmt die Kompensation +automatisch — E selbst braucht nicht angepasst zu werden. + +--- + +## Implementierung + +**`server/server.js`** — Funktion `fkStateToDriverG92()`: + +```js +function fkStateToDriverG92(s) { + const d = { ...s }; + if (d.b != null) d.b = 180 - d.b; // Hand-Knick: 180°-Dreher + if (d.c != null) d.c = d.c + 90; // Palm-Roll: +90° Offset + if (d.z != null && d.y != null) d.z = d.y + d.z; // Ellbogen: relativ → absolut + return d; +} +``` + +Aufruf im `POST /api/homing/send-state`-Handler: + +```js +const gcode = buildG92(fkStateToDriverG92(state)); +``` + +Null-Werte (unbeobachtbare Achsen) bleiben null → werden von `buildG92` weggelassen → +Driver lässt die entsprechenden Achsen unverändert. + +--- + +## Kontext: Woher kommen die FK-Werte? + +``` +4b_revolute_angle.py × N → accumulated_state {x, y, z, a, b} + (Hand/b schlägt fehl, wenn keine Marker am Hand-Link) + ↓ (falls Hand/b fehlt) +5_pose_estimation.py → movements {x,y,z,a,b,c,e} als FK-Winkel (Bundle-Adjustment) + ↓ +homingOrchestrator.js → finalState (flaches {x,y,z,a,b,c,e}) + ↓ +POST /api/homing/send-state + ↓ +fkStateToDriverG92() → Konvertierung FK → Driver + ↓ +buildG92() → "G92 X… Y… Z… A… B… C… E…" + ↓ +sendGcode() → appRobotDriver (WebSocket) +``` + +Im aktuellen `robot_1781069752019.json` hat der Hand-Link **keine eigenen Marker**; +der b-Wert kommt daher immer aus `5_pose_estimation.py` (Bundle-Adjustment über +die FingerA/FingerB-Marker, die durch die kinematische Kette Hand→Palm→Finger +indirekt b einschränken). diff --git a/server/server.js b/server/server.js index 09f55fd..ae466c5 100755 --- a/server/server.js +++ b/server/server.js @@ -513,24 +513,31 @@ async function runBoardPipeline(runDir, send, { refSet } = {}) { send({ type: 'log', text: `▶ Kameras: ${cameraIds.join(', ')}` }); send({ type: 'log', text: '' }); - // Pro Kamera: Foto → Script 1 → Script 2 - for (const camId of cameraIds) { + // Phase 1: alle Kameras gleichzeitig fotografieren (Modus-Umschaltung parallel) + send({ type: 'log', text: 'Fotos aufnehmen …' }); + const snapResults = await Promise.all( + cameraIds.map(async (camId) => { + let snapResp; + for (let attempt = 1; attempt <= 2; attempt++) { + snapResp = await new WebcamClient(WEBCAM_URL).getSnapshot(camId, true); + if (snapResp.status !== 503) break; + if (attempt < 2) await new Promise(r => setTimeout(r, 2000)); + } + if (!snapResp.ok) return { camId, imgPath: null, error: `HTTP ${snapResp.status}` }; + const imgPath = path.join(runDir, `${camId}.jpg`); + await fsPromises.writeFile(imgPath, Buffer.from(await snapResp.arrayBuffer())); + return { camId, imgPath, error: null }; + }) + ); + + // Phase 2: Scripts 1 + 2 pro Kamera (sequenziell, damit Logs lesbar bleiben) + for (const { camId, imgPath, error } of snapResults) { send({ type: 'log', text: `─── ${camId} ${'─'.repeat(40 - camId.length)}` }); - // Snapshot - send({ type: 'log', text: 'Foto aufnehmen …' }); - let snapResp; - for (let attempt = 1; attempt <= 2; attempt++) { - snapResp = await new WebcamClient(WEBCAM_URL).getSnapshot(camId, true); - if (snapResp.status !== 503) break; - if (attempt < 2) await new Promise(r => setTimeout(r, 2000)); - } - if (!snapResp.ok) { - send({ type: 'log', text: `⚠ HTTP ${snapResp.status} – Kamera übersprungen` }); + if (error) { + send({ type: 'log', text: `⚠ ${error} – Kamera übersprungen` }); continue; } - const imgPath = path.join(runDir, `${camId}.jpg`); - await fsPromises.writeFile(imgPath, Buffer.from(await snapResp.arrayBuffer())); send({ type: 'log', text: `✅ Foto: ${camId}.jpg` }); // NPZ suchen – neueste Session, die eine NPZ für diese Kamera enthält