From aa78116837d743e742862379408e667efed224a3 Mon Sep 17 00:00:00 2001 From: chk <79915315+ChKendel@users.noreply.github.com> Date: Fri, 19 Jun 2026 06:43:06 +0200 Subject: [PATCH] boardViewer --- doc/accessRobotAPI.md | 25 +++-- doc/homingAPI.md | 135 ++++++++++++++++++++--- package-lock.json | 98 ++++++++++++++++- package.json | 3 +- public/boardViewer.html | 28 +++-- public/sceneViewer.html | 67 +++++------ server/homingOrchestrator.js | 102 +++++++++++++++++ server/server.js | 208 ++++++++++++++++++++++++++++++++++- 8 files changed, 593 insertions(+), 73 deletions(-) diff --git a/doc/accessRobotAPI.md b/doc/accessRobotAPI.md index db4be8e..53e3f23 100644 --- a/doc/accessRobotAPI.md +++ b/doc/accessRobotAPI.md @@ -1,14 +1,25 @@ # robot.json – Zugriff via appRobotDriver -> Stand: 2026-06-15 -> Beschreibt die geplante Umstellung: robot.json kommt vom appRobotDriver, nicht -> mehr aus einer lokalen Datei. +> **Status: umgesetzt** (2026-06-17) — `server/robotConfig.js` ist aktiv. +> Dieses Dokument beschreibt Entwurf und Implementierung. Der Implementierungsplan +> (Schritte 1–3) ist vollständig abgearbeitet. --- -## Ist-Zustand +## Verhalten je Env-Variable -`appRobotHoming` liest und schreibt die Roboter-Konfiguration direkt aus einer +| Variable | nicht gesetzt | gesetzt | +|----------|--------------|---------| +| `ROBOT_URL` | Kein Driver-Kontakt; alle Lese-/Schreibvorgänge direkt auf die lokale Datei | `fetchRobot()` liest von `GET {ROBOT_URL}/api/robot/config`; `pushRobot()` schreibt nach `POST {ROBOT_URL}/api/robot/config` | +| `ROBOT_JSON` | Standardpfad `scripts/robot_1781069752019.json` | Angegebener Pfad wird als lokale Cache-Datei verwendet | + +Beide Variablen nicht gesetzt = **reiner Lokal-Modus**, identisch zum Verhalten vor dem Umbau. + +--- + +## Ehemaliger Ist-Zustand (vor 2026-06-17) + +`appRobotHoming` las und schrieb die Roboter-Konfiguration direkt aus einer lokalen Datei: ``` @@ -16,8 +27,8 @@ ROBOT_JSON = process.env.ROBOT_JSON || 'scripts/robot_1781069752019.json' ``` -Die Python-Skripte erhalten den Dateipfad als CLI-Argument (`-robot`, `--robot`). -Alle Kalibrierungs-Endpoints schreiben ebenfalls in diese Datei. +Die Python-Skripte erhielten den Dateipfad als CLI-Argument (`-robot`, `--robot`). +Alle Kalibrierungs-Endpoints schrieben ebenfalls in diese Datei. **Problem:** Der appRobotDriver besitzt die maßgebliche Konfiguration — nicht das Homing-System. Nach einem Neustart könnten Konfiguration und Driver auseinanderlaufen. diff --git a/doc/homingAPI.md b/doc/homingAPI.md index 6ec772a..95de4b5 100644 --- a/doc/homingAPI.md +++ b/doc/homingAPI.md @@ -1,10 +1,12 @@ # Homing Offline-API -> Beschreibung eines geplanten HTTP-Endpunkts, der die vollständige Homing-Pipeline -> (Schritte 1–5) ohne Live-Kameras betreibt. Gedacht für die **Simulations-Pipeline** -> (`appRobotRendering`), die synthetische oder aufgezeichnete Bilder liefert und die -> aktuelle Pose-Erkennung von appRobotHoming nutzen möchte — ohne den Umweg über -> echte Kameras und den WebCam-Service. Stand: 2026-06-16, **noch nicht implementiert**. +> **Status: implementiert** (2026-06-18). +> `POST /api/homing/run-offline` ist aktiv in `server/server.js`. +> Abhängigkeit: `multer` (npm-Package, bereits installiert). +> +> Gedacht für die **Simulations-Pipeline** (`appRobotRendering`), die synthetische oder +> aufgezeichnete Bilder liefert und die aktuelle Pose-Erkennung von appRobotHoming nutzen +> möchte — ohne den Umweg über echte Kameras und den WebCam-Service. --- @@ -35,7 +37,7 @@ Ergebnisdateien zurückgeben. |---|---|---| | Bilder | WebCam-Service liefert auf Abruf | Caller liefert im Request | | NPZ | Server sucht neueste Session in `data/calibration/` | Caller liefert im Request | -| `robot.json` | Server nutzt `ROBOT_JSON`-Env | Caller liefert im Request | +| `robot.json` | Server nutzt `robotConfig.js` (Driver oder lokale Cache-Datei) | Caller liefert im Request | | Pipeline (1→2→3b→4b→5) | identisch | identisch | | Antwort | SSE-Stream während Lauf | Synchrones JSON mit allen Ergebnisdateien | | Zweck | Produktions-Homing | Simulation / Replay | @@ -119,6 +121,110 @@ Fehlerresponse immer `{ "error": "…", "log": ["…"] }`. --- +## Aufruf-Beispiele + +### curl + +```bash +curl -X POST https://thinkcentre.local:2093/api/homing/run-offline \ + -F "images=@cam0.jpg" \ + -F "images=@cam1.jpg" \ + -F "images=@cam2.jpg" \ + -F "calibrations=@cam0_calibration.npz" \ + -F "calibrations=@cam1_calibration.npz" \ + -F "calibrations=@cam2_calibration.npz" \ + -F "robot=@robot_1781069752019.json;type=application/json" \ + -F "refSet=A0" +``` + +Ohne `refSet` (alle Board-Marker als Referenz): +```bash +curl -X POST https://thinkcentre.local:2093/api/homing/run-offline \ + -F "images=@cam0.jpg" -F "images=@cam1.jpg" -F "images=@cam2.jpg" \ + -F "calibrations=@cam0_calibration.npz" -F "calibrations=@cam1_calibration.npz" \ + -F "calibrations=@cam2_calibration.npz" \ + -F "robot=@robot_1781069752019.json;type=application/json" +``` + +### JavaScript / fetch (Browser oder Node) + +```javascript +const formData = new FormData(); + +// Bilder – Dateiname MUSS {cameraId}.jpg sein +formData.append('images', cam0Blob, 'cam0.jpg'); +formData.append('images', cam1Blob, 'cam1.jpg'); +formData.append('images', cam2Blob, 'cam2.jpg'); + +// Kalibrierungen – Dateiname MUSS {cameraId}_calibration.npz sein +formData.append('calibrations', cam0NpzBlob, 'cam0_calibration.npz'); +formData.append('calibrations', cam1NpzBlob, 'cam1_calibration.npz'); +formData.append('calibrations', cam2NpzBlob, 'cam2_calibration.npz'); + +// robot.json +formData.append('robot', new Blob([JSON.stringify(robotJson)], { type: 'application/json' }), 'robot.json'); + +// optional +formData.append('refSet', 'A0'); + +const res = await fetch('/api/homing/run-offline', { method: 'POST', body: formData }); +const data = await res.json(); +// data.state → { x, y, z, a, b, c, e } +// data.files → { "aruco_marker_poses.json": {...}, "state_Arm1.json": {...}, … } +// data.log → ["▶ Kameras: cam0, cam1, cam2", …] +// data.runDir → "20260618_143022" +``` + +### Python (requests) + +```python +import requests + +url = 'https://thinkcentre.local:2093/api/homing/run-offline' + +files = [ + ('images', ('cam0.jpg', open('cam0.jpg', 'rb'), 'image/jpeg')), + ('images', ('cam1.jpg', open('cam1.jpg', 'rb'), 'image/jpeg')), + ('images', ('cam2.jpg', open('cam2.jpg', 'rb'), 'image/jpeg')), + ('calibrations', ('cam0_calibration.npz', open('cam0_calibration.npz', 'rb'), 'application/octet-stream')), + ('calibrations', ('cam1_calibration.npz', open('cam1_calibration.npz', 'rb'), 'application/octet-stream')), + ('calibrations', ('cam2_calibration.npz', open('cam2_calibration.npz', 'rb'), 'application/octet-stream')), + ('robot', ('robot.json', open('robot.json', 'rb'), 'application/json')), +] +data = {'refSet': 'A0'} # optional + +resp = requests.post(url, files=files, data=data, verify=False, timeout=120) +result = resp.json() +print(result['state']) # {'x': 312.4, 'y': 44.8, 'z': -12.1, ...} +``` + +### Wichtige Regeln für Dateinamen + +| Feld | Pflichtformat | Beispiel | +|------|--------------|---------| +| `images` | `{cameraId}.jpg` | `cam0.jpg`, `cam1.jpg` | +| `calibrations` | `{cameraId}_calibration.npz` | `cam0_calibration.npz` | +| `robot` | beliebig (wird intern als `robot_run.json` gespeichert) | `robot.json` | + +Das Pairing Bild ↔ NPZ erfolgt rein über den `cameraId`-Prefix. +Eine Kamera ohne passende NPZ wird übersprungen (kein Fehler, aber Log-Eintrag). + +### HTTP-Statuscodes + +| Code | Bedeutung | +|------|-----------| +| `200` | Erfolg: `{ ok: true, runDir, state, files, log }` | +| `400` | Pflichtfelder fehlen oder `robot.json` ist kein valides JSON | +| `422` | Pipeline abgebrochen: `aruco_marker_poses.json` nicht erzeugt (< 2 Kamera-Posen) | +| `500` | Homing fehlgeschlagen (4b-Kette + Fallback gescheitert) | + +### Timeout-Hinweis + +Die Pipeline kann 20–60 s dauern. Client-Timeout auf **≥ 120 s** setzen +(curl: `--max-time 120`, requests: `timeout=120`, nginx/Caddy: `proxy_read_timeout 120s`). + +--- + ## Datenfluss (detailliert) ``` @@ -179,7 +285,7 @@ holt, sondern aus dem vorab befüllten `{runDir}`. --- -## Umsetzungsplan +## Umsetzungsplan (abgeschlossen 2026-06-18) ### Schritt 1 — `runBoardPipelineOffline(runDir, send, opts)` (Kern) @@ -260,7 +366,7 @@ setzen. Alternativ: SSE-Variante (gleiche Daten, aber inkrementell gestreamt). ### Schritt 5 — Aufräumen temporärer `robot.json` **Was:** Die hochgeladene `robot.json` wird nur für diesen Lauf im `{runDir}` als -`robot_run.json` gespeichert. Sie liegt **nicht** an der Stelle von `ROBOT_JSON` +`robot_run.json` gespeichert. Sie liegt **nicht** an der Stelle von `robotCachePath` (aus `robotConfig.js`) und wird daher nie vom Live-Modus versehentlich gelesen. Kein Konflikt. **Risiko:** keines — rein additive Datei in einem neuen Verzeichnis. @@ -329,15 +435,12 @@ dieser Implementierung). --- -## Offene Entscheidungen +## Offene Entscheidungen (getroffen) -- [ ] `multer` hinzufügen (`npm install multer`) oder `busboy` nutzen? -- [ ] Offline-Runs in `data/homing-offline/` oder `data/homing/` speichern? - (`data/homing/` würde sie im boardViewer sichtbar machen, was praktisch sein kann) -- [ ] Timeout-Handling: synchrone Antwort mit Timeout-Header-Hinweis **oder** SSE-Stream - wie Live-Modus (Client liest bis `done`-Event)? -- [ ] Soll `5_pose_estimation.py` immer laufen (auch nach vollständiger 4b-Kette), - oder nur als Fallback wie im Live-Modus? +- [x] `multer` (v2.2.0) — installiert, `diskStorage` schreibt direkt in `{runDir}` +- [x] Offline-Runs in `data/homing-offline/` (eigenes Verzeichnis, nicht im boardViewer) +- [x] Synchrone Antwort — Pipeline läuft durch, dann JSON; Client-Timeout ≥ 120 s empfohlen +- [x] `5_pose_estimation.py` nur als Fallback (identisch zum Live-Modus) --- diff --git a/package-lock.json b/package-lock.json index 530d495..182db1c 100644 --- a/package-lock.json +++ b/package-lock.json @@ -9,7 +9,8 @@ "version": "0.1.0", "dependencies": { "dotenv": "^16.4.5", - "express": "^4.19.2" + "express": "^4.19.2", + "multer": "^2.2.0" }, "devDependencies": { "jest": "^29.7.0", @@ -1256,6 +1257,12 @@ "node": ">= 8" } }, + "node_modules/append-field": { + "version": "1.0.0", + "resolved": "https://registry.npmjs.org/append-field/-/append-field-1.0.0.tgz", + "integrity": "sha512-klpgFSWLW1ZEs8svjfb7g4qWY0YS5imI82dTg+QahUvJ8YqAY0P10Uk8tTyh9ZGuYEZEMaeJYCF5BFuX552hsw==", + "license": "MIT" + }, "node_modules/argparse": { "version": "1.0.10", "resolved": "https://registry.npmjs.org/argparse/-/argparse-1.0.10.tgz", @@ -1524,9 +1531,19 @@ "version": "1.1.2", "resolved": "https://registry.npmjs.org/buffer-from/-/buffer-from-1.1.2.tgz", "integrity": "sha512-E+XQCRwSbaaiChtv6k6Dwgc+bx+Bs6vuKJHHl5kox/BaKbhiXzqQOwK4cO22yElGp2OCmjwVhT3HmxgyPGnJfQ==", - "dev": true, "license": "MIT" }, + "node_modules/busboy": { + "version": "1.6.0", + "resolved": "https://registry.npmjs.org/busboy/-/busboy-1.6.0.tgz", + "integrity": "sha512-8SFQbg/0hQ9xy3UNTB0YEnsNBbWfhf7RtnzpL7TkBiTBRfrQ9Fxcnz7VJsleJpyp6rVLvXiuORqjlHi5q+PYuA==", + "dependencies": { + "streamsearch": "^1.1.0" + }, + "engines": { + "node": ">=10.16.0" + } + }, "node_modules/bytes": { "version": "3.1.2", "resolved": "https://registry.npmjs.org/bytes/-/bytes-3.1.2.tgz", @@ -1754,6 +1771,21 @@ "dev": true, "license": "MIT" }, + "node_modules/concat-stream": { + "version": "2.0.0", + "resolved": "https://registry.npmjs.org/concat-stream/-/concat-stream-2.0.0.tgz", + "integrity": "sha512-MWufYdFw53ccGjCA+Ol7XJYpAlW6/prSMzuPOTRnJGcGzuhLn4Scrz7qf6o8bROZ514ltazcIFJZevcfbo0x7A==", + "engines": [ + "node >= 6.0" + ], + "license": "MIT", + "dependencies": { + "buffer-from": "^1.0.0", + "inherits": "^2.0.3", + "readable-stream": "^3.0.2", + "typedarray": "^0.0.6" + } + }, "node_modules/content-disposition": { "version": "0.5.4", "resolved": "https://registry.npmjs.org/content-disposition/-/content-disposition-0.5.4.tgz", @@ -3978,6 +4010,25 @@ "integrity": "sha512-Tpp60P6IUJDTuOq/5Z8cdskzJujfwqfOTkrwIwj7IRISpnkJnT6SyJ4PCPnGMoFjC9ddhal5KVIYtAt97ix05A==", "license": "MIT" }, + "node_modules/multer": { + "version": "2.2.0", + "resolved": "https://registry.npmjs.org/multer/-/multer-2.2.0.tgz", + "integrity": "sha512-6rdyFg2kLrMh9Jee7/BMPuV9lEAd7lLW2YUpF9/YxR7njyoUwwQ0ZPh3TaIY50Sw6vlyD2HW3wGOkTS4P79xrQ==", + "license": "MIT", + "dependencies": { + "append-field": "^1.0.0", + "busboy": "^1.6.0", + "concat-stream": "^2.0.0", + "type-is": "^1.6.18" + }, + "engines": { + "node": ">= 10.16.0" + }, + "funding": { + "type": "opencollective", + "url": "https://opencollective.com/express" + } + }, "node_modules/natural-compare": { "version": "1.4.0", "resolved": "https://registry.npmjs.org/natural-compare/-/natural-compare-1.4.0.tgz", @@ -4554,6 +4605,20 @@ "dev": true, "license": "MIT" }, + "node_modules/readable-stream": { + "version": "3.6.2", + "resolved": "https://registry.npmjs.org/readable-stream/-/readable-stream-3.6.2.tgz", + "integrity": "sha512-9u/sniCrY3D5WdsERHzHE4G2YCXqoG5FTHUiCC4SIbr6XcLZBY05ya9EKjYek9O5xOAwjGq+1JdGBAS7Q9ScoA==", + "license": "MIT", + "dependencies": { + "inherits": "^2.0.3", + "string_decoder": "^1.1.1", + "util-deprecate": "^1.0.1" + }, + "engines": { + "node": ">= 6" + } + }, "node_modules/readdirp": { "version": "3.6.0", "resolved": "https://registry.npmjs.org/readdirp/-/readdirp-3.6.0.tgz", @@ -4934,6 +4999,23 @@ "node": ">= 0.8" } }, + "node_modules/streamsearch": { + "version": "1.1.0", + "resolved": "https://registry.npmjs.org/streamsearch/-/streamsearch-1.1.0.tgz", + "integrity": "sha512-Mcc5wHehp9aXz1ax6bZUyY5afg9u2rv5cqQI3mRrYkGC8rW2hM02jWuwjtL++LS5qinSyhj2QfLyNsuc+VsExg==", + "engines": { + "node": ">=10.0.0" + } + }, + "node_modules/string_decoder": { + "version": "1.3.0", + "resolved": "https://registry.npmjs.org/string_decoder/-/string_decoder-1.3.0.tgz", + "integrity": "sha512-hkRX8U1WjJFd8LsDJ2yQ/wWWxaopEsABU1XfkM8A+j0+85JAGppt16cr1Whg6KIbb4okU6Mql6BOj+uup/wKeA==", + "license": "MIT", + "dependencies": { + "safe-buffer": "~5.2.0" + } + }, "node_modules/string-length": { "version": "4.0.2", "resolved": "https://registry.npmjs.org/string-length/-/string-length-4.0.2.tgz", @@ -5161,6 +5243,12 @@ "node": ">= 0.6" } }, + "node_modules/typedarray": { + "version": "0.0.6", + "resolved": "https://registry.npmjs.org/typedarray/-/typedarray-0.0.6.tgz", + "integrity": "sha512-/aCDEGatGvZ2BIk+HmLf4ifCJFwvKFNb9/JeZPMulfgFracn9QFcAf5GO8B/mweUjSoblS5In0cWhqpfs/5PQA==", + "license": "MIT" + }, "node_modules/undefsafe": { "version": "2.0.5", "resolved": "https://registry.npmjs.org/undefsafe/-/undefsafe-2.0.5.tgz", @@ -5236,6 +5324,12 @@ "requires-port": "^1.0.0" } }, + "node_modules/util-deprecate": { + "version": "1.0.2", + "resolved": "https://registry.npmjs.org/util-deprecate/-/util-deprecate-1.0.2.tgz", + "integrity": "sha512-EPD5q1uXyFxJpCrLnCc1nHnq3gOa6DZBocAIiI2TaSCA7VCJ1UJDMagCzIkXNsUYfD1daK//LTEQ8xiIbrHtcw==", + "license": "MIT" + }, "node_modules/utils-merge": { "version": "1.0.1", "resolved": "https://registry.npmjs.org/utils-merge/-/utils-merge-1.0.1.tgz", diff --git a/package.json b/package.json index 6a7ace8..69fea4a 100755 --- a/package.json +++ b/package.json @@ -15,7 +15,8 @@ }, "dependencies": { "dotenv": "^16.4.5", - "express": "^4.19.2" + "express": "^4.19.2", + "multer": "^2.2.0" }, "devDependencies": { "jest": "^29.7.0", diff --git a/public/boardViewer.html b/public/boardViewer.html index a653815..48656fe 100644 --- a/public/boardViewer.html +++ b/public/boardViewer.html @@ -367,20 +367,24 @@ function buildSkeletonFK(robot, angles) { const markerSizeM = (m.size ?? 25) * S; const [nx, ny, nz] = m.normal ?? [0, 0, 1]; - // Marker-Orientierung ZUERST im lokalen Link-Frame bauen, DANN die volle - // childFrame-Rotation anwenden. So wird der Roll (Drehung des Markers um - // seine eigene Normale) korrekt mitgeführt — auch wenn die Link-Drehachse - // parallel zur Marker-Normale liegt (z.B. Marker 197: normal [-1,0,0] ∥ - // Arm1-Achse [-1,0,0]). Eine reine Welt-Normalen-Rekonstruktion würde - // genau diesen Anteil verlieren. - const nLocal = new THREE.Vector3(nx, nz, -ny).normalize(); // robot→three.js + // Marker-Orientierung ZUERST im lokalen ROBOT-Frame bauen (rohe Normale: + // Minimal-Rotation [0,0,1]→Normale + Spin um die Normale), DANN über qView + // in three.js-Achsen und mit qFrame in die Welt drehen. Würde man die + // Normale wie früher schon VOR der Minimal-Rotation nach three.js drehen + // (nx,nz,-ny), verdreht eine schräg liegende Normale (z.B. [-1,0,1]) das + // Quadrat zusätzlich um ihren Azimut (~45°) um die eigene Achse; der Spin + // kann das nicht kompensieren. Der Link-Roll um die Normale bleibt + // erhalten, weil qFrame zuletzt wirkt (z.B. Marker 197: normal [-1,0,0] ∥ + // Arm1-Achse). Gegen triangulierte Ecken geprüft (Capture 20260616_133151, + // Marker 146): diese Reihenfolge 0.8°, die alte 45.5°. + const nRobot = new THREE.Vector3(nx, ny, nz).normalize(); // rohe robot-Normale const spinRad = ((m.spin ?? 0) * Math.PI) / 180; - const qNormalLoc = new THREE.Quaternion().setFromUnitVectors(new THREE.Vector3(0, 0, 1), nLocal); - const qSpinLoc = new THREE.Quaternion().setFromAxisAngle(nLocal, spinRad); - const qMarkerLoc = qSpinLoc.multiply(qNormalLoc); // Q_spin ∘ Q_normal (lokal) + const qNormalLoc = new THREE.Quaternion().setFromUnitVectors(new THREE.Vector3(0, 0, 1), nRobot); + const qSpinLoc = new THREE.Quaternion().setFromAxisAngle(nRobot, spinRad); + const qView = new THREE.Quaternion().setFromAxisAngle(new THREE.Vector3(1, 0, 0), -Math.PI / 2); // robot→three.js const qFrame = new THREE.Quaternion().setFromRotationMatrix(childFrame); - const qMarkerW = qFrame.clone().multiply(qMarkerLoc); // in Welt drehen - const normalW = nLocal.clone().applyQuaternion(qFrame).normalize(); + const qMarkerW = qFrame.clone().multiply(qView).multiply(qSpinLoc.multiply(qNormalLoc)); // lokal → three.js → Welt + const normalW = new THREE.Vector3(nx, nz, -ny).applyQuaternion(qFrame).normalize(); // P1: orientiertes Quadrat (Normale + Roll + Spin in einem Quaternion). // PlaneGeometry hat nativ die +Z-Normale, qMarkerW dreht +Z auf die diff --git a/public/sceneViewer.html b/public/sceneViewer.html index 9a45daa..3f131b7 100644 --- a/public/sceneViewer.html +++ b/public/sceneViewer.html @@ -483,39 +483,23 @@ function transformDirByT(T, dir) { ]; } -function makeMarkerSquare(pos, normal, size, color) { +// Marker-Quadrat mit vorab berechneter Orientierung (Quaternion). Die +// BoxGeometry ist dünn in lokal-Z, ihre Normale ist also lokal +Z — quat +// dreht +Z auf die Marker-Normale inkl. Link-Rotation und Spin. +// +// Die Orientierung MUSS im lokalen Link-Frame gebaut und erst danach in die +// Szene gedreht werden (siehe Aufrufer). Würde man wie früher +// setFromUnitVectors([0,0,1], welt_normale) NACH dem robot→three.js- +// Achsentausch anwenden, verdreht eine schräg liegende Normale (z.B. +// [-1,0,1]) das Quadrat zusätzlich um ihren Azimut (~45°) um die eigene +// Achse, und der Spin fehlt ganz. Gegen triangulierte Ecken geprüft +// (Capture 20260616_133151, Marker 146): lokale Variante 0.8°, alte 45.5°. +function makeMarkerSquareQuat(pos, quat, size, color) { const geo = new THREE.BoxGeometry(size, size, size * 0.1); - const mat = new THREE.MeshPhongMaterial({ - color, - shininess: 40 - }); - + const mat = new THREE.MeshPhongMaterial({ color, shininess: 40 }); const m = new THREE.Mesh(geo, mat); m.position.copy(pos); - - // Fallback falls keine gültige Normale vorhanden - let nx = 0, ny = 0, nz = 1; - - if (Array.isArray(normal) && normal.length >= 3) { - nx = Number(normal[0]) || 0; - ny = Number(normal[1]) || 0; - nz = Number(normal[2]) || 1; - } else if (normal instanceof THREE.Vector3) { - nx = normal.x; - ny = normal.y; - nz = normal.z; - } - - const n = new THREE.Vector3(nx, ny, nz); - - if (n.lengthSq() > 1e-12) { - n.normalize(); - m.quaternion.setFromUnitVectors( - new THREE.Vector3(0, 0, 1), - n - ); - } - + m.quaternion.copy(quat); return m; } @@ -837,8 +821,19 @@ function rebuild() { // ── model markers + normals ── const modelPositions = {}; const modelNormals = {}; + // robot→three.js view rotation (x,y,z)->(x,z,-y) == Rot_x(-90°). Applied LAST, + // so the marker orientation can be built in the robot/link frame first. + const qView = new THREE.Quaternion().setFromAxisAngle(new THREE.Vector3(1, 0, 0), -Math.PI / 2); for (const [lname, ld] of Object.entries(links)) { const col = linkColor(lname); + // link rotation in the robot frame (from FK), as a quaternion + const Tl = T[lname] || I4(); + const qLink = new THREE.Quaternion().setFromRotationMatrix(new THREE.Matrix4().set( + Tl[0], Tl[1], Tl[2], 0, + Tl[4], Tl[5], Tl[6], 0, + Tl[8], Tl[9], Tl[10], 0, + 0, 0, 0, 1 + )); for (const m of (ld.markers||[])) { if (!m.position) continue; const mid = m.id; @@ -849,8 +844,16 @@ function rebuild() { modelPositions[mid] = wp; modelNormals[mid] = nWorld; - const sq = makeMarkerSquare(r2vArr(wp), r2vDir(...nWorld), 0.022, col); - gModel.add(sq); + // Orientierung ZUERST im lokalen Link-Frame (robot): Minimal-Rotation + // [0,0,1]→Normale, dann Spin um diese Normale; DANN qLink (Link-Drehung) + // und qView (in die Szene). So bleibt der Roll des Links um die Normale + // erhalten und der Spin-Azimut-Twist entfällt (siehe makeMarkerSquareQuat). + const nLR = new THREE.Vector3(nLocal[0], nLocal[1], nLocal[2]).normalize(); + const spinRad = ((m.spin ?? 0) * Math.PI) / 180; + const qNormal = new THREE.Quaternion().setFromUnitVectors(new THREE.Vector3(0, 0, 1), nLR); + const qSpin = new THREE.Quaternion().setFromAxisAngle(nLR, spinRad); + const qMarker = qView.clone().multiply(qLink).multiply(qSpin.multiply(qNormal)); + gModel.add(makeMarkerSquareQuat(r2vArr(wp), qMarker, 0.022, col)); // normal arrow (length = half a marker size = ~12.5mm → 0.0125m) const arr = makeNormalArrow(r2vArr(wp), nWorld, 0.018, col); diff --git a/server/homingOrchestrator.js b/server/homingOrchestrator.js index 5c7ede2..05ae2a5 100644 --- a/server/homingOrchestrator.js +++ b/server/homingOrchestrator.js @@ -151,6 +151,108 @@ export async function runHoming({ } } +/** + * Führt den Homing-Ablauf offline aus (Bilder und NPZ bereits im runDir). + * Identische 4b-Kette wie runHoming — ohne Webcam-Zugriff und ohne SSE-Stream. + * send() akkumuliert Logs; done-Event trägt den finalen State. + * + * @param {{ + * robotJsonPath: string, + * runDir: string, + * send: (obj: object) => void, + * runScript: (args: string[], send: Function) => Promise, + * runBoardPipelineOffline:(runDir: string, send: Function) => Promise, + * SCRIPT_4B: string, + * SCRIPT_5POSE: string, + * }} opts + */ +export async function runHomingOffline({ + robotJsonPath, + runDir, + send, + runScript, + runBoardPipelineOffline, + SCRIPT_4B, + SCRIPT_5POSE, +}) { + send({ type: 'log', text: `▶ Homing-Offline: ${path.basename(runDir)}` }); + send({ type: 'log', text: `▶ Robot-JSON: ${robotJsonPath}` }); + send({ type: 'log', text: '' }); + + // ── Schritt 1: Marker-Triangulierung (Bilder liegen bereits im runDir) ────── + send({ type: 'step', step: 1, total: 5, text: 'Marker-Triangulierung …' }); + await runBoardPipelineOffline(runDir, send); + + const arucoJson = path.join(runDir, 'aruco_marker_poses.json'); + try { + await fsPromises.access(arucoJson); + } catch { + send({ type: 'error', text: '❌ aruco_marker_poses.json fehlt – Script 3b hat nicht funktioniert.' }); + send({ type: 'done', exitCode: -1 }); + return; + } + + // ── Schritt 2: X-Position schätzen ────────────────────────────────────────── + const xMm = estimateXFromMarkers(arucoJson, robotJsonPath); + send({ type: 'log', text: `▶ Geschätzte X-Position: ${xMm.toFixed(1)} mm` }); + send({ type: 'analysis', key: 'x_mm', value: xMm }); + + // ── Schritte 3–5 (2–4): 4b-Kette Arm1 → Ellbow → Arm2 → Hand ─────────────── + const links = ['Arm1', 'Ellbow', 'Arm2', 'Hand']; + let fromState = null; + let chainComplete = true; + + for (let i = 0; i < links.length; i++) { + const link = links[i]; + send({ type: 'step', step: 2 + i, total: 5, text: `Gelenkwinkel ${link} …` }); + send({ type: 'log', text: `\n─── 4b: ${link} ${'─'.repeat(35 - link.length)}` }); + + const outputPath = path.join(runDir, `state_${link}.json`); + const args = [SCRIPT_4B, '--robot', robotJsonPath, '--aruco', arucoJson, '--link', link, '--output', outputPath]; + if (fromState) args.push('--from-state', fromState); + else args.push('--x-mm', String(xMm)); + + const exit = await runScript(args, send); + if (exit !== 0) { + send({ type: 'log', text: `⚠ 4b ${link} Exit ${exit} — falle auf 5_pose_estimation.py zurück` }); + chainComplete = false; + break; + } + fromState = outputPath; + + try { + const stateData = JSON.parse(await fsPromises.readFile(outputPath, 'utf8')); + send({ type: 'analysis', key: `state_${link}`, value: stateData.accumulated_state ?? stateData }); + } catch { /* ignorieren */ } + } + + // ── Endergebnis ────────────────────────────────────────────────────────────── + try { + let finalState; + if (chainComplete) { + const finalData = JSON.parse(await fsPromises.readFile(fromState, 'utf8')); + finalState = finalData.accumulated_state ?? finalData; + } else { + send({ type: 'step', step: 5, total: 5, text: '5_pose_estimation.py (Fallback) …' }); + const poseOut = path.join(runDir, 'robot_state.json'); + const args = [SCRIPT_5POSE, arucoJson, '-robot', robotJsonPath, '-out', poseOut]; + if (fromState) args.push('--from-state', fromState); + const exit = await runScript(args, send); + if (exit !== 0) throw new Error(`5_pose_estimation.py Exit ${exit}`); + const poseData = JSON.parse(await fsPromises.readFile(poseOut, 'utf8')); + finalState = Object.fromEntries( + Object.entries(poseData.movements).map(([k, v]) => [k, v.value]) + ); + } + send({ type: 'log', text: '' }); + send({ type: 'log', text: '✅ Homing-Offline abgeschlossen' }); + send({ type: 'done', exitCode: 0, state: finalState }); + } catch (err) { + send({ type: 'error', text: `❌ Endzustand konnte nicht gelesen werden: ${err.message}` }); + send({ type: 'done', exitCode: -1 }); + } +} + /** Timestamp-String YYYYMMDD_HHmmss */ function makeTimestamp() { const now = new Date(); diff --git a/server/server.js b/server/server.js index 03a0a78..1a8eb26 100755 --- a/server/server.js +++ b/server/server.js @@ -9,7 +9,8 @@ import process from 'process'; import { spawn } from 'child_process'; import { WebcamClient } from './webcamClient.js'; import { assignByZRange, removeMarkerAssignment, alignSetToMeasured, assignMarkerId, adoptXAxis, assignFixedMarkersToLink, setJointOriginYZ, setArmMarkerSpin } from './editRobot.js'; -import { runHoming } from './homingOrchestrator.js'; +import multer from 'multer'; +import { runHoming, runHomingOffline } from './homingOrchestrator.js'; import { fetchRobot, robotCachePath } from './robotConfig.js'; const __filename = fileURLToPath(import.meta.url); @@ -438,8 +439,9 @@ app.post('/api/calibration/compute', async (req, res) => { // ── Board-Erkennung ─────────────────────────────────────────────────────────── -const boardDataDir = path.join(__dirname, '..', 'data', 'board'); -const homingDataDir = path.join(__dirname, '..', 'data', 'homing'); +const boardDataDir = path.join(__dirname, '..', 'data', 'board'); +const homingDataDir = path.join(__dirname, '..', 'data', 'homing'); +const homingOfflineDataDir = path.join(__dirname, '..', 'data', 'homing-offline'); const SCRIPT_1 = path.join(__dirname, '..', 'scripts', '1_detect_aruco_observations.py'); const SCRIPT_2 = path.join(__dirname, '..', 'scripts', '2_estimate_camera_from_observations.py'); const SCRIPT_3B = path.join(__dirname, '..', 'scripts', '3b_corner_marker_poses.py'); @@ -588,6 +590,120 @@ async function runBoardPipeline(runDir, send, { refSet } = {}) { send({ type: 'log', text: '' }); } +/** + * Board-Pipeline für Offline-Homing: Bilder und NPZs liegen bereits im runDir. + * Kein Webcam-Zugriff, keine NPZ-Suche — Scripts 1, 2, 3b werden identisch aufgerufen. + * + * Dateinamen-Konvention im runDir: + * {cameraId}.jpg – Kamerabild + * {cameraId}_calibration.npz – Kalibrierung + * robot_run.json – robot.json für diesen Lauf + * + * @param {string} runDir + * @param {Function} send + * @param {{ refSet?: string }} [opts] + */ +async function runBoardPipelineOffline(runDir, send, { refSet } = {}) { + const robotRunPath = path.join(runDir, 'robot_run.json'); + + const allFiles = await fsPromises.readdir(runDir); + const cameraIds = allFiles + .filter(f => /^[a-zA-Z0-9]+\.jpg$/.test(f)) + .map(f => path.basename(f, '.jpg')) + .sort(); + + send({ type: 'log', text: `▶ Kameras: ${cameraIds.join(', ')}` }); + send({ type: 'log', text: '' }); + + for (const camId of cameraIds) { + send({ type: 'log', text: `─── ${camId} ${'─'.repeat(40 - camId.length)}` }); + + const imgPath = path.join(runDir, `${camId}.jpg`); + const npzPath = path.join(runDir, `${camId}_calibration.npz`); + + try { await fsPromises.access(npzPath); } catch { + send({ type: 'log', text: `⚠ Keine NPZ für ${camId} – übersprungen` }); + continue; + } + + send({ type: 'log', text: '\n▷ 1_detect_aruco_observations' }); + const exit1 = await runScript([ + SCRIPT_1, + '-i', imgPath, + '-npz', npzPath, + '-robot', robotRunPath, + '-cameraId', camId, + '-outDir', runDir, + '--saveDebugImage', + ], send); + if (exit1 !== 0) { + send({ type: 'log', text: `❌ Script 1 Exit ${exit1}` }); + continue; + } + + const detJson = path.join(runDir, `${camId}_aruco_detection.json`); + try { await fsPromises.access(detJson); } catch { + send({ type: 'log', text: '⚠ Detection-JSON fehlt – Script 2 übersprungen' }); + continue; + } + send({ type: 'log', text: '\n▷ 2_estimate_camera_from_observations' }); + const script2Args = [SCRIPT_2, '-i', detJson, '-robot', robotRunPath, '-outDir', runDir]; + if (refSet) script2Args.push('--refSet', refSet); + const exit2 = await runScript(script2Args, send); + if (exit2 !== 0) send({ type: 'log', text: `❌ Script 2 Exit ${exit2}` }); + + send({ type: 'log', text: '' }); + } + + send({ type: 'log', text: '─── 3b: Marker-Triangulierung ────────────────────────────' }); + const runFiles3b = await fsPromises.readdir(runDir); + const numPoses = runFiles3b.filter(f => f.endsWith('_camera_pose.json')).length; + if (numPoses >= 2) { + send({ type: 'log', text: `▷ 3b_corner_marker_poses (${numPoses} Kamera-Posen)` }); + const exit3b = await runScript([SCRIPT_3B, '--evalDir', runDir, '--robot', robotRunPath], send); + if (exit3b !== 0) send({ type: 'log', text: `❌ Script 3b Exit ${exit3b}` }); + } else { + send({ type: 'log', text: `⚠ Nur ${numPoses} Kamera-Pose(n) – Script 3b braucht ≥2 Kameras` }); + } + send({ type: 'log', text: '' }); +} + +// ── Multer-Setup für Offline-Homing ────────────────────────────────────────── + +async function prepareOfflineRunDir(req, res, next) { + try { + const ts = makeTimestamp(); + const runDir = path.join(homingOfflineDataDir, ts); + await fsPromises.mkdir(runDir, { recursive: true }); + req.offlineRunDir = runDir; + req.offlineTs = ts; + next(); + } catch (err) { + res.status(500).json({ error: String(err) }); + } +} + +const offlineMulter = multer({ + storage: multer.diskStorage({ + destination: (req, file, cb) => cb(null, req.offlineRunDir), + filename: (req, file, cb) => { + const safe = path.basename(file.originalname).replace(/[^a-zA-Z0-9_.-]/g, '_'); + cb(null, safe); + }, + }), +}).fields([ + { name: 'images', maxCount: 10 }, + { name: 'calibrations', maxCount: 10 }, + { name: 'robot', maxCount: 1 }, +]); + +function runOfflineUpload(req, res, next) { + offlineMulter(req, res, (err) => { + if (err) return res.status(400).json({ error: `Upload-Fehler: ${err.message}` }); + next(); + }); +} + /** * POST /api/board/run * 1. Erstellt data/board/{timestamp}/ @@ -866,6 +982,92 @@ app.get('/api/homing/run-data', async (req, res) => { } }); +/** + * POST /api/homing/run-offline + * Vollständiger Homing-Ablauf ohne Live-Kameras. + * Bilder, NPZs und robot.json werden per multipart/form-data hochgeladen. + * Antwortet synchron mit { ok, runDir, state, files, log }. + * + * Felder: + * images – ein oder mehrere JPEG-Dateien, Name muss {cameraId}.jpg sein + * calibrations – je eine NPZ pro Kamera, Name muss {cameraId}_calibration.npz sein + * robot – robot.json für diesen Lauf (einmalig, wird nicht dauerhaft gespeichert) + * refSet – (Text, optional) Referenz-Set für Script 2, z. B. "A0" + */ +app.post('/api/homing/run-offline', + prepareOfflineRunDir, + runOfflineUpload, + async (req, res) => { + const runDir = req.offlineRunDir; + const ts = req.offlineTs; + const log = []; + + // robot.json validieren und als robot_run.json speichern + const robotFile = req.files?.robot?.[0]; + if (!robotFile) { + return res.status(400).json({ error: '"robot" fehlt – robot.json muss hochgeladen werden', log }); + } + let robotRunPath; + try { + const content = await fsPromises.readFile(robotFile.path, 'utf8'); + JSON.parse(content); // Syntaxprüfung + robotRunPath = path.join(runDir, 'robot_run.json'); + await fsPromises.rename(robotFile.path, robotRunPath); + } catch (err) { + return res.status(400).json({ error: `robot.json ungültig: ${err.message}`, log }); + } + + // Mindestens ein Bild erforderlich + if (!req.files?.images?.length) { + return res.status(400).json({ error: 'Mindestens ein Bild ("images") fehlt', log }); + } + + const refSet = req.body?.refSet || undefined; + + // Logs und done-Event akkumulieren + let finalState = null; + let exitCode = -1; + const send = (obj) => { + if (obj.type === 'log') log.push(obj.text); + if (obj.type === 'done') { finalState = obj.state ?? null; exitCode = obj.exitCode; } + }; + + try { + await runHomingOffline({ + robotJsonPath: robotRunPath, + runDir, + send, + runScript, + runBoardPipelineOffline: (dir, s) => runBoardPipelineOffline(dir, s, { refSet }), + SCRIPT_4B, + SCRIPT_5POSE, + }); + } catch (err) { + console.error('homing/run-offline error:', err); + return res.status(500).json({ error: String(err), log }); + } + + // Zu wenige Kameras → aruco_marker_poses.json fehlt + if (exitCode !== 0) { + const arucoExists = await fsPromises.access(path.join(runDir, 'aruco_marker_poses.json')) + .then(() => true).catch(() => false); + const status = arucoExists ? 500 : 422; + return res.status(status).json({ error: 'Homing fehlgeschlagen', log }); + } + + // Alle JSON-Ausgabedateien einlesen (robot_run.json ausgenommen) + const allFiles = await fsPromises.readdir(runDir).catch(() => []); + const files = {}; + for (const f of allFiles.sort()) { + if (f.endsWith('.json') && f !== 'robot_run.json') { + try { files[f] = JSON.parse(await fsPromises.readFile(path.join(runDir, f), 'utf8')); } catch {} + } + } + + return res.json({ ok: true, runDir: ts, state: finalState, files, log }); + } +); + // ── Robot-JSON bearbeiten ───────────────────────────────────────────────────── /**