Compare commits

...

62 Commits

Author SHA1 Message Date
chk
1bbcb535aa Marker Swap 2026-06-26 07:30:14 +02:00
chk
c1f29bc1ee parallel foto 2026-06-26 07:18:30 +02:00
chk
dba2744687 G92 Angles according to Driver 2026-06-26 07:00:27 +02:00
chk
33dcbe72bf Multipoint zurück 2026-06-25 20:36:09 +02:00
chk
fab7032d56 Multipoint Schritt 4 2026-06-25 19:58:23 +02:00
chk
9bf49eff8d Multipoint Schritt 3 2026-06-25 19:23:37 +02:00
chk
da2a5d5ae6 G92 senden besser 2026-06-25 17:34:41 +02:00
chk
7818604c02 G92 senden 2026-06-25 17:16:30 +02:00
ChK
1db62e08df Punkte 2026-06-25 16:48:34 +02:00
chk
ce829d3875 Finger B korrekt drehen 2026-06-24 06:52:40 +02:00
ChK
fe08ebc08c zweiter Finger - verdreht 2026-06-24 06:30:24 +02:00
ChK
b9df99540d Finger1 Marker 2026-06-24 06:24:58 +02:00
ChK
2c0aeb718a new Marker 2026-06-23 22:44:38 +02:00
ChK
f9db05d073 Marker 2026-06-19 11:31:32 +02:00
chk
aa78116837 boardViewer 2026-06-19 06:43:06 +02:00
chk
d36ef6189d Homing API 2026-06-17 23:23:55 +02:00
chk
eb403dab36 Multipoint 2026-06-17 22:57:52 +02:00
chk
5f8e1a0189 Marker mit einer Kamera - Gaurds 2026-06-16 22:39:54 +02:00
chk
498499bf13 Draft 4ecken 2026-06-16 22:25:48 +02:00
chk
a3986beb6e Draft MultiPoint 2026-06-16 21:05:09 +02:00
chk
855f917d24 Reihenfolge sinnvoll gestalten 2026-06-16 19:46:33 +02:00
chk
f585c83689 Zustand Funktioniert 2026-06-16 19:37:01 +02:00
chk
366de4aad9 Wenn Hand abbricht > Weiter 2026-06-16 17:47:25 +02:00
chk
eae6b6098a Axis automatisch 2026-06-16 17:36:46 +02:00
chk
5f6d28673a MultiPose 2026-06-16 17:04:11 +02:00
chk
08d1c21d1e Homing Marker Rotation 2026-06-16 16:01:32 +02:00
chk
f929c19f4b Dokumentieren 2026-06-16 15:31:25 +02:00
chk
578b955508 4b kind-marker für winkel beachten 2026-06-16 15:28:14 +02:00
chk
0234c1ef1d 4b_revolute mit Fallback 2026-06-16 14:52:18 +02:00
chk
ad208b7d21 Neue Marker Unterarm 2026-06-16 13:58:20 +02:00
chk
f983d69a0c Neue Marker Unterarm 2026-06-16 13:55:42 +02:00
chk
ef4c7e6144 Base-Skeleton=Axis 2026-06-16 07:24:36 +02:00
chk
90f84d1221 Base-Skeleton=Axis 2026-06-16 06:34:25 +02:00
chk
13316b0d03 Base-Skeleton=Axis 2026-06-16 06:21:27 +02:00
chk
7ea5eda908 Y-Achsis callibration 2026-06-16 06:16:15 +02:00
chk
8ec601a087 Marker Spin 2026-06-16 05:52:01 +02:00
chk
2582b2adc9 Claude: Rotation-Fix 2026-06-16 05:48:34 +02:00
chk
a79729ca1f Marker Spin 2026-06-16 05:35:14 +02:00
chk
b854b44111 Marker Name falsch 2026-06-15 23:12:28 +02:00
chk
e8b48d17b5 Marker Spin 2026-06-15 23:06:04 +02:00
chk
487d35473a Arbeiten an Normalen 2026-06-15 23:00:38 +02:00
chk
f88e73c02f Marker - Callibration 2026-06-15 22:51:00 +02:00
chk
15d4175fd1 spin Marker Callibration 2026-06-15 09:23:21 +02:00
chk
375ee4cf69 Refactor Claude 2026-06-14 22:35:44 +02:00
chk
42f042c9d0 Marker error +110 2026-06-14 22:13:13 +02:00
chk
d56ccd8094 Marker im Robot.json 2026-06-14 21:55:23 +02:00
chk
1db66d06c0 Marker Update Live 2026-06-14 21:50:42 +02:00
chk
42742485f1 Marker fix 2026-06-14 21:31:30 +02:00
chk
dbfc915b66 Logs & Marker anzeigen 2026-06-14 21:18:29 +02:00
chk
8be4e2ff95 Marker - Robot 2026-06-14 20:08:31 +02:00
chk
659e2845a1 Arm 1 Log 2026-06-14 19:49:08 +02:00
chk
43e5991ffa Arm 1 Marker 2026-06-14 19:42:33 +02:00
chk
fc4d2b3c84 UI verbessern 2 2026-06-14 18:38:33 +02:00
chk
c23fbf75f2 UI verbessern 2026-06-14 18:24:12 +02:00
chk
7d76caa00b UI Homing umgestalten 2026-06-14 18:05:37 +02:00
chk
3a7ca828dc Script fehlte 2026-06-14 17:51:58 +02:00
chk
273146c726 Callibration Mathe 2026-06-14 17:47:57 +02:00
chk
fdbdb5f1e7 Homing nicht als SubPage 2026-06-14 17:14:22 +02:00
chk
dd8de5674d Phase 0, 1, 2 2026-06-14 16:58:45 +02:00
chk
0cb2bae554 Phasen aufteilung ToDo 2026-06-14 16:42:17 +02:00
chk
cce53cda54 Roadmap Homing 2026-06-14 16:29:01 +02:00
chk
275ab083fa Roadmap planen 2026-06-14 16:23:18 +02:00
159 changed files with 78998 additions and 2488 deletions

11
.claude/launch.json Normal file
View File

@@ -0,0 +1,11 @@
{
"version": "0.0.1",
"configurations": [
{
"name": "appRobotHoming",
"runtimeExecutable": "node",
"runtimeArgs": ["server/server.js"],
"port": 2093
}
]
}

197
README.md
View File

@@ -1,105 +1,152 @@
# appRobotHoming
`appRobotHoming` ist die browserbasierte Bedienoberfläche für die WebCam-gestützte
Ermittlung der Roboterpose. Das Frontend bleibt der Einstieg; die eigentliche
Bildverarbeitung läuft hinter der Firewall auf eigenen Services (WebCam,
BodyTracker), die der Homing-Backend als schlanker Proxy anspricht.
Browserbasierte Bedienoberfläche für das kameragestützte **Homing** und die
**Kalibrierung** eines Roboterarms. Das Frontend kommuniziert mit einem Node.js-Backend
(BFF-Proxy), das Kamera-Bilder, ArUco-Erkennung und Gelenk-Winkel-Schätzung
über Python-Skripte orchestriert.
## Architektur im Überblick
## Architektur
```
Browser ──HTTPS──▶ Reverse-Proxy ──HTTPS/WSS──▶ appRobotHoming-Backend
(statisches UI) (öffentliches TLS) (server/server.js, Port 2093)
intern (hinter der Firewall, HTTP):
├──▶ WebCam-Service (Bilder)
├──▶ BodyTracker-Service (Pose)
└──▶ … weitere Schritte (später)
Browser ──HTTPS──▶ Reverse-Proxy ──HTTPS──▶ appRobotHoming-Backend (Port 2093)
server/server.js
intern (HTTP):
├──▶ WebCam-Service (Bilder, NPZ)
└──▶ Robot-Driver (POST /api/state)
```
- **Frontend (`public/`)** statische Seite: zeigt Infos, Buttons und die
Rückmeldungen (Result als JSON + Tree-View, Snapshot-Tabelle, Bilder). Kein
direkter Zugriff auf die internen Services.
- **Backend (`server/server.js`)** BFF-Proxy. Liefert das statische Frontend
aus und stellt eine kleine API bereit, über die das UI an die internen
Services kommt. Läuft auf **HTTPS, Port 2093**.
**Frontend (`public/`):** statische Seiten — Homing, Kalibrierung, Board-Viewer,
Scene-Viewer. Kein direkter Zugriff auf interne Services.
## Ablauf
**Backend (`server/server.js`):** HTTPS-BFF auf Port 2093. Liefert Frontend aus,
orchestriert Python-Skripte (SSE-Stream) und liest/schreibt `robot.json`.
1. Das UI lädt den aktuellen Stand über `GET /api/latest-snapshot`.
2. **Bilder und Kamera-Intrinsics kommen vom WebCam-Service** (eigener Server
hinter der Firewall; die Kamera ist Source of Truth ihrer eigenen Kalibrierung).
3. Auf Knopfdruck schickt das UI eine Pose-Anfrage an `POST /api/estimate`.
4. Der Backend reicht **Bilder + Intrinsics** zur Verarbeitung an den
**BodyTracker** weiter und erhält die Roboterpose zurück.
5. Das Ergebnis wird im UI ausgegeben (JSON, Tree, Tabelle, annotierte Bilder).
6. **Eventuell folgen weitere Schritte** (z. B. Pose an `appRobotDriver` geben).
## Funktionen
Fällt der BodyTracker aus, rechnet das Frontend ersatzweise lokal mit
`public/calculateAngles.js`.
| Seite | Pfad | Beschreibung |
|-------|------|--------------|
| Homing | `/` (`index.html`) | Homing-Run starten, Status, GCode-Ausgabe |
| Kalibrierung | `/calibration.html` | Tabs: Camera NPZ · Board · X-Achse · Arm1-Y · **Marker** |
| Board-Viewer | `/boardViewer.html` | 3D-Viewer: Board-Marker, Skeleton FK, Arm-Marker mit Spin |
| Scene-Viewer | `/sceneViewer.html` | Standalone-Viewer (Datei-Upload, keine Server-Abhängigkeit) |
| Homing-Detail | `/homing.html` | Detail-Ansicht eines Homing-Laufs |
## HTTPS (bewusste Entscheidung)
## Homing-Ablauf
Der Backend läuft selbst auf **HTTPS** auch wenn davor schon ein Reverse-Proxy
die öffentliche TLS-Terminierung übernimmt. Grund: **WebSocket-Verbindungen (WSS)
kommen nur sauber durch den Proxy, wenn auch der Backend-Hop TLS spricht.**
```
Foto alle Kameras
→ 1_detect_aruco_observations.py (ArUco-Erkennung, pro Kamera)
→ 2_estimate_camera_from_observations.py (Kamera-Pose)
→ 3b_corner_marker_poses.py (Marker-Triangulierung)
→ X-Position schätzen (JS: server/homingXEstimate.cjs)
→ 4b_revolute_angle.py Arm1 / Ellbow / Arm2 / Hand (Gelenk-Winkel)
→ POST ROBOT_URL/api/state
```
- Das verwendete Zertifikat ist **self-signed** (`https/`, Passphrase `abcd`).
Das ist Absicht: Dieser Hop ist nur **Proxy ↔ Backend**, nie öffentlich. Die
vertrauenswürdige Kette stellt der vorgelagerte Reverse-Proxy bereit.
- Zugriff im internen Netz z. B. über `https://thinkcentre.local:2093/`.
SSE-Events (`log` / `step` / `analysis` / `done`) streamen den Fortschritt live
ins Frontend. Der Board-Viewer zeigt das Skeleton progressiv nach jedem erkannten Gelenk.
## API (Backend)
Details: [`doc/Homing_ROADMAP.md`](doc/Homing_ROADMAP.md)
## Kalibrierung
Einmaliger Vorgang nach mechanischen Änderungen:
| Schritt | Tab | Ergebnis |
|---------|-----|---------|
| 1 Camera NPZ | Camera NPZ | Kamera-Intrinsics als `.npz` |
| 2 Board | Board | `links.Board.markers` in `robot.json` |
| 3 X-Achse | Robot X Axis | alle Marker-Positionen rotiert |
| 4 Arm1-Y | Arm1 Y | `links.Arm1.jointToParent.origin[1,2]` |
| 5 Arm-Marker | Marker | Spin-Korrektur, Orientierungs-Verifikation |
Details: [`doc/Kalibrierung.md`](doc/Kalibrierung.md) ·
[`doc/Kalibrierung_Marker.md`](doc/Kalibrierung_Marker.md) ·
[`doc/accessRobotAPI.md`](doc/accessRobotAPI.md) (robot.json via Driver)
## robot.json
Zentrale Konfiguration aller Gelenke, Marker und Kinematik-Parameter.
```
ROBOT_JSON = process.env.ROBOT_JSON || 'scripts/robot_1781069752019.json'
```
Enthält: `links.{Link}.markers[].{id, position, normal, size, spin}`,
`links.{Link}.jointToParent`, `defaultPosition`, `robot_test_poses`.
## API-Übersicht
| Endpoint | Methode | Zweck |
|---|---|---|
| `/api/health` | GET | Status + konfigurierte Service-URLs |
| `/api/latest-snapshot` | GET | Aktuelle Bilder/Daten (vom WebCam-Service bzw. lokalem Fallback) |
| `/api/estimate` | POST | Bilder an BodyTracker geben → Pose zurück |
|----------|---------|-------|
| `/api/robot` | GET | robot.json lesen |
| `/api/robot/set-arm-marker-spin` | POST | Spin eines Arm-Markers setzen |
| `/api/robot/set-joint-origin` | POST | Joint-Origin Y/Z setzen |
| `/api/robot/assign-by-z` | POST | Marker nach Z-Bereich zuordnen |
| `/api/robot/adopt-x-axis` | POST | X-Achse übernehmen |
| `/api/board/run` | POST | Board-Pipeline starten (SSE) |
| `/api/board/latest` | GET | Letzter Board-Run (Marker + Robot) |
| `/api/homing/run` | POST | Homing-Lauf starten (SSE) |
| `/api/homing/send-state` | POST | State an Robot-Driver senden |
| `/api/homing/run-data` | GET | Debug-Daten eines Runs |
| `/api/calibration/*` | POST/GET | Kalibrierungs-Session verwalten |
## Konfiguration (Umgebungsvariablen)
## Konfiguration
| Variable | Bedeutung |
|---|---|
| `HTTPS_PORT` | Port des Backends (Default `2093`) |
| `WEBCAM_URL` | Basis-URL des internen WebCam-Services |
| `BODYTRACKER_URL` | Basis-URL des internen BodyTracker-Services |
| `HTTPS_KEY_PATH` / `HTTPS_CERT_PATH` / `HTTPS_PASSPHRASE` | self-signed Cert für den Proxy-Hop |
Ist `WEBCAM_URL` nicht gesetzt, nutzt der Backend lokale Dateien aus
`public/snapshots` als Fallback (Entwicklung ohne Kamera).
|----------|-----------|
| `HTTPS_PORT` | Port (Default `2093`) |
| `WEBCAM_URL` | Interner WebCam-Service |
| `ROBOT_URL` | Interner Robot-Driver |
| `ROBOT_JSON` | Pfad zu robot.json (Default `scripts/robot_1781069752019.json`) |
| `HTTPS_KEY_PATH` / `HTTPS_CERT_PATH` / `HTTPS_PASSPHRASE` | self-signed Cert |
## Dateien & Struktur
- `public/` statisches Frontend (UI, Client-Logik, Anzeige).
- `server/server.js` HTTPS-Backend / BFF-Proxy.
- `https/` self-signed Zertifikate für den Proxy-Hop (nicht eingecheckt).
- `doc/README_WebCam.md` WebCam-Service (Bildquelle).
- `doc/README_BodyTracker.md` BodyTracker-Service (Pose-Ermittlung).
- `doc/ToDo.md` offene Punkte & nächste Umsetzungsschritte.
- `test/` Tests für Berechnung und Auswertung.
```
public/ Frontend (HTML, JS, CSS)
boardViewer.html 3D-Viewer mit Three.js FK, Arm-Markern, Spin-Rendering
sceneViewer.html Standalone-Viewer (nur Datei-Upload)
calibration*.html Kalibrierungs-Tabs (lazy-geladen)
client.js Homing-Frontend-Logik
calibration.js Kalibrierungs-Frontend-Logik
server/
server.js Express-Backend, alle API-Routes
editRobot.js robot.json lesen/schreiben
homingOrchestrator.js Homing-Ablauf (SSE-Stream)
homingXEstimate.cjs X-Schätzung (reine Geometrie, unit-getestet)
spinNormalize.cjs Spin-Normalisierung [0,360) (unit-getestet)
scripts/
robot_1781069752019.json Haupt-Konfiguration (robot.json)
1_detect_aruco_observations.py
2_estimate_camera_from_observations.py
3b_corner_marker_poses.py
4b_revolute_angle.py
test/
homingXEstimate.test.js X-Schätzungs-Geometrie (9 Tests, inkl. Regression)
spinNormalize.test.js Spin-Normalisierung (5 Tests)
yAxisComputeJs.test.js Y-Achsen-Berechnung
yAxisRotation.test.js Rotations-Mathe
doc/
Homing_ROADMAP.md Homing-Ablauf und Implementierungs-Status
Kalibrierung.md Kalibrierungs-Schritte 14
Kalibrierung_Marker.md Arm-Marker: Datenmodell, Spin-Verifikation, Roadmap P1P5
ToDo.md Offene Punkte
```
## Nutzung
```bash
npm install
npm test
npm start # startet den HTTPS-Backend auf Port 2093
npm test # Jest-Tests (14+ Tests)
npm start # HTTPS-Backend auf Port 2093
```
Danach im internen Netz `https://<host>:2093/` öffnen (self-signed → einmalige
Zertifikatswarnung im Browser bestätigen).
Danach: `https://<host>:2093/`
> Hinweis: Das Frontend ist auf den Backend angewiesen `/api/latest-snapshot`
> und `/api/estimate` funktionieren **nicht**, wenn man `index.html` rein
> statisch öffnet. Immer über `npm start` (bzw. den Container) laufen lassen.
## Geplante Erweiterungen
1. Pose an `appRobotDriver` weitergeben.
2. Wenn die Hand nicht erkannt wird: Vorschlag für eine bessere Arm-/Foto-Position.
3. Manuelle Eingabe von `x, y, z, a, b, c, e`.
4. Erkennungsergebnis und Pose klarer im UI ausgeben.
Konkrete nächste Schritte und offene Schnittstellen-Fragen: siehe
[`doc/ToDo.md`](doc/ToDo.md).
> self-signed Zertifikat → einmalige Browser-Warnung bestätigen.
> Frontend benötigt laufendes Backend (API-Calls beim Laden).

View File

@@ -0,0 +1,49 @@
marker_id,link,set,num_cameras,x_mm,y_mm,z_mm,nx,ny,nz,model_x_mm,model_y_mm,model_z_mm,dist_to_model_mm,delta_z_mm,edge_length_mm
0,unknown,,2,505.2,-100.88,-7.16,0.02189,0.03624,0.9991,,,,,,24.24
46,Board,A0,2,537.83,185.51,-27.88,-0.08568,-0.05354,0.99488,537.44,185.2,-27.2,0.841,-0.679,23.2
47,Board,A0,2,343.18,-286.29,-27.16,-0.03123,0.01004,0.99946,343.18,-286.05,-27.49,0.407,0.326,24.03
50,Board,A0,2,574.07,210.38,-26.23,-0.00073,0.03209,0.99948,574.23,211.48,-27.15,1.448,0.925,24.06
51,Board,A0,2,166.73,-171.08,-27.09,-0.03386,0.01172,0.99936,167.18,-170.93,-27.76,0.818,0.667,24.31
53,Board,A0,2,487.37,212.32,-27.38,-0.07361,-0.01564,0.99716,487.08,212.19,-27.28,0.33,-0.103,23.53
54,Board,A0,3,341.07,-330.3,-27.22,-0.03362,0.04017,0.99863,341.05,-330.09,-27.5,0.351,0.28,24.36
55,Arm1,A0,2,282.65,-261.75,-26.65,-0.05274,0.01696,0.99846,,,,,,24.32
58,Board,A0,2,48.62,-216.5,-27.86,-0.00162,-0.0094,0.99995,49.3,-216.52,-27.93,0.684,0.068,24.24
62,Board,A0,3,404.15,-174.92,-26.96,0.01614,-0.01502,0.99976,404.07,-174.84,-27.4,0.454,0.439,23.97
64,Board,A0,2,-22.59,-186.68,-26.97,-0.00648,0.01133,0.99991,-21.95,-186.37,-28.04,1.286,1.074,24.24
66,Board,A0,2,208.51,-363.21,-27.64,-0.04961,0.03216,0.99825,208.41,-362.24,-27.7,0.98,0.055,24.38
68,Board,A0,2,574.39,169.06,-26.11,-0.00044,0.05167,0.99866,574.4,170.26,-27.15,1.591,1.04,24.52
73,Board,A0,2,221.97,337.22,-30.29,-0.05172,0.1462,0.9879,223.01,334.05,-27.67,4.242,-2.621,26.49
76,Board,A0,2,686.21,165.02,-27.13,-0.04548,-0.03114,0.99848,685.86,166,-26.98,1.054,-0.149,23.28
79,Board,A0,2,311.51,-157.95,-27.04,0.05023,-0.04899,0.99754,311.73,-158.5,-27.54,0.776,0.496,23.36
82,Board,A0,2,219.37,300.46,-29.79,0.00751,0.1232,0.99235,220.31,298.19,-27.68,3.238,-2.111,25.14
85,Board,A0,3,503.91,-313.51,-27.06,0.00515,-0.00866,0.99995,503.43,-312.87,-27.25,0.816,0.191,24.1
90,Board,A0,2,644.93,316.2,-28.16,-0.02384,-0.04569,0.99867,644.39,315.79,-27.04,1.307,-1.118,23.32
91,Board,A0,2,725.49,327.85,-27.51,-0.02708,0.03694,0.99895,724.61,327.11,-26.92,1.289,-0.587,24.21
92,Board,A0,2,644.7,-186.93,-25.55,-0.05883,-0.0185,0.9981,644.42,-185.49,-27.04,2.094,1.489,23.69
95,Board,A0,3,184.77,-273.26,-27.61,-0.03697,0.01835,0.99915,185.04,-272.99,-27.73,0.401,0.121,24.34
96,Board,A0,3,369.2,-185.74,-27.61,-0.04836,0.02152,0.9986,369.1,-186.1,-27.46,0.398,-0.148,24.06
103,Board,A0,3,104.63,-186.32,-27.25,-0.01721,0.02455,0.99955,105.03,-186.33,-27.85,0.721,0.6,24.42
105,Board,A0,3,524.3,-267.15,-27.08,-0.016,0.01938,0.99968,523.86,-266.44,-27.22,0.848,0.137,23.92
118,unknown,,3,322.99,-174.22,47.16,0.02335,-0.99462,0.10089,,,,,,24.14
122,Ellbow,,3,359.95,-173.78,46.07,0.00204,-0.99361,0.11286,,,,,,24.26
143,Arm2,,2,340.07,-138.22,229.66,-0.72695,-0.68151,0.08421,,,,,,24.03
144,Arm2,,3,362.93,-157.15,158.16,-0.05456,-0.9917,0.11642,,,,,,24.31
146,Arm2,,2,337.31,-147.71,160.02,-0.68833,-0.72303,0.05854,,,,,,24.0
147,FingerA,,3,383.25,-144.05,226.5,0.43778,-0.89607,0.07357,,,,,,23.85
148,Arm2,,3,367.21,-142.91,264.34,-0.04936,-0.99586,0.07639,,,,,,24.28
178,FingerB,,2,287.73,-121.4,315.33,-0.67179,-0.7215,-0.16775,,,,,,23.25
179,FingerB,,2,329.15,-139.64,311.4,-0.61251,-0.17201,0.77152,,,,,,23.53
198,Arm1,,2,268.53,-53.98,84.4,0.00864,0.01319,0.99988,,,,,,24.79
200,unknown,,2,199.66,-28.82,110.04,-0.15134,-0.01695,0.98834,,,,,,23.71
204,unknown,,2,198.48,115.43,120.91,0.03546,0.04428,0.99839,,,,,,24.17
208,Board,rail,2,626.35,-98.62,-6.79,-0.00084,0.01537,0.99988,631.01,-98.43,-7.71,4.748,0.917,24.04
210,Board,rail,2,129.9,-7.86,-5.67,0.06219,0.02253,0.99781,122.63,-13.98,-0.72,10.715,-4.946,23.24
214,unknown,,2,531.62,-8.59,-6.83,-0.02922,0.04196,0.99869,,,,,,24.25
217,Board,rail,2,730.01,-8.51,-5.0,-0.08513,-0.01438,0.99627,732.39,-23.88,7.39,19.881,-12.386,23.12
229,Arm1,,3,271.17,-142.11,79.1,0.01062,-0.04516,0.99892,,,,,,23.94
243,Arm1,,2,270.3,-176.61,43.73,0.02044,-0.9996,-0.01934,,,,,,24.13
camera_id,x_mm,y_mm,z_mm,dir_x,dir_y,dir_z
cam0,335.33,-885.66,468.37,-0.06588,0.89247,-0.44628
cam1,296.81,-462.82,780.61,0.09503,0.4331,-0.89632
cam2,712.91,-665.53,724.31,-0.42887,0.59445,-0.68022
1 marker_id link set num_cameras x_mm y_mm z_mm nx ny nz model_x_mm model_y_mm model_z_mm dist_to_model_mm delta_z_mm edge_length_mm
2 0 unknown 2 505.2 -100.88 -7.16 0.02189 0.03624 0.9991 24.24
3 46 Board A0 2 537.83 185.51 -27.88 -0.08568 -0.05354 0.99488 537.44 185.2 -27.2 0.841 -0.679 23.2
4 47 Board A0 2 343.18 -286.29 -27.16 -0.03123 0.01004 0.99946 343.18 -286.05 -27.49 0.407 0.326 24.03
5 50 Board A0 2 574.07 210.38 -26.23 -0.00073 0.03209 0.99948 574.23 211.48 -27.15 1.448 0.925 24.06
6 51 Board A0 2 166.73 -171.08 -27.09 -0.03386 0.01172 0.99936 167.18 -170.93 -27.76 0.818 0.667 24.31
7 53 Board A0 2 487.37 212.32 -27.38 -0.07361 -0.01564 0.99716 487.08 212.19 -27.28 0.33 -0.103 23.53
8 54 Board A0 3 341.07 -330.3 -27.22 -0.03362 0.04017 0.99863 341.05 -330.09 -27.5 0.351 0.28 24.36
9 55 Arm1 A0 2 282.65 -261.75 -26.65 -0.05274 0.01696 0.99846 24.32
10 58 Board A0 2 48.62 -216.5 -27.86 -0.00162 -0.0094 0.99995 49.3 -216.52 -27.93 0.684 0.068 24.24
11 62 Board A0 3 404.15 -174.92 -26.96 0.01614 -0.01502 0.99976 404.07 -174.84 -27.4 0.454 0.439 23.97
12 64 Board A0 2 -22.59 -186.68 -26.97 -0.00648 0.01133 0.99991 -21.95 -186.37 -28.04 1.286 1.074 24.24
13 66 Board A0 2 208.51 -363.21 -27.64 -0.04961 0.03216 0.99825 208.41 -362.24 -27.7 0.98 0.055 24.38
14 68 Board A0 2 574.39 169.06 -26.11 -0.00044 0.05167 0.99866 574.4 170.26 -27.15 1.591 1.04 24.52
15 73 Board A0 2 221.97 337.22 -30.29 -0.05172 0.1462 0.9879 223.01 334.05 -27.67 4.242 -2.621 26.49
16 76 Board A0 2 686.21 165.02 -27.13 -0.04548 -0.03114 0.99848 685.86 166 -26.98 1.054 -0.149 23.28
17 79 Board A0 2 311.51 -157.95 -27.04 0.05023 -0.04899 0.99754 311.73 -158.5 -27.54 0.776 0.496 23.36
18 82 Board A0 2 219.37 300.46 -29.79 0.00751 0.1232 0.99235 220.31 298.19 -27.68 3.238 -2.111 25.14
19 85 Board A0 3 503.91 -313.51 -27.06 0.00515 -0.00866 0.99995 503.43 -312.87 -27.25 0.816 0.191 24.1
20 90 Board A0 2 644.93 316.2 -28.16 -0.02384 -0.04569 0.99867 644.39 315.79 -27.04 1.307 -1.118 23.32
21 91 Board A0 2 725.49 327.85 -27.51 -0.02708 0.03694 0.99895 724.61 327.11 -26.92 1.289 -0.587 24.21
22 92 Board A0 2 644.7 -186.93 -25.55 -0.05883 -0.0185 0.9981 644.42 -185.49 -27.04 2.094 1.489 23.69
23 95 Board A0 3 184.77 -273.26 -27.61 -0.03697 0.01835 0.99915 185.04 -272.99 -27.73 0.401 0.121 24.34
24 96 Board A0 3 369.2 -185.74 -27.61 -0.04836 0.02152 0.9986 369.1 -186.1 -27.46 0.398 -0.148 24.06
25 103 Board A0 3 104.63 -186.32 -27.25 -0.01721 0.02455 0.99955 105.03 -186.33 -27.85 0.721 0.6 24.42
26 105 Board A0 3 524.3 -267.15 -27.08 -0.016 0.01938 0.99968 523.86 -266.44 -27.22 0.848 0.137 23.92
27 118 unknown 3 322.99 -174.22 47.16 0.02335 -0.99462 0.10089 24.14
28 122 Ellbow 3 359.95 -173.78 46.07 0.00204 -0.99361 0.11286 24.26
29 143 Arm2 2 340.07 -138.22 229.66 -0.72695 -0.68151 0.08421 24.03
30 144 Arm2 3 362.93 -157.15 158.16 -0.05456 -0.9917 0.11642 24.31
31 146 Arm2 2 337.31 -147.71 160.02 -0.68833 -0.72303 0.05854 24.0
32 147 FingerA 3 383.25 -144.05 226.5 0.43778 -0.89607 0.07357 23.85
33 148 Arm2 3 367.21 -142.91 264.34 -0.04936 -0.99586 0.07639 24.28
34 178 FingerB 2 287.73 -121.4 315.33 -0.67179 -0.7215 -0.16775 23.25
35 179 FingerB 2 329.15 -139.64 311.4 -0.61251 -0.17201 0.77152 23.53
36 198 Arm1 2 268.53 -53.98 84.4 0.00864 0.01319 0.99988 24.79
37 200 unknown 2 199.66 -28.82 110.04 -0.15134 -0.01695 0.98834 23.71
38 204 unknown 2 198.48 115.43 120.91 0.03546 0.04428 0.99839 24.17
39 208 Board rail 2 626.35 -98.62 -6.79 -0.00084 0.01537 0.99988 631.01 -98.43 -7.71 4.748 0.917 24.04
40 210 Board rail 2 129.9 -7.86 -5.67 0.06219 0.02253 0.99781 122.63 -13.98 -0.72 10.715 -4.946 23.24
41 214 unknown 2 531.62 -8.59 -6.83 -0.02922 0.04196 0.99869 24.25
42 217 Board rail 2 730.01 -8.51 -5.0 -0.08513 -0.01438 0.99627 732.39 -23.88 7.39 19.881 -12.386 23.12
43 229 Arm1 3 271.17 -142.11 79.1 0.01062 -0.04516 0.99892 23.94
44 243 Arm1 2 270.3 -176.61 43.73 0.02044 -0.9996 -0.01934 24.13
45 camera_id x_mm y_mm z_mm dir_x dir_y dir_z
46 cam0 335.33 -885.66 468.37 -0.06588 0.89247 -0.44628
47 cam1 296.81 -462.82 780.61 0.09503 0.4331 -0.89632
48 cam2 712.91 -665.53 724.31 -0.42887 0.59445 -0.68022

File diff suppressed because it is too large Load Diff

Binary file not shown.

After

Width:  |  Height:  |  Size: 98 KiB

File diff suppressed because it is too large Load Diff

View File

@@ -0,0 +1,481 @@
{
"schema_version": "1.0",
"created_utc": "2026-06-25T17:25:09Z",
"source": {
"detection_json": "/app/data/homing/20260625_172504/cam0_aruco_detection.json",
"robot_json": "/app/scripts/robot_1781069752019.json"
},
"camera": {
"camera_id": "cam0",
"camera_matrix": [
[
1424.7584228515625,
0.0,
635.95947265625
],
[
0.0,
1421.5770263671875,
482.1744384765625
],
[
0.0,
0.0,
1.0
]
],
"distortion_coefficients": [
0.05634751915931702,
0.33765655755996704,
0.002130246954038739,
-0.004022662527859211,
-1.182201862335205
]
},
"estimation": {
"method": "single_camera_marker_center_lm",
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
"marker_size_m": 0.025,
"num_used_markers": 20,
"used_marker_ids": [
97,
66,
85,
54,
105,
69,
47,
95,
58,
64,
103,
62,
96,
208,
51,
79,
210,
68,
50,
91
],
"history": {
"iters": [
0,
1,
2,
3
],
"rms": [
0.011403454671871994,
0.001728926659833975,
0.0016691755968926973,
0.0016691753847773315
],
"lambda": [
0.001,
0.0005,
0.00025,
0.000125
]
},
"residual_rms_px": 3.4096867660331136,
"residual_median_px": 1.5691231791727809,
"residual_max_px": 12.026369061549262,
"sigma2_normalized": 3.2778193707572165e-06
},
"camera_pose": {
"world_to_camera": {
"rotation_matrix": [
[
0.9973888397216797,
0.04563671350479126,
-0.05597161129117012
],
[
-0.02958603762090206,
-0.4488010108470917,
-0.8931418061256409
],
[
-0.0658801719546318,
0.8924656510353088,
-0.4462788999080658
]
],
"translation_m": [
-0.2678244411945343,
0.030760858207941055,
1.0215346813201904
],
"rvec_rad": [
2.0344335845398724,
0.011289329253343375,
-0.08570511152514877
]
},
"camera_in_world": {
"position_m": [
0.33533409237861633,
-0.885656476020813,
0.4683726131916046
],
"position_mm": [
335.3341064453125,
-885.656494140625,
468.37261962890625
],
"orientation_deg": {
"roll": 116.56741333007812,
"pitch": 3.777391195297241,
"yaw": -1.6990946531295776
}
},
"uncertainty": {
"pose_covariance_6x6": [
[
9.702707532310775e-06,
1.2273484057683694e-06,
3.575388647845507e-06,
-1.2199370555784652e-07,
-2.1440042443296295e-06,
1.039618282302021e-07
],
[
1.22734840576837e-06,
3.358446334387221e-06,
1.8878909236797321e-07,
4.651297819399596e-07,
-8.835069052569738e-07,
6.206962238461149e-07
],
[
3.5753886478454758e-06,
1.8878909236796024e-07,
1.143731789507973e-05,
-7.216885572151019e-07,
-2.2145565854988443e-06,
-3.0088157401175667e-06
],
[
-1.219937055578466e-07,
4.651297819399599e-07,
-7.216885572151033e-07,
2.2942348340442282e-07,
3.6332485943604063e-08,
3.2363949947789566e-07
],
[
-2.1440042443296215e-06,
-8.835069052569716e-07,
-2.2145565854988506e-06,
3.633248594360388e-08,
9.86494679764419e-07,
6.579597825341255e-07
],
[
1.039618282302198e-07,
6.206962238461218e-07,
-3.0088157401175595e-06,
3.2363949947789524e-07,
6.579597825341204e-07,
2.970696318843356e-06
]
],
"parameter_std": {
"rvec_std_deg": [
0.1784715940965861,
0.10500061405868079,
0.19376919211544102
],
"tvec_std_m": [
0.0004789817151044733,
0.0009932243854056438,
0.0017235708047084566
]
},
"camera_center_std_m": [
0.0024386451901829424,
0.0013863680263389562,
0.002737631446074117
],
"camera_center_std_mm": [
2.4386451901829425,
1.3863680263389562,
2.7376314460741167
],
"orientation_std_deg": {
"roll": 0.19011944203297743,
"pitch": 0.14270986978198957,
"yaw": 0.1181609242753551
}
}
},
"observations": {
"markers": [
{
"marker_id": 97,
"observed_center_px": [
676.25,
910.5
],
"projected_center_px": [
675.9551391601562,
911.3131713867188
],
"reprojection_error_px": 0.8649801263910383,
"confidence": 0.42385670146087856
},
{
"marker_id": 66,
"observed_center_px": [
480.5,
921.0
],
"projected_center_px": [
480.5653381347656,
919.2376098632812
],
"reprojection_error_px": 1.763600880544741,
"confidence": 0.2797311732321247
},
{
"marker_id": 85,
"observed_center_px": [
1080.0,
843.5
],
"projected_center_px": [
1077.551025390625,
842.6098022460938
],
"reprojection_error_px": 2.605749158768581,
"confidence": 0.6761152978036918
},
{
"marker_id": 54,
"observed_center_px": [
753.5,
868.5
],
"projected_center_px": [
753.16845703125,
868.4518432617188
],
"reprojection_error_px": 0.33502210609070604,
"confidence": 0.6745646371332027
},
{
"marker_id": 105,
"observed_center_px": [
1098.25,
783.5
],
"projected_center_px": [
1096.3798828125,
782.7687377929688
],
"reprojection_error_px": 2.0080046589625047,
"confidence": 0.53843639257073
},
{
"marker_id": 69,
"observed_center_px": [
130.25,
818.25
],
"projected_center_px": [
131.2628631591797,
816.5780639648438
],
"reprojection_error_px": 1.9548048201489217,
"confidence": 0.6309056746154269
},
{
"marker_id": 47,
"observed_center_px": [
755.0,
810.25
],
"projected_center_px": [
754.8045043945312,
810.226318359375
],
"reprojection_error_px": 0.19692473653729947,
"confidence": 0.558404255319149
},
{
"marker_id": 95,
"observed_center_px": [
461.25,
799.75
],
"projected_center_px": [
462.01556396484375,
799.7348022460938
],
"reprojection_error_px": 0.7657148006869643,
"confidence": 0.5573620390355706
},
{
"marker_id": 58,
"observed_center_px": [
243.0,
742.5
],
"projected_center_px": [
244.86248779296875,
742.42578125
],
"reprojection_error_px": 1.8639659872994379,
"confidence": 0.4721784486231997
},
{
"marker_id": 64,
"observed_center_px": [
139.75,
714.0
],
"projected_center_px": [
141.61831665039062,
715.1568603515625
],
"reprojection_error_px": 2.197483328524737,
"confidence": 0.4471706966400147
},
{
"marker_id": 103,
"observed_center_px": [
350.75,
708.0
],
"projected_center_px": [
352.2232971191406,
708.7933959960938
],
"reprojection_error_px": 1.6733444379103959,
"confidence": 0.3922909999211629
},
{
"marker_id": 62,
"observed_center_px": [
851.5,
685.25
],
"projected_center_px": [
851.7431640625,
686.1382446289062
],
"reprojection_error_px": 0.9209274032584249,
"confidence": 0.3564780454484243
},
{
"marker_id": 96,
"observed_center_px": [
793.25,
698.0
],
"projected_center_px": [
793.7991333007812,
698.2991943359375
],
"reprojection_error_px": 0.6253516072450701,
"confidence": 0.3348906742607279
},
{
"marker_id": 208,
"observed_center_px": [
1202.5,
583.0
],
"projected_center_px": [
1209.5457763671875,
583.9437866210938
],
"reprojection_error_px": 7.108705775496229,
"confidence": 0.28965074531908364
},
{
"marker_id": 51,
"observed_center_px": [
458.0,
690.5
],
"projected_center_px": [
459.341064453125,
691.0894775390625
],
"reprojection_error_px": 1.4649019204351656,
"confidence": 0.34674061533734285
},
{
"marker_id": 79,
"observed_center_px": [
696.25,
672.25
],
"projected_center_px": [
697.0543212890625,
673.733154296875
],
"reprojection_error_px": 1.6872105394342276,
"confidence": 0.3256815826862768
},
{
"marker_id": 210,
"observed_center_px": [
439.25,
532.5
],
"projected_center_px": [
427.3565673828125,
530.716796875
],
"reprojection_error_px": 12.026369061549262,
"confidence": 0.15571821530659996
},
{
"marker_id": 68,
"observed_center_px": [
1027.25,
434.25
],
"projected_center_px": [
1027.0755615234375,
434.6865539550781
],
"reprojection_error_px": 0.47011502613700773,
"confidence": 0.09323781394061914
},
{
"marker_id": 50,
"observed_center_px": [
1017.0,
413.5
],
"projected_center_px": [
1016.9035034179688,
413.9068298339844
],
"reprojection_error_px": 0.41811733301008686,
"confidence": 0.08307692198670814
},
{
"marker_id": 91,
"observed_center_px": [
1165.5,
355.75
],
"projected_center_px": [
1165.020751953125,
355.35980224609375
],
"reprojection_error_px": 0.6180072633772071,
"confidence": 0.06871920537654881
}
]
},
"qa": {
"sanity_notes": []
}
}

Binary file not shown.

After

Width:  |  Height:  |  Size: 209 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 108 KiB

File diff suppressed because it is too large Load Diff

View File

@@ -0,0 +1,761 @@
{
"schema_version": "1.0",
"created_utc": "2026-06-25T17:25:13Z",
"source": {
"detection_json": "/app/data/homing/20260625_172504/cam1_aruco_detection.json",
"robot_json": "/app/scripts/robot_1781069752019.json"
},
"camera": {
"camera_id": "cam1",
"camera_matrix": [
[
1367.5723876953125,
0.0,
672.1165771484375
],
[
0.0,
1372.3011474609375,
445.8396911621094
],
[
0.0,
0.0,
1.0
]
],
"distortion_coefficients": [
0.01016925647854805,
0.7656787633895874,
-0.0031530377455055714,
-0.00288817984983325,
-2.490830183029175
]
},
"estimation": {
"method": "single_camera_marker_center_lm",
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
"marker_size_m": 0.025,
"num_used_markers": 40,
"used_marker_ids": [
54,
95,
58,
85,
47,
103,
59,
48,
105,
51,
102,
96,
62,
71,
92,
63,
208,
210,
217,
74,
75,
52,
68,
76,
46,
53,
101,
50,
100,
82,
60,
67,
73,
94,
70,
104,
98,
90,
91,
88
],
"history": {
"iters": [
0,
1,
2,
3
],
"rms": [
0.011372574297308506,
0.0021949616225605375,
0.002143709970633132,
0.002143709690342172
],
"lambda": [
0.001,
0.0005,
0.00025,
0.000125
]
},
"residual_rms_px": 4.23364353800265,
"residual_median_px": 1.0679787737296105,
"residual_max_px": 19.348939630537274,
"sigma2_normalized": 4.968098633993014e-06
},
"camera_pose": {
"world_to_camera": {
"rotation_matrix": [
[
-0.9951881766319275,
0.019756384193897247,
-0.0959698036313057
],
[
-0.02385592833161354,
0.9011316895484924,
0.4328886866569519
],
[
0.09503374248743057,
0.43309515714645386,
-0.8963242769241333
]
],
"translation_m": [
0.37943923473358154,
0.0862211212515831,
0.8719164729118347
],
"rvec_rad": [
0.0032072073338993575,
-2.967110340660477,
-0.6774876643558738
]
},
"camera_in_world": {
"position_m": [
0.2968088686466217,
-0.4628157615661621,
0.7806104421615601
],
"position_mm": [
296.8088684082031,
-462.8157653808594,
780.6104125976562
],
"orientation_deg": {
"roll": 154.21060180664062,
"pitch": -5.453261375427246,
"yaw": -178.62680053710938
}
},
"uncertainty": {
"pose_covariance_6x6": [
[
2.2460757709580997e-06,
-1.0592111231264017e-07,
-5.911479445197522e-07,
9.807677828663091e-09,
6.656021402664132e-07,
1.8807173299942092e-07
],
[
-1.059211123126279e-07,
7.819602866884685e-06,
-8.896995228371429e-07,
-8.928123497147304e-07,
8.447874310868068e-07,
-3.883503279890717e-06
],
[
-5.911479445197253e-07,
-8.896995228370399e-07,
1.8982339503369444e-05,
-6.420456812663701e-07,
-7.934435641861599e-07,
3.082335762957178e-07
],
[
9.807677828661003e-09,
-8.928123497147341e-07,
-6.42045681266356e-07,
2.4448642118127635e-07,
-7.775978286233749e-08,
3.809119252748882e-07
],
[
6.656021402664136e-07,
8.447874310868029e-07,
-7.934435641861794e-07,
-7.775978286233639e-08,
4.178437168687293e-07,
-3.614395664399821e-07
],
[
1.880717329994141e-07,
-3.883503279890717e-06,
3.08233576295777e-07,
3.809119252748862e-07,
-3.614395664399844e-07,
2.809559727575109e-06
]
],
"parameter_std": {
"rvec_std_deg": [
0.08586868930820345,
0.16021935571577825,
0.24963041613501347
],
"tvec_std_m": [
0.0004944556817160425,
0.000646408320544166,
0.001676174134025194
]
},
"camera_center_std_m": [
0.0028646163342597826,
0.0025089246532298257,
0.001967934179216506
],
"camera_center_std_mm": [
2.8646163342597823,
2.5089246532298257,
1.967934179216506
],
"orientation_std_deg": {
"roll": 0.1994418033453169,
"pitch": 0.16062277696661237,
"yaw": 0.07987033548886921
}
}
},
"observations": {
"markers": [
{
"marker_id": 54,
"observed_center_px": [
735.25,
38.5
],
"projected_center_px": [
735.0857543945312,
38.939788818359375
],
"reprojection_error_px": 0.4694580105501786,
"confidence": 0.2985929580100399
},
{
"marker_id": 95,
"observed_center_px": [
1005.5,
139.0
],
"projected_center_px": [
1004.7282104492188,
139.58082580566406
],
"reprojection_error_px": 0.9659284275868637,
"confidence": 0.9357112460666233
},
{
"marker_id": 58,
"observed_center_px": [
1234.75,
235.25
],
"projected_center_px": [
1234.0849609375,
235.2240447998047
],
"reprojection_error_px": 0.6655453606389707,
"confidence": 0.36770453019575644
},
{
"marker_id": 85,
"observed_center_px": [
458.0,
68.5
],
"projected_center_px": [
458.2991027832031,
69.70124816894531
],
"reprojection_error_px": 1.2379255382753527,
"confidence": 0.8685664374455692
},
{
"marker_id": 47,
"observed_center_px": [
731.5,
117.25
],
"projected_center_px": [
731.38525390625,
117.72209930419922
],
"reprojection_error_px": 0.485844027498816,
"confidence": 0.9524913713047937
},
{
"marker_id": 103,
"observed_center_px": [
1128.75,
284.5
],
"projected_center_px": [
1128.236083984375,
284.30267333984375
],
"reprojection_error_px": 0.5504974858473882,
"confidence": 0.9144001450649527
},
{
"marker_id": 59,
"observed_center_px": [
262.25,
123.5
],
"projected_center_px": [
263.9138488769531,
125.33772277832031
],
"reprojection_error_px": 2.479035718842208,
"confidence": 0.9376517069038668
},
{
"marker_id": 48,
"observed_center_px": [
148.5,
51.0
],
"projected_center_px": [
149.4849395751953,
51.862728118896484
],
"reprojection_error_px": 1.3093531891436279,
"confidence": 0.5517245546325658
},
{
"marker_id": 105,
"observed_center_px": [
432.0,
147.75
],
"projected_center_px": [
432.3002014160156,
149.29176330566406
],
"reprojection_error_px": 1.5707179826022124,
"confidence": 0.9442241264421224
},
{
"marker_id": 51,
"observed_center_px": [
1019.75,
308.0
],
"projected_center_px": [
1019.1754150390625,
307.9459228515625
],
"reprojection_error_px": 0.5771240900522866,
"confidence": 0.9297130863840988
},
{
"marker_id": 102,
"observed_center_px": [
239.25,
213.75
],
"projected_center_px": [
240.9429473876953,
215.61300659179688
],
"reprojection_error_px": 2.5173129361648683,
"confidence": 0.9498751926981491
},
{
"marker_id": 96,
"observed_center_px": [
689.75,
280.5
],
"projected_center_px": [
689.872314453125,
280.85595703125
],
"reprojection_error_px": 0.37638575097840954,
"confidence": 0.8766456914396368
},
{
"marker_id": 62,
"observed_center_px": [
634.75,
296.75
],
"projected_center_px": [
634.6029052734375,
297.34588623046875
],
"reprojection_error_px": 0.6137729696270053,
"confidence": 0.8625841801430425
},
{
"marker_id": 71,
"observed_center_px": [
63.5,
113.5
],
"projected_center_px": [
64.10832977294922,
114.1216812133789
],
"reprojection_error_px": 0.8698003470479402,
"confidence": 0.6962965934908386
},
{
"marker_id": 92,
"observed_center_px": [
256.25,
273.25
],
"projected_center_px": [
257.945556640625,
274.84429931640625
],
"reprojection_error_px": 2.327381067178508,
"confidence": 0.8785732433921414
},
{
"marker_id": 63,
"observed_center_px": [
36.75,
191.25
],
"projected_center_px": [
37.489715576171875,
191.14720153808594
],
"reprojection_error_px": 0.7468243819019208,
"confidence": 0.23403229407345516
},
{
"marker_id": 208,
"observed_center_px": [
295.25,
413.75
],
"projected_center_px": [
288.3956298828125,
413.3526916503906
],
"reprojection_error_px": 6.8658752994838395,
"confidence": 0.7372980849807326
},
{
"marker_id": 210,
"observed_center_px": [
1059.0,
559.75
],
"projected_center_px": [
1073.6019287109375,
555.7957763671875
],
"reprojection_error_px": 15.127861931469159,
"confidence": 0.6136138322804316
},
{
"marker_id": 217,
"observed_center_px": [
162.25,
532.5
],
"projected_center_px": [
146.7255401611328,
520.9512939453125
],
"reprojection_error_px": 19.348939630537274,
"confidence": 0.6320655742155583
},
{
"marker_id": 74,
"observed_center_px": [
1086.0,
734.75
],
"projected_center_px": [
1084.8804931640625,
735.0576782226562
],
"reprojection_error_px": 1.1610174177882524,
"confidence": 0.49861888008985367
},
{
"marker_id": 75,
"observed_center_px": [
1251.5,
801.75
],
"projected_center_px": [
1249.17626953125,
800.1643676757812
],
"reprojection_error_px": 2.8131749250632194,
"confidence": 0.06713952659324364
},
{
"marker_id": 52,
"observed_center_px": [
1075.25,
807.5
],
"projected_center_px": [
1073.9066162109375,
807.3248291015625
],
"reprojection_error_px": 1.354756379713829,
"confidence": 0.44660041827617547
},
{
"marker_id": 68,
"observed_center_px": [
421.5,
733.0
],
"projected_center_px": [
422.2747497558594,
733.5101318359375
],
"reprojection_error_px": 0.9276161243969003,
"confidence": 0.453656743367513
},
{
"marker_id": 76,
"observed_center_px": [
274.0,
722.75
],
"projected_center_px": [
274.78045654296875,
723.6485595703125
],
"reprojection_error_px": 1.19017717876916,
"confidence": 0.42914121819077033
},
{
"marker_id": 46,
"observed_center_px": [
472.5,
751.25
],
"projected_center_px": [
472.7325744628906,
751.6512451171875
],
"reprojection_error_px": 0.4637763737575183,
"confidence": 0.42705375163400877
},
{
"marker_id": 53,
"observed_center_px": [
541.0,
783.25
],
"projected_center_px": [
541.320556640625,
783.3278198242188
],
"reprojection_error_px": 0.32986737469810884,
"confidence": 0.4239630899600427
},
{
"marker_id": 101,
"observed_center_px": [
1031.25,
900.5
],
"projected_center_px": [
1029.7542724609375,
900.5302124023438
],
"reprojection_error_px": 1.4960326401403623,
"confidence": 0.3366794154513476
},
{
"marker_id": 50,
"observed_center_px": [
427.25,
777.5
],
"projected_center_px": [
427.572998046875,
777.9152221679688
],
"reprojection_error_px": 0.5260581593870906,
"confidence": 0.3955545216798782
},
{
"marker_id": 100,
"observed_center_px": [
120.0,
723.5
],
"projected_center_px": [
122.01116180419922,
723.669677734375
],
"reprojection_error_px": 2.018306799327716,
"confidence": 0.4621067158671878
},
{
"marker_id": 82,
"observed_center_px": [
894.75,
890.0
],
"projected_center_px": [
893.3291625976562,
890.5499267578125
],
"reprojection_error_px": 1.523547952267044,
"confidence": 0.37908960001789416
},
{
"marker_id": 60,
"observed_center_px": [
613.0,
860.75
],
"projected_center_px": [
612.6180419921875,
860.841552734375
],
"reprojection_error_px": 0.39277706514463534,
"confidence": 0.3907528879258137
},
{
"marker_id": 67,
"observed_center_px": [
497.75,
837.75
],
"projected_center_px": [
497.95806884765625,
838.1924438476562
],
"reprojection_error_px": 0.48892658313275084,
"confidence": 0.3865272468846743
},
{
"marker_id": 73,
"observed_center_px": [
889.25,
925.75
],
"projected_center_px": [
887.6505126953125,
926.299560546875
],
"reprojection_error_px": 1.6912647434798707,
"confidence": 0.15682847151496923
},
{
"marker_id": 94,
"observed_center_px": [
29.0,
720.5
],
"projected_center_px": [
31.336706161499023,
720.2244873046875
],
"reprojection_error_px": 2.3528924604549735,
"confidence": 0.09146699566173024
},
{
"marker_id": 70,
"observed_center_px": [
401.25,
866.75
],
"projected_center_px": [
401.60943603515625,
867.041259765625
],
"reprojection_error_px": 0.46262999734212534,
"confidence": 0.35896437880813437
},
{
"marker_id": 104,
"observed_center_px": [
107.5,
792.0
],
"projected_center_px": [
109.24372100830078,
792.2886352539062
],
"reprojection_error_px": 1.7674482353344945,
"confidence": 0.3880719051456888
},
{
"marker_id": 98,
"observed_center_px": [
436.5,
882.25
],
"projected_center_px": [
436.5618896484375,
883.1439819335938
],
"reprojection_error_px": 0.8961216581333906,
"confidence": 0.36026641726859315
},
{
"marker_id": 90,
"observed_center_px": [
351.75,
880.25
],
"projected_center_px": [
352.1582336425781,
880.5147094726562
],
"reprojection_error_px": 0.4865447685943754,
"confidence": 0.3567245846660417
},
{
"marker_id": 91,
"observed_center_px": [
254.25,
887.0
],
"projected_center_px": [
255.22390747070312,
886.9551391601562
],
"reprojection_error_px": 0.9749401296709685,
"confidence": 0.3494656541641164
},
{
"marker_id": 88,
"observed_center_px": [
199.75,
872.5
],
"projected_center_px": [
201.13644409179688,
872.6148681640625
],
"reprojection_error_px": 1.3911944201992585,
"confidence": 0.347903013426907
}
]
},
"qa": {
"sanity_notes": []
}
}

Binary file not shown.

After

Width:  |  Height:  |  Size: 257 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 220 KiB

File diff suppressed because it is too large Load Diff

View File

@@ -0,0 +1,495 @@
{
"schema_version": "1.0",
"created_utc": "2026-06-25T17:25:15Z",
"source": {
"detection_json": "/app/data/homing/20260625_172504/cam2_aruco_detection.json",
"robot_json": "/app/scripts/robot_1781069752019.json"
},
"camera": {
"camera_id": "cam2",
"camera_matrix": [
[
1388.99072265625,
0.0,
933.082763671875
],
[
0.0,
1394.8729248046875,
562.4996948242188
],
[
0.0,
0.0,
1.0
]
],
"distortion_coefficients": [
0.019531700760126114,
-0.11213663965463638,
0.0026758278254419565,
0.0007694826927036047,
0.05339815095067024
]
},
"estimation": {
"method": "single_camera_marker_center_lm",
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
"marker_size_m": 0.025,
"num_used_markers": 21,
"used_marker_ids": [
85,
92,
105,
54,
93,
66,
217,
62,
96,
95,
79,
76,
103,
64,
46,
90,
53,
86,
84,
82,
73
],
"history": {
"iters": [
0,
1,
2,
3
],
"rms": [
0.014228673477920699,
0.0012360717022841035,
0.0009070175499997285,
0.0009070142696966918
],
"lambda": [
0.001,
0.0005,
0.00025,
0.000125
]
},
"residual_rms_px": 1.7852212006525887,
"residual_median_px": 1.0469264348760319,
"residual_max_px": 6.647095648204168,
"sigma2_normalized": 9.597873663078857e-07
},
"camera_pose": {
"world_to_camera": {
"rotation_matrix": [
[
0.7617526054382324,
0.6427322030067444,
0.08141452819108963
],
[
0.48559483885765076,
-0.4832412004470825,
-0.7284748554229736
],
[
-0.42887139320373535,
0.5944520831108093,
-0.6802176833152771
]
],
"translation_m": [
-0.1742696762084961,
-0.1401522308588028,
1.1940648555755615
],
"rvec_rad": [
2.1767839356074785,
0.8396398702141942,
-0.2585585732707637
]
},
"camera_in_world": {
"position_m": [
0.7129078507423401,
-0.6655329465866089,
0.7243147492408752
],
"position_mm": [
712.9078369140625,
-665.532958984375,
724.3147583007812
],
"orientation_deg": {
"roll": 138.84930419921875,
"pitch": 25.395954132080078,
"yaw": 32.51630783081055
}
},
"uncertainty": {
"pose_covariance_6x6": [
[
2.4924502189037485e-06,
7.284908255672299e-07,
2.0024504778922095e-07,
6.993863992084647e-08,
-3.359070245275866e-07,
5.600958340179941e-09
],
[
7.284908255672354e-07,
1.064107327675121e-06,
-2.3219104124960886e-08,
2.319589057300187e-07,
-2.846525717659831e-07,
1.5012786826699554e-07
],
[
2.0024504778920997e-07,
-2.3219104124972556e-08,
4.278959925593973e-06,
-1.4661732034947215e-07,
-6.23228741847165e-07,
-1.5703080494058352e-06
],
[
6.993863992084778e-08,
2.3195890573001912e-07,
-1.4661732034946982e-07,
1.0962701995068561e-07,
-2.8204197612513988e-08,
1.4125379662533936e-07
],
[
-3.359070245275866e-07,
-2.8465257176598095e-07,
-6.232287418471685e-07,
-2.8204197612513405e-08,
2.305112527858795e-07,
2.649685210729187e-07
],
[
5.600958340183888e-09,
1.5012786826699972e-07,
-1.5703080494058346e-06,
1.4125379662534013e-07,
2.6496852107291726e-07,
1.078572601871589e-06
]
],
"parameter_std": {
"rvec_std_deg": [
0.09045568752546955,
0.05910379253809281,
0.11852002706372318
],
"tvec_std_m": [
0.0003310997130030251,
0.00048011587433231107,
0.0010385435002307746
]
},
"camera_center_std_m": [
0.0015926294915772563,
0.001282414773137875,
0.0014297273307869451
],
"camera_center_std_mm": [
1.5926294915772563,
1.282414773137875,
1.4297273307869451
],
"orientation_std_deg": {
"roll": 0.12744173458500993,
"pitch": 0.08524351737408967,
"yaw": 0.06595345807707714
}
}
},
"observations": {
"markers": [
{
"marker_id": 85,
"observed_center_px": [
943.25,
1039.0
],
"projected_center_px": [
943.3576049804688,
1037.9586181640625
],
"reprojection_error_px": 1.0469264348760319,
"confidence": 0.25017342512163626
},
{
"marker_id": 92,
"observed_center_px": [
1262.25,
1041.75
],
"projected_center_px": [
1262.399658203125,
1040.8380126953125
],
"reprojection_error_px": 0.9241852745384849,
"confidence": 0.18638720024967376
},
{
"marker_id": 105,
"observed_center_px": [
1019.5,
1007.25
],
"projected_center_px": [
1019.3496704101562,
1006.0000610351562
],
"reprojection_error_px": 1.2589465443049024,
"confidence": 0.7893291944675445
},
{
"marker_id": 54,
"observed_center_px": [
726.75,
892.25
],
"projected_center_px": [
727.0918579101562,
891.9924926757812
],
"reprojection_error_px": 0.4279916503422691,
"confidence": 0.712278812924601
},
{
"marker_id": 93,
"observed_center_px": [
1893.5,
971.5
],
"projected_center_px": [
1894.048095703125,
971.9451904296875
],
"reprojection_error_px": 0.70611855836639,
"confidence": 0.042321015168188404
},
{
"marker_id": 66,
"observed_center_px": [
548.25,
804.25
],
"projected_center_px": [
549.4153442382812,
803.2486572265625
],
"reprojection_error_px": 1.5364616961092168,
"confidence": 0.5883494177188935
},
{
"marker_id": 217,
"observed_center_px": [
1527.25,
916.5
],
"projected_center_px": [
1529.0672607421875,
922.8938598632812
],
"reprojection_error_px": 6.647095648204168,
"confidence": 0.45290577054800996
},
{
"marker_id": 62,
"observed_center_px": [
962.0,
802.5
],
"projected_center_px": [
961.2557373046875,
802.291748046875
],
"reprojection_error_px": 0.7728491674409725,
"confidence": 0.6057547967936824
},
{
"marker_id": 96,
"observed_center_px": [
912.25,
783.25
],
"projected_center_px": [
911.0736694335938,
783.0982055664062
],
"reprojection_error_px": 1.18608395635878,
"confidence": 0.5545391608146377
},
{
"marker_id": 95,
"observed_center_px": [
630.5,
709.5
],
"projected_center_px": [
631.0589599609375,
709.0905151367188
],
"reprojection_error_px": 0.6929026563578145,
"confidence": 0.4695004591117706
},
{
"marker_id": 79,
"observed_center_px": [
875.75,
715.0
],
"projected_center_px": [
875.3380126953125,
715.4541625976562
],
"reprojection_error_px": 0.6131861090513578,
"confidence": 0.5142115324222218
},
{
"marker_id": 76,
"observed_center_px": [
1551.75,
746.5
],
"projected_center_px": [
1552.177490234375,
744.9830322265625
],
"reprojection_error_px": 1.576051752365359,
"confidence": 0.29593493306278906
},
{
"marker_id": 103,
"observed_center_px": [
648.5,
590.0
],
"projected_center_px": [
648.8373413085938,
590.6143798828125
],
"reprojection_error_px": 0.7009007054415375,
"confidence": 0.39305998326235797
},
{
"marker_id": 64,
"observed_center_px": [
541.0,
511.25
],
"projected_center_px": [
541.9498901367188,
512.197021484375
],
"reprojection_error_px": 1.3413206043684687,
"confidence": 0.3099175748319284
},
{
"marker_id": 46,
"observed_center_px": [
1382.0,
629.0
],
"projected_center_px": [
1381.5179443359375,
628.320556640625
],
"reprojection_error_px": 0.8330791930264032,
"confidence": 0.26236530151152454
},
{
"marker_id": 90,
"observed_center_px": [
1573.0,
614.25
],
"projected_center_px": [
1573.05712890625,
612.7874145507812
],
"reprojection_error_px": 1.4637007577355878,
"confidence": 0.21907023701319386
},
{
"marker_id": 53,
"observed_center_px": [
1341.0,
580.0
],
"projected_center_px": [
1340.56494140625,
579.7637939453125
],
"reprojection_error_px": 0.49504472552160556,
"confidence": 0.22146320976627604
},
{
"marker_id": 86,
"observed_center_px": [
1260.75,
466.5
],
"projected_center_px": [
1259.335205078125,
466.6405944824219
],
"reprojection_error_px": 1.4217635103809503,
"confidence": 0.19461123347835588
},
{
"marker_id": 84,
"observed_center_px": [
1285.5,
508.25
],
"projected_center_px": [
1284.366455078125,
507.854248046875
],
"reprojection_error_px": 1.200643035340168,
"confidence": 0.17943691614102802
},
{
"marker_id": 82,
"observed_center_px": [
1130.0,
393.5
],
"projected_center_px": [
1129.2742919921875,
393.4766540527344
],
"reprojection_error_px": 0.7260834289920938,
"confidence": 0.18215071600887703
},
{
"marker_id": 73,
"observed_center_px": [
1154.0,
379.0
],
"projected_center_px": [
1152.795654296875,
379.095703125
],
"reprojection_error_px": 1.2081422353226527,
"confidence": 0.17728829216377978
}
]
},
"qa": {
"sanity_notes": []
}
}

Binary file not shown.

After

Width:  |  Height:  |  Size: 353 KiB

View File

@@ -0,0 +1,59 @@
{
"schema_version": "1.0",
"created_utc": "2026-06-25T17:25:19Z",
"method": "hybrid",
"seeded": true,
"movements": {
"x": {
"value": 162.42504894251783,
"unit": "mm",
"observable": true,
"confidence": "high",
"n_markers": 4
},
"y": {
"value": -1.169420311588133,
"unit": "deg",
"observable": true,
"confidence": "high",
"n_markers": 4
},
"z": {
"value": 98.46924561003269,
"unit": "deg",
"observable": true,
"confidence": "medium",
"n_markers": 1
},
"a": {
"value": 89.51769789582495,
"unit": "deg",
"observable": true,
"confidence": "high",
"n_markers": 4
},
"b": {
"value": -47.89793601064412,
"unit": "deg",
"observable": true,
"confidence": "medium",
"n_markers": 3
},
"c": {
"value": -64.66160343747586,
"unit": "deg",
"observable": true,
"confidence": "medium",
"n_markers": 3
},
"e": {
"value": 15.8937800585978,
"unit": "mm",
"observable": true,
"confidence": "medium",
"n_markers": 3
}
},
"residual_rms": 22.202829667894964,
"num_markers": 43
}

View File

@@ -0,0 +1,105 @@
{
"status": "ok",
"link": "Arm1",
"joint": "y",
"method": "primary",
"joint_origin_world_mm": [
216.50906842923365,
108.3968,
46.3163
],
"joint_axis_world": [
-1.0,
0.0,
0.0
],
"mean_angle_deg": 8.73396517409613,
"circular_variance": 0.07161679045871494,
"circular_std_deg": 22.088350114431325,
"num_pairs_used": 6,
"num_markers_matched": 4,
"per_pair": [
{
"marker_ids": [
55,
198
],
"link": "Arm1",
"tier": "primary",
"skipped": false,
"angle_deg": 2.7136316277566275,
"baseline_model_mm": 120.70271952197267,
"baseline_obs_mm": 235.58340824705388,
"weight": 28435.558049674528
},
{
"marker_ids": [
55,
229
],
"link": "Arm1",
"tier": "primary",
"skipped": false,
"angle_deg": 36.09512656392027,
"baseline_model_mm": 63.35571402801804,
"baseline_obs_mm": 159.67340571245236,
"weight": 10116.222630197835
},
{
"marker_ids": [
55,
243
],
"link": "Arm1",
"tier": "primary",
"skipped": false,
"angle_deg": 88.90480962311312,
"baseline_model_mm": 34.32559540634365,
"baseline_obs_mm": 110.45934908729735,
"weight": 3791.582925618644
},
{
"marker_ids": [
198,
229
],
"link": "Arm1",
"tier": "primary",
"skipped": false,
"angle_deg": -3.440668721283733,
"baseline_model_mm": 89.99999999999997,
"baseline_obs_mm": 88.29072557685865,
"weight": 7946.165301917275
},
{
"marker_ids": [
198,
243
],
"link": "Arm1",
"tier": "primary",
"skipped": false,
"angle_deg": -2.7040713039960176,
"baseline_model_mm": 129.8075498574717,
"baseline_obs_mm": 129.20056719980755,
"weight": 16771.209068402644
},
{
"marker_ids": [
229,
243
],
"link": "Arm1",
"tier": "primary",
"skipped": false,
"angle_deg": -0.7105184107327543,
"baseline_model_mm": 49.49747468305837,
"baseline_obs_mm": 49.40955380679017,
"weight": 2445.6481386528067
}
],
"accumulated_state": {
"x": 106.50906842923364,
"y": 8.73396517409613
}
}

View File

@@ -0,0 +1,101 @@
{
"status": "ok",
"link": "Arm2",
"joint": "a",
"method": "primary",
"joint_origin_world_mm": [
306.50906842923365,
-138.70421127250364,
84.27799422070137
],
"joint_axis_world": [
0.0,
0.13361549880725562,
0.9910332479177922
],
"mean_angle_deg": 93.37084948672933,
"circular_variance": 0.0007324693649812808,
"circular_std_deg": 2.193370075603702,
"num_pairs_used": 4,
"num_markers_matched": 4,
"per_pair": [
{
"marker_ids": [
143,
144
],
"link": "Arm2",
"tier": "primary",
"skipped": false,
"angle_deg": 89.94688841244773,
"baseline_model_mm": 26.68445427585132,
"baseline_obs_mm": 24.643765187064083,
"weight": 657.6054253190281
},
{
"marker_ids": [
143,
146
],
"link": "Arm2",
"tier": "primary",
"skipped": true,
"reason": "bl_model=0.3 bl_obs=2.8 < 15.0"
},
{
"marker_ids": [
143,
148
],
"link": "Arm2",
"tier": "primary",
"skipped": false,
"angle_deg": 93.80406057538599,
"baseline_model_mm": 26.836171485515575,
"baseline_obs_mm": 28.679452701596173,
"weight": 769.6467108107678
},
{
"marker_ids": [
144,
146
],
"link": "Arm2",
"tier": "primary",
"skipped": false,
"angle_deg": 92.82624404009415,
"baseline_model_mm": 26.791733799812224,
"baseline_obs_mm": 27.197454261300408,
"weight": 728.6669546013292
},
{
"marker_ids": [
144,
148
],
"link": "Arm2",
"tier": "primary",
"skipped": true,
"reason": "bl_model=0.4 bl_obs=4.3 < 15.0"
},
{
"marker_ids": [
146,
148
],
"link": "Arm2",
"tier": "primary",
"skipped": false,
"angle_deg": 96.11666161271195,
"baseline_model_mm": 26.946706663338293,
"baseline_obs_mm": 31.282017166008377,
"weight": 842.9473404099408
}
],
"accumulated_state": {
"x": 106.50906842923364,
"y": 8.73396517409613,
"z": 88.94460334069886,
"a": 93.37084948672933
}
}

View File

@@ -0,0 +1,54 @@
{
"status": "ok",
"link": "Ellbow",
"joint": "z",
"method": "fallback_1_child_axis",
"joint_origin_world_mm": [
216.50906842923365,
-138.70421127250364,
84.27799422070137
],
"joint_axis_world": [
-1.0,
0.0,
0.0
],
"mean_angle_deg": 88.94460334069886,
"circular_variance": 5.079874864089007e-07,
"circular_std_deg": 0.05775162282873076,
"num_pairs_used": 2,
"num_markers_matched": 1,
"per_pair": [
{
"marker_ids": [
143,
146
],
"link": "Arm2",
"tier": "fallback_1_child_axis",
"skipped": false,
"angle_deg": 89.03275897168004,
"baseline_model_mm": 70.00000071428568,
"baseline_obs_mm": 70.28215356251216,
"weight": 4919.750799577387
},
{
"marker_ids": [
144,
148
],
"link": "Arm2",
"tier": "fallback_1_child_axis",
"skipped": false,
"angle_deg": 88.90676969215383,
"baseline_model_mm": 106.99999999999999,
"baseline_obs_mm": 107.1349128404376,
"weight": 11463.43567392682
}
],
"accumulated_state": {
"x": 106.50906842923364,
"y": 8.73396517409613,
"z": 88.94460334069886
}
}

View File

@@ -0,0 +1,61 @@
marker_id,link,set,num_cameras,x_mm,y_mm,z_mm,nx,ny,nz,model_x_mm,model_y_mm,model_z_mm,dist_to_model_mm,delta_z_mm,edge_length_mm
0,unknown,,3,505.79,-100.06,-8.17,0.00654,-0.01478,0.99987,,,,,,23.71
46,Board,A0,2,537.9,185.52,-27.5,0.0061,0.00117,0.99998,537.44,185.2,-27.2,0.638,-0.305,23.83
47,Board,A0,3,343.34,-286.18,-27.52,-0.00826,0.02432,0.99967,343.18,-286.05,-27.49,0.21,-0.034,24.1
50,Board,A0,3,574.64,211.77,-27.34,0.00242,0.0109,0.99994,574.23,211.48,-27.15,0.539,-0.186,24.07
51,Board,A0,3,166.89,-171.06,-27.49,-0.04142,0.00081,0.99914,167.18,-170.93,-27.76,0.417,0.272,24.2
53,Board,A0,3,487.16,212.93,-27.58,0.06677,0.02171,0.99753,487.08,212.19,-27.28,0.798,-0.296,23.92
54,Board,A0,3,341.31,-330.18,-27.55,-0.01014,0.01262,0.99987,341.05,-330.09,-27.5,0.275,-0.049,24.21
55,Arm1,A0,3,282.71,-261.9,-26.63,-0.03413,0.00638,0.9994,,,,,,24.37
56,Arm1,A0,2,500.02,169.42,-27.88,-0.01336,0.00216,0.99991,,,,,,23.7
58,Board,A0,3,48.69,-216.76,-27.87,-0.0193,0.00188,0.99981,49.3,-216.52,-27.93,0.658,0.059,24.44
60,Board,A0,2,435.69,286.11,-29.34,-0.04419,0.06927,0.99662,435.46,283.95,-27.36,2.937,-1.981,24.83
62,Board,A0,3,404.22,-174.75,-27.14,0.00893,-0.00881,0.99992,404.07,-174.84,-27.4,0.313,0.259,23.96
64,Board,A0,2,-22.33,-187.15,-26.56,0.02977,0.00062,0.99956,-21.95,-186.37,-28.04,1.719,1.484,23.66
66,Board,A0,2,207.97,-362.78,-28.65,-0.08,0.0928,0.99247,208.41,-362.24,-27.7,1.18,-0.951,25.14
67,Board,A0,2,524.61,268.02,-28.51,-0.0163,-0.00123,0.99987,524.1,266.85,-27.22,1.818,-1.29,23.6
68,Board,A0,3,575.05,170.15,-27.14,0.01722,0.03205,0.99934,574.4,170.26,-27.15,0.663,0.006,24.1
69,Board,A0,2,6.91,-280.99,-27.23,-0.00089,0.03112,0.99952,6.58,-279.46,-28,1.744,0.77,24.07
70,Board,A0,2,603.56,300.57,-28.34,-0.06715,0.04327,0.9968,603.03,299.84,-27.11,1.527,-1.233,23.99
73,Board,A0,2,222.01,337.1,-29.9,-0.0437,0.11256,0.99268,223.01,334.05,-27.67,3.914,-2.235,25.6
75,Board,A0,2,-27.07,199.16,-28.78,-0.06027,0.14192,0.98804,-24.94,196.46,-28.04,3.523,-0.736,26.71
76,Board,A0,2,686.17,164.68,-26.47,-0.08323,0.00998,0.99648,685.86,166,-26.98,1.45,0.514,23.85
77,Arm1,A0,2,17.53,194.86,-28.25,-0.02333,0.14876,0.9886,,,,,,26.07
79,Board,A0,2,311.53,-157.96,-26.95,0.02919,-0.01163,0.99951,311.73,-158.5,-27.54,0.827,0.59,24.03
82,Board,A0,2,219.4,300.84,-30.04,0.00365,0.102,0.99478,220.31,298.19,-27.68,3.663,-2.363,25.25
85,Board,A0,3,504.05,-313.36,-27.37,-0.00147,-0.03538,0.99937,503.43,-312.87,-27.25,0.799,-0.124,24.1
88,Board,A0,2,767.2,313.41,-25.61,0.05089,-0.00066,0.9987,767.09,314.94,-26.86,1.978,1.252,23.68
90,Board,A0,2,644.9,315.86,-27.76,-0.00476,-0.03483,0.99938,644.39,315.79,-27.04,0.882,-0.718,23.64
91,Board,A0,2,725.06,326.79,-26.9,-0.04318,-0.00677,0.99904,724.61,327.11,-26.92,0.553,0.023,23.76
92,Board,A0,2,644.97,-186.59,-26.03,0.03072,-0.01393,0.99943,644.42,-185.49,-27.04,1.592,1.007,24.18
94,Board,A0,2,875.22,168.95,-22.69,0.07264,0.05485,0.99585,876.38,172.13,-26.7,5.244,4.006,24.44
95,Board,A0,3,184.79,-273.16,-27.83,-0.00258,0.02727,0.99962,185.04,-272.99,-27.73,0.315,-0.097,24.35
96,Board,A0,3,369.18,-185.71,-27.51,0.02262,0.01808,0.99958,369.1,-186.1,-27.46,0.4,-0.049,24.16
97,Board,A0,2,303.36,-359.54,-26.35,-0.00502,0.02288,0.99973,303.02,-359.03,-27.55,1.344,1.197,24.46
98,Board,A0,2,577.15,315.74,-29.24,-0.02274,0.04433,0.99876,576.48,314.67,-27.15,2.445,-2.094,24.33
100,Board,A0,2,803.8,169.01,-24.93,0.01883,-0.01657,0.99969,803.92,171.12,-26.81,2.829,1.882,23.24
103,Board,A0,3,104.64,-186.37,-27.12,-0.00058,0.0069,0.99998,105.03,-186.33,-27.85,0.825,0.726,24.45
104,Board,A0,2,827.17,235.41,-24.77,0.08284,0.04199,0.99568,827.64,237.83,-26.77,3.174,1.996,24.77
105,Board,A0,3,524.5,-266.88,-27.45,-0.00083,0.01085,0.99994,523.86,-266.44,-27.22,0.811,-0.234,24.19
118,unknown,,3,323.07,-174.22,47.26,0.01395,-0.99423,0.10635,,,,,,24.41
122,Ellbow,,3,360.07,-173.45,46.18,0.01114,-0.99429,0.10615,,,,,,24.51
143,Arm2,,2,339.81,-138.06,229.48,-0.73183,-0.67663,0.08126,,,,,,24.18
144,Arm2,,3,362.95,-156.86,158.05,-0.03961,-0.99309,0.11048,,,,,,24.37
147,FingerA,,3,382.82,-143.67,226.6,0.42827,-0.9012,0.06646,,,,,,24.01
148,Arm2,,3,366.84,-142.43,264.66,-0.04437,-0.99493,0.09027,,,,,,24.39
178,FingerB,,2,287.31,-120.45,315.29,-0.66387,-0.73035,-0.1608,,,,,,22.88
179,FingerB,,2,329.61,-140.32,313.07,-0.6954,-0.15177,0.70241,,,,,,26.39
198,Arm1,,2,268.82,-53.79,84.52,0.01757,0.02587,0.99951,,,,,,24.56
200,unknown,,2,199.61,-28.62,110.03,-0.06794,-0.01026,0.99764,,,,,,23.25
204,unknown,,2,198.89,116.09,120.31,0.05577,0.03761,0.99774,,,,,,24.15
208,Board,rail,3,626.32,-98.56,-6.78,0.00383,-0.01334,0.9999,631.01,-98.43,-7.71,4.781,0.93,23.72
210,Board,rail,2,129.76,-7.26,-6.27,0.028,0.01002,0.99956,122.63,-13.98,-0.72,11.263,-5.553,23.35
214,unknown,,3,532.15,-7.88,-7.33,-0.02081,0.05531,0.99825,,,,,,24.05
217,Board,rail,2,730.24,-8.43,-5.0,-0.01833,-0.01881,0.99966,732.39,-23.88,7.39,19.917,-12.387,23.61
229,Arm1,,3,271.15,-142.19,79.27,0.01827,-0.03009,0.99938,,,,,,24.04
243,Arm1,,3,270.52,-175.76,42.77,0.0038,-0.99933,-0.03654,,,,,,24.16
camera_id,x_mm,y_mm,z_mm,dir_x,dir_y,dir_z
cam0,335.07,-885.3,469.64,-0.06533,0.89189,-0.44751
cam1,297.15,-462.72,780.65,0.09465,0.43305,-0.89639
cam2,714.94,-666.29,727.22,-0.42921,0.59338,-0.68094
1 marker_id link set num_cameras x_mm y_mm z_mm nx ny nz model_x_mm model_y_mm model_z_mm dist_to_model_mm delta_z_mm edge_length_mm
2 0 unknown 3 505.79 -100.06 -8.17 0.00654 -0.01478 0.99987 23.71
3 46 Board A0 2 537.9 185.52 -27.5 0.0061 0.00117 0.99998 537.44 185.2 -27.2 0.638 -0.305 23.83
4 47 Board A0 3 343.34 -286.18 -27.52 -0.00826 0.02432 0.99967 343.18 -286.05 -27.49 0.21 -0.034 24.1
5 50 Board A0 3 574.64 211.77 -27.34 0.00242 0.0109 0.99994 574.23 211.48 -27.15 0.539 -0.186 24.07
6 51 Board A0 3 166.89 -171.06 -27.49 -0.04142 0.00081 0.99914 167.18 -170.93 -27.76 0.417 0.272 24.2
7 53 Board A0 3 487.16 212.93 -27.58 0.06677 0.02171 0.99753 487.08 212.19 -27.28 0.798 -0.296 23.92
8 54 Board A0 3 341.31 -330.18 -27.55 -0.01014 0.01262 0.99987 341.05 -330.09 -27.5 0.275 -0.049 24.21
9 55 Arm1 A0 3 282.71 -261.9 -26.63 -0.03413 0.00638 0.9994 24.37
10 56 Arm1 A0 2 500.02 169.42 -27.88 -0.01336 0.00216 0.99991 23.7
11 58 Board A0 3 48.69 -216.76 -27.87 -0.0193 0.00188 0.99981 49.3 -216.52 -27.93 0.658 0.059 24.44
12 60 Board A0 2 435.69 286.11 -29.34 -0.04419 0.06927 0.99662 435.46 283.95 -27.36 2.937 -1.981 24.83
13 62 Board A0 3 404.22 -174.75 -27.14 0.00893 -0.00881 0.99992 404.07 -174.84 -27.4 0.313 0.259 23.96
14 64 Board A0 2 -22.33 -187.15 -26.56 0.02977 0.00062 0.99956 -21.95 -186.37 -28.04 1.719 1.484 23.66
15 66 Board A0 2 207.97 -362.78 -28.65 -0.08 0.0928 0.99247 208.41 -362.24 -27.7 1.18 -0.951 25.14
16 67 Board A0 2 524.61 268.02 -28.51 -0.0163 -0.00123 0.99987 524.1 266.85 -27.22 1.818 -1.29 23.6
17 68 Board A0 3 575.05 170.15 -27.14 0.01722 0.03205 0.99934 574.4 170.26 -27.15 0.663 0.006 24.1
18 69 Board A0 2 6.91 -280.99 -27.23 -0.00089 0.03112 0.99952 6.58 -279.46 -28 1.744 0.77 24.07
19 70 Board A0 2 603.56 300.57 -28.34 -0.06715 0.04327 0.9968 603.03 299.84 -27.11 1.527 -1.233 23.99
20 73 Board A0 2 222.01 337.1 -29.9 -0.0437 0.11256 0.99268 223.01 334.05 -27.67 3.914 -2.235 25.6
21 75 Board A0 2 -27.07 199.16 -28.78 -0.06027 0.14192 0.98804 -24.94 196.46 -28.04 3.523 -0.736 26.71
22 76 Board A0 2 686.17 164.68 -26.47 -0.08323 0.00998 0.99648 685.86 166 -26.98 1.45 0.514 23.85
23 77 Arm1 A0 2 17.53 194.86 -28.25 -0.02333 0.14876 0.9886 26.07
24 79 Board A0 2 311.53 -157.96 -26.95 0.02919 -0.01163 0.99951 311.73 -158.5 -27.54 0.827 0.59 24.03
25 82 Board A0 2 219.4 300.84 -30.04 0.00365 0.102 0.99478 220.31 298.19 -27.68 3.663 -2.363 25.25
26 85 Board A0 3 504.05 -313.36 -27.37 -0.00147 -0.03538 0.99937 503.43 -312.87 -27.25 0.799 -0.124 24.1
27 88 Board A0 2 767.2 313.41 -25.61 0.05089 -0.00066 0.9987 767.09 314.94 -26.86 1.978 1.252 23.68
28 90 Board A0 2 644.9 315.86 -27.76 -0.00476 -0.03483 0.99938 644.39 315.79 -27.04 0.882 -0.718 23.64
29 91 Board A0 2 725.06 326.79 -26.9 -0.04318 -0.00677 0.99904 724.61 327.11 -26.92 0.553 0.023 23.76
30 92 Board A0 2 644.97 -186.59 -26.03 0.03072 -0.01393 0.99943 644.42 -185.49 -27.04 1.592 1.007 24.18
31 94 Board A0 2 875.22 168.95 -22.69 0.07264 0.05485 0.99585 876.38 172.13 -26.7 5.244 4.006 24.44
32 95 Board A0 3 184.79 -273.16 -27.83 -0.00258 0.02727 0.99962 185.04 -272.99 -27.73 0.315 -0.097 24.35
33 96 Board A0 3 369.18 -185.71 -27.51 0.02262 0.01808 0.99958 369.1 -186.1 -27.46 0.4 -0.049 24.16
34 97 Board A0 2 303.36 -359.54 -26.35 -0.00502 0.02288 0.99973 303.02 -359.03 -27.55 1.344 1.197 24.46
35 98 Board A0 2 577.15 315.74 -29.24 -0.02274 0.04433 0.99876 576.48 314.67 -27.15 2.445 -2.094 24.33
36 100 Board A0 2 803.8 169.01 -24.93 0.01883 -0.01657 0.99969 803.92 171.12 -26.81 2.829 1.882 23.24
37 103 Board A0 3 104.64 -186.37 -27.12 -0.00058 0.0069 0.99998 105.03 -186.33 -27.85 0.825 0.726 24.45
38 104 Board A0 2 827.17 235.41 -24.77 0.08284 0.04199 0.99568 827.64 237.83 -26.77 3.174 1.996 24.77
39 105 Board A0 3 524.5 -266.88 -27.45 -0.00083 0.01085 0.99994 523.86 -266.44 -27.22 0.811 -0.234 24.19
40 118 unknown 3 323.07 -174.22 47.26 0.01395 -0.99423 0.10635 24.41
41 122 Ellbow 3 360.07 -173.45 46.18 0.01114 -0.99429 0.10615 24.51
42 143 Arm2 2 339.81 -138.06 229.48 -0.73183 -0.67663 0.08126 24.18
43 144 Arm2 3 362.95 -156.86 158.05 -0.03961 -0.99309 0.11048 24.37
44 147 FingerA 3 382.82 -143.67 226.6 0.42827 -0.9012 0.06646 24.01
45 148 Arm2 3 366.84 -142.43 264.66 -0.04437 -0.99493 0.09027 24.39
46 178 FingerB 2 287.31 -120.45 315.29 -0.66387 -0.73035 -0.1608 22.88
47 179 FingerB 2 329.61 -140.32 313.07 -0.6954 -0.15177 0.70241 26.39
48 198 Arm1 2 268.82 -53.79 84.52 0.01757 0.02587 0.99951 24.56
49 200 unknown 2 199.61 -28.62 110.03 -0.06794 -0.01026 0.99764 23.25
50 204 unknown 2 198.89 116.09 120.31 0.05577 0.03761 0.99774 24.15
51 208 Board rail 3 626.32 -98.56 -6.78 0.00383 -0.01334 0.9999 631.01 -98.43 -7.71 4.781 0.93 23.72
52 210 Board rail 2 129.76 -7.26 -6.27 0.028 0.01002 0.99956 122.63 -13.98 -0.72 11.263 -5.553 23.35
53 214 unknown 3 532.15 -7.88 -7.33 -0.02081 0.05531 0.99825 24.05
54 217 Board rail 2 730.24 -8.43 -5.0 -0.01833 -0.01881 0.99966 732.39 -23.88 7.39 19.917 -12.387 23.61
55 229 Arm1 3 271.15 -142.19 79.27 0.01827 -0.03009 0.99938 24.04
56 243 Arm1 3 270.52 -175.76 42.77 0.0038 -0.99933 -0.03654 24.16
57 camera_id x_mm y_mm z_mm dir_x dir_y dir_z
58 cam0 335.07 -885.3 469.64 -0.06533 0.89189 -0.44751
59 cam1 297.15 -462.72 780.65 0.09465 0.43305 -0.89639
60 cam2 714.94 -666.29 727.22 -0.42921 0.59338 -0.68094

File diff suppressed because it is too large Load Diff

Binary file not shown.

After

Width:  |  Height:  |  Size: 103 KiB

File diff suppressed because it is too large Load Diff

View File

@@ -0,0 +1,481 @@
{
"schema_version": "1.0",
"created_utc": "2026-06-25T17:59:21Z",
"source": {
"detection_json": "/app/data/homing/20260625_175916/cam0_aruco_detection.json",
"robot_json": "/app/scripts/robot_1781069752019.json"
},
"camera": {
"camera_id": "cam0",
"camera_matrix": [
[
1424.7584228515625,
0.0,
635.95947265625
],
[
0.0,
1421.5770263671875,
482.1744384765625
],
[
0.0,
0.0,
1.0
]
],
"distortion_coefficients": [
0.05634751915931702,
0.33765655755996704,
0.002130246954038739,
-0.004022662527859211,
-1.182201862335205
]
},
"estimation": {
"method": "single_camera_marker_center_lm",
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
"marker_size_m": 0.025,
"num_used_markers": 20,
"used_marker_ids": [
97,
66,
85,
54,
105,
69,
47,
95,
58,
64,
103,
96,
62,
51,
79,
208,
210,
68,
50,
53
],
"history": {
"iters": [
0,
1,
2,
3
],
"rms": [
0.011436738437523918,
0.0017627388219078695,
0.0017082741597643208,
0.0017082738298200922
],
"lambda": [
0.001,
0.0005,
0.00025,
0.000125
]
},
"residual_rms_px": 3.4895581122703203,
"residual_median_px": 1.3632084656881505,
"residual_max_px": 12.179917236270976,
"sigma2_normalized": 3.433175856049273e-06
},
"camera_pose": {
"world_to_camera": {
"rotation_matrix": [
[
0.99742591381073,
0.04507605358958244,
-0.055764634162187576
],
[
-0.029563840478658676,
-0.4500020146369934,
-0.8925380110740662
],
[
-0.06532628834247589,
0.891889214515686,
-0.44751107692718506
]
],
"translation_m": [
-0.26810967922210693,
0.0306912399828434,
1.021647572517395
],
"rvec_rad": [
2.035801112416453,
0.010908616696792009,
-0.08515448496251522
]
},
"camera_in_world": {
"position_m": [
0.33506736159324646,
-0.8853000402450562,
0.46964067220687866
],
"position_mm": [
335.0673522949219,
-885.300048828125,
469.64068603515625
],
"orientation_deg": {
"roll": 116.64550018310547,
"pitch": 3.745587110519409,
"yaw": -1.6977574825286865
}
},
"uncertainty": {
"pose_covariance_6x6": [
[
1.0402983243682025e-05,
9.607061849723336e-07,
3.175419172829312e-06,
-1.413862159845341e-07,
-2.12955432250041e-06,
2.312142235999549e-07
],
[
9.607061849723307e-07,
3.7788686477185887e-06,
-1.6924039364567714e-07,
5.562539816390373e-07,
-8.841983839385075e-07,
6.874403655011122e-07
],
[
3.175419172829249e-06,
-1.692403936456681e-07,
1.3583347408234157e-05,
-9.175016077988046e-07,
-2.2808655354386924e-06,
-3.2773776598692575e-06
],
[
-1.4138621598453382e-07,
5.562539816390341e-07,
-9.175016077987983e-07,
2.6164306620048117e-07,
4.0225581275842607e-08,
3.496020393352873e-07
],
[
-2.1295543225004017e-06,
-8.841983839385094e-07,
-2.280865535438699e-06,
4.02255812758387e-08,
9.903676474491735e-07,
6.66954432381659e-07
],
[
2.3121422359997266e-07,
6.874403655011114e-07,
-3.277377659869258e-06,
3.4960203933526876e-07,
6.669544323816537e-07,
3.1141328985088255e-06
]
],
"parameter_std": {
"rvec_std_deg": [
0.18479983617802564,
0.11137905544845887,
0.2111669934581791
],
"tvec_std_m": [
0.0005115105729117251,
0.0009951721697521356,
0.0017646905956877612
]
},
"camera_center_std_m": [
0.0026703652741227927,
0.0018374002535456223,
0.0027539784841622054
],
"camera_center_std_mm": [
2.670365274122793,
1.8374002535456224,
2.7539784841622055
],
"orientation_std_deg": {
"roll": 0.1722531066068174,
"pitch": 0.1564002711188888,
"yaw": 0.12201801252145385
}
}
},
"observations": {
"markers": [
{
"marker_id": 97,
"observed_center_px": [
676.0,
910.5
],
"projected_center_px": [
675.7621459960938,
911.7245483398438
],
"reprojection_error_px": 1.247434633072338,
"confidence": 0.42385670146087856
},
{
"marker_id": 66,
"observed_center_px": [
479.5,
922.25
],
"projected_center_px": [
480.50054931640625,
919.6788330078125
],
"reprojection_error_px": 2.7589850735869397,
"confidence": 0.27078087716396265
},
{
"marker_id": 85,
"observed_center_px": [
1079.0,
843.75
],
"projected_center_px": [
1076.984619140625,
842.904296875
],
"reprojection_error_px": 2.1856289218368885,
"confidence": 0.6759853525830334
},
{
"marker_id": 54,
"observed_center_px": [
753.5,
868.75
],
"projected_center_px": [
752.8966064453125,
868.8074340820312
],
"reprojection_error_px": 0.606120825922678,
"confidence": 0.6610788202218779
},
{
"marker_id": 105,
"observed_center_px": [
1098.25,
784.0
],
"projected_center_px": [
1095.7879638671875,
782.9950561523438
],
"reprojection_error_px": 2.6592356150248286,
"confidence": 0.5825558453258135
},
{
"marker_id": 69,
"observed_center_px": [
130.5,
818.0
],
"projected_center_px": [
131.20993041992188,
816.9083862304688
],
"reprojection_error_px": 1.302160444400257,
"confidence": 0.5841161997457144
},
{
"marker_id": 47,
"observed_center_px": [
755.0,
810.5
],
"projected_center_px": [
754.5029907226562,
810.5115966796875
],
"reprojection_error_px": 0.4971445511574386,
"confidence": 0.5487375886524822
},
{
"marker_id": 95,
"observed_center_px": [
461.0,
800.25
],
"projected_center_px": [
461.86224365234375,
800.0243530273438
],
"reprojection_error_px": 0.8912803555986298,
"confidence": 0.5215660558894334
},
{
"marker_id": 58,
"observed_center_px": [
243.25,
743.0
],
"projected_center_px": [
244.70860290527344,
742.6357421875
],
"reprojection_error_px": 1.5033982137941357,
"confidence": 0.468912299986827
},
{
"marker_id": 64,
"observed_center_px": [
139.5,
714.0
],
"projected_center_px": [
141.43846130371094,
715.3218383789062
],
"reprojection_error_px": 2.346249970897008,
"confidence": 0.4334442420959473
},
{
"marker_id": 103,
"observed_center_px": [
350.5,
708.0
],
"projected_center_px": [
352.01885986328125,
708.9456787109375
],
"reprojection_error_px": 1.78920191946218,
"confidence": 0.3938267188343575
},
{
"marker_id": 96,
"observed_center_px": [
793.0,
697.75
],
"projected_center_px": [
793.4209594726562,
698.4274291992188
],
"reprojection_error_px": 0.7975695565737103,
"confidence": 0.36075364145684324
},
{
"marker_id": 62,
"observed_center_px": [
851.25,
685.5
],
"projected_center_px": [
851.3250732421875,
686.2469482421875
],
"reprojection_error_px": 0.7507114413671468,
"confidence": 0.34135471017020086
},
{
"marker_id": 51,
"observed_center_px": [
457.75,
690.75
],
"projected_center_px": [
459.0975646972656,
691.2110595703125
],
"reprojection_error_px": 1.4242564869760441,
"confidence": 0.3676955439548635
},
{
"marker_id": 79,
"observed_center_px": [
695.75,
672.0
],
"projected_center_px": [
696.7139892578125,
673.8245849609375
],
"reprojection_error_px": 2.0635856097717857,
"confidence": 0.3187317073170732
},
{
"marker_id": 208,
"observed_center_px": [
1201.75,
583.0
],
"projected_center_px": [
1208.861572265625,
583.9735107421875
],
"reprojection_error_px": 7.177895461370356,
"confidence": 0.28568227626765064
},
{
"marker_id": 210,
"observed_center_px": [
439.0,
532.75
],
"projected_center_px": [
427.0049133300781,
530.6361694335938
],
"reprojection_error_px": 12.179917236270976,
"confidence": 0.16251118951659882
},
{
"marker_id": 68,
"observed_center_px": [
1027.25,
434.25
],
"projected_center_px": [
1026.4912109375,
434.3568420410156
],
"reprojection_error_px": 0.7662741435661348,
"confidence": 0.09654486228169104
},
{
"marker_id": 50,
"observed_center_px": [
1016.5,
413.5
],
"projected_center_px": [
1016.3207397460938,
413.5331726074219
],
"reprojection_error_px": 0.18230375891269784,
"confidence": 0.08377973241313476
},
{
"marker_id": 53,
"observed_center_px": [
909.0,
416.5
],
"projected_center_px": [
909.6261596679688,
416.78497314453125
],
"reprojection_error_px": 0.6879575734700244,
"confidence": 0.07916194626380657
}
]
},
"qa": {
"sanity_notes": []
}
}

Binary file not shown.

After

Width:  |  Height:  |  Size: 223 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 110 KiB

File diff suppressed because it is too large Load Diff

View File

@@ -0,0 +1,761 @@
{
"schema_version": "1.0",
"created_utc": "2026-06-25T17:59:25Z",
"source": {
"detection_json": "/app/data/homing/20260625_175916/cam1_aruco_detection.json",
"robot_json": "/app/scripts/robot_1781069752019.json"
},
"camera": {
"camera_id": "cam1",
"camera_matrix": [
[
1367.5723876953125,
0.0,
672.1165771484375
],
[
0.0,
1372.3011474609375,
445.8396911621094
],
[
0.0,
0.0,
1.0
]
],
"distortion_coefficients": [
0.01016925647854805,
0.7656787633895874,
-0.0031530377455055714,
-0.00288817984983325,
-2.490830183029175
]
},
"estimation": {
"method": "single_camera_marker_center_lm",
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
"marker_size_m": 0.025,
"num_used_markers": 40,
"used_marker_ids": [
54,
95,
58,
85,
47,
103,
59,
105,
48,
51,
102,
96,
62,
71,
92,
208,
63,
210,
217,
74,
75,
52,
76,
68,
46,
53,
50,
101,
82,
100,
73,
60,
67,
70,
94,
90,
104,
98,
91,
88
],
"history": {
"iters": [
0,
1,
2,
3
],
"rms": [
0.011863062568042395,
0.0021882720514259653,
0.0021293863444762583,
0.0021293859894700134
],
"lambda": [
0.001,
0.0005,
0.00025,
0.000125
]
},
"residual_rms_px": 4.205255463207004,
"residual_median_px": 1.1858116613812175,
"residual_max_px": 19.118438507028245,
"sigma2_normalized": 4.9019293968833265e-06
},
"camera_pose": {
"world_to_camera": {
"rotation_matrix": [
[
-0.995226263999939,
0.019625136628746986,
-0.09560120105743408
],
[
-0.023808155208826065,
0.9011572599411011,
0.4328380525112152
],
[
0.09464621543884277,
0.4330478608608246,
-0.8963881134986877
]
],
"translation_m": [
0.37944144010543823,
0.08616430312395096,
0.8720222115516663
],
"rvec_rad": [
0.0032728478486759383,
-2.9675124187665194,
-0.6774800690352322
]
},
"camera_in_world": {
"position_m": [
0.2971478998661041,
-0.4627215266227722,
0.7806501984596252
],
"position_mm": [
297.14788818359375,
-462.7215270996094,
780.6502075195312
],
"orientation_deg": {
"roll": 154.2146453857422,
"pitch": -5.430957317352295,
"yaw": -178.6295928955078
}
},
"uncertainty": {
"pose_covariance_6x6": [
[
2.216368778099703e-06,
-1.0420377127470311e-07,
-5.846035681663414e-07,
9.638753004284797e-09,
6.568277931365646e-07,
1.8540529905122184e-07
],
[
-1.0420377127470325e-07,
7.716359917594955e-06,
-8.780258686751835e-07,
-8.798584488389978e-07,
8.337615617564991e-07,
-3.831747509912712e-06
],
[
-5.846035681663423e-07,
-8.780258686751325e-07,
1.8740126796208265e-05,
-6.332974421687337e-07,
-7.826466185078999e-07,
3.040527671220899e-07
],
[
9.638753004284528e-09,
-8.798584488389991e-07,
-6.332974421687222e-07,
2.409326607990254e-07,
-7.666433629844329e-08,
3.751828231192777e-07
],
[
6.568277931365642e-07,
8.337615617564963e-07,
-7.826466185079048e-07,
-7.66643362984426e-08,
4.122731304646649e-07,
-3.5672545335086356e-07
],
[
1.8540529905122184e-07,
-3.831747509912709e-06,
3.040527671221047e-07,
3.7518282311927725e-07,
-3.567254533508643e-07,
2.7716081204320043e-06
]
],
"parameter_std": {
"rvec_std_deg": [
0.08529894195541628,
0.15915814568050488,
0.24803267374807242
],
"tvec_std_m": [
0.0004908489185065252,
0.0006420849869485074,
0.0016648147405738587
]
},
"camera_center_std_m": [
0.0027539612430056413,
0.002145451671454086,
0.0019549734255131183
],
"camera_center_std_mm": [
2.7539612430056413,
2.145451671454086,
1.9549734255131184
],
"orientation_std_deg": {
"roll": 0.19816528973825537,
"pitch": 0.158142424860459,
"yaw": 0.04543296464811841
}
}
},
"observations": {
"markers": [
{
"marker_id": 54,
"observed_center_px": [
735.25,
38.25
],
"projected_center_px": [
735.1257934570312,
38.84978103637695
],
"reprojection_error_px": 0.6125067811164705,
"confidence": 0.29867757253933086
},
{
"marker_id": 95,
"observed_center_px": [
1005.25,
139.0
],
"projected_center_px": [
1004.7445678710938,
139.50558471679688
],
"reprojection_error_px": 0.714896875632619,
"confidence": 0.9333333333333333
},
{
"marker_id": 58,
"observed_center_px": [
1234.75,
235.25
],
"projected_center_px": [
1234.0479736328125,
235.1490020751953
],
"reprojection_error_px": 0.7092542569779415,
"confidence": 0.36770453019575644
},
{
"marker_id": 85,
"observed_center_px": [
458.0,
68.0
],
"projected_center_px": [
458.30291748046875,
69.59838104248047
],
"reprojection_error_px": 1.6268315699341729,
"confidence": 0.875342401743355
},
{
"marker_id": 47,
"observed_center_px": [
731.75,
117.0
],
"projected_center_px": [
731.4143676757812,
117.63695526123047
],
"reprojection_error_px": 0.7199729591239207,
"confidence": 0.9759139241315488
},
{
"marker_id": 103,
"observed_center_px": [
1128.75,
284.5
],
"projected_center_px": [
1128.21533203125,
284.2250061035156
],
"reprojection_error_px": 0.6012416152520633,
"confidence": 0.9144001450649527
},
{
"marker_id": 59,
"observed_center_px": [
262.0,
123.5
],
"projected_center_px": [
263.8606872558594,
125.23478698730469
],
"reprojection_error_px": 2.5439424041120047,
"confidence": 0.9428090418229809
},
{
"marker_id": 105,
"observed_center_px": [
432.25,
147.75
],
"projected_center_px": [
432.2873840332031,
149.19923400878906
],
"reprojection_error_px": 1.449716102610904,
"confidence": 0.9442241264421224
},
{
"marker_id": 48,
"observed_center_px": [
148.5,
50.75
],
"projected_center_px": [
149.40765380859375,
51.741458892822266
],
"reprojection_error_px": 1.3441823434382303,
"confidence": 0.5647167996853729
},
{
"marker_id": 51,
"observed_center_px": [
1019.5,
307.5
],
"projected_center_px": [
1019.1678466796875,
307.8690490722656
],
"reprojection_error_px": 0.49651087191997795,
"confidence": 0.8824030145295596
},
{
"marker_id": 102,
"observed_center_px": [
239.5,
213.75
],
"projected_center_px": [
240.8688507080078,
215.52737426757812
],
"reprojection_error_px": 2.24339286525173,
"confidence": 0.9331395642876747
},
{
"marker_id": 96,
"observed_center_px": [
690.0,
280.75
],
"projected_center_px": [
689.8768310546875,
280.7818908691406
],
"reprojection_error_px": 0.12723056481812098,
"confidence": 0.9012948266053811
},
{
"marker_id": 62,
"observed_center_px": [
634.75,
296.75
],
"projected_center_px": [
634.6005249023438,
297.2737731933594
],
"reprojection_error_px": 0.5446844617769289,
"confidence": 0.8643569912878776
},
{
"marker_id": 71,
"observed_center_px": [
63.25,
113.75
],
"projected_center_px": [
63.99217224121094,
114.01245880126953
],
"reprojection_error_px": 0.7872129686354927,
"confidence": 0.6618010073234457
},
{
"marker_id": 92,
"observed_center_px": [
256.25,
273.25
],
"projected_center_px": [
257.8680114746094,
274.7709045410156
],
"reprojection_error_px": 2.220610671605798,
"confidence": 0.8785732433921414
},
{
"marker_id": 208,
"observed_center_px": [
295.25,
413.75
],
"projected_center_px": [
288.319091796875,
413.3032531738281
],
"reprojection_error_px": 6.945291300358831,
"confidence": 0.7372980849807326
},
{
"marker_id": 63,
"observed_center_px": [
36.0,
190.75
],
"projected_center_px": [
37.349727630615234,
191.05543518066406
],
"reprojection_error_px": 1.3838552404184128,
"confidence": 0.23122213258874602
},
{
"marker_id": 210,
"observed_center_px": [
1059.0,
560.0
],
"projected_center_px": [
1073.5731201171875,
555.7078857421875
],
"reprojection_error_px": 15.192039848292012,
"confidence": 0.6479471317434429
},
{
"marker_id": 217,
"observed_center_px": [
162.0,
532.25
],
"projected_center_px": [
146.59190368652344,
520.931640625
],
"reprojection_error_px": 19.118438507028245,
"confidence": 0.6089399477645175
},
{
"marker_id": 74,
"observed_center_px": [
1085.75,
734.75
],
"projected_center_px": [
1084.8079833984375,
734.9703979492188
],
"reprojection_error_px": 0.9674557011249624,
"confidence": 0.4941465175073702
},
{
"marker_id": 75,
"observed_center_px": [
1251.5,
801.5
],
"projected_center_px": [
1249.0706787109375,
800.055419921875
],
"reprojection_error_px": 2.8263781642957686,
"confidence": 0.07129070123540064
},
{
"marker_id": 52,
"observed_center_px": [
1075.0,
807.0
],
"projected_center_px": [
1073.82666015625,
807.2386474609375
],
"reprojection_error_px": 1.1973633531819778,
"confidence": 0.4747611026689374
},
{
"marker_id": 76,
"observed_center_px": [
273.75,
722.5
],
"projected_center_px": [
274.6397705078125,
723.6699829101562
],
"reprojection_error_px": 1.4698814804706541,
"confidence": 0.4464638052540792
},
{
"marker_id": 68,
"observed_center_px": [
421.5,
733.0
],
"projected_center_px": [
422.1742858886719,
733.5136108398438
],
"reprojection_error_px": 0.847618755377099,
"confidence": 0.453656743367513
},
{
"marker_id": 46,
"observed_center_px": [
472.25,
751.5
],
"projected_center_px": [
472.6407165527344,
751.65087890625
],
"reprojection_error_px": 0.41883632713964764,
"confidence": 0.4433609532391809
},
{
"marker_id": 53,
"observed_center_px": [
541.25,
783.25
],
"projected_center_px": [
541.2369384765625,
783.3223876953125
],
"reprojection_error_px": 0.07355665725931064,
"confidence": 0.41918904165916515
},
{
"marker_id": 50,
"observed_center_px": [
427.0,
777.75
],
"projected_center_px": [
427.4671630859375,
777.926025390625
],
"reprojection_error_px": 0.49922568744740276,
"confidence": 0.4069334148740479
},
{
"marker_id": 101,
"observed_center_px": [
1031.0,
900.25
],
"projected_center_px": [
1029.668212890625,
900.4525146484375
],
"reprojection_error_px": 1.3470965397955643,
"confidence": 0.34672467375183735
},
{
"marker_id": 82,
"observed_center_px": [
894.5,
890.25
],
"projected_center_px": [
893.2540283203125,
890.4962158203125
],
"reprojection_error_px": 1.2700660048814185,
"confidence": 0.39232527116476396
},
{
"marker_id": 100,
"observed_center_px": [
120.0,
723.5
],
"projected_center_px": [
121.81710815429688,
723.7096557617188
],
"reprojection_error_px": 1.8291630826238716,
"confidence": 0.4207088246724057
},
{
"marker_id": 73,
"observed_center_px": [
889.0,
926.25
],
"projected_center_px": [
887.5708618164062,
926.2489624023438
],
"reprojection_error_px": 1.4291385602573807,
"confidence": 0.1469791980355697
},
{
"marker_id": 60,
"observed_center_px": [
613.0,
860.5
],
"projected_center_px": [
612.533935546875,
860.8355102539062
],
"reprojection_error_px": 0.5742675377756796,
"confidence": 0.36564926628565203
},
{
"marker_id": 67,
"observed_center_px": [
497.75,
838.0
],
"projected_center_px": [
497.8586120605469,
838.2026977539062
],
"reprojection_error_px": 0.22996295165716743,
"confidence": 0.363884422773956
},
{
"marker_id": 70,
"observed_center_px": [
401.25,
866.75
],
"projected_center_px": [
401.48388671875,
867.0733032226562
],
"reprojection_error_px": 0.39903379679866513,
"confidence": 0.35896437880813437
},
{
"marker_id": 94,
"observed_center_px": [
29.0,
720.25
],
"projected_center_px": [
31.11460304260254,
720.2709350585938
],
"reprojection_error_px": 2.1147066710213593,
"confidence": 0.09031730482356817
},
{
"marker_id": 90,
"observed_center_px": [
351.5,
880.25
],
"projected_center_px": [
352.0174560546875,
880.55810546875
],
"reprojection_error_px": 0.6022372857988869,
"confidence": 0.36026641726859315
},
{
"marker_id": 104,
"observed_center_px": [
107.5,
792.25
],
"projected_center_px": [
109.0373764038086,
792.3465576171875
],
"reprojection_error_px": 1.540405654502856,
"confidence": 0.40599116793083023
},
{
"marker_id": 98,
"observed_center_px": [
436.5,
882.0
],
"projected_center_px": [
436.4425048828125,
883.1728515625
],
"reprojection_error_px": 1.174259969580457,
"confidence": 0.35391110957760685
},
{
"marker_id": 91,
"observed_center_px": [
254.25,
887.0
],
"projected_center_px": [
255.05325317382812,
887.015869140625
],
"reprojection_error_px": 0.8034099146071898,
"confidence": 0.3494656541641164
},
{
"marker_id": 88,
"observed_center_px": [
199.5,
873.0
],
"projected_center_px": [
200.95053100585938,
872.6802978515625
],
"reprojection_error_px": 1.4853449642002237,
"confidence": 0.33862598740745903
}
]
},
"qa": {
"sanity_notes": []
}
}

Binary file not shown.

After

Width:  |  Height:  |  Size: 260 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 282 KiB

File diff suppressed because it is too large Load Diff

View File

@@ -0,0 +1,761 @@
{
"schema_version": "1.0",
"created_utc": "2026-06-25T17:59:28Z",
"source": {
"detection_json": "/app/data/homing/20260625_175916/cam2_aruco_detection.json",
"robot_json": "/app/scripts/robot_1781069752019.json"
},
"camera": {
"camera_id": "cam2",
"camera_matrix": [
[
1388.99072265625,
0.0,
933.082763671875
],
[
0.0,
1394.8729248046875,
562.4996948242188
],
[
0.0,
0.0,
1.0
]
],
"distortion_coefficients": [
0.019531700760126114,
-0.11213663965463638,
0.0026758278254419565,
0.0007694826927036047,
0.05339815095067024
]
},
"estimation": {
"method": "single_camera_marker_center_lm",
"description": "Rigid init from per-marker pose estimates, followed by LM on normalized marker-center reprojection residuals.",
"marker_size_m": 0.025,
"num_used_markers": 40,
"used_marker_ids": [
85,
92,
105,
97,
54,
208,
217,
47,
93,
66,
62,
96,
94,
95,
79,
100,
104,
76,
69,
103,
51,
58,
68,
50,
88,
46,
91,
64,
90,
53,
72,
98,
67,
70,
60,
84,
86,
82,
75,
73
],
"history": {
"iters": [
0,
1,
2,
3
],
"rms": [
0.00929587415047541,
0.0010748985788974661,
0.001013950655392918,
0.001013950511849939
],
"lambda": [
0.001,
0.0005,
0.00025,
0.000125
]
},
"residual_rms_px": 1.9984633198313304,
"residual_median_px": 1.1048397349885588,
"residual_max_px": 8.012141964697209,
"sigma2_normalized": 1.1114547464625753e-06
},
"camera_pose": {
"world_to_camera": {
"rotation_matrix": [
[
0.7610290050506592,
0.6436193585395813,
0.08117286115884781
],
[
0.4864314794540405,
-0.4833734929561615,
-0.7278286218643188
],
[
-0.42920777201652527,
0.5933837294578552,
-0.6809379458427429
]
],
"translation_m": [
-0.17428335547447205,
-0.14054450392723083,
1.1974174976348877
],
"rvec_rad": [
2.177352630181243,
0.841105236753405,
-0.2590449733390141
]
},
"camera_in_world": {
"position_m": [
0.7149407863616943,
-0.6662914156913757,
0.7272217273712158
],
"position_mm": [
714.9407958984375,
-666.2914428710938,
727.2217407226562
],
"orientation_deg": {
"roll": 138.93040466308594,
"pitch": 25.41729164123535,
"yaw": 32.58573913574219
}
},
"uncertainty": {
"pose_covariance_6x6": [
[
1.7325268790245518e-06,
5.981655510564799e-07,
1.9008081078750225e-07,
6.084320732905797e-08,
-2.3354702118446124e-07,
-3.602729870649897e-08
],
[
5.981655510564792e-07,
7.185403999584639e-07,
-1.733161988537022e-07,
1.3590794895526218e-07,
-1.804162779559688e-07,
1.5814835072347804e-07
],
[
1.9008081078749777e-07,
-1.733161988537059e-07,
2.5200605751354745e-06,
-7.330065793590657e-08,
-3.490611313350714e-07,
-1.0443589786543664e-06
],
[
6.084320732905791e-08,
1.3590794895526253e-07,
-7.330065793590603e-08,
6.430560370646186e-08,
-2.0737599537247602e-08,
9.272757844523572e-08
],
[
-2.3354702118446045e-07,
-1.8041627795596826e-07,
-3.490611313350726e-07,
-2.073759953724742e-08,
1.4131779018249e-07,
1.552658806009075e-07
],
[
-3.6027298706497375e-08,
1.5814835072348006e-07,
-1.0443589786543662e-06,
9.272757844523604e-08,
1.552658806009069e-07,
7.158822480370525e-07
]
],
"parameter_std": {
"rvec_std_deg": [
0.07541584873239728,
0.048567777257391795,
0.09095532342017693
],
"tvec_std_m": [
0.0002535854958519155,
0.0003759225853583288,
0.0008460982496359702
]
},
"camera_center_std_m": [
0.001238859522904557,
0.0010725601282025278,
0.0013428043149285806
],
"camera_center_std_mm": [
1.2388595229045571,
1.0725601282025277,
1.3428043149285807
],
"orientation_std_deg": {
"roll": 0.09670418188338105,
"pitch": 0.07031307438362219,
"yaw": 0.04214847006011442
}
}
},
"observations": {
"markers": [
{
"marker_id": 85,
"observed_center_px": [
943.0,
1037.5
],
"projected_center_px": [
942.2024536132812,
1035.979736328125
],
"reprojection_error_px": 1.7167649434302272,
"confidence": 0.28339330788046485
},
{
"marker_id": 92,
"observed_center_px": [
1260.5,
1040.25
],
"projected_center_px": [
1260.0,
1039.166259765625
],
"reprojection_error_px": 1.1935212170729015,
"confidence": 0.186820630468236
},
{
"marker_id": 105,
"observed_center_px": [
1018.75,
1005.5
],
"projected_center_px": [
1017.9473266601562,
1004.2492065429688
],
"reprojection_error_px": 1.48619270710366,
"confidence": 0.868630415221559
},
{
"marker_id": 97,
"observed_center_px": [
650.75,
884.0
],
"projected_center_px": [
651.09619140625,
883.6798095703125
],
"reprojection_error_px": 0.47156166195399984,
"confidence": 0.7472812484733762
},
{
"marker_id": 54,
"observed_center_px": [
727.5,
891.5
],
"projected_center_px": [
727.0736083984375,
890.49853515625
],
"reprojection_error_px": 1.0884583736414755,
"confidence": 0.7927270034536174
},
{
"marker_id": 208,
"observed_center_px": [
1312.75,
909.5
],
"projected_center_px": [
1318.954833984375,
914.5689697265625
],
"reprojection_error_px": 8.012141964697209,
"confidence": 0.5865958415608975
},
{
"marker_id": 217,
"observed_center_px": [
1525.0,
915.5
],
"projected_center_px": [
1526.02783203125,
921.9722290039062
],
"reprojection_error_px": 6.553334034174421,
"confidence": 0.48103075735306616
},
{
"marker_id": 47,
"observed_center_px": [
779.5,
850.5
],
"projected_center_px": [
779.2447509765625,
849.6724243164062
],
"reprojection_error_px": 0.8660447887040601,
"confidence": 0.7324762772043267
},
{
"marker_id": 93,
"observed_center_px": [
1890.0,
969.75
],
"projected_center_px": [
1890.317138671875,
971.253173828125
],
"reprojection_error_px": 1.5362644612040617,
"confidence": 0.0615171510658591
},
{
"marker_id": 66,
"observed_center_px": [
549.25,
803.25
],
"projected_center_px": [
550.2374877929688,
801.9854125976562
],
"reprojection_error_px": 1.6044667149644478,
"confidence": 0.526209880769093
},
{
"marker_id": 62,
"observed_center_px": [
961.5,
801.5
],
"projected_center_px": [
960.479248046875,
801.3424072265625
],
"reprojection_error_px": 1.0328455993265524,
"confidence": 0.5777637144454008
},
{
"marker_id": 96,
"observed_center_px": [
911.5,
782.5
],
"projected_center_px": [
910.5089721679688,
782.1806640625
],
"reprojection_error_px": 1.0412068021481435,
"confidence": 0.5986996695409863
},
{
"marker_id": 94,
"observed_center_px": [
1813.75,
889.5
],
"projected_center_px": [
1813.9437255859375,
890.62109375
],
"reprojection_error_px": 1.1377085738166388,
"confidence": 0.321218994735393
},
{
"marker_id": 95,
"observed_center_px": [
631.25,
708.75
],
"projected_center_px": [
631.6299438476562,
708.2338256835938
],
"reprojection_error_px": 0.640931550393094,
"confidence": 0.5102620497459525
},
{
"marker_id": 79,
"observed_center_px": [
875.5,
714.5
],
"projected_center_px": [
875.01416015625,
714.7401733398438
],
"reprojection_error_px": 0.5419627173032531,
"confidence": 0.5178115536596077
},
{
"marker_id": 100,
"observed_center_px": [
1710.25,
832.5
],
"projected_center_px": [
1710.6463623046875,
831.8109130859375
],
"reprojection_error_px": 0.7949489617009169,
"confidence": 0.31927346015206026
},
{
"marker_id": 104,
"observed_center_px": [
1769.75,
795.5
],
"projected_center_px": [
1770.6925048828125,
794.8803100585938
],
"reprojection_error_px": 1.1279765412478604,
"confidence": 0.2820489477598529
},
{
"marker_id": 76,
"observed_center_px": [
1550.0,
745.5
],
"projected_center_px": [
1549.9093017578125,
744.649169921875
],
"reprojection_error_px": 0.8556506255348009,
"confidence": 0.2957408773689774
},
{
"marker_id": 69,
"observed_center_px": [
465.5,
587.5
],
"projected_center_px": [
467.173583984375,
587.0509643554688
],
"reprojection_error_px": 1.7327770666811397,
"confidence": 0.3850070054256877
},
{
"marker_id": 103,
"observed_center_px": [
649.0,
589.5
],
"projected_center_px": [
649.4606323242188,
590.1286010742188
],
"reprojection_error_px": 0.7793083142275168,
"confidence": 0.4188423377379467
},
{
"marker_id": 51,
"observed_center_px": [
720.75,
621.0
],
"projected_center_px": [
720.8779907226562,
620.9248046875
],
"reprojection_error_px": 0.14844514174617424,
"confidence": 0.43643037974141263
},
{
"marker_id": 58,
"observed_center_px": [
569.25,
573.75
],
"projected_center_px": [
570.5189208984375,
573.7750244140625
],
"reprojection_error_px": 1.2691676279320252,
"confidence": 0.37500699513057323
},
{
"marker_id": 68,
"observed_center_px": [
1415.5,
663.25
],
"projected_center_px": [
1414.463134765625,
662.5946044921875
],
"reprojection_error_px": 1.2266347402207098,
"confidence": 0.25940485673792224
},
{
"marker_id": 50,
"observed_center_px": [
1438.0,
635.25
],
"projected_center_px": [
1437.16650390625,
634.8544921875
],
"reprojection_error_px": 0.922573665375857,
"confidence": 0.270957436835909
},
{
"marker_id": 88,
"observed_center_px": [
1720.5,
694.0
],
"projected_center_px": [
1720.8408203125,
693.2443237304688
],
"reprojection_error_px": 0.8289783530016113,
"confidence": 0.21331579725472802
},
{
"marker_id": 46,
"observed_center_px": [
1380.75,
628.5
],
"projected_center_px": [
1379.9970703125,
628.1585693359375
],
"reprojection_error_px": 0.8267272903932755,
"confidence": 0.23649772201893263
},
{
"marker_id": 91,
"observed_center_px": [
1672.75,
657.5
],
"projected_center_px": [
1672.596923828125,
656.64208984375
],
"reprojection_error_px": 0.8714597813971704,
"confidence": 0.22421739747061642
},
{
"marker_id": 64,
"observed_center_px": [
541.75,
511.0
],
"projected_center_px": [
543.0106811523438,
511.8523864746094
],
"reprojection_error_px": 1.5218014555032349,
"confidence": 0.3194003123985894
},
{
"marker_id": 90,
"observed_center_px": [
1571.25,
614.0
],
"projected_center_px": [
1571.232177734375,
612.768798828125
],
"reprojection_error_px": 1.2313301583159495,
"confidence": 0.21534147510121324
},
{
"marker_id": 53,
"observed_center_px": [
1340.25,
579.75
],
"projected_center_px": [
1339.28076171875,
579.6795654296875
],
"reprojection_error_px": 0.9717941523468644,
"confidence": 0.2131836038878541
},
{
"marker_id": 72,
"observed_center_px": [
1282.0,
562.75
],
"projected_center_px": [
1280.0421142578125,
563.0445556640625
],
"reprojection_error_px": 1.9799190939764175,
"confidence": 0.2329113678114362
},
{
"marker_id": 98,
"observed_center_px": [
1493.25,
573.75
],
"projected_center_px": [
1492.691650390625,
572.2074584960938
],
"reprojection_error_px": 1.6404842509340254,
"confidence": 0.19836555127576866
},
{
"marker_id": 67,
"observed_center_px": [
1411.0,
570.0
],
"projected_center_px": [
1410.1534423828125,
569.2794799804688
],
"reprojection_error_px": 1.111669419280521,
"confidence": 0.22146320976627604
},
{
"marker_id": 70,
"observed_center_px": [
1516.0,
598.25
],
"projected_center_px": [
1515.696044921875,
597.093505859375
],
"reprojection_error_px": 1.1957706246675956,
"confidence": 0.17917987028547397
},
{
"marker_id": 60,
"observed_center_px": [
1326.5,
509.5
],
"projected_center_px": [
1325.19873046875,
509.093017578125
],
"reprojection_error_px": 1.3634284303456619,
"confidence": 0.19672907121069258
},
{
"marker_id": 84,
"observed_center_px": [
1284.5,
508.0
],
"projected_center_px": [
1283.4097900390625,
507.8693542480469
],
"reprojection_error_px": 1.0980100506965966,
"confidence": 0.1992299923059727
},
{
"marker_id": 86,
"observed_center_px": [
1259.0,
466.5
],
"projected_center_px": [
1258.5447998046875,
466.7055358886719
],
"reprojection_error_px": 0.4994519189518402,
"confidence": 0.20697942435059868
},
{
"marker_id": 82,
"observed_center_px": [
1129.75,
393.75
],
"projected_center_px": [
1128.9288330078125,
393.5796813964844
],
"reprojection_error_px": 0.8386439386067119,
"confidence": 0.16919547786983627
},
{
"marker_id": 75,
"observed_center_px": [
862.5,
325.25
],
"projected_center_px": [
861.7324829101562,
326.8608703613281
],
"reprojection_error_px": 1.7843726640496438,
"confidence": 0.21628605150283992
},
{
"marker_id": 73,
"observed_center_px": [
1153.5,
379.0
],
"projected_center_px": [
1152.4403076171875,
379.2215881347656
],
"reprojection_error_px": 1.08261223328565,
"confidence": 0.1485104612681437
}
]
},
"qa": {
"sanity_notes": []
}
}

Binary file not shown.

After

Width:  |  Height:  |  Size: 458 KiB

File diff suppressed because it is too large Load Diff

View File

@@ -0,0 +1,59 @@
{
"schema_version": "1.0",
"created_utc": "2026-06-25T17:59:33Z",
"method": "hybrid",
"seeded": true,
"movements": {
"x": {
"value": 110.01986467359067,
"unit": "mm",
"observable": true,
"confidence": "high",
"n_markers": 6
},
"y": {
"value": 1.0330890750947754,
"unit": "deg",
"observable": true,
"confidence": "high",
"n_markers": 6
},
"z": {
"value": 96.2744445958484,
"unit": "deg",
"observable": true,
"confidence": "medium",
"n_markers": 1
},
"a": {
"value": 86.75801106951552,
"unit": "deg",
"observable": true,
"confidence": "high",
"n_markers": 3
},
"b": {
"value": 62.05866240456044,
"unit": "deg",
"observable": true,
"confidence": "medium",
"n_markers": 3
},
"c": {
"value": -80.56572105590129,
"unit": "deg",
"observable": true,
"confidence": "medium",
"n_markers": 3
},
"e": {
"value": 35.00722900692152,
"unit": "mm",
"observable": true,
"confidence": "medium",
"n_markers": 3
}
},
"residual_rms": 33.572073291771,
"num_markers": 55
}

View File

@@ -0,0 +1,222 @@
{
"status": "ok",
"link": "Arm1",
"joint": "y",
"method": "primary",
"joint_origin_world_mm": [
154.64058080413625,
108.3154,
53.4964
],
"joint_axis_world": [
-1.0,
0.0,
0.0
],
"mean_angle_deg": 7.073578785211089,
"circular_variance": 0.007972526914742928,
"circular_std_deg": 7.249434372855015,
"num_pairs_used": 15,
"num_markers_matched": 6,
"per_pair": [
{
"marker_ids": [
55,
56
],
"link": "Arm1",
"tier": "primary",
"skipped": false,
"angle_deg": 0.20885786426191633,
"baseline_model_mm": 430.4301189508002,
"baseline_obs_mm": 431.321568455382,
"weight": 185653.79401629578
},
{
"marker_ids": [
55,
77
],
"link": "Arm1",
"tier": "primary",
"skipped": false,
"angle_deg": 0.1534985213298885,
"baseline_model_mm": 455.14017577005876,
"baseline_obs_mm": 456.76224811573417,
"weight": 207890.84989252244
},
{
"marker_ids": [
55,
198
],
"link": "Arm1",
"tier": "primary",
"skipped": false,
"angle_deg": 2.7294530137510984,
"baseline_model_mm": 120.70271952197267,
"baseline_obs_mm": 235.9270100140718,
"weight": 28477.031717386148
},
{
"marker_ids": [
55,
229
],
"link": "Arm1",
"tier": "primary",
"skipped": false,
"angle_deg": 36.070256539732,
"baseline_model_mm": 63.35571402801802,
"baseline_obs_mm": 159.8295466346313,
"weight": 10126.115049811471
},
{
"marker_ids": [
55,
243
],
"link": "Arm1",
"tier": "primary",
"skipped": false,
"angle_deg": 89.62500483491822,
"baseline_model_mm": 34.32559540634365,
"baseline_obs_mm": 110.61945936866468,
"weight": 3797.0788063572545
},
{
"marker_ids": [
56,
77
],
"link": "Arm1",
"tier": "primary",
"skipped": false,
"angle_deg": -0.8278666326016993,
"baseline_model_mm": 24.720487454740837,
"baseline_obs_mm": 25.442346313142185,
"weight": 628.9472028532032
},
{
"marker_ids": [
56,
198
],
"link": "Arm1",
"tier": "primary",
"skipped": false,
"angle_deg": 16.060958275347453,
"baseline_model_mm": 332.5358726513577,
"baseline_obs_mm": 249.91702454137288,
"weight": 83106.37584629621
},
{
"marker_ids": [
56,
229
],
"link": "Arm1",
"tier": "primary",
"skipped": false,
"angle_deg": 10.575835506835253,
"baseline_model_mm": 421.3102260804976,
"baseline_obs_mm": 329.51739043275734,
"weight": 138829.0462606806
},
{
"marker_ids": [
56,
243
],
"link": "Arm1",
"tier": "primary",
"skipped": false,
"angle_deg": 8.204562901697962,
"baseline_model_mm": 452.5694494770941,
"baseline_obs_mm": 352.3360154416916,
"weight": 159456.51653939928
},
{
"marker_ids": [
77,
198
],
"link": "Arm1",
"tier": "primary",
"skipped": false,
"angle_deg": 14.349880680055369,
"baseline_model_mm": 356.9731122927888,
"baseline_obs_mm": 273.03257450221435,
"weight": 97465.28787736819
},
{
"marker_ids": [
77,
229
],
"link": "Arm1",
"tier": "primary",
"skipped": false,
"angle_deg": 9.66556769141446,
"baseline_model_mm": 445.86971516352173,
"baseline_obs_mm": 353.78452354827397,
"weight": 157741.80474373116
},
{
"marker_ids": [
77,
243
],
"link": "Arm1",
"tier": "primary",
"skipped": false,
"angle_deg": 7.573295647776105,
"baseline_model_mm": 477.27969043318825,
"baseline_obs_mm": 377.3635068156209,
"weight": 180107.93771374188
},
{
"marker_ids": [
198,
229
],
"link": "Arm1",
"tier": "primary",
"skipped": false,
"angle_deg": -3.397605318552869,
"baseline_model_mm": 90.00000000000003,
"baseline_obs_mm": 88.54992394070814,
"weight": 7969.493154663735
},
{
"marker_ids": [
198,
243
],
"link": "Arm1",
"tier": "primary",
"skipped": false,
"angle_deg": -3.2531512518968957,
"baseline_model_mm": 129.8075498574717,
"baseline_obs_mm": 128.9115321451203,
"weight": 16733.690136130772
},
{
"marker_ids": [
229,
243
],
"link": "Arm1",
"tier": "primary",
"skipped": false,
"angle_deg": -2.3933465309361535,
"baseline_model_mm": 49.49747468305833,
"baseline_obs_mm": 49.589805881590635,
"weight": 2454.5701611618097
}
],
"accumulated_state": {
"x": 44.640580804136256,
"y": 7.073578785211089
}
}

View File

@@ -0,0 +1,65 @@
{
"status": "ok",
"link": "Arm2",
"joint": "a",
"method": "primary",
"joint_origin_world_mm": [
244.64058080413622,
-139.78180740318305,
84.28236561327859
],
"joint_axis_world": [
0.0,
0.13416044351804374,
0.9909596234938344
],
"mean_angle_deg": 92.49229573636204,
"circular_variance": 0.0005036790852294137,
"circular_std_deg": 1.8187344232244496,
"num_pairs_used": 2,
"num_markers_matched": 3,
"per_pair": [
{
"marker_ids": [
143,
144
],
"link": "Arm2",
"tier": "primary",
"skipped": false,
"angle_deg": 90.53857082520275,
"baseline_model_mm": 26.684454275851333,
"baseline_obs_mm": 24.841154956793282,
"weight": 662.8726636038881
},
{
"marker_ids": [
143,
148
],
"link": "Arm2",
"tier": "primary",
"skipped": false,
"angle_deg": 94.18507945848495,
"baseline_model_mm": 26.836171485515596,
"baseline_obs_mm": 28.506933940749555,
"weight": 765.0169677602199
},
{
"marker_ids": [
144,
148
],
"link": "Arm2",
"tier": "primary",
"skipped": true,
"reason": "bl_model=0.4 bl_obs=3.9 < 15.0"
}
],
"accumulated_state": {
"x": 44.640580804136256,
"y": 7.073578785211089,
"z": 90.63649643342701,
"a": 92.49229573636204
}
}

View File

@@ -0,0 +1,41 @@
{
"status": "ok",
"link": "Ellbow",
"joint": "z",
"method": "fallback_1_child_axis",
"joint_origin_world_mm": [
154.64058080413625,
-139.78180740318305,
84.28236561327859
],
"joint_axis_world": [
-1.0,
0.0,
0.0
],
"mean_angle_deg": 90.63649643342701,
"circular_variance": 0.0,
"circular_std_deg": 0.0,
"num_pairs_used": 1,
"num_markers_matched": 1,
"per_pair": [
{
"marker_ids": [
144,
148
],
"link": "Arm2",
"tier": "fallback_1_child_axis",
"skipped": false,
"angle_deg": 90.63649643342701,
"baseline_model_mm": 107.0,
"baseline_obs_mm": 107.57847026231472,
"weight": 11510.896318067675
}
],
"accumulated_state": {
"x": 44.640580804136256,
"y": 7.073578785211089,
"z": 90.63649643342701
}
}

View File

@@ -1,595 +0,0 @@
{
"coordinateSystem": {
"handedness": "right",
"x": "right",
"y": "backward",
"z": "up"
},
"units": {
"length": "mm",
"rotation": "degree"
},
"vision_config": {
"MarkerType": "DICT_4X4_250",
"MarkerSize": 0.025
},
"renderingInfo": {
"width": 1280,
"height": 720,
"cameraPosition__1": [-10, -800, 500],
"cameraPosition__2": [-500, 300, 1200],
"cameraPosition__3": [-200, -900, 200],
"cameraPosition__4": [1200, 200, 300],
"cameraPosition_a":[-300, -800,500],
"cameraPosition":[300, -500,1450],
"cameraPosition_c":[600, -500,600],
"cameraTarget": [210, -200, 180],
"cameraUpVector": [0, 0, 1],
"lightPosition": [-500, -500, 500],
"lightTarget": [0, 0, 0],
"lightUpVector": [0, 0, 1],
"metric": "mm",
"showSkeleton": true,
"showMarkers": true,
"backgroundColor": [0.70, 0.85, 1.0],
"backgroundStrength": 0.20,
"sunEnergy": 0.35,
"areaEnergy": 120,
"exposure": -1.5,
"lensDirt": true,
"lensDirtStrength": 0.08,
"dofEnabled": true,
"dofFStop": 11,
"arucoDust": false,
"arucoDustStrength": 0.00005,
"localizedBlur": false,
"localizedBlurStrength": 0.15,
"vignette": true,
"vignetteStrength": 0.08,
"sensorNoise": true,
"sensorNoiseStrength": 0.01,
"lensDistortion": true,
"lensDistortionStrength": 0.002,
"materials": {
"wood": {
"baseColor": [0.72, 0.52, 0.33],
"roughness": 0.85,
"metallic": 0.0
},
"plaWhite": {
"baseColor": [0.95, 0.95, 0.95],
"roughness": 0.45,
"metallic": 0.0
},
"steel": {
"baseColor": [0.72, 0.72, 0.75],
"roughness": 0.25,
"metallic": 1.0
},
"powderCoatBlue": {
"baseColor": [0.15, 0.25, 0.7],
"roughness": 0.55,
"metallic": 0.0
},
"defaultPlastic": {
"baseColor": [0.95, 0.95, 0.95],
"roughness": 0.4,
"metallic": 0.0
},
"skeletonRed": {
"baseColor": [0.85, 0.20, 0.20],
"roughness": 0.35,
"metallic": 0.0
},
"markerBlack": {
"baseColor": [0.04, 0.04, 0.04],
"roughness": 0.80,
"metallic": 0.0
}
},
"skeletonDefaults": {
"radius": 4,
"color": [0.85, 0.20, 0.20]
},
"markerDefaults": {
"size": 25,
"thickness": 1,
"color": [0.04, 0.04, 0.04]
}
},
"defaultPosition__": {
"x": 10,
"y": 4,
"z": 20,
"a": 10,
"b": 2,
"c": 9,
"e": 1
},
"defaultPosition": {
"x": 180,
"y": 86,
"z": -120,
"a": -60,
"b": 22,
"c": 91,
"e": 10
},
"recognized": {
"x": null,
"y": null,
"z": null,
"a": null,
"b": null,
"c": null,
"e": null
},
"constraint_rules": {
"rigid_distance": {
"enabled": true,
"mode": "mst",
"weight": 1.0
},
"joint_axis_projection": {
"enabled": true,
"max_pairs": 2,
"weight": 0.35
},
"chain_axis_projection": {
"enabled": false,
"max_depth": 3,
"max_pairs": 2,
"weight": 0.15
},
"axis_alignment_threshold": 0.95
},
"observation_weighting": {
"enabled": true,
"distance_weight": true,
"marker_size_weight": true,
"view_angle_weight": true
},
"multiview_calculation": {
"combine_mode": "mean",
"size_ref_px": 50.0,
"border_ref_px": 120.0,
"center_ref_norm": 0.01,
"sharpness_ref": 2500.0,
"homography_ref": 0.18,
"size_factor": 0.3,
"aspect_factor": 0.3,
"border_factor": 0.01,
"center_factor": 0.01,
"sharpness_factor": 0.5,
"homography_factor": 0.2,
"normal_visibility_factor": 0.01,
"spin_factor": 0.3,
"weight_floor": 0.3
},
"robot_test_poses": {
"sim04": {"x": 70, "y": 50,"z": -70,"a": 120,"b": 50,"c": 30,"e": 20},
"sim05": {"x": 180,"y": 86,"z": -120,"a": -60,"b": 22,"c": 91,"e": 10},
"sim06": {"x": 80, "y": 20, "z": 80, "a": -120, "b": 23, "c": 9, "e": 3},
"sim07": {"x": 30, "y": -2, "z": 95, "a": 20, "b": 23, "c": 9, "e": 9},
"sim08": {"x": 50, "y": -2, "z": 95, "a": 20, "b": 60, "c": 9, "e": 3},
"sim09": {"x": 60, "y": -2, "z": 95, "a": 200, "b": 60, "c": 9, "e": 8},
"sim09a": {"x": 60, "y": -2, "z": 95, "a": 200, "b": 60, "c": 9, "e": 8},
"sim09b": {"x": 60, "y": -2, "z": 95, "a": 200, "b": 60, "c": 9, "e": 8},
"sim010": {"x": 120, "y": 60, "z": -110, "a": 20, "b": 30, "c": 180, "e": 4},
"sim011": {"x": 50, "y": 4, "z": 176, "a": 20, "b": 60, "c": 9, "e": 5},
"sim012": {"x": 50, "y": 0, "z": 178, "a": 210, "b": 80, "c": 90, "e": 6}
},
"movements": {
"x": null,
"y": null,
"z": null,
"a": null,
"b": null,
"c": null,
"e": null
},
"state_pose_params":{
"numbers_of_Elements_to_consider_start": 3,
"numbers_of_Elements_to_consider_final": 4,
"solver_in_between_geometrical": false,
"solver_after_geometrical": false,
"geometric_passes_per_stage": 2,
"revolute_search_coarse_deg": 5.0,
"revolute_search_fine_deg": 1.0,
"root_pose_min_markers": 3,
"use_marker_normals_flip_tiebreak": true,
"normal_flip_weight": 0.05
},
"links": {
"Board": {
"parent": null,
"size": [1000, 200, 25],
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"skeleton": {
"from": [0, 0, 16],
"to": [1000, 0, 16],
"radius": 4,
"color": [0.85, 0.20, 0.20]
},
"markers":[
{"id":210,"position":[20, -20, 0.3], "normal":[0,0,1]},
{"id":211,"position":[250, -10, 0.3], "normal":[0,0,1]},
{"id":215,"position":[250, -90, 0.3], "normal":[0,0,1]},
{"id":214,"position":[350, -10, 0.3], "normal":[0,0,1]},
{"id":208,"position":[350, -90, 0.3], "normal":[0,0,1]},
{"id":206,"position":[650, -10, 0.3], "normal":[0,0,1]},
{"id":205,"position":[750, -90, 0.3], "normal":[0,0,1]},
{"id":207,"position":[750, -10, 0.3], "normal":[0,0,1]},
{"id":217,"position":[650, -90, 0.3], "normal":[0,0,1]},
{"id": 46, "position": [536.71, 185.44, -27.3], "normal": [0, 0, 1], "spin": 90, "info": "is placed on a white paper, A0_60Arucos_25mm_Seet223.pdf, with the following marker placements:"},
{"id": 47, "position": [344.23, -286.54, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 48, "position": [688.69, -320.72, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 49, "position": [1006.0, 158.33, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 50, "position": [573.41, 211.86, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 51, "position": [167.8, -172.08, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 52, "position": [94.68, 208.66, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 53, "position": [486.25, 212.24, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 54, "position": [342.27, -330.59, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 55, "position": [283.72, -262.58, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 56, "position": [498.68, 168.67, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 57, "position": [602.86, -364.05, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 58, "position": [50.09, -218.11, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 59, "position": [626.21, -278.75, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 60, "position": [434.36, 283.81, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 61, "position": [-22.42, 335.83, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 62, "position": [404.7, -175.1, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 63, "position": [777.4, -236.15, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 64, "position": [-21.27, -188.23, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 65, "position": [803.39, -297.37, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 66, "position": [209.75, -363.23, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 67, "position": [523.07, 267.04, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 68, "position": [573.73, 170.64, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 69, "position": [7.61, -281.21, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 70, "position": [601.87, 300.33, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 71, "position": [749.75, -284.01, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 72, "position": [440.99, 194.32, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 73, "position": [221.73, 333.11, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 74, "position": [93.78, 144.5, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 75, "position": [-25.7, 194.58, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 76, "position": [685.21, 166.8, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 77, "position": [18.19, 191.57, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 78, "position": [823.11, -344.38, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 79, "position": [312.3, -159.11, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 80, "position": [863.59, -335.92, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 81, "position": [132.14, 169.03, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 82, "position": [219.16, 297.24, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 83, "position": [44.16, 339.22, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 84, "position": [407.49, 258.42, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 85, "position": [504.58, -312.75, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 86, "position": [362.89, 292.01, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 87, "position": [943.63, -245.76, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 88, "position": [765.87, 316.04, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 89, "position": [988.02, -369.14, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 90, "position": [643.17, 316.43, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 91, "position": [723.35, 328.05, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 92, "position": [645.09, -184.84, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 93, "position": [934.88, 143.6, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 94, "position": [875.7, 173.65, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 95, "position": [186.04, -274.07, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 96, "position": [369.77, -186.49, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 97, "position": [304.35, -359.67, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 98, "position": [575.27, 315.06, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 99, "position": [959.16, -321.55, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 100, "position": [803.25, 172.36, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 101, "position": [117.7, 298.66, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 102, "position": [649.69, -223.0, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 103, "position": [105.71, -187.71, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 104, "position": [826.71, 239.16, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 105, "position": [524.84, -266.25, -27.3], "normal": [0, 0, 1], "spin": 90}
],
"model": [
{
"stlFile": "surfaces/Board.stl",
"originOfModel": [0, 0, 0],
"rotationOfModelDegree": [0, 0, -90],
"material": "wood"
},
{
"stlFile": "surfaces/BoardRail.stl",
"originOfModel": [0, 0, 0],
"rotationOfModelDegree": [0, 0, -90],
"material": "steel"
}
]
},
"Base": {
"parent": "Board",
"size": [150, 200, 150],
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Slider",
"type": "linear",
"axis": [1, 0, 0],
"origin": [0, 0, 16],
"rotation": [0, 0, 0],
"variable": "x"
},
"skeleton": {
"from": [0, 108, 45],
"to": [110, 108, 45],
"radius": 4,
"color": [0.20, 0.80, 0.20]
},
"markers": [
],
"model": [
{
"stlFile": "surfaces/Base.stl",
"originOfModel": [-30, 0, -35],
"rotationOfModelDegree": [0, 0, 0],
"material": "plaWhite"
}
]
},
"Arm1": {
"parent": "Base",
"size": [70, 250, 70],
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Joint1",
"type": "revolute",
"axis": [-1, 0, 0],
"origin": [110, 108, 45],
"rotation": [0, 0, 0],
"variable": "y"
},
"skeleton": {
"from": [0, 0, 0],
"to": [0, -250, 0],
"radius": 4,
"color": [0.20, 0.20, 0.90]
},
"markers": [
{
"id": 198,
"name": "aruco_198",
"position": [0, -160, 35],
"normal": [0, 0, 1],
"size": 25,
"spin": 0
},
{
"id": 229,
"name": "aruco_229",
"position": [0, -250, 35],
"normal": [0, 0, 1],
"size": 25,
"spin": 0
},
{
"id": 242,
"name": "aruco_242",
"position": [0, -250, -35],
"normal": [0, 0, -1],
"size": 25,
"spin": 0
},
{
"id": 243,
"name": "aruco_243",
"position": [0, -285, 0],
"normal": [0, -1, 0],
"size": 25,
"spin": 0
}
],
"model": [
{
"stlFile": "surfaces/Holm.stl",
"originOfModel__": [-25,29,-28.5],
"originOfModel": [-29,25,28.5],
"rotationOfModelDegree__": [0, 0, 0],
"rotationOfModelDegree": [180, 0, -90],
"material": "powderCoatBlue"
}
]
},
"Ellbow": {
"parent": "Arm1",
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Joint2",
"type": "revolute",
"axis": [-1, 0, 0],
"origin": [0, -250, 0],
"rotation": [0, 0, 0],
"variable": "z"
},
"skeleton": {
"from": [0, 0, 0],
"to": [90, 0, 0],
"radius": 4,
"color": [0.90, 0.20, 0.20]
},
"model": [
{
"stlFile": "surfaces/Ellebogen.stl",
"originOfModel": [90,0,0],
"rotationOfModelDegree": [0,-90,-90],
"material": "defaultPlastic"
}
],
"markers": [
{"id": 244, "name": "aruco_244", "position": [125, 0, 0], "normal": [1, 0, 0], "size": 25, "spin": 0},
{"id": 245, "name": "aruco_245", "position": [90, 0, -35], "normal": [0, 0, -1], "size": 25, "spin": 0},
{"id": 246, "name": "aruco_246", "position": [90, 0, 35], "normal": [0, 0, 1], "size": 25},
{"id": 247, "name": "aruco_247", "position": [52.5, 0, 35], "normal": [0, 0, 1], "size": 25},
{"id": 248, "name": "aruco_248", "position": [52.5, 0, -35], "normal": [0, 0, -1], "size": 25},
{"id": 232, "name": "aruco_232", "position": [90, 24.75, -24.75], "normal": [0, 1, -1], "size": 25},
{"id": 231, "name": "aruco_231", "position": [90, 24.75, 24.75], "normal": [0, 1, 1], "size": 25}
]
},
"Arm2": {
"parent": "Ellbow",
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Joint3",
"type": "revolute",
"axis": [0, -1, 0],
"origin": [90, 0, 0],
"rotation": [0, 0, 0],
"variable": "a"
},
"skeleton": {
"from": [0, 0, 0],
"to": [0, -250, 0],
"radius": 4,
"color": [0.95, 0.85, 0.20]
},
"model": [
{
"stlFile": "surfaces/Unterarm.stl",
"originOfModel": [0,-250,0],
"rotationOfModelDegree": [180, 0, -90],
"material": "defaultPlastic"
}
],
"markers":[
{"id":120, "position":[24.75, -112, -24.75],"normal":[1,0,-1]},
{"id":122, "name": "aruco_122", "position":[-35,-112,0], "normal":[-1,0,0]},
{"id":218, "name": "aruco_218", "position":[35,-112,0], "normal":[1,0,0]},
{"id":113, "name": "aruco_113", "position":[0, -182, 30],"normal":[0,0,1]},
{"id":114, "name": "aruco_114", "position":[ 24.75, -182, -24.75],"normal":[ 1,0,-1]},
{"id":115, "name": "aruco_115", "position":[-24.75, -182, -24.75],"normal":[-1,0,-1]},
{"id":124, "name": "aruco_124", "position":[-35,-219,0], "normal":[-1,0,0]},
{"id":219, "name": "aruco_219", "position":[35,-219,0], "normal":[1,0,0]}
]
},
"Hand": {
"parent": "Arm2",
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Joint4",
"type": "revolute",
"axis": [1, 0, 0],
"origin": [0, -250, 0],
"rotation": [0, 0, 0],
"variable": "b"
},
"skeleton": {
"from": [0, 0, 0],
"to": [0, -35, 0],
"radius": 4,
"color": [0.95, 0.55, 0.15]
}
},
"Palm": {
"parent": "Hand",
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Joint3",
"type": "revolute",
"axis": [0, -1, 0],
"origin": [0, 0, 0],
"rotation": [0, 0, 0],
"variable": "c"
},
"skeleton": {
"from": [-50, -35, 0],
"to": [50, -35, 0],
"radius": 7,
"color": [0.95, 0.20, 0.20]
}
},
"FingerA": {
"parent": "Palm",
"size": [80, 60, 20],
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Slider",
"type": "linear",
"axis": [1, 0, 0],
"origin": [4, -35,0],
"rotation": [0, 0, 0],
"variable": "e"
},
"skeleton": {
"from": [0, 0,0],
"to": [0, -60, 0],
"radius": 4,
"color": [0.20, 0.80, 0.20]
},
"markers":[
{"id":40,"position":[12,-24,-17.1],"normal":[-10.98,0,-23.56]},
{"id":41,"position":[1.5,-2.2,25.8],"normal":[0,-25.6,9.5]},
{"id":42,"position":[13.9,-40,0],"normal":[1,-0.35,0.40], "spin": 27}
],
"model": [
{
"stlFile": "surfaces/Finger.stl",
"originOfModel": [24,0,-9.1],
"rotationOfModelDegree": [90, -90,0],
"material": "defaultPlastic"
}
]
},
"FingerB": {
"parent": "Palm",
"size": [80, 60, 20],
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Slider",
"type": "linear",
"axis": [-1, 0, 0],
"origin": [-4, -35,0],
"rotation": [0, 0, 0],
"variable": "e"
},
"skeleton": {
"from": [0, 0,0],
"to": [0, -60, 0],
"radius": 4,
"color": [0.20, 0.80, 0.20]
},
"markers":[
{"id":43,"position":[-12,-24,17.1],"normal":[10.98,0,23.56], "spin":90 },
{"id":44,"position":[-1.5,-2.2,-25.8],"normal":[0,-25.6,-9.5], "spin":90},
{"id":45,"position":[-13.9,-40,0],"normal":[-1,-0.35,-0.40], "spin": -27}
],
"model": [
{
"stlFile": "surfaces/Finger.stl",
"originOfModel": [-24,0,9.1],
"rotationOfModelDegree": [90, 90,0],
"material": "defaultPlastic"
}
]
}
}
}

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Before

Width:  |  Height:  |  Size: 2.9 MiB

View File

@@ -1,127 +0,0 @@
# Bestimmung der Y-Rotationsachse aus Marker-Positionen
## Problem
Wenn der Roboter um seine Y-Achse rotiert, bewegt sich jeder erkannte Marker auf einem **Kreisbogen** im 3D-Raum. Ziel ist es, Lage und Richtung dieser Rotationsachse aus den beobachteten Marker-Positionen (x, y, z) zu mehreren Zeitstempeln zu berechnen.
---
## Reichen zwei Positionen?
**Nein.** Zwei Punkte P₁, P₂ desselben Markers definieren nur eine Strecke. Die Rotationsachse muss durch die senkrechte Mittelebene dieser Strecke laufen aber *wo* genau in dieser Ebene und in welcher Richtung bleibt unbestimmt. Das System ist unterbestimmt.
| Beobachtungen | Bestimmbar? | Begründung |
|---|---|---|
| 1 Marker, 2 Positionen | Nein | Achse liegt irgendwo auf einer Halbebene |
| 1 Marker, 3 Positionen | **Ja** | Eindeutiger Umkreis → eindeutige Achse |
| N Marker, je ≥ 3 Positionen | Ja, überbestimmt | Least-Squares, robuster gegenüber Messrauschen |
---
## Mathematik: 1 Marker, 3 Positionen
Gegeben: P₁, P₂, P₃ ∈ ℝ³ (derselbe Marker zu drei verschiedenen Rotationswinkeln).
Da die Rotation starr ist, liegen P₁, P₂, P₃ auf einem **Kreis**, dessen Ebene senkrecht zur Rotationsachse steht.
### Schritt 1 Achsenrichtung (Normalenvektor der Kreisebene)
```
v₁ = P₂ P₁
v₂ = P₃ P₁
n = (v₁ × v₂) / |v₁ × v₂| ← Einheitsvektor entlang der Rotationsachse
```
### Schritt 2 Mittelpunkt des Kreises (Umkreismittelpunkt)
Der Umkreismittelpunkt C liegt in der Ebene der drei Punkte und ist von allen drei Punkten gleich weit entfernt. Er ist ein Punkt **auf der Rotationsachse**.
Berechnung über Baryzentrischen Koordinaten:
```
a² = |P₂ P₃|²
b² = |P₁ P₃|²
c² = |P₁ P₂|²
w₁ = a²·(b² + c² a²)
w₂ = b²·(a² + c² b²)
w₃ = c²·(a² + b² c²)
C = (w₁·P₁ + w₂·P₂ + w₃·P₃) / (w₁ + w₂ + w₃)
```
### Schritt 3 Rotationsachse
```
Achse: r(t) = C + t·n, t ∈
```
---
## Mathematik: N Marker, je ≥ 3 Positionen (Least Squares)
Jeder Marker i liefert:
- einen Schätzwert **Cᵢ** (Umkreismittelpunkt, Punkt auf der Achse)
- einen Schätzwert **nᵢ** (Normalenvektor, Achsenrichtung)
### Achsenrichtung (gemittelt / PCA)
Wegen möglichem Vorzeichenambiguität alle nᵢ auf dasselbe Halbraum ausrichten, dann:
```
n̄ = mean(nᵢ) / |mean(nᵢ)|
```
Robuster: **PCA** über die Matrix der nᵢ-Vektoren → erster Hauptkomponentenvektor.
### Achsenposition (Least Squares)
Minimiere die quadratischen Abstände aller Umkreismittelpunkte Cᵢ zur gesuchten Geraden `r(t) = A + t·n̄`:
```
Abstand² von Cᵢ zur Achse = |Cᵢ A|² ((Cᵢ A)·n̄)²
```
Ableitung null setzen liefert:
```
A = C̄ (C̄·n̄)·n̄ + (C̄·n̄)·n̄ = C̄ (Referenzpunkt = Schwerpunkt der Cᵢ)
```
d. h., der **Schwerpunkt der Umkreismittelpunkte** ist der optimale Referenzpunkt auf der Achse. Die Achsenrichtung n̄ bleibt unverändert.
**Verbleibender Fehler** (Residuum pro Marker):
```
εᵢ = |(Cᵢ C̄) ((Cᵢ C̄)·n̄)·n̄|
```
Große εᵢ deuten auf einen fehlerhaften Marker oder eine nicht-rotatorische Bewegungskomponente hin.
---
## Praktische Empfehlung
- **Mindestens 3 Zeitstempel** pro Marker, verteilt über einen Winkelbereich von ≥ 30° (sonst ist der Kreis numerisch schlecht konditioniert).
- **34 Marker** mit je 3 Positionen sind ausreichend für eine stabile Schätzung.
- Die Rotationswinkel müssen nicht bekannt sein nur die 3D-Koordinaten der Marker.
- Bei sehr kleinen Winkeln (< 10°) ist die Bestimmung numerisch instabil; größere Drehungen bevorzugen.
---
## Zusammenfassung
```
Eingabe: Marker-Positionen P[marker][timestamp] ∈ ℝ³
(mind. 3 Timestamps pro Marker, mind. 1 Marker)
Pro Marker:
1. n_i = normalize((P2-P1) × (P3-P1)) ← Achsenrichtung
2. C_i = Umkreismittelpunkt(P1, P2, P3) ← Punkt auf Achse
Kombination:
3. n̄ = normalize(mean(n_i)) ← beste Achsenrichtung
4. C̄ = mean(C_i) ← bester Referenzpunkt
Ergebnis: Rotationsachse r(t) = C̄ + t·n̄
```

View File

@@ -1,123 +0,0 @@
# Calibration Roadmap appRobotHoming
> Stand: 2026-06-10
> Ziel: Vor dem Homing muss das System einmalig (oder nach mechanischen Änderungen) kalibriert werden.
---
## Übersicht der Kalibrierungsschritte
```
[1] Camera NPZ → [2] Board → [3] Robot X-Axis → [4] Arm1 / Arm2
```
Jede Stufe baut auf der vorherigen auf. Die Ergebnisse werden als Dateien gespeichert und vom Homing-Prozess geladen.
---
## [1] Camera NPZ Kamerakalibrierung
**Ziel:** Intrinsische Kameraparameter (Brennweite, Verzerrungskoeffizienten, Kameramatrix) für jede Kamera ermitteln und als `.npz`-Datei speichern.
**Vorgehen (noch offen):**
- Schachbrettmuster / ChArUco-Board aus verschiedenen Winkeln fotografieren
- OpenCV `calibrateCamera()` ausführen
- Ergebnis speichern: `cam0_calib.npz`, `cam1_calib.npz`, `cam2_calib.npz`
**Aktionen auf der Seite (geplant):**
- Fotos aufnehmen (mehrere Posen)
- Kalibrierung berechnen
- `.npz`-Datei herunterladen / auf Server speichern
- Reprojektionsfehler anzeigen
**Offene Fragen:**
- Welches Muster wird verwendet (Schachbrett vs. ChArUco)?
- Automatische Erfassung oder manueller Upload der Bilder?
---
## [2] Board Referenz-Board-Kalibrierung
**Ziel:** Die extrinsische Position des Marker-Boards im Kamera-Koordinatensystem bestimmen.
**Vorgehen (noch offen):**
- Board in definierter Position aufstellen
- Foto aufnehmen, Marker erkennen (`aruco.detectMarkers`)
- `solvePnP` → Rotations- und Translationsvektor berechnen
- Transformation Board → Kamera speichern
**Aktionen auf der Seite (geplant):**
- Kamerabild anzeigen mit erkannten Markern
- Pose berechnen und anzeigen
- Kalibrierungsdatei speichern
**Offene Fragen:**
- Feste Board-Position oder Referenzpunkte einmessen?
---
## [3] Robot X-Axis Achsenrichtung kalibrieren
**Ziel:** Die X-Achse des Roboters im Weltkoordinatensystem verorten (Ausrichtung und Nullpunkt).
**Vorgehen (noch offen):**
- Roboter an bekannte X-Positionen fahren (z. B. X=0 und X=max)
- Kamera beobachtet den Endeffektor / Marker am Roboter
- Achsvektor aus zwei Messpunkten berechnen
- Ergebnis speichern
**Aktionen auf der Seite (geplant):**
- Roboter zu Referenzposition 1 fahren → Foto → Marker-Position merken
- Roboter zu Referenzposition 2 fahren → Foto → Marker-Position merken
- Achsvektor berechnen und anzeigen
- Speichern
**Offene Fragen:**
- Wie viele Referenzpunkte werden benötigt?
- Wird die Z-Achse separat kalibriert?
---
## [4] Arm1 / Arm2 Gelenk-Kalibrierung
**Ziel:** Nullposition und Kinematikparameter von Arm1 und Arm2 einmessen.
**Vorgehen (noch offen):**
- Arm in mechanische Nullposition fahren (physischer Anschlag oder Markierung)
- Kamera prüft die tatsächliche Arm-Pose
- Offset zwischen Soll und Ist berechnen und speichern
**Aktionen auf der Seite (geplant):**
- Arm1 auf Nullposition → Foto → Winkel ablesen
- Arm2 auf Nullposition → Foto → Winkel ablesen
- Offset-Korrektur berechnen und speichern
**Offene Fragen:**
- Separater Marker pro Armgelenk?
- Kalibrierung bei jedem Start oder nur nach Umbau?
---
## Dateistruktur (geplant)
```
calibration/
cam0_calib.npz
cam1_calib.npz
cam2_calib.npz
board_pose.json
robot_xaxis.json
arm_offsets.json
```
---
## Status
| Schritt | Status | Anmerkung |
|------------------|--------------|----------------------------------|
| Camera NPZ | offen | Konzept unklar |
| Board | offen | Konzept unklar |
| Robot X-Axis | offen | Konzept unklar |
| Arm1 / Arm2 | offen | Konzept unklar |
| Calibration-UI | in Arbeit | HTML-Seite angelegt |

164
doc/Homing.md Normal file
View File

@@ -0,0 +1,164 @@
# Homing appRobotHoming
> Stand: 2026-06-15
> Homing läuft bei **jedem Einschalten** — schnell, vollautomatisch, ohne mechanische Endschalter.
---
## Was ist Homing?
Der Roboter weiss beim Einschalten nicht, wo er steht.
Die Kameras schauen auf die ArUco-Marker am Roboter und berechnen daraus
die vollständige Pose aller Gelenke.
**Homing** (dieser Prozess): bei jedem Einschalten, automatisch.
**Kalibrierung** (`doc/Kalibrierung.md`): nur nach mechanischen Änderungen.
Dieses Dokument ist der **allgemeine Ablauf**. Technische Detail-Doku je Teilstrecke:
| Doku | Inhalt |
|---|---|
| [`Homing_0_Camera.md`](Homing_0_Camera.md) | Foto → Kamera-Pose → trianguliertes `aruco_marker_poses.json` (Schritte 13b) |
| [`Homing_1_StepByStep.md`](Homing_1_StepByStep.md) | `aruco_marker_poses.json` → Gelenkwinkel je Link (4b-Kette, Schätz-Methoden/Fallbacks) |
| `Homing_2_improvement.md` *(geplant)* | Bekannte Probleme, Genauigkeits-Befunde, Verbesserungsideen |
| [`Homing_5_Pose.md`](Homing_5_Pose.md) | Verfeinerungsschritt NACH der 4b-Kette (Bundle-Adjustment, `5_pose_estimation.py`) — noch nicht verdrahtet |
---
## Kinematik-Kette
```
Board (ROOT, fest) ← Referenz aller Kameras
├── Base linear x axis=[1,0,0] ← Slider-Position
├── Arm1 revolute y axis=[-1,0,0] ← Schultergelenk
├── Ellbow revolute z axis=[-1,0,0] ← Ellbogen
├── Arm2 revolute a axis=[0,-1,0] ← Unterarm-Drehung
├── Hand revolute b axis=[1,0,0] ← Handgelenk
├── Palm revolute c axis=[0,-1,0] ← Handfläche
└── FingerA/B linear e axis=±[1,0,0] ← Greifer (symmetrisch)
```
**Ergebnis-State:** `{ x_mm, y_deg, z_deg, a_deg, b_deg, c_deg, e_mm }`
---
## Voraussetzungen
Homing setzt eine abgeschlossene Kalibrierung voraus:
| Was | Status |
|-----|--------|
| Kamera-Intrinsik (NPZ) | ✅ |
| Board-Marker-Positionen | ✅ |
| X-Achsen-Richtung | ✅ |
| Arm1 Joint-Origin Y/Z | ✅ Button vorhanden und ausführbar |
| Arm-Marker in robot.json | 🔶 Position manuell eintragen; Spin-Verifikation via Kalibrierung → Tab „Marker" (→ `doc/Kalibrierung_Marker.md`) |
---
## Ablauf
```
Foto alle Kameras
1_detect_aruco_observations.py (pro Kamera, mit NPZ)
2_estimate_camera_from_observations.py (pro Kamera)
3b_corner_marker_poses.py (einmal, benötigt ≥2 Kamera-Posen)
│ → aruco_marker_poses.json
X-Position aus Marker-Positionen schätzen
│ → x_mm (pro Arm-Marker: beobachtetes_x Modell_x(slider=0), gemittelt)
4b_revolute_angle.py --link Arm1 --x-mm {x_mm}
│ → state_Arm1.json (accumulated_state)
4b_revolute_angle.py --link Ellbow --from-state state_Arm1.json
│ → state_Ellbow.json
4b_revolute_angle.py --link Arm2 --from-state state_Ellbow.json
│ → state_Arm2.json
4b_revolute_angle.py --link Hand --from-state state_Arm2.json
│ → state_Hand.json ← accumulated_state: x,y,z,a,b (4b-Primärkette)
│ Fallback 5_pose_estimation.py liefert alle 7 (…,c,e). Nur bekannte
│ Achsen gehen ins G92; wirklich fehlende werden weggelassen.
G92 über Driver-WebSocket (DRIVER_WS_URL) — setzt Motorposition ohne Bewegung
```
**Schritte 13b** sind dieselbe Board-Pipeline wie in der Kalibrierung.
Sie sind in `runBoardPipeline()` (`server/server.js`) als gemeinsame Funktion ausgelagert.
**4b-Schleife**: sequenziell von root nach tip; jedes Script bekommt den Zustand des
vorherigen Schritts über `--from-state`. Der erste Aufruf erhält die geschätzte
X-Slider-Position über `--x-mm`.
---
## Implementierung
| Komponente | Datei | Beschreibung |
|-----------|-------|--------------|
| 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()` | 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 |
| Backend-Route | `POST /api/homing/run` | SSE-Stream, startet `runHoming()` |
| State senden | `POST /api/homing/send-state` | Baut `G92` (fehlende c/e → 0) und sendet es als Plain-Text-G-Code über den Driver-WebSocket (`DRIVER_WS_URL`, `server/driverClient.js`). Der Driver verarbeitet G92 intern als M92 = Motorposition setzen ohne Bewegung. Kein HTTP `/api/state` (gibt es am Driver nicht) |
| 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; schreibt Teil-Pose als `G92`-GCode ins Eingabefeld |
| Board-Viewer (Homing) | `public/boardViewer.html?mode=homing` | Skelett + Arm-Marker per FK (Three.js): Marker-Quadrat spin-korrekt rotiert + Orientierungszeiger zu Ecke 0 (Modell-Seite); gemessene Marker als Kugeln + Fehlerlinien; progressiver Update je erkanntem Gelenk |
**Lauf-Verzeichnisse:** `data/homing/{timestamp}/`
---
## SSE-Event-Typen
Das Backend streamt während des Homing-Laufs folgende Events:
| `type` | Felder | Bedeutung |
|--------|--------|-----------|
| `log` | `text` | Zeile aus Script-Ausgabe |
| `step` | `step`, `total`, `text` | Fortschritt (16) |
| `analysis` | `key`, `value` | Zwischenergebnis (x_mm, state_Arm1, …) |
| `error` | `text` | Fehler (Script-Exit ≠ 0 o.ä.) |
| `done` | `exitCode`, `state?`, `runDir` | Abschluss; `state` nur bei Erfolg |
---
## robot.json — Ladestrategie
**Aktuell:** Lokale Datei
```javascript
ROBOT_JSON = process.env.ROBOT_JSON || 'scripts/robot_1781069752019.json'
```
**Geplant** (wenn Driver `GET ROBOT_URL/api/robot/config` implementiert):
```javascript
async function loadRobotConfig() {
if (ROBOT_URL) {
const res = await fetch(new URL('/api/robot/config', ROBOT_URL));
return res.json();
}
return JSON.parse(await fs.readFile(ROBOT_JSON, 'utf8'));
}
```
Auswirkung: nur `ROBOT_JSON`-Variable ändern — alle Scripts bekommen automatisch
die aktuelle Konfiguration.
---
## Offene Punkte
- [~] **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 — Arm1 (x, y) und Ellbow (z) laufen durch; Arm2/Hand noch zu prüfen. Ellbow brach zunächst ab (4b: nur 2 Marker sichtbar, deren Verbindungsvektor parallel zur Gelenkachse liegt → keine Baseline), seit 2026-06-16 über Fallback-1/-2 in `4b_revolute_angle.py` gelöst — Details: [`Homing_1_StepByStep.md`](Homing_1_StepByStep.md)
- [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
- [x] **robot.json via Driver-API** (2026-06-17): `server/robotConfig.js``fetchRobot()`/`pushRobot()`/`robotCachePath`; automatischer Fallback auf lokale Datei wenn `ROBOT_URL` nicht gesetzt

34
doc/Homing_0_Camera.md Normal file
View File

@@ -0,0 +1,34 @@
# Homing 0 Kamera & Triangulation
> Technische Detail-Doku zu [`Homing.md`](Homing.md) — dieser Teil: Foto → trianguliertes
> `aruco_marker_poses.json`.
> Status: Gerüst, wird sukzessive ausgebaut.
---
## Scope
Schritte 13b der Homing-Pipeline (siehe `Homing.md` → Ablauf), a.k.a. „Board-Pipeline":
| Schritt | Script | Pro | Ergebnis |
|---|---|---|---|
| 1 | `1_detect_aruco_observations.py` | Kamera | 2D-Marker-Ecken (mit NPZ-Intrinsik entzerrt) |
| 2 | `2_estimate_camera_from_observations.py` | Kamera | Kamera-Pose relativ zum Board (`cam*_camera_pose.json`) |
| 3b | `3b_corner_marker_poses.py` | einmal, ≥2 Kamera-Posen | `aruco_marker_poses.json` (3D-Weltpositionen aller Marker) |
Ausgelagert in `runBoardPipeline()` (`server/server.js`) — gemeinsam genutzt von
Kalibrierung und Homing.
## Offene Punkte / noch zu dokumentieren
- [ ] Kamera-Intrinsik-Format (NPZ-Inhalt, Verzeichnis)
- [ ] Wie `3b` mehrere Kamera-Beobachtungen eines Markers kombiniert (Triangulation/Mittelung, `num_cameras`-Feld)
- [ ] Fehlerquellen: Verdeckung, Belichtung, `num_cameras < 2` → Marker fehlt komplett
- [ ] Format von `aruco_marker_poses.json` (Felder `position_mm`, `corners_m`, `normal`, `edge_length_mm`)
- [ ] Zusammenhang mit `Kalibrierung.md` / `Kalibrierung_Marker.md`
## Verweise
- Allgemeiner Ablauf: [`Homing.md`](Homing.md)
- Nächster Schritt (Gelenkwinkel aus den triangulierten Positionen): [`Homing_1_StepByStep.md`](Homing_1_StepByStep.md)
- Kalibrierung: `Kalibrierung.md`, `Kalibrierung_Marker.md`

View File

@@ -0,0 +1,84 @@
# Homing 1 Schritt für Schritt: Gelenkwinkel-Schätzung (4b)
> Technische Detail-Doku zu [`Homing.md`](Homing.md) — dieser Teil: `aruco_marker_poses.json`
> → Gelenkwinkel je Link.
> Status: Gerüst, wird sukzessive ausgebaut.
---
## Scope
Die 4b-Kette aus `Homing.md` → Ablauf: `scripts/4b_revolute_angle.py`, sequenziell
Arm1 → Ellbow → Arm2 → Hand. Jeder Aufruf bekommt den Zustand des vorherigen Schritts
über `--from-state` (`accumulated_state`).
## Schätz-Methoden (Prioritätsreihenfolge je Gelenk)
Pro Gelenk wird in dieser Reihenfolge versucht, eine Schätzung zu finden. Jede
nächste Stufe ist ein **reiner Fallback** — sie greift nur, wenn die vorherige Stufe
**keine einzige** brauchbare Baseline liefert (z. B. Marker nicht sichtbar, oder
Marker-Paar zufällig parallel zur Drehachse):
1. **Primär** (`TIER_PRIMARY`) — zwei Marker auf dem Ziel-Link selbst.
`v_model`/`v_obs`-Differenzvektor, ⟂ zur Gelenkachse projiziert, Winkel zwischen
beiden gemessen. Braucht nur die Achs*richtung* (aus FK der schon gelösten
Vorgänger), nicht die Pivot-*Position*.
2. **Fallback-1** (`TIER_FALLBACK_1`, implementiert 2026-06-16) — zwei Marker auf
dem **direkten Kind-Link**, deren Verbindungsvektor (im Kind-Lokalframe) parallel
zur **eigenen** Achse des Kind-Links liegt (Toleranz `--child-axis-tol`, default
1mm) → invariant gegen dessen noch unbekannten Winkel, daher als Stellvertreter
für „zwei Marker am Ziel-Link" nutzbar. Beispiel: Ellbow (Achse X) ← Arm2-Marker
144↔148 bzw. 143↔146 (Arm2-Achse Y, ⟂ zu X, beide Paare exakt achsparallel in
Arm2s Lokalframe). Wie Primär unabhängig von der Pivot-Position — eine separate
„ist die nächste Achse senkrecht"-Prüfung war nicht nötig, das ergibt sich
automatisch aus der bestehenden Mindest-Baseline-Prüfung nach der Projektion.
3. **Fallback-2** (`TIER_FALLBACK_2`, implementiert) — ein einzelner Marker auf dem
Ziel-Link gegen den Gelenk-**Pivot** selbst (Pivot + Achse aus FK der Vorgänger).
Einzige Stufe, die mit nur 1 sichtbaren Marker funktioniert — aber zusätzlich
abhängig von der Pivot-*Position* (also den geschätzten Vorgänger-*Werten*, nicht
nur deren Achsrichtung). Letzter Rückfall, nur falls Fallback-1 auch nichts findet
(kein Kind-Link, oder dessen Marker nicht sichtbar/nicht achsparallel).
→ Code: `scripts/4b_revolute_angle.py` (`estimate_revolute_angle()`); Konstanten
`TIER_PRIMARY`/`TIER_FALLBACK_1`/`TIER_FALLBACK_2`/`PIVOT_FALLBACK_ID`; Felder
`"method"` (top-level) und `"tier"`/`"link"` (je `per_pair`-Eintrag) im Ergebnis-JSON.
### Befund 2026-06-16 (Anlass für Fallback-1)
Im Testlauf `test/homing/20260616_120456` waren am Ellbow nur Marker 129/132 sichtbar,
deren Verbindungsvektor exakt parallel zur Ellbow-Achse liegt → Primär-Methode liefert
nichts. **Vor** Fallback-1 sprang Fallback-2 (Pivot) ein und meldete `z ≈ -4.33°`
(intern konsistent, Exit 0) — eine unabhängige Gegenrechnung (Least-Squares über alle
Ellbow- *und* Arm2-Marker, `z`+`a` frei) zeigte aber ihr Minimum bei `z ≈ -38°`, also
ca. 3540° daneben. **Mit** Fallback-1 (Arm2-Marker 144↔148/143↔146) liefert derselbe
Lauf jetzt `z ≈ -44.15°` (circular_σ 0.80°) — deutlich näher am Least-Squares-Minimum.
Bestätigt auch downstream: die anschließende Arm2-Schätzung (Primär, eigene Marker)
hatte mit der alten Fallback-2-Kette einen Ausreißer und `circular_σ 45.8°`; mit der
Fallback-1-Kette sind alle vier Arm2-Paare konsistent, `circular_σ 5.1°`.
→ Fallback-1 war hier kein „nice to have", sondern ca. 35° Genauigkeitsgewinn.
Weitere/künftige Befunde: siehe `Homing_2_improvement.md` (geplant).
## robot.json-Struktur (Kurzreferenz)
- `links.<Name>.parent` — Name des Eltern-Links (Kette/Baum)
- `links.<Name>.jointToParent``{type, axis, origin, rotation, variable}`
- `links.<Name>.markers[]``{id, position, normal, size}`; `position` ist relativ
zum **Pivot** des eigenen Links, im lokalen, noch nicht eigen-rotierten Frame.
- FK-Engine: `scripts/robot_fk.py` (`RobotFK.compute()`, `.joint_origin_world()`,
`.joint_axis_world()`, `.marker_world()`).
## Offene Punkte / noch zu dokumentieren
- [ ] State-JSON-Schema im Detail (`accumulated_state`, `per_pair`, `method`, `circular_std_deg`)
- [ ] `--min-baseline` / `--child-axis-tol` Tuning / Auswirkung
- [x] Fallback-1 Implementierung (2026-06-16, `scripts/4b_revolute_angle.py`)
- [ ] Mehrstufige Rekursion (Enkel-Links) für Fallback-1 — aktuell bewusst nur
direkter Kind-Link, siehe Code-Kommentar bei `_child_links()`
- [ ] y-Restfehler (~2°) aus `Homing.md` → Offene Punkte
## Verweise
- Allgemeiner Ablauf: [`Homing.md`](Homing.md)
- Vorheriger Schritt (Kamera/Triangulation): [`Homing_0_Camera.md`](Homing_0_Camera.md)
- Nächster Schritt (Verfeinerung nach 4b): [`Homing_5_Pose.md`](Homing_5_Pose.md)
- Bekannte Probleme / Ideen: `Homing_2_improvement.md` (geplant)

439
doc/Homing_5_Pose.md Normal file
View File

@@ -0,0 +1,439 @@
# Homing 5 Pose-Schätzung per Bundle-Adjustment (`hybrid`)
> Technische Detail-Doku zu [`Homing.md`](Homing.md) — **Verfeinerungsschritt NACH
> der 4b-Kette** ([`Homing_1_StepByStep.md`](Homing_1_StepByStep.md)), **nicht**
> deren Ersatz: `5_pose_estimation.py` braucht den `accumulated_state` von 4b als
> Startwert. Ohne guten Startwert läuft die interne Optimierung mangels eigener
> verlässlicher Initialisierung leicht in ein lokales Minimum (siehe „Wichtige
> Einschränkung" unten).
> Status (2026-06-16): **Code-Hooks umgesetzt und gegen drei echte Homing-Captures
> verifiziert** — `--from-state` (Startwert aus 4b, mit Multi-Start-Schutz für alles,
> was der Startwert nicht abdeckt), `null` statt `0` für unbeobachtbare Gelenke
> (z. B. `Hand`/`Palm`/Finger, aktuell ohne Marker in `robot_1781069752019.json`),
> `scipy` in `docker-compose.yaml` ergänzt, sowie der **eine Schalter**
> `pose_estimation.fit_origin_link` (siehe eigener Abschnitt unten), der einen
> Gelenk-Drehpunkt automatisch mitbestimmt und in `robot.json` übernimmt. **Noch
> offen:** Anbindung in `homingOrchestrator.js`/`server.js`/Frontend (siehe
> Integrationsschritte — Umfang/Fehlerfall/Robotersteuerung-Politik dafür sind
> noch nicht festgelegt).
---
## Herkunft
`scripts/5_pose_estimation.py` ist 1:1 (byte-identisch, per Diff verifiziert)
aus dem Schwesterprojekt **`appRobotRendering`** übernommen
(`pipeline/pose_estimation.py`, dort Stufe 4 der allgemeinen Pose-Pipeline).
Mitgewandert und ebenfalls identisch: `scripts/robot_fk.py`. Dort ist das
Verfahren an zehn simulierten Szenen mit bekannter Grundwahrheit validiert
(`doc/pipeline.tex` im Rendering-Projekt) — diese Zahlen unten sind **Simulationsergebnisse
aus appRobotRendering**, keine Messung an appRobotHoming/echter Hardware.
`scripts/robot_1781069752019.json` enthält bereits den passenden
`pose_estimation`-Konfigurationsblock (identisch zu den Rendering-Defaults:
`method: hybrid`, `normal_weight: 100`, `huber_delta_mm: 8.0`, …) — die Datenseite
ist also schon vorbereitet, nur die Prozess-Verdrahtung fehlt noch.
---
## Einordnung in den Homing-Ablauf
```
1_detect_aruco_observations.py ┐
2_estimate_camera_from_observations.py │ = "Board-Pipeline" (Homing_0_Camera.md)
3b_corner_marker_poses.py ┘
▼ aruco_marker_poses.json
4b_revolute_angle.py × N (sequenziell root→tip, über --from-state verkettet)
│ Homing_1_StepByStep.md — liefert pro Gelenk eine Schätzung
│ (Primär/Fallback-1/Fallback-2), abhängig von Sichtbarkeit
accumulated_state {x,y,z,a,b,c,e} ← Startwert, NICHT optional überspringbar
5_pose_estimation.py (method=global_ba, accumulated_state als Startwert x0)
│ dieses Dokument — EIN gemeinsamer Bündelausgleich über alle 7
│ Variablen gleichzeitig, verfeinert/korrigiert den 4b-Zustand global
robot_state.json { movements: {…}, residual_rms, … }
```
**Wichtig:** `5_pose_estimation.py` ist **kein** Ersatz für die 4b-Kette, sondern
ein **Verfeinerungsschritt danach**, der deren `accumulated_state` als Startwert
braucht. Lässt man die 4b-Kette weg, fehlt dieser Startwert — die interne
Optimierung initialisiert dann faktisch bei `0` für jede Variable, und bei einer
beim Einschalten unbekannten Roboterpose ist das ein guter Weg in ein lokales
Minimum (Mechanismus s. „Wichtige Einschränkung" unten).
| Stufe | Eingabe | Aufruf | Ausgabe |
|---|---|---|---|
| **4b-Kette** (liefert den Startwert) | `aruco_marker_poses.json` + extern geschätztes `x_mm` | N Prozesse, je Link einer, `--from-state` verkettet | `accumulated_state` (flach: `x,y,z,a,b,c,e`) |
| **`5_pose_estimation.py`** (verfeinert global) | `aruco_marker_poses.json` **+ `accumulated_state` aus 4b als Startwert** | 1 Prozess | `robot_state.json``movements.<var>.{value,unit,observable,confidence,n_markers}` |
---
## Wie es funktioniert (kurz)
Das Skript parametrisiert über **Gelenkvariablen** (nicht Links) und liest pro
Marker Position **und gemessene Normale** aus `aruco_marker_poses.json`
(3b-Ausgabe). Vier austauschbare Verfahren (`robot.json``pose_estimation.method`),
`hybrid` ist Standard und kombiniert die letzten beiden:
1. `sequential_vector` — analytische Winkel aus Marker-Paar-Vektoren (schnell, braucht ≥2 Marker/Gelenk)
2. `sequential_fk` — blockweiser nichtlinearer Fit entlang der Kette, vorherige Variablen eingefroren, Multi-Start `{0,60,…,300}°` gegen lokale Minima
3. `global_ba`**einziges** Bündelausgleichsproblem über **alle 7 Variablen gleichzeitig** (`scipy.optimize.least_squares`, Huber-Loss)
4. **`hybrid`** = 2 als Startwert → 3 als Verfeinerung
Die Blockbildung in `analyze_chain()` ist generisch aus der FK-Topologie
abgeleitet (keine festen Link-Namen) — passt damit zur Projekt-Konvention
„Scripts müssen Szenen/Ketten automatisch erkennen, nichts hartkodieren".
Für *dieses* Robot-Modell ergibt sich u. a. der Block `{x, y}`: `Base` (Variable
`x`) hat **keine eigenen Marker** (`"markers": []` in `robot_1781069752019.json`)
und wird automatisch mit `Arm1` (Variable `y`, 5 Marker) zu einem gemeinsamen
Least-Squares-Fit zusammengefasst.
Jedes Ergebnis kommt mit einer Konfidenz pro Variable (`high/medium/low/none`,
abgeleitet aus sichtbaren Markern je Block) — analog zur 4b-Kette, aber pro
Block statt pro Einzel-Fallback-Stufe.
### Wichtige Einschränkung: Startwert und lokale Minima
**Ursprünglicher Befund (vor dem Code-Hook vom 2026-06-16):** `estimate_pose()`
rief für `global_ba`/`hybrid` immer selbst `estimate_sequential_fk()` als
„billigen, robusten Init" auf — es gab keinen Parameter, um stattdessen einen
extern vorgegebenen Startwert einzuspeisen, obwohl `estimate_global_ba()`
selbst intern bereits ein `x0`-Dict entgegennahm (`:272-273`). **Das ist jetzt
behoben** (`seed`-Parameter auf `estimate_pose()`/`estimate_sequential_fk()`,
CLI `--from-state`) — die folgende Beschreibung des Multi-Start-Mechanismus
gilt weiterhin unverändert für **alles, was der Seed nicht abdeckt** (bzw. für
den reinen Kaltstart ohne `--from-state`):
`estimate_sequential_fk()` initialisiert jede nicht geseedete Variable bei
`0.0` und rastert den Multi-Start `{0,60,120,180,240,300}°` **nur über die
erste Variable eines Blocks** (`bvars[0]`) — und auch das **nur, wenn diese
selbst `revolute` ist** (`:348-356`). Für dieses Robotermodell heißt das konkret:
- Block `{x, y}` (Base markerlos → mit Arm1 zusammengefasst): `bvars[0]` ist
`x` (linear) → `lead_type != "revolute"`**kein** Multi-Start. `y`
(Schultergelenk, Arm1) wird in einem einzigen Lauf ab `0°` gefittet.
- Block `{b, c, e}` (Hand/Palm markerlos → mit den Fingermarkern zusammengefasst):
nur `b` bekommt den 6-Punkte-Raster; `c` und `e` starten in **jedem** der
6 Läufe fix bei `0`.
- Einzelvariablen-Blöcke wie Ellbow (`{z}`) oder Arm2 (`{a}`) bekommen den
vollen Raster auf sich selbst — dort ist das Risiko deutlich kleiner.
Liegt die echte Pose in `y`, `c` oder `e` weit von `0` entfernt (beim Homing
nach dem Einschalten der Normalfall, nicht die Ausnahme), kann schon die
`sequential_fk`-Vorstufe in einem falschen lokalen Minimum landen — die
anschließende `global_ba`-Verfeinerung poliert dieses falsche Minimum dann nur
noch, statt es zu verlassen. Das deckt sich mit dem in der Validierungstabelle
unten sichtbaren großen Abstand zwischen Mittelwert (0,253°) und Schlechtestfall
(1,568°) bei sonst niedriger Streuung (0,134°) — ein Muster, das zu „meist
gut, gelegentlich falsches Minimum" passt.
**Konsequenz / Status:** `5_pose_estimation.py` sollte in appRobotHoming **nicht
kalt** laufen, sondern mit dem `accumulated_state` der 4b-Kette als Startwert.
**Umgesetzt:** `--from-state <json>` lädt einen flachen oder
`{"accumulated_state": {...}}`-verpackten Zustand; `estimate_sequential_fk()`
überspringt nur Blöcke, die **vollständig** im Startwert enthalten sind, und
wendet auf alles andere weiterhin seinen normalen Multi-Start an — auch bei
einem **unvollständigen** Seed (z. B. nur `x,y` aus einem abgebrochenen
4b-Lauf) bleiben `z`/`a` also Multi-Start-geschützt, statt ungeschützt bei `0`
zu starten. Verifiziert an drei echten Captures (s. „Validierung an echten
appRobotHoming-Daten" unten).
### Validierung im Rendering-Projekt (Simulation, 10 Posen, bekannte GT)
| Verfahren | Winkel Ø [°] | Winkel schlechtest. [°] | Position Ø [mm] | Position schlechtest. [mm] |
|---|---|---|---|---|
| `sequential_vector` | 0,315 | 1,717 | 0,144 | 0,712 |
| `sequential_fk` | 0,434 | 1,838 | 0,158 | 0,851 |
| `global_ba` | 0,253 | 1,568 | 0,103 | 0,390 |
| **`hybrid`** | **0,253** | **1,568** | **0,103** | **0,390** |
(Quelle: `appRobotRendering/doc/pipeline.tex`, Abschnitt „Validierung und Ergebnisse".)
### Validierung an echten appRobotHoming-Daten (2026-06-16)
Drei echte Captures aus `test/homing/` (nicht simuliert; vom Nutzer bereitgestellt,
inkl. eines bereits aktualisierten 4b-Laufs):
| Fixture | 4b kam bis | Arm1-Marker gesehen | Ellbow-Marker gesehen |
|---|---|---|---|
| `20260616_120456` | Arm1 (Ellbow nicht gespeichert) | 197, 243 | 129, 132 |
| `20260616_133151` | Arm2 | 198, 229 | 129, 132 |
| `20260616_135403` | Arm2 | 197, 243 | 129, 132, 121 |
Geprüft für jede Fixture (`python scripts/5_pose_estimation.py … --from-state state_Arm2.json`,
bzw. `state_Arm1.json` für die unvollständige erste Fixture):
- **Kein Crash**, trotz `Hand`/`Palm`/`FingerA`/`FingerB` aktuell ganz ohne
Marker in `robot_1781069752019.json` (`"markers": []` an allen vieren) —
`b`, `c`, `e` kommen als `confidence:"none"`, `"value": null` heraus, exakt
wie gefordert ("Hand als unbekannt stehen lassen").
- **Kalt vs. geseedet liefert dieselben Werte** (z. B. `133151`: `x=193.96mm,
y=25.74°, z=-28.00°, a=-0.81°` in beiden Fällen) — der Seed verändert das
Ergebnis nicht, wenn der Kaltstart bereits im richtigen Minimum lag; er
schützt nur die Fälle, in denen das nicht so ist.
- **Unvollständiger Seed** (`120456`, nur `x,y` aus `state_Arm1.json`, `z`/`a`
fehlen): liefert dieselben Werte wie der volle Kaltstart — und durchläuft
jetzt nachweislich den Multi-Start-Pfad für `z`/`a` (Code-Pfad geprüft, nicht
nur Zufall des Ergebnisses).
- Residuum über alle Marker: **4,34,5 mm RMS** (deutlich über der
Simulationsvalidierung oben — erwartbar, reale Marker/Kameras sind
verrauschter als der appRobotRendering-Renderfehler-Boden; noch keine
`huber_delta_mm`/`normal_weight`-Nachjustierung vorgenommen).
---
## Vorteile
- **Bestes/stabilstes Verfahren im Rendering-Benchmark** (s. Tabelle oben) — unter
allen vier Methoden der niedrigste Mittel- *und* Worst-Case-Fehler.
- **Überbrückt markerlose Gelenke automatisch.** `Hand` (Variable `b`) und `Palm`
(`c`) tragen keine eigenen Marker — `global_ba` zieht die Information aus den
Fingermarkern *rückwärts* durch die Kette. Die 4b-Kette braucht dafür explizit
einen Fallback pro Gelenk; hier passiert es als Nebenprodukt der gemeinsamen
Optimierung.
- **Fittet `x` und `y` gemeinsam aus denselben Arm1-Markern** (Block `{x,y}`,
weil `Base` markerlos ist) — konsistenter als zwei getrennte Schätzungen.
Ersetzt `estimateXFromMarkers()` aber **nicht**: dieser Block ist genau einer
der beiden, die ohne guten Startwert anfällig für ein lokales Minimum sind
(s. „Wichtige Einschränkung" unten) — die gemeinsame Schätzung ist also ein
Mehrwert *nach* einer 4b-Vorschätzung, kein Grund, diese zu überspringen.
- **Funktioniert mit nur 1 sichtbarem Marker pro Gelenk**, weil das Residuum
Position **und** Normale nutzt (Gl. in `residual_vector()`) — die 4b-Primärmethode
braucht dafür mindestens 2.
- **Ist die automatisierte Form der bereits manuell durchgeführten Gegenrechnung.**
Der Befund vom 2026-06-16 in `Homing_1_StepByStep.md` (Ellbow: Fallback-2 lag
3540° neben einer von Hand gerechneten Least-Squares-Kontrolle über Ellbow- *und*
Arm2-Marker) ist exakt das, was `global_ba`/`hybrid` automatisch und für **alle**
Gelenke gleichzeitig macht. Ein Lauf hätte den Fallback-2-Fehler vermutlich direkt
erkennbar gemacht.
- **Robuste Verlustfunktion (Huber)** dämpft einzelne Ausreißer-Marker (Fehldetektion,
Verdeckung) automatisch, statt dass ein einzelner schlechter Marker das ganze
Gelenk verfälscht.
- **Multi-Start über mehrere Startwinkel** hilft dort, wo er greift (Blöcke mit
genau einer Variable, z. B. Ellbow/`z`, Arm2/`a`) — für Homing wertvoller als
für Kalibrierung, weil beim Einschalten die Pose komplett unbekannt ist. Greift
aber **nicht** bei den gekoppelten Blöcken `{x,y}` und `{b,c,e}` (s. u.) — genau
dort ist ein externer Startwert aus 4b nötig.
## Nachteile
- **Kein verlässlicher eigener Kaltstart — Startwert von außen zwingend nötig.**
Wie im Abschnitt „Wichtige Einschränkung" hergeleitet: der interne Multi-Start
deckt nur Einzelvariablen-Blöcke ab, nicht die gekoppelten Blöcke `{x,y}` und
`{b,c,e}`. Allein aufgerufen (ohne `accumulated_state` aus 4b) ist
`5_pose_estimation.py` daher beim Homing real gefährdet, in einem lokalen
Minimum zu landen, statt die echte Pose zu finden — kein Rand-, sondern ein
Kernfall, weil die Pose beim Einschalten grundsätzlich unbekannt ist.
- ~~`scipy` fehlt im appRobotHoming-Container~~ — **behoben (2026-06-16):**
`docker-compose.yaml` installierte nur `opencv-python-headless numpy`; ohne
`scipy` greift `HAVE_SCIPY=False` und `estimate_sequential_fk`/`estimate_global_ba`
fallen lautlos auf den Nullzustand zurück (nur eine `[WARN]`-Zeile, kein
Fehler-Exit) — ein stiller Fehlmodus. `scipy` ist jetzt in der
`pip3 install`-Zeile ergänzt (kein separater Image-Build nötig — `pip3
install` läuft laut `command:` bei jedem Containerstart neu). **Noch nicht**
auf einem laufenden Container wirksam geprüft — wirkt erst nach dessen
nächstem Neustart.
- **Zwei nichtlineare Least-Squares-Läufe statt eines geschlossenen Ausdrucks** —
langsamer als `sequential_vector` und langsamer als ein einzelner
`4b_revolute_angle.py`-Aufruf. Für „schnell, vollautomatisch" (Anspruch aus
`Homing.md`) noch nicht auf echter Hardware gemessen.
- **Kein progressives Zwischenergebnis.** Die 4b-Kette liefert nach jedem Link
ein SSE-`analysis`-Event und aktualisiert den Board-Viewer live
(„progressiver Update je erkanntem Gelenk", `Homing.md` → Implementierung).
`estimate_pose()` gibt nur den fertigen Endzustand zurück — für dieselbe UX
müsste man zusätzlich die internen Zwischenstände von `estimate_sequential_fk()`
exponieren.
- **Verliert die dokumentierte Fallback-Diagnostik.** `Homing_1_StepByStep.md`
protokolliert pro Gelenk, *welche* Stufe gegriffen hat (`method`: primary /
fallback_1 / fallback_2). `5_pose_estimation.py` liefert nur eine
Block-Konfidenz (`high/medium/low/none`), nicht *welche* Heuristik intern
gewirkt hat — weniger Transparenz beim Debuggen einzelner Gelenke.
- **Ausgabeformat passt nicht direkt auf `/api/state`.** Der Endpunkt erwartet
ein flaches `{x,y,z,a,b,c,e}` (`accumulated_state`, siehe
`server/server.js` → `POST /api/homing/send-state`), `5_pose_estimation.py`
schreibt verschachtelt (`movements.<var>.value`). Eine kleine Adapterfunktion
ist nötig, kein Drop-in-Ersatz.
- ~~Unbeobachtbare Gelenke werden als `0.0` ausgegeben, nicht als `null`~~ —
**behoben (2026-06-16):** `main()`s Output-Writer schreibt jetzt `"value":
null`, wenn `observable:false`, statt der internen `0.0` (die intern bleibt,
weil die FK für die *anderen* Gelenke einen Zahlenwert braucht — nur der
*Output*-Vertrag ändert sich). Verifiziert an allen drei Fixtures (`Hand`,
`Palm`, `e` → `null`). Gilt nur für `5_pose_estimation.py` selbst — der
Adapter zu `/api/homing/send-state` (nächster Punkt) muss `null` weiterhin
korrekt durchreichen, nicht wieder in `0` zurückverwandeln.
- **Noch nicht an echten Kamerabildern/Markern validiert.** Die Zahlen oben sind
Simulation aus appRobotRendering (saubere FK-Marker-Positionen, definierter
Renderfehler-Rauschboden). Reale Marker-Ungenauigkeiten (s.
`Kalibrierung_Marker.md`) und reale Kameranoise könnten andere `huber_delta_mm`/
`normal_weight`-Werte als die übernommenen Defaults verlangen.
## Besonderheiten
- **Reiner, unveränderter Import-Stand** — momentan git-`??` (untracked), noch
nicht in `homingOrchestrator.js`/`server.js` referenziert (nur `4b_revolute_angle.py`
ist dort als `SCRIPT_4B` verdrahtet).
- **Schema-Kompatibilität zur lokalen `3b_corner_marker_poses.py` bereits
geprüft:** Feldnamen `marker_id`, `position_mm`/`position_m`, `normal`,
`num_cameras` stimmen 1:1 — `load_observations()` braucht keine Anpassung.
- **Namens-Kollision mit `5_camera_z_refine.py`** — zwei Skripte teilen sich das
Präfix `5_`. Entspricht der Konvention aus appRobotRendering, wo mehrere
Dateien sich ein Stufen-Präfix teilen (z. B. `3_*`, `4_*`); kein Bug, aber beim
Lesen der `scripts/`-Liste leicht zu verwechseln.
- **Die `pose_estimation.method`-Option erlaubt gezieltes A/B-Testen** ohne
Codeänderung: `--method sequential_vector|sequential_fk|global_ba|hybrid` per
CLI-Override, oder dauerhaft über `robot_1781069752019.json` →
`pose_estimation.method`. Nützlich, um den Effekt des Startwerts zu isolieren:
einmal kalt (zeigt das Problem aus „Wichtige Einschränkung"), einmal mit
`--from-state` und 4b-Startwert — als Regressionstest für genau diese
Einschränkung (beide Aufrufe stehen unter „Aufruf (Stand-alone, zum Testen)").
- **`finger_block_joints`/`per_link_method`** stehen schon (leer) in der
robot.json — vorbereitete, aber im Skript bisher ungenutzte Erweiterungspunkte
aus appRobotRendering.
---
## Kalibrier-Switch: Gelenk-Origin (`pose_estimation.fit_origin_link`)
**Motivation:** `doc/Kalibrierung.md` Schritt [4] bestimmt
`links.Arm1.jointToParent.origin[1,2]` (Y/Z des Schultergelenk-Drehpunkts) geometrisch
aus einer dedizierten 3-Pose-Aufnahme (Marker-Mittelpunkte, keine Normalen). Diese
Y/Z-Werte sind laut Nutzer „etwas ungenau gemessen" — `5_pose_estimation.py` hat mit
Position+Normale und einem robusten Least-Squares-Löser bereits die Bausteine, um
denselben Drehpunkt aus den ohnehin vorhandenen Homing-Aufnahmen zu verfeinern.
**Ein Schalter, eine Stelle:** `robot.json` → `pose_estimation.fit_origin_link`
(aktuell `"Arm1"`). Wenn gesetzt, gibt `estimate_global_ba()` für diesen Link
`jointToParent.origin[1,2]` (Y,Z) als **2 zusätzliche Variablen derselben
Optimierung** frei (kein separater Lauf, keine Restore-Logik, keine eigene
Funktion) — die Gelenkvariable und der Drehpunkt werden **gemeinsam** bestimmt.
Bei Erfolg übernimmt `main()` das Ergebnis automatisch: `patch_robot_json_origin()`
schreibt nur die eine `"origin": [...]`-Zeile des Links in `robot.json` zurück
(Text-Patch, nicht `json.dump()` — der Rest der handgepflegten, kompakten Datei
bleibt unverändert). `null`/Feld weglassen = aus, keine Verhaltensänderung.
### Befund an echten Daten (drei reale Captures, sequenziell, 2026-06-16)
| Lauf | Fixture | Origin Y / Z danach | Δ zum Vorlauf |
|---|---|---|---|
| 1 | `20260616_133151` | 108,28 / 34,81 | +7,18 / 20,39 (ggü. ursprünglich 101,1 / 55,2) |
| 2 | `20260616_135403` | 108,84 / 34,89 | +0,57 / +0,08 |
| 3 | `20260616_120456` (unvollst. Seed) | 107,42 / 35,33 | 1,43 / +0,45 |
Drei unabhängige Aufnahmen (unterschiedliche Marker, unterschiedliche Posen) landen
im selben Bereich, und die Schritte werden **kleiner statt größer** — spricht für
Konvergenz, nicht für Rauschen/Drift. (Für die Doku danach wieder auf den
Ursprungswert `101.1, 55.2` zurückgesetzt — die Tabelle zeigt nur den Testlauf.)
**Konsequenz des „bei jedem Lauf automatisch":** der Wert wandert mit jeder neuen
Aufnahme leicht weiter, statt einmalig fixiert zu werden — gewünscht laut Nutzer,
aber gut zu wissen. Schalter auf `null` setzen, um das einzufrieren.
Ergänzt, ersetzt nicht: `doc/Kalibrierung.md` Schritt [4] (3-Pose-Kreis-Fit, nur
Marker-Mittelpunkte) bleibt die unabhängige Gegenmessung. Die Achs**richtung**
(`jointToParent.axis`) fitten beide Verfahren nicht.
---
## Aufruf (Stand-alone, zum Testen)
**Empfohlen — mit Startwert aus der 4b-Kette** (z. B. dem letzten vorhandenen
`state_*.json`; unvollständig ist ok, fehlende Variablen bleiben Multi-Start-geschützt):
```bash
python scripts/5_pose_estimation.py data/homing/<run>/aruco_marker_poses.json \
-robot scripts/robot_1781069752019.json \
--from-state data/homing/<run>/state_Arm2.json \
-out data/homing/<run>/robot_state.json
```
**Kalt** (kein `--from-state`) — funktioniert weiterhin identisch wie vor diesem
Code-Hook, aber ohne den oben beschriebenen Schutz für gekoppelte Blöcke;
nützlich, um das Kaltstart-/Lokales-Minimum-Verhalten aus „Wichtige
Einschränkung" gezielt zu reproduzieren/regressionszutesten:
```bash
python scripts/5_pose_estimation.py data/homing/<run>/aruco_marker_poses.json \
-robot scripts/robot_1781069752019.json
# Verfahren erzwingen, z.B. zum gezielten Vergleich einzelner Methoden:
python scripts/5_pose_estimation.py data/homing/<run>/aruco_marker_poses.json \
-robot scripts/robot_1781069752019.json --method global_ba
```
Gegen die echten Testdaten in `test/homing/*/` ausprobiert — siehe
„Validierung an echten appRobotHoming-Daten" oben.
---
## Integrationsschritte (Offene Punkte)
**Erledigt (2026-06-16):**
- [x] **Architektur entschieden:** 4b-Kette läuft zuerst und liefert den
`accumulated_state` als Startwert; `5_pose_estimation.py` läuft danach als
globaler Verfeinerungsschritt darüber. Kein Ersatz, keine parallele
Alternative — siehe „Wichtige Einschränkung" oben.
- [x] **`scipy` in `docker-compose.yaml` ergänzt** (`pip3 install … numpy scipy`).
- [x] **Code-Hook `--from-state`:** `load_seed_state()` (akzeptiert flach oder
`{accumulated_state:{...}}`) + `estimate_sequential_fk(..., seed=...)`
überspringt nur vollständig geseedete Blöcke, alles andere bleibt
Multi-Start-geschützt. `estimate_pose(..., seed=...)` reicht das durch.
Verifiziert an 3 echten Fixtures (s. „Validierung an echten
appRobotHoming-Daten").
- [x] **Robustheit gegen fehlende Marker:** `Hand`/`Palm`/`FingerA`/`FingerB`
(aktuell `"markers": []`) laufen ohne Crash durch, Output zeigt `null`/
`confidence:"none"` statt erfundener `0`. `main()`s Output-Writer mappt
`observable:false → value:null` (intern bleibt `0.0` für die FK-Rechnung
der anderen Gelenke — nur der Output-Vertrag ändert sich).
- [x] **Kalibrier-Switch `pose_estimation.fit_origin_link`** umgesetzt — ein
Konfig-Feld, direkt in `estimate_global_ba()` integriert (keine separate
Funktion/Report/CLI-Flag mehr), übernimmt das Ergebnis automatisch in
`robot.json` (`patch_robot_json_origin()`, Text-Patch). Generisch für jeden
Link, aktuell für `Arm1` aktiviert. Details: eigener Abschnitt oben.
**Noch offen:**
- [ ] **Adapter** `movements.<var>.value` → flaches `{x,…,e}`-State-Objekt für
`POST /api/homing/send-state`; `null` muss `null` bleiben (nicht zurück zu `0`).
- [ ] **Anbindung in `homingOrchestrator.js`** (neuer Schritt nach der 4b-Schleife,
SSE-Events) — **Umfang/Fehlerfall/Sende-Politik an die Robotersteuerung sind
noch nicht festgelegt** (offene Rückfrage vom 2026-06-16, noch unbeantwortet:
Minimal-Fix vs. Voll-Integration; Abbruch vs. Fallback bei Fehler; Senden vs.
nur Anzeigen). Diese drei Entscheidungen zuerst klären, dann verdrahten.
- [ ] **Arm1-Origin-Wert beobachten:** wandert bei jedem Lauf mit `fit_origin_link`
leicht weiter (s. Befund-Tabelle oben, wird kleiner/konvergiert bisher). Falls
das nicht erwünscht ist: Schalter nach dem Einschwingen auf `null` setzen, oder
gegen eine frische Verfahren-B-3-Pose-Messung gegenchecken.
- [ ] **`fit_origin_link` in der Kalibrierung-UI sichtbar machen** (`doc/Kalibrierung.md`
Schritt [4]) — aktuell nur in `robot.json` umschaltbar; Tab „Arm1 Y" könnte
den aktuellen Wert/letzte Änderung neben Verfahren B anzeigen.
- [ ] **Mehrpose-Erweiterung** (mehrere `aruco_marker_poses.json` mit
gemeinsamem `origin`, je Pose ein eigener Gelenkwinkel, ein gemeinsamer Solve)
— würde die Winkel/Origin-Korrelation aus einer Einzelpose weiter reduzieren,
analog zur bestehenden 3-Pose-Aufnahme.
- [ ] `huber_delta_mm`/`normal_weight` ggf. gegen reale Marker-Genauigkeit
nachjustieren — reales Residuum (2,44,5 mm RMS) liegt deutlich über der
Simulation; Defaults sind unverändert aus appRobotRendering übernommen.
- [ ] Python-Tests (`pytest`) für `load_seed_state()`, den Block-Skip in
`estimate_sequential_fk()` und die Origin-Erweiterung in `estimate_global_ba()`
— aktuell nur manuell gegen die drei Fixtures verifiziert (s. oben);
appRobotHoming hat bisher keine Python-Testinfrastruktur (nur Jest/JS).
- [ ] Eintrag in `Homing.md`-Tabelle (Doku-Übersicht) ergänzen, sobald
`homingOrchestrator.js` verdrahtet ist.
---
## Verweise
- Allgemeiner Ablauf: [`Homing.md`](Homing.md)
- Vorheriger Schritt (Kamera/Triangulation, liefert den gemeinsamen Input):
[`Homing_0_Camera.md`](Homing_0_Camera.md)
- Vorstufe (4b-Kette, liefert den hier benötigten Startwert):
[`Homing_1_StepByStep.md`](Homing_1_StepByStep.md)
- Ursprung & Validierung: Projekt **`appRobotRendering`**,
`pipeline/pose_estimation.py` + `doc/pipeline.tex` (Abschnitte „Pose-Estimation:
Vier Schätzverfahren" und „Validierung und Ergebnisse").

View File

@@ -0,0 +1,365 @@
# Homing 5 Verbesserungspfade: Mehrpunkt-Residuen & Gewichtung
> Ergänzt [`Homing_5_Pose.md`](Homing_5_Pose.md) um drei mögliche
> Verbesserungen, die alle an derselben Stelle ansetzen: *was* ein Marker als
> Messung beiträgt und *wie stark* sie zählt.
>
> **Status (2026-06-25):** **Qualitäts-Gewichtung** (Doc-Punkt 3 / Schritte 1+2)
> umgesetzt. **Mehrpunkt-Eckresiduen** (Doc-Punkt 1 / Schritte 3+4) als
> Opt-in-Modus `corner_points` implementiert, aber nach gescheitertem
> Live-Test wieder **DEAKTIVIERT** — robot.json steht auf `corner_pose`
> (bewährter Zustand). Der Code bleibt inaktiv im Repo. Ursache + Vorbedingungen
> für eine erneute Aktivierung: Notiz unten. **Einzelkamera-Einbindung**
> (Doc-Punkt 2 / Schritte 5+6) weiterhin **offen**.
---
## Ausgangslage (aktueller Code)
Pro Marker fließen heute genau **6 Residuen** in `5_pose_estimation.py`s
`residual_vector()` ein: 3× Position (`pos_mm`) + 3× Normale (skaliert mit
`normal_weight`, Default 100). Beide kommen aus `aruco_marker_poses.json`,
geschrieben von `3b_corner_marker_poses.py`:
```
3b: trianguliert 4 Ecken (DLT, je Ecke eigene Multi-View-Triangulation)
→ position_m/mm = Schwerpunkt der 4 Ecken
→ normal = SVD-Ebenen-Fit durch die 4 Ecken
→ corners_m = die 4 Ecken selbst (steht im JSON, wird aber von
5_pose_estimation.py nie gelesen)
```
Alle drei folgenden Punkte hängen an genau dieser Kette.
---
## 1. Vier Eckpunkte statt Center+Normal
**Frage des Nutzers:** „Mit vier Eckpunkten hätten wir weit mehr Statistik,
und nicht die zusammengefassten Werte, oder?" — ja.
**Befund:** `corners_m` steht bereits in `aruco_marker_poses.json`
(`3b_corner_marker_poses.py:166`), wird aber von
`load_observations()` (`5_pose_estimation.py:110`) nicht gelesen. Center und
Normale sind beide bereits verlustbehaftete Zusammenfassungen der 4 Ecken
(Mittelwert bzw. SVD-Ebenen-Fit) — der eigentliche Fit in `residual_vector()`
sieht nur noch diese zusammengefasste Version, nicht die Rohdaten.
**Idee:** Pro Marker 4×3=12 Eck-Residuen statt 3+3 Center/Normal-Residuen.
Vorteile:
- Mehr unabhängige Messpunkte → realistischere Statistik/Unsicherheit.
- Eine einzelne schlecht erkannte Ecke (Verdeckung, Blur) verzerrt nicht mehr
automatisch Center *und* Normale des ganzen Markers — der robuste
Huber-Verlust könnte direkt auf Eck-Ebene greifen statt auf Marker-Ebene.
- Lage *und* Orientierung kommen aus denselben 4 Punkten — keine separate,
zusätzlich verlustbehaftete Normalen-Schätzung nötig.
**Nötig dafür:**
- `robot_fk.py`: neue Methode (analog `marker_world()`), die die 4 lokalen
Eckpunkte eines Markers (aus `marker["size"]` + ArUco-Eckreihenfolge/Spin-
Konvention, schon bekannt aus `corner_plane_normal()` und
`doc/Kalibrierung_Marker.md`) ins Weltsystem transformiert.
- `5_pose_estimation.py`: `load_observations()` um `corners_m` erweitern,
`residual_vector()` um einen dritten Modus neben den bestehenden
`"corner_pose"`/`"center_point"`-Werten von `marker_observation`.
**Risiko:** mittel. Die Spin/Eckreihenfolge-Konvention muss exakt stimmen,
sonst systematischer Bias statt Verbesserung. Gut gegen Simulations-GT
(appRobotRendering) prüfbar, bevor an reale Daten.
---
## 2. Einzelkamera-Marker einbeziehen statt verwerfen
**Frage des Nutzers:** Komplett verworfene 1-Kamera-Marker seien vermutlich
schlechter als sie schwach gewichtet einzubeziehen — insbesondere relevant für
zukünftige Finger-Marker, wo oft zwei Kameras je einen *anderen* Marker sehen.
**Befund — zwei Stellen verwerfen heute hart:**
- `3b_corner_marker_poses.py:142`: `if len(cam_ids) < args.minCams: continue`
(Default `--minCams 2`) — ein 1-Kamera-Marker bekommt **gar keinen Eintrag**
in `aruco_marker_poses.json`. Das passiert schon **vor** `5_pose_estimation.py`.
- `5_pose_estimation.py`s `load_observations(..., min_cams=2)` filtert
zusätzlich — meist wirkungslos, weil 3b den Marker oft schon gar nicht erst
schreibt.
### Reprojektions-Residuum statt Single-View-PnP (bevorzugter Weg)
**Ursprünglich angenommener Weg — Single-View-PnP — explizit nicht bevorzugt.**
`triangulate_multiview()` (3b) ist Multi-View-DLT — mit 1 Kamera unterbestimmt,
ein einzelnes Bild kann keinen 3D-Punkt triangulieren. Der naheliegende Ersatz
wäre Single-View-PnP (bekannte Kantenlänge `size` als Skalen-Anker) — das hat
aber die bekannte **Flip-Ambiguität** (zwei spiegelbildliche Lösungen aus
einer Ansicht, vgl. appRobotRendering `scene_reconstruct.py`). Nur als
**Fallback** vorgesehen ("besser als nichts"), explizit nicht der Hauptweg.
**Bevorzugt: Reprojektion in den bestehenden globalen Fit, keine eigene
Marker-Pose.** Kamera A ist gegen das Board bereits voll posenbestimmt
(Stufe 2: `K`,`D`,`R`,`t` bekannt). Statt für einen 1-Kamera-Marker einen
eigenständigen (zwangsläufig mehrdeutigen) 3D-Punkt zu rekonstruieren, geht er
direkt als **Bild-Residuum** in denselben globalen Zustand `q` ein:
```
für den Gelenkzustand q (einzige Unbekannte — keine separate Marker-Pose):
P_k(q) = T_link(q) · p_k_lokal (4 Eckpunkte via FK)
û_k = project(K_A, D_A, R_A, t_A; P_k(q)) (Reprojektion in Kamera A)
residual_k = û_k u_k_beobachtet (Pixel-Differenz je Ecke)
```
Keine Flip-Ambiguität, weil keine unabhängige Marker-Pose gesucht wird — nur
`q` (7 Variablen, durch alles andere im Modell schon mitbestimmt). Ein falscher
`q` projiziert einfach sichtbar daneben, statt eine zweite gültige Lösung zu
erzeugen.
**„Kette von Triangulations-Optionen" (Nutzeridee):** Geteilte Variablen
(z. B. `e` bei FingerA/FingerB) werden in `analyze_chain()` schon heute in
**einen** Block zusammengefasst — sind beide Finger-Marker einzeln
triangulierbar (je ≥2 Kameras), bestimmen sie `e` schon jetzt gemeinsam. Neu
wäre: auch wenn FingerA nur von Kamera 1 und FingerB nur von Kamera 2 gesehen
wird (keiner einzeln triangulierbar), könnten zwei Reprojektions-Residuen
(beide Funktionen desselben `q`) den geteilten Freiheitsgrad **gemeinsam**
einschränken — ohne dass einer der beiden Marker für sich eine vollständige
Pose bräuchte. Passt zur vom Nutzer ausdrücklich erlaubten groben Genauigkeit
(±10°/±10 mm reicht für die Finger völlig).
**Bezug zu Finger-Positionen:** `Hand`/`Palm`/`FingerA`/`FingerB` haben aktuell
**0 Marker** in `robot.json`. Sobald dort Marker ergänzt werden, dürften sie
wegen Größe/Beweglichkeit/Verdeckung oft nur von 1 Kamera gesehen werden — mit
der harten ≥2-Kamera-Regel blieben diese Gelenke dann trotz vorhandener Marker
oft `confidence:"none"`. Die Teilbaum-Logik in `observability()` hilft nur,
wenn irgendein Marker *im Teilbaum* ≥2 Kameras hat — nicht, wenn alle
Finger-Marker isoliert nur je 1 Kamera sehen.
**Nötig dafür (deutlich größer als „Schwellwert ändern"):**
- 3b (oder ein neuer Zwischenschritt): 1-Kamera-Beobachtungen nicht verwerfen,
sondern mit Kamera-Referenz + rohen 2D-Eckpunkten aufnehmen.
- `5_pose_estimation.py`: `load_observations()`/`estimate_pose()` müssten
zusätzlich Kamerakalibrierung (`{cam}_camera_pose.json`, NPZ-Intrinsik)
einlesen können — heute reicht `aruco_marker_poses.json` allein.
- `residual_vector()`: zweiter Residuumstyp (Pixel statt mm) **gemeinsam** mit
den bestehenden mm-Residuen optimiert, mit eigenem Gewicht (analog
`normal_weight`, aber für „Pixel vs. Millimeter").
**Risiko:** primär die **Gewichtung zwischen den beiden Residuumstypen**
falsch skaliert, dominiert einer den Fit und verschlechtert sogar die heute
schon gut funktionierenden ≥2-Kamera-Marker. Architektonisch größer als
Punkt 1/3, aber ohne die Flip-Problematik des ursprünglich angenommenen Wegs.
---
## 3. Marker-Qualitäts-Gewichtung (Größe/Schärfe/Distanz/Kontrast)
**Vom Nutzer bestätigt:** bewusst (provisorisch) entfernt — brachte in
Simulationen wenig, Option soll aber offen bleiben.
**Befund — die Bausteine existieren bereits, sind aber nicht verbunden:**
- `1_detect_aruco_observations.py` berechnet pro Detektion bereits:
`area_px`, `sharpness` (Laplace-Varianz, `compute_sharpness()`), `contrast`/
`dynamic_range` (`compute_contrast()`), `distance_to_border_px`, kombiniert
zu einem `confidence`-Score (`compute_confidence()`) — geschrieben ins
`quality`-Feld jeder Detektion in `{cam}_aruco_detection.json`.
- `robot.json` hat dafür ein fertiges Schema: `observation_weighting`
(`distance_weight`, `marker_size_weight`, `view_angle_weight`) und
`multiview_calculation` (`size_factor`, `sharpness_factor`, `border_factor`,
`homography_factor`, `spin_factor`, `weight_floor`, ...).
- **Aber:** Dieses Schema liest nur `3_multiview_bundle_adjustment_v4.py`
ein Skript, das im Homing-Pfad **nicht** läuft (Homing nutzt
`3b_corner_marker_poses.py`).
- `3b_corner_marker_poses.py`s `load_cameras()` liest aus der Detection-JSON
nur `camera_matrix`, `distortion_coefficients`, `image_points_px` — das
`quality`-Feld wird **nie gelesen**. Es geht also schon hier verloren, bevor
es überhaupt zu `aruco_marker_poses.json` käme.
**Idee (falls reaktiviert):** Die Qualitätsmaße existieren schon ganz am
Anfang der Pipeline — nur durchreichen nötig: 3b liest `quality`/`confidence`
aus der Detection-JSON und schreibt einen `weight`-Wert pro Marker in
`aruco_marker_poses.json`; `5_pose_estimation.py`s `residual_vector()` skaliert
das jeweilige Markerresiduum damit (analog zu `normal_weight`, aber pro Marker
statt global für alle).
**Mögliche Erklärung, warum es in Simulation wenig brachte:** Der
appRobotRendering-Renderfehler-Boden (`markerOffsetMaxMm`, `sensorNoise`, …
aus `renderingInfo`) ist recht gleichförmig über alle Marker — wenig echte
Qualitätsunterschiede zum Gewichten. Bei echten Kameras (Beleuchtung,
Entfernung, Bewegungsunschärfe) könnte die Streuung größer und der Effekt
dadurch sichtbarer sein — das ist der Grund, die Option offen zu halten.
**Nötig dafür:** klein und lokal begrenzt — nur 2 Stellen (3b: `quality`
durchreichen; `5_pose_estimation.py`: Gewicht im Residuum nutzen).
`1_detect_aruco_observations.py` und das `robot.json`-Schema müssen nicht
angefasst werden, die liegen schon bereit.
**Risiko:** niedrig (reines Durchreichen + ein Faktor) — der Aufwand liegt im
erneuten Validieren (Simulation + reale Daten), nicht im Code selbst.
---
## Zusammenhang der drei Punkte
Alle drei ändern letztlich dasselbe: was als „eine Messung" zählt und wie
stark sie zählt. Sie sind unabhängig umsetzbar, aber am Ende würde man sie
zusammenführen wollen:
```
heute: residual = [Δposition, Δnormal × normal_weight] (6 Werte/Marker, nur ≥2-Kamera-Marker)
1: residual = [Δcorner_0 .. Δcorner_3] × marker_weight (12 Werte/Marker, weiterhin ≥2-Kamera)
2: + Reprojektions-Residuen für 1-Kamera-Marker (neuer Typ, eigene Gewichtung)
3: marker_weight zusätzlich nach Quality-Score aus Stufe 1
```
**Geschätzte Reihenfolge nach Aufwand/Risiko** (keine Festlegung, nur
Einschätzung): 3 (niedrig, reines Durchreichen) → 1 (mittel, FK-Erweiterung,
gut simulationstestbar) → 2 (architektonisch am größten — neuer Residuumstyp
+ Kamerakalibrierung als zusätzlicher Input —, aber ohne Flip-Risiko, seit
Reprojektion statt PnP der bevorzugte Weg ist).
---
## Umsetzungsplan (ToDo)
Reihenfolge folgt der Risiko-Einschätzung oben: erst risikoarmes Durchreichen,
dann die beiden strukturellen Erweiterungen. Jeder Schritt ist einzeln
umsetzbar und (wo möglich) einzeln testbar, bevor der nächste beginnt.
> **Status-Legende:** ✅ erledigt · ⬜ **offen**
>
> **Punkt-Zuordnung** (Doc-Abschnitt ↔ Chat-Nummerierung): Doc-Punkt 1 „Vier
> Eckpunkte" = Schritte 3+4 · Doc-Punkt 2 „Einzelkamera" = Schritte 5+6 ·
> Doc-Punkt 3 „Qualitäts-Gewichtung" = Schritte 1+2 (erledigt).
| # | Status | Schritt | Testbar an | Bricht Bestehendes? |
|---|---|---|---|---|
| 1 | ✅ erledigt (2026-06-17) | **(Doc-Punkt 3)** `quality`/`confidence` aus `{cam}_aruco_detection.json` bis nach `aruco_marker_poses.json` durchreichen (3b liest es, schreibt ein neues `weight`-Feld pro Marker) | Diff der Ausgabedatei: nur das neue Feld ist zusätzlich da, alles andere (Position, Normale, …) identisch zu vorher — reiner Additivitätstest | **Nein.** Rein additives Feld, kein Pflichtfeld, alte Leser ignorieren es |
| 2 | ✅ erledigt (2026-06-17) | **(Doc-Punkt 3)** `residual_vector()` nutzt das neue Gewicht, hinter einem Schalter (`pose_estimation.use_marker_weight`, Default `false`) | A/B-Vergleich auf den appRobotRendering-Simulationsszenen (mit/ohne Schalter) gegen bekannte Grundwahrheit — genau der Test, der laut Nutzer beim ersten Versuch wenig brachte, jetzt wiederholbar | **Nein bei Default aus.** Mit `true`: Ergebnisse ändern sich gewollt — muss vor Produktiv-Default separat validiert werden |
| 3 | ✅ erledigt (2026-06-25) | **(Doc-Punkt 1)** `robot_fk.py`: neue Methode liefert die 4 lokalen Eckpunkte eines Markers im Weltsystem (Baustein, noch ohne Anbindung) | Isoliert testbar, ganz ohne `5_pose_estimation.py`: gegen die wahren Eckpositionen aus `render_*.json` (Simulation liefert das schon) | **Nein.** Neue, bisher von niemandem aufgerufene Methode |
| 4 | 🔴 Code da, aber DEAKTIVIERT (live gescheitert 2026-06-25 → robot.json zurück auf `corner_pose`) | **(Doc-Punkt 1)** Neuer `marker_observation`-Modus (z. B. `"corner_points"`) nutzt die 12 Eck-Residuen statt 6 Center/Normal-Residuen | Direkter Vorher/Nachher-Vergleich gegen dieselbe Validierungstabelle wie in `Homing_5_Pose.md` (10 Simulationsposen, bekannte GT) | **Nein als Opt-in** (Default bleibt `"corner_pose"`). Tuning-Punkt: `huber_delta_mm` ist auf die heutige Residuumsgröße kalibriert — mit 12 statt 6 Werten/Marker verschiebt sich die RMS-Größenordnung, müsste neu eingeordnet werden |
| 5 | ⬜ **offen** (Vorarbeit/Guards ✅) | **(Doc-Punkt 2)** 3b nimmt 1-Kamera-Beobachtungen mit auf (rohe 2D-Ecken + Kamera-Referenz), statt sie zu verwerfen | Output-Diff: nur neue Einträge für vorher fehlende Marker, bestehende ≥2-Kamera-Einträge unverändert | **Ja, konkret** — siehe Konsumenten-Recherche direkt unter der Tabelle. Mehrere Stellen brauchen einen Guard, **bevor** dieser Schritt scharf geschaltet wird |
| 6 | ⬜ **offen** | **(Doc-Punkt 2)** `residual_vector()` um Reprojektions-Residuen erweitert; `load_observations()`/`estimate_pose()` lesen zusätzlich Kamerakalibrierung | Zuerst an Simulationsszenen, bei denen gut beobachtete Marker künstlich auf „nur 1 Kamera" reduziert werden (GT bekannt) — saubere Kontrolle, ob das Residuum tatsächlich hilft, bevor reale Finger-Marker überhaupt existieren | **Nein strukturell** (additiver Residuumstyp), aber Regressionsrisiko durch falsche mm/px-Gewichtung — vor Produktiv-Default gegen die bestehende Validierungstabelle gegenprüfen (wie Schritt 4) |
| 7 | ⬜ **offen** | Zusammenführen: ein gemeinsames Gewichtsschema (Quality × Kamera-Anzahl × Residuumstyp) statt drei separater Schalter | End-to-End gegen Simulationsbenchmark **und** die drei realen Fixtures aus `Homing_5_Pose.md` | **Nein**, wenn alle Vorstufen additive Defaults hatten |
### Recherche zu Schritt 5: wer liest `aruco_marker_poses.json`? (2026-06-16)
Alle Dateien mit Referenz auf `aruco_marker_poses` im Projekt durchsucht und
geprüft, ob sie `position_mm` (o. ä.) ungeschützt voraussetzen oder schon einen
Guard haben:
| Datei | Nutzung | Wenn `position_mm` fehlt | Guard schon da? |
|---|---|---|---|
| `public/yAxisCompute.js:109-111` | Y-Rotationsachse Base↔Arm1 (Kalibrierung [4], Kreisfit über 3 Posen) | Guard eingefügt (Z. 109114): `Array.isArray()`-Check auf alle drei Posen, fehlende landen im `skipped`-Log statt Crash | ✅ Ja (2026-06-16) |
| `public/boardViewer.html` | X-Achsen-Richtung (Kalibrierung [3]) **und** Y-Achsen-Viewer **und** allgemeiner Pos-A/B/C-Vergleich | `hasXYZ()`-Helper (Z. 220226) + Pre-Filterung der `_*FremdMarkers`-Arrays beim Laden (Z. 1069/1107/1140); direkte Zugriffe in `buildCompareLines()` (Z. 892, 915916) sicher, weil nur pre-gefilterte Marker in den Arrays stehen | ✅ Ja (2026-06-16) |
| `public/homing.js:96` | Homing-Marker-Tabelle | Keiner — `m.position_mm ?? [null,null,null]` | ✅ Ja |
| `server/editRobot.js``assignByZRange()` | Marker-Z-Bereich-Zuordnung (Kalibrierung „Board"-Tab) | Keiner — `Array.isArray(emPos)`-Check, sonst `continue` | ✅ Ja |
| `server/editRobot.js``alignSetToMeasured()` | Set-Ausrichtung (Kabsch-Fit) | Keiner — Marker ohne `position_mm` werden beim Aufbau der Messwert-Map einfach ausgelassen | ✅ Ja |
| `server/editRobot.js``assignMarkerId()` | Einzelnen Marker manuell per ID zuordnen | Guard eingefügt (vor Z. 379): `Array.isArray(em.position_mm)`-Check — fehlende Position gibt klare Fehlermeldung statt Crash | ✅ Ja (2026-06-16) |
| `scripts/4_yAxis_rotation_reconstruction.py` | Python-Variante der Y-Achsen-Rekonstruktion (offline, parallel zu `yAxisCompute.js`) | Guard eingefügt (Z. 165174): expliziter `None`-Check statt `.get(..., [0,0,0])` — fehlende Messung landet mit klarem Grund im `skipped`-Log | ✅ Ja (2026-06-16) |
| `scripts/9_evaluateMarker.py` | Offline-Benchmark gegen Simulations-GT, **nicht** im Live-Homing-Pfad | **Crash**`o["position_m"]` ohne `.get()` | ❌ Nein, aber kein Produktionscode |
| `public/client.js` | Nur CSV-Anzeige/Zahlenformat | Keine Berechnung, nur Darstellung | n/a |
**Bestätigt deine Vermutung, aber breiter als gedacht:** Es betrifft tatsächlich
nur die **X-Achsen-** und **Y-Rotationsachsen-Kalibrierung** (Schritt [3]/[4]
in `Kalibrierung.md`) plus den Offline-Benchmark — aber **nicht nur ein
Filter an einer Stelle**, sondern `yAxisCompute.js` **und** mehrere Stellen in
`boardViewer.html` (das Viewer-File bedient beide Kalibrierschritte). Die
Homing-Seite selbst (`editRobot.js`, `homing.js`) ist bereits robust.
**Guards umgesetzt (2026-06-16) — alle relevanten Stellen:**
- `yAxisCompute.js` (Z. 109114): `Array.isArray()`-Check, fehlende landen im `skipped`-Log.
- `boardViewer.html`: `hasXYZ()`-Helper (Z. 220226) + Pre-Filterung der `_*FremdMarkers`-Arrays; Viewer in allen Situationen getestet und stabil.
- `4_yAxis_rotation_reconstruction.py` (Z. 165174): expliziter `None`-Check ersetzt irreführendes `.get(..., [0,0,0])`; fehlende Messung landet mit klarem Grund im `skipped`-Log.
- `editRobot.js``assignMarkerId()` (vor Z. 379): `Array.isArray(em.position_mm)`-Check gibt klare Fehlermeldung zurück statt Crash.
Alle anderen Konsumenten (`homing.js`, `editRobot.js``assignByZRange`/`alignSetToMeasured`, `scripts/9_evaluateMarker.py`) waren schon robust oder sind Offline-Benchmark-Code ohne Produktionsrelevanz.
### Umsetzung Schritt 3+4 (Doc-Punkt 1) — Befunde (2026-06-25)
- **Schritt 3** (`robot_fk.py`): `marker_corners_local/_world` + `all_markers_world`
liefern jetzt `corners_world` (4×3, in `corners_m`-Reihenfolge). Orientierung =
Spin um die Normale ∘ Minimal-Rotation [0,0,1]→Normale (exakt wie
boardViewer.html). Verifiziert in `test/test_robot_fk_corners.py`:
Selbst-Konsistenz (Center/Kanten/planar/Normalen-Rückgewinnung) **und** gegen
echte triangulierte Roboter-Ecken am Seed-Pose (~1 mm RMS, Identitäts-Paarung
schlägt jede Umordnung). Eckreihenfolge = `(+h,+h),(+h,-h),(-h,-h),(-h,+h)`
(= boardViewer-Zeiger (1,1,0)).
- **Wichtiger Konventions-Befund:** Die `spin`-Werte sind nur für die
**Roboter-Marker** kalibriert/visuell verifiziert, **nicht** für die Board/
Rail-Marker (Set A0/rail, alle auf dem Root-Link `Board`). Deren Eckreihenfolge
liegt ~90° daneben. Lösung: `corner_points` nutzt 12 Eck-Residuen nur für
Roboter-Links (`corner_point_links`, Default = alle Nicht-Root-Links) und 1
Center-Residuum für die Board/Rail-Marker. Da `Board` Root ist (Residuum
konstant bzgl. der Gelenke), kostet das nichts an Information.
- **Datenbefund (nicht Code):** 6 Marker des Board-Sets **A0 sind auf `Arm1`
fehlzugeordnet** (`[55,56,57,77,78,99]`), ~230 mm neben dem Modell. Sie
destabilisieren `corner_points` (12 statt 3 Residuen → falsches Minimum) und
ziehen auch `corner_pose` leicht. Auf bereinigten Markern konvergiert
`corner_points``corner_pose`. → Marker-Zuordnung korrigieren (separate
Kalibrier-Aufgabe).
- **DEAKTIVIERT (2026-06-25) — Aktivierung am echten Roboter gescheitert,
zurückgedreht.** robot.json steht wieder auf `marker_observation:
"corner_pose"` (der bewährte Zustand). Der `corner_points`-Code bleibt im
Repo, ist aber **inaktiv** (opt-in).
**Was probiert wurde:** `corner_points` mit `corner_point_links:
["Hand","Palm","FingerA","FingerB"]` scharfgeschaltet (nur Hand/Finger über
Ecken, Arme/Board unverändert). Am echten Lauf (data/homing/20260625_175916)
kippte die Hand (`b` 52° → +62°) und `x` wanderte (160 → 110 mm).
**Belegte Ursache (Gegenprobe an denselben Daten):**
- `corner_pose` → gutes Ergebnis (`b`52, `x`≈160); `corner_points` → das
schlechte. Also eindeutig der Modus.
- Die **Eck-Konvention ist NICHT der Fehler**: 2 der 3 Finger-Marker passen am
guten Pose exakt (FingerB #178/#179, ~23 mm, korrekte Eck-Paarung fwd+r0).
- **Eigentliche Ursache: ein einzelner schlechter Marker.** FingerA **#147**
liegt **132 mm neben dem Modell** (Position/Zuordnung in robot.json noch
provisorisch, vgl. Commits „Finger1 Marker"/„zweiter Finger verdreht").
Im Eck-Modus liefert ein Marker **12 statt 3 Residuen** → ein einzelner
Ausreißer hat ~4× Zugkraft und reißt `global_ba` ins falsche Minimum.
`corner_pose` (3 Residuen) dämpft ihn per Huber weg.
**Damit `corner_points` je produktiv taugt, fehlen ZWEI Dinge:**
1. **Daten:** FingerA #147 sauber einmessen / Zuordnung prüfen.
2. **Code-Robustheit:** Ausreißer-Schutz im Eck-Modus (grob daneben liegende
Marker pro Marker auf Center zurückfallen lassen oder verwerfen), sonst
kippt jede reale Aufnahme mit *einem* schlechten Marker. Erst danach
gegen GT (Simulation oder Finger-Capture bekannter Pose), **nicht** live
erneut testen. CLI zum Vergleichen: `--marker-observation corner_points`.
- **Offen (Schritt 4 Tuning):** `huber_delta_mm` ist auf 6 Residuen/Marker
kalibriert; mit 12 verschiebt sich die RMS-Größenordnung. Sauberes A/B + Tuning
der Hand/Finger-Ecken gegen appRobotRendering-Simulations-GT steht aus (hier
fehlten Finger-Marker in den Captures). CLI: `--marker-observation corner_pose`
schaltet zum Vergleich zurück.
## Offene Punkte
- [ ] Keiner der drei Punkte/Schritte ist priorisiert/entschieden — reine Optionen.
- [x] Schritt 5: Konsumenten-Recherche erledigt (Tabelle oben) — vor Schritt 5
müssen mindestens `yAxisCompute.js` (1 Stelle) und `boardViewer.html`
(≥9 Stellen) einen Guard bekommen (fehlende Marker überspringen statt
crashen), sonst brechen X-/Y-Achsen-Kalibrierung beim nächsten Lauf mit
1-Kamera-Markern.
- [x] Guards für Schritt 5 umgesetzt (2026-06-16): alle vier offenen Stellen
(`yAxisCompute.js`, `boardViewer.html`, `4_yAxis_rotation_reconstruction.py`,
`editRobot.js → assignMarkerId`) schützen nun gegen fehlende `position_mm`.
Schritt 5 selbst (1-Kamera-Marker in 3b aufnehmen) ist noch offen.
- [x] Schritt 1 (Punkt 3) umgesetzt (2026-06-17): `3b_corner_marker_poses.py`
liest `confidence` aus der Detection-JSON pro Kamera und schreibt
`weight` (Mittelwert über alle beteiligten Kameras) als neues Feld in
`aruco_marker_poses.json`. Alles andere identisch zu vorher — rein additiv.
- [x] Schritt 2 (Punkt 3) umgesetzt (2026-06-17): `5_pose_estimation.py`
liest `weight` aus `aruco_marker_poses.json` in `load_observations()` und
wendet es in `residual_vector()` an, gesteuert durch
`pose_estimation.use_marker_weight` (Default `false`). Kein Verhalten bei
Default; aktivierbar sobald Simulationsvalidierung erfolgt.
- [ ] Für Schritt 3/4: Eckreihenfolge/Spin-Konvention zuerst exakt verifizieren,
bevor Residuen darauf aufbauen.
- [ ] Für Schritt 2: Simulationsvalidierung (A/B-Vergleich mit/ohne
`use_marker_weight`) vor Umstellung des Defaults auf `true`.
## Verweise
- [`Homing_5_Pose.md`](Homing_5_Pose.md) — Hauptdokument zu `5_pose_estimation.py`
- `scripts/1_detect_aruco_observations.py` — Qualitätsmaße je Detektion
- `scripts/3b_corner_marker_poses.py` — Triangulation, Center/Normal-Aggregation,
≥2-Kamera-Filter
- `scripts/3_multiview_bundle_adjustment_v4.py` — einziger Ort, der das
bestehende Gewichtungsschema (`observation_weighting`/`multiview_calculation`)
aktuell liest (nicht im Homing-Pfad)

View File

@@ -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<mm> Y<°> Z<°> A<°> B<°> C<°> E<mm>
```
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).

View File

@@ -1,381 +0,0 @@
# Homing Roadmap appRobotHoming
> Stand: 2026-06-13
> Ziel: Aus einem einzigen Kamera-Snapshot die aktuellen Gelenkwinkel/-positionen
> des Roboters bestimmen und an den Controller senden.
---
## Was ist Homing?
Homing = der Roboter weiss **nicht**, wo er ist.
Die Kameras schauen auf das Board + die ArUco-Marker am Roboter und berechnen
daraus die vollständige Pose aller Gelenke — ohne mechanische Endschalter.
Homing läuft bei **jedem** Einschalten ab, also muss es schnell und robust sein.
Kalibrierung hingegen läuft nur nach mechanischen Änderungen (≈ einmalig).
---
## Voraussetzungen (Kalibrierung muss abgeschlossen sein)
| Was | Wo gespeichert | Status |
|-----|----------------|--------|
| Kamera-Intrinsik (NPZ) | `data/calibration/camX_calibration.npz` | ✅ |
| Board-Marker-Positionen | `robot.json → links.Board.markers[]` | ✅ |
| X-Achsen-Richtung | `robot.json → links.*.position` (rotiert) | ✅ |
| **Arm1-Gelenkursprung Y/Z** | `robot.json → links.Arm1.jointToParent.origin[1,2]` | 🔶 in Arbeit |
| Arm-Marker-Zuordnung | `robot.json → links.Arm1/Ellbow/Arm2/Hand.markers[]` | ❌ offen |
> **Schlüssel-Erkenntnis:** Sobald `Arm1.jointToParent.origin` korrekt gesetzt ist (aus
> der Y-Achsen-Kalibrierung), ist die gesamte Kinematikkette in `robot.json` geometrisch
> definiert. Dann kann Homing starten.
---
## Kinematik-Kette (aus `robot.json`)
```
Board (ROOT, fest)
├── Base linear variable=x axis=[1,0,0] origin=[0,0,16]
│ → Slider-Position entlang Board-X
├── Arm1 revolute variable=y axis=[-1,0,0] origin=[110, 108*, 45*]
│ → Schultergelenk (heben/senken) *aus Y-Achsen-Kalib.
├── Ellbow revolute variable=z axis=[-1,0,0] origin=[0,-250,0]
│ → Ellbogen (relativ zu Arm1-Ende)
├── Arm2 revolute variable=a axis=[0,-1,0] origin=[90,0,0]
│ → Unterarm-Drehung
├── Hand revolute variable=b axis=[1,0,0] origin=[0,-250,0]
│ → Handgelenk
├── Palm revolute variable=c axis=[0,-1,0] origin=[0,0,0]
│ → Handflächen-Rotation
└── FingerA/B linear variable=e axis=±[1,0,0] origin=[±4,-35,0]
→ Greifer (symmetrisch)
```
**Gelenkvariablen:** `x` (mm), `y z a b c` (Grad), `e` (mm)
---
## Homing-Ablauf (Schritt für Schritt)
### Schritt 1 — Snapshot aufnehmen
**Was:** Alle Kameras gleichzeitig ein Foto.
**Script:** — (direkt via Webcam-API)
**UI-Aktion:** Button `[Foto aufnehmen]`
**Ausgabe:**
- `cam0.jpg`, `cam1.jpg`, `cam2.jpg` im aktuellen Run-Verzeichnis
- **Snapshots-Sektion:** Kamerabilder erscheinen
---
### Schritt 2 — ArUco-Marker erkennen
**Was:** Pixel-Koordinaten aller sichtbaren Marker in jedem Kamerabild.
**Script:** `1_detect_aruco_observations.py`
```bash
python 1_detect_aruco_observations.py \
-i cam0.jpg cam1.jpg cam2.jpg \
-robot robot.json \
-outDir data/homing/TIMESTAMP/
```
**Output-Dateien:**
- `cam0_aruco_detection.json` — Marker-IDs + Pixel-Ecken + Kamera-Pose-Kandidaten
**Snapshot CSV:** Tabelle: MarkerID | Kamera | px-Koordinaten | confidence
**Fehlerfälle:**
- Marker verdeckt → weniger Marker in CSV sichtbar
- Schlechte Beleuchtung → confidence niedrig
---
### Schritt 3 — Kamera-Posen schätzen
**Was:** Aus den Board-Markern (fix im Raum) die extrinsische Lage jeder Kamera
berechnen.
**Script:** `2_estimate_camera_from_observations.py`
```bash
python 2_estimate_camera_from_observations.py \
-i data/homing/TIMESTAMP/ \
-robot robot.json \
-outDir data/homing/TIMESTAMP/
```
**Output-Dateien:**
- `cam0_camera_pose.json` — 4×4 Transformationsmatrix Kamera→Welt
**Analysis & Reasoning:** Reprojektions-Fehler pro Kamera (sollte < 3 px)
**Fehlerfälle:**
- Zu wenig Board-Marker sichtbar (< 3) → Kamera-Pose nicht berechenbar
- Hoher Reprojektions-Fehler → Kalibrierung möglicherweise veraltet
---
### Schritt 3b — 3D-Marker-Positionen triangulieren
**Was:** Aus den Kamera-Posen und den Pixel-Koordinaten die echten 3D-Positionen
aller Marker berechnen (inkl. Arm-Marker).
**Script:** `3b_corner_marker_poses.py`
```bash
python 3b_corner_marker_poses.py \
-i data/homing/TIMESTAMP/ \
-robot robot.json \
--outDir data/homing/TIMESTAMP/
```
**Output-Dateien:**
- `aruco_marker_poses.json` — für jeden Marker: `position_mm`, `normal`, `corners_m`, `link`
**Snapshot CSV:** Vollständige Marker-Tabelle mit triangulierten 3D-Positionen
**Fehlerfälle:**
- Marker nur in 1 Kamera → keine Triangulation möglich (braucht ≥ 2)
---
### Schritt 4 — Gelenkwinkel bestimmen
Zwei Methoden — **4b** ist sequenziell und robuster:
#### Methode A — Vollständige State-Schätzung (schnell)
**Script:** `4_robotState_estimation_v6.py`
```bash
python 4_robotState_estimation_v6.py \
aruco_marker_poses.json \
--robot robot.json \
--output homing_state.json
```
Schätzt alle Gelenke in einem Durchlauf mit geometrischen Gleichungen
(Kabsch-Fit, Projektions-Residuen, 2D-Winkel).
#### Methode B — Sequenziell Gelenk für Gelenk (robuster)
**Script:** `4b_revolute_angle.py` — einmal pro Gelenk, von root nach tip:
```bash
# Slider-Position (x) aus Board-Markern bekannt → manuell oder aus 4a
python 4b_revolute_angle.py --robot robot.json --aruco aruco_marker_poses.json \
--link Arm1 --x-mm 180 --output state_arm1.json
python 4b_revolute_angle.py --robot robot.json --aruco aruco_marker_poses.json \
--link Ellbow --from-state state_arm1.json --output state_ellbow.json
python 4b_revolute_angle.py --robot robot.json --aruco aruco_marker_poses.json \
--link Arm2 --from-state state_ellbow.json --output state_arm2.json
python 4b_revolute_angle.py --robot robot.json --aruco aruco_marker_poses.json \
--link Hand --from-state state_arm2.json --output state_hand.json
```
Jedes `--from-state` enthält den akkumulierten Gelenk-State aus allen
vorherigen Schritten.
**Warum sequenziell?** Arm2-Winkel kann nur korrekt berechnet werden wenn
Arm1 und Ellbow bereits bekannt sind (der Weltachsenvektor des Arm2-Joints
ändert sich mit den vorherigen Gelenkwinkeln).
**Output-JSON-Struktur (pro Schritt):**
```json
{
"link": "Arm1",
"joint": "y",
"mean_angle_deg": 23.4,
"circular_std_deg": 0.8,
"num_pairs": 6,
"joint_origin_world_mm": [290, 108, 45],
"joint_axis_world": [-1, 0, 0],
"accumulated_state": { "x": 180, "y": 23.4, "z": null, "a": null, ... }
}
```
**Analysis & Reasoning:** Gelenk-für-Gelenk-Ergebnis als JSON-Baum
**Fehlerfälle:**
- `circular_std_deg` > 5° → Marker-Konfiguration fragwürdig, Messung wiederholen
- `num_pairs` < 2 → Arm-Marker nicht genug sichtbar
---
### Schritt 5 (optional) — Kamera-Z verfeinern
**Was:** Kamera-Höhe (Z) ist aus Board-Markern schlecht bestimmt (Board ist flach).
Wenn Ellbow-Winkel bekannt → FK-Vorhersage als Z-Referenz nutzen.
**Script:** `5_camera_z_refine.py`
```bash
python 5_camera_z_refine.py \
--angle state_ellbow.json \
--robot robot.json \
--aruco aruco_marker_poses.json \
-pose cam0_camera_pose.json \
-pose cam1_camera_pose.json \
--outDir data/homing/TIMESTAMP/
```
**Output:** Korrigierte `*_camera_pose_v8.json` + `z_correction.json`
**Wann nötig:** Wenn Residuen in Schritt 4 systematisch in Z-Richtung driften.
---
### Schritt 6 — Ergebnis senden
**Was:** Vollständigen Joint-State an Roboter-Controller übermitteln.
**UI-Aktion:** Button `[An Roboter senden]`
**Body:** `{ x, y, z, a, b, c, e }`
`POST ROBOT_URL/api/state` (oder Motion-Controller-Protokoll)
**Result Raw JSON:**
```json
{
"status": "ok",
"timestamp": "2026-06-13T19:00:00",
"state": { "x": 180.0, "y": 23.4, "z": -12.1, "a": 5.0, "b": 0.0, "c": 0.0, "e": 0.0 },
"confidence": { "y_std_deg": 0.8, "z_std_deg": 1.2 }
}
```
**Result Tree View:** Gelenk-Werte als lesbare Tabelle
---
## UI-Seitenstruktur (analog index.html)
```
┌─────────────────────────────────────────┐
│ AKTIONEN │
│ [Foto aufnehmen] [Homing berechnen] │
│ [An Roboter senden] │
├─────────────────────────────────────────┤
│ AUSGABE / LOG │
│ Schritt-für-Schritt Log aller Scripts │
├─────────────────────────────────────────┤
│ ANALYSIS & REASONING │
│ Kamera-Posen, Reprojektionsfehler, │
│ Gelenk-Schätzungen je Schritt als JSON │
├──────────────────┬──────────────────────┤
│ RESULT RAW JSON │ RESULT TREE │
│ Vollst. State-JSON │ Gelenk-Tabelle │
├─────────────────────────────────────────┤
│ SNAPSHOT CSV │
│ Tabelle aller Marker: ID, Position, │
│ Link, Residual, num_cameras │
├─────────────────────────────────────────┤
│ SNAPSHOTS (Bilder) │
│ Annotierte Kamerabilder mit Markern │
└─────────────────────────────────────────┘
```
---
## Offene Punkte (Voraussetzungen für Homing)
### 🔶 A — Arm-Marker in robot.json eintragen
Die Arm-Links haben noch **keine Marker** in `robot.json` (links.Arm1/Ellbow/Arm2/Hand.markers).
Mögliche Quellen:
- Aus der Y-Achsen-Kalibrierung: Marker 197, 218, 219 wurden als rotierend erkannt
→ diese können mit relativen Positionen in den jeweiligen Links eingetragen werden
- Mit `4b_revolute_angle.py` lässt sich pro Link prüfen, welche Marker
"zu diesem Link gehören" (paarweise Baseline-Methode)
**Aktion:** Für jedes Arm-Segment mind. 2 Marker bestimmen und in robot.json eintragen.
### 🔶 B — Arm1.jointToParent.origin[Y, Z] finalisieren
Aus der Arm1-Y-Achsen-Kalibrierung (calibration_arm.html):
- Berechneter Wert: Y ≈ 115.6 mm, Z ≈ 50.3 mm
- Aktueller Wert in robot.json: origin = [110, 108, 45]
- **Aktion:** Button „Joint-Origin Y/Z übernehmen" klicken → schreibt in robot.json
### 🔶 C — X-Position (Slider) im Homing
Die Slider-Position `x` ist für `4b_revolute_angle.py --x-mm` nötig.
Optionen:
1. Mechanischer Anschlag / Encoder → immer bekannt
2. Aus Board-Markern schätzen (funktioniert wenn A0-Marker sichtbar)
3. Manuell eingeben (Fallback)
### 🔶 D — Homing-Seite implementieren
Eine neue `homing.html`-Seite (oder Tab in `calibration.html`) die:
- Die Script-Kette 1 → 2 → 3b → 4b(je Link) → (5) als SSE-Stream orchestriert
- Alle Ausgabe-Sektionen wie in index.html befüllt
- „An Roboter senden" Button mit finalem State-JSON verdrahtet
---
## Script-Abhängigkeitsgraph
```
Kamera-Snapshot
[1] detect_aruco → cam*_aruco_detection.json
[2] estimate_camera → cam*_camera_pose.json
│ (aus Board-Markern)
[3b] corner_marker_poses → aruco_marker_poses.json
│ (alle Marker trianguliert)
├──────────────────────────────────────┐
▼ ▼
[4b] revolute Arm1 [4] state_v6 (alternativ)
[4b] revolute Ellbow ──► [5] camera_z_refine (optional, nutzt Ellbow)
[4b] revolute Arm2
[4b] revolute Hand
State-JSON {x, y, z, a, b, c, e}
→ Roboter-Controller
```
---
## Status-Tabelle
| Schritt | Script | Status | Blockiert durch |
|---------|--------|--------|-----------------|
| 1 Snapshot | Webcam-API | ✅ vorhanden | — |
| 2 Marker erkennen | `1_detect_aruco.py` | ✅ vorhanden | — |
| 3 Kamera-Pose | `2_estimate_camera.py` | ✅ vorhanden | — |
| 3b Triangulation | `3b_corner_marker_poses.py` | ✅ vorhanden | — |
| 4 State-Schätzung | `4_robotState_estimation_v6.py` | ✅ vorhanden | Arm-Marker fehlen |
| 4b Sequenziell | `4b_revolute_angle.py` | ✅ vorhanden | Arm-Marker fehlen |
| 5 Z-Verfeinern | `5_camera_z_refine.py` | ✅ vorhanden | Ellbow-Marker fehlen |
| **Arm1-Origin** | calibration_arm.html | 🔶 bereit | Joint-Origin Y/Z übernehmen |
| **Arm-Marker** | robot.json | ❌ fehlen | manuelle Zuordnung |
| **Homing-UI** | homing.html | ❌ fehlt | erst nach Markern sinnvoll |

347
doc/Kalibrierung.md Normal file
View File

@@ -0,0 +1,347 @@
# Kalibrierung appRobotHoming
> Stand: 2026-06-15
> Einmaliger Vorgang — nur nach mechanischen Änderungen wiederholen.
> Jede Stufe baut auf der vorherigen auf.
---
## Übersicht
```
[1] Camera NPZ → [2] Board → [3] X-Achse → [4] Arm1 Y-Achse → [5] Arm-Marker
```
| Schritt | UI-Tab | Ergebnis | Status |
|---------|--------|----------|--------|
| [1] Camera NPZ | Camera NPZ | `data/calibration/{session}/{cam}_calibration.npz` | ✅ |
| [2] Board | Board | `links.Board.markers[].position` in `robot.json` | ✅ |
| [3] X-Achse | Robot X Axis | Alle Marker-Positionen in `robot.json` rotiert | ✅ |
| [4] Arm1 Y-Achse | Arm1 Y | `links.Arm1.jointToParent.origin[1,2]` in `robot.json` | ✅ |
| [5] Arm-Marker | Marker | `links.*.markers[].{position,spin}` visuell prüfen und korrigieren | ✅ |
---
## [1] Camera NPZ — Kameraparameter
**Ziel:** Intrinsische Parameter (Brennweite, Verzerrungskoeffizienten, Kameramatrix)
für jede Kamera als `.npz`-Datei speichern.
**Ablauf:**
1. ChArUco-Board aus verschiedenen Winkeln fotografieren (mehrere Posen)
2. OpenCV `calibrateCamera()` berechnet Kameraparameter
3. Ergebnis als `.npz` speichern und an Webcam-Service übertragen
**Speichert:** `data/calibration/{session}/{cam}_calibration.npz`
**Wird verwendet von:** Script `1_detect_aruco_observations.py` beim Board-Run und Homing
(Argument `-npz`).
---
## [2] Board — Referenz-Marker-Positionen
**Ziel:** Absolute 3D-Positionen aller Board-ArUco-Marker im Welt-Koordinatensystem
bestimmen und in `robot.json` speichern.
**Ablauf:**
1. Foto mit allen Kameras (Snapshot)
2. Script `1_detect_aruco_observations.py``{cam}_aruco_detection.json`
3. Script `2_estimate_camera_from_observations.py``{cam}_camera_pose.json`
4. Script `3b_corner_marker_poses.py``aruco_marker_poses.json` (Triangulierung)
5. Positionen in `robot.json` unter `links.Board.markers` eintragen / bestätigen
**Aktionen im Board-Tab:**
- **Board erkennen**: startet den vollständigen Foto+Script-Durchlauf (SSE-Stream)
- **Marker zuordnen** (Z-Bereich, Set, Link): Marker manuell kategorisieren
- **Sets justieren** (Kabsch 2D+Z): Zwei Marker-Sets aufeinander ausrichten
- **Marker ID zuordnen / entfernen**: einzelne Marker-Korrekturen
**Speichert:** `links.Board.markers` in `robot.json`
---
## [3] X-Achse — Schieberichtung des Sliders
**Ziel:** X-Achse des Roboters (Slider-Richtung `Base → Arm1`) im Board-Koordinatensystem
ausrichten, sodass `[1, 0, 0]` der realen Bewegungsrichtung entspricht.
**Ablauf:**
1. Roboter auf Position A fahren → Board erkennen
2. Roboter auf Position B fahren (mind. 20 mm Unterschied) → Board erkennen
3. Board-Viewer berechnet den Richtungsvektor der Markerbewegungen
4. „X-Achse übernehmen": alle Marker-Positionen in `robot.json` werden rotiert,
sodass die gemessene Richtung zur neuen X-Achse `[1, 0, 0]` wird
**Implementierung:** `editRobot.js``adoptXAxis()` — rotiert alle `links.*.markers[].position`
um den A0-Schwerpunkt als Ursprung.
**Speichert:** alle `links.*.markers[].position` in `robot.json` (rotiert)
---
## [4] Arm1 — Y-Rotationsachse (Schultergelenk)
**Ziel:** Lage und Richtung der Rotationsachse zwischen `Base` und `Arm1` bestimmen
und als `jointToParent.origin` in `robot.json` speichern.
**Ablauf:**
1. Board erkennen mit Arm in **Position A**
2. Arm1 um ≥ 15° drehen
3. Board erkennen (**Position B**)
4. Arm1 nochmals ≥ 15° drehen
5. Board erkennen (**Position C**)
6. Board-Viewer berechnet automatisch die Rotationsachse (magenta Linie im 3D-Viewer)
7. Aktion **„Joint-Origin Y/Z übernehmen"**: speichert Y/Z des Achspunkts in `robot.json`
**Optional — Aktion „Fixe Marker → Link Base":**
Marker mit Bewegung < 10 mm über alle drei Positionen sind physisch am Basis-Körper
befestigt. Diese werden in `links.Base.markers` eingetragen.
**Speichert:**
- `links.Arm1.jointToParent.origin[1]` (Y) und `[2]` (Z) in `robot.json`
- Optional: `links.Base.markers` ergänzt
### Alternative/Ergänzung — `pose_estimation.fit_origin_link` (`5_pose_estimation.py`)
🔶 *Experimentell, noch nicht in dieses UI eingebunden* — Details, Befund und
Vergleich zu Verfahren B: [`doc/Homing_5_Pose.md`](Homing_5_Pose.md) (Abschnitt
„Kalibrier-Switch: Gelenk-Origin").
Ein Schalter in `robot.json` (`pose_estimation.fit_origin_link: "Arm1"`, aktuell
gesetzt): bestimmt `origin[1,2]` (Y,Z) **zusammen mit** der Gelenkvariable in
derselben Pose-Schätzung — aus einer einzelnen vorhandenen Homing-Aufnahme
(Position + gemessene Normale aller Arm1-Marker), keine eigene Mess-Session
nötig. Bei Erfolg **automatisch übernommen**: jeder Lauf schreibt den neuen
Wert direkt in `robot.json` zurück (wie der „Joint-Origin Y/Z übernehmen"-Button,
nur automatisch statt per Klick). Auf drei realen Captures ergab sich eine
konsistente, sich einschwingende Korrektur von ca. **+6 bis +7 mm (Y) / 19 bis
20 mm (Z)** gegenüber dem ursprünglichen Wert — bisher **nicht** gegen eine
frische Messung mit Verfahren B gegengeprüft.
---
### Mathematik: Bestimmung der Rotationsachse
Bei einer Rotation um die Y-Achse bewegt sich jeder erkannte Marker auf einem
**Kreisbogen** im 3D-Raum. Aus den beobachteten Marker-Positionen zu mehreren
Zeitstempeln lassen sich Lage und Richtung der Rotationsachse berechnen.
#### Wie viele Beobachtungen sind nötig?
Entscheidend ist nicht „2 oder 3 Positionen" allein, sondern die Kombination aus
Anzahl Marker und Anzahl Positionen:
| Beobachtungen | Bestimmbar? | Begründung |
|---|---|---|
| 1 Marker, 2 Positionen | **Nein** | Achse liegt *irgendwo* in der Mittelsenkrechten-Ebene der Sehne; Richtung und Lage bleiben unterbestimmt |
| **2 Marker, je 2 Positionen** | **Ja** | **Verfahren A**: Richtung aus d₁ × d₂, Lage aus Schnitt zweier Mittelsenkrechten-Ebenen |
| 1 Marker, 3 Positionen | **Ja** | **Verfahren B**: drei Punkte definieren einen eindeutigen Umkreis → eindeutige Achse |
| N Marker, je ≥ 2 bzw. ≥ 3 Positionen | Ja, überbestimmt | Least-Squares, robust gegenüber Messrauschen, erlaubt Fehlerabschätzung |
Die häufige Kurzbehauptung „zwei Positionen reichen nicht" gilt nur für **einen
einzigen** Marker. Sobald **zwei** (nicht-entartete) Marker vorliegen, genügen
zwei Positionen — siehe Verfahren A.
> Die aktuelle Implementierung (`public/yAxisCompute.js`) verwendet **Verfahren B**.
#### Verfahren A — Zwei Marker, je zwei Positionen
Gegeben: M₁ₐ, M₁ᵦ (Marker 1 zu Zeit a, b) und M₂ₐ, M₂ᵦ (Marker 2 zu Zeit a, b).
Beide Marker sitzen am selben starren Körper, der zwischen a und b um die gesuchte
Achse rotiert. Verschiebungsvektoren (Sehnen der Kreisbögen):
```
d₁ = M₁ᵦ M₁ₐ
d₂ = M₂ᵦ M₂ₐ
```
**Schritt 1 Achsenrichtung.** Jeder Punkt bewegt sich in einer Ebene **senkrecht**
zur Achse; die Sehne liegt in dieser Ebene, also `d₁ ⊥ n` und `d₂ ⊥ n`:
```
n = (d₁ × d₂) / |d₁ × d₂| ← Einheitsvektor entlang der Rotationsachse
```
**Schritt 2 Achsenlage.** Der Kreismittelpunkt eines Markers ist von Start- und
Endposition gleich weit entfernt, liegt also in der **Mittelsenkrechten-Ebene** der
Sehne. Da zusätzlich `n ⊥ dᵢ`, liegt die **gesamte Achse** in dieser Ebene.
```
Ebene 1: d₁ · (x P₁) = 0, P₁ = (M₁ₐ + M₁ᵦ) / 2
Ebene 2: d₂ · (x P₂) = 0, P₂ = (M₂ₐ + M₂ᵦ) / 2
```
Die Achse ist die **Schnittgerade** beider Ebenen mit Richtung `n`. Ein Punkt A auf
ihr (Ansatz A = α·d₁ + β·d₂, Komponente entlang n zu 0 gesetzt):
```
c₁ = d₁ · P₁, c₂ = d₂ · P₂
D = |d₁|²·|d₂|² (d₁ · d₂)² (= |d₁ × d₂|²)
α = (c₁·|d₂|² c₂·(d₁ · d₂)) / D
β = (c₂·|d₁|² c₁·(d₁ · d₂)) / D
A = α·d₁ + β·d₂ ← Referenzpunkt auf der Achse
```
Ergebnis: `r(t) = A + t·n`.
**Entartung:** `D = 0``d₁ ∥ d₂` ⟺ beide Marker liegen **mit der Achse in einer
gemeinsamen Ebene**. Dann sind die Mittelsenkrechten-Ebenen parallel und schneiden
sich nicht eindeutig. Erkennung über `|d₁ × d₂| / (|d₁|·|d₂|) ≈ 0`; auch ein Marker
direkt auf der Achse liefert `dᵢ ≈ 0`. → anderen Marker wählen oder Verfahren B.
#### Verfahren B — Ein Marker, drei Positionen *(implementiert)*
Gegeben P₁, P₂, P₃ desselben Markers zu drei Drehwinkeln. Die drei Punkte liegen
auf einem Kreis, dessen Ebene senkrecht zur Achse steht.
**Schritt 1 — Achsenrichtung** (Normalenvektor der Kreisebene):
```
v₁ = P₂ P₁
v₂ = P₃ P₁
n = normalize(v₁ × v₂)
```
**Schritt 2 — Umkreismittelpunkt** (Punkt auf der Achse), über baryzentrische Gewichte:
```
a² = |P₂ P₃|², b² = |P₁ P₃|², c² = |P₁ P₂|²
w₁ = a²·(b² + c² a²)
w₂ = b²·(a² + c² b²)
w₃ = c²·(a² + b² c²)
C = (w₁·P₁ + w₂·P₂ + w₃·P₃) / (w₁ + w₂ + w₃)
```
Ergebnis: `r(t) = C + t·n`. **Entartung:** P₁,P₂,P₃ kollinear (zu kleiner Drehwinkel)
`|v₁ × v₂| ≈ 0`, Umkreis numerisch schlecht bestimmt.
#### Kombination über N Marker (Least Squares)
Beide Verfahren lassen sich über mehrere Marker mitteln — robuster und mit
Fehlerabschätzung.
**Achsenrichtung gemeinsam.** Aus `dᵢ ⊥ n` (bzw. Ebenen-Normalen) minimiert die beste
Richtung `Σ (dᵢ · n)²` unter `|n| = 1`:
```
M = Σ dᵢ · dᵢᵀ (3×3-Matrix)
n = Eigenvektor von M zum kleinsten Eigenwert
```
(Verallgemeinert das Kreuzprodukt. Verfahren B alternativ: alle Normalen nᵢ aufs
gleiche Halbraum ausrichten und mitteln.)
**Achsenlage gemeinsam.** Für Verfahren B (Umkreismittelpunkte Cᵢ) ergibt sich der
einfache Schwerpunkt:
```
axisDir = normalize( mean(nᵢ) ) ← Vorzeichen vorher angleichen
axisPoint = mean(Cᵢ) ← Schwerpunkt der Umkreismittelpunkte
```
**Residuum pro Marker** (Verfahren B, Fehler- / Ausreißer-Erkennung):
```
εᵢ = |(Cᵢ C̄) ((Cᵢ C̄)·n̄)·n̄|
```
Große εᵢ deuten auf einen fehlerhaften Marker oder eine nicht-rotatorische
Bewegungskomponente hin.
Die Y/Z-Koordinaten von `axisPoint` geben an, wo die Rotationsachse durch die
YZ-Ebene geht — das ist der gesuchte `jointToParent.origin`.
#### Genauigkeit & Verfahrenswahl
Beide Verfahren sind im **rauschfreien** Fall exakt; die Unterschiede liegen in
Robustheit und Aufwand:
| Kriterium | Verfahren A (2 Marker, 2 Bilder) | Verfahren B (1 Marker, 3 Bilder) |
|---|---|---|
| Mindest-Aufnahmen | 2 Bilder | 3 Bilder |
| Mindest-Marker | 2 (gut separiert) | 1 |
| Eigenständige Fehler-Schätzung | nur über mehrere Marker | pro Marker (Residuum εᵢ) |
| Kritische Entartung | d₁ ∥ d₂ (Marker koplanar mit Achse) | P₁,P₂,P₃ kollinear (kleiner Winkel) |
| Empfindlichkeit | hoch, wenn Sehnen fast parallel oder Drehwinkel klein | hoch, wenn Drehwinkel klein (Bogen fast gerade) |
**Beide** brauchen einen ausreichend großen Drehwinkel: kleine Winkel liefern kurze
Sehnen bzw. fast gerade Bögen, in denen das Messrauschen dominiert.
- **Verfahren A** ist schneller (zwei statt drei Aufnahmen), sinnvoll bei ≥ 2 gut
getrennten Markern.
- **Verfahren B** ist robuster für die **Fehlerrechnung**: jeder Marker liefert
unabhängig eine vollständige Achsen-Schätzung, das Residuum εᵢ erlaubt
Ausreißer-Erkennung, keine Entartung zwischen Markern. Für belastbare Genauigkeit
daher vorzuziehen — und der Grund, weshalb die Implementierung Verfahren B nutzt.
**Praktische Werte (beide Verfahren):**
- Drehwinkel zwischen den Positionen ≥ 15° (besser ≥ 30°), sonst numerisch instabil.
- 34 Marker mit guter räumlicher Verteilung um die Achse.
- Die Rotationswinkel müssen **nicht** bekannt sein nur die 3D-Koordinaten.
**Marker-Filter (`min_movement_mm = 10`):**
Marker, die sich zwischen A/B/C weniger als 10 mm bewegen, sind nicht am rotierenden
Teil befestigt und werden aus der Achsenberechnung ausgeschlossen. Ihre Position A
wird für die optionale Base-Link-Zuweisung gespeichert.
---
### Implementierungsnachweis
Die Implementierung in `public/yAxisCompute.js` setzt **Verfahren B** um:
| Schritt | Implementierung | Verifikation |
|---------|----------------|--------------|
| Normalenvektor | `yAxisCompute.js` Z. 5161 | Kreuzprodukt v₁×v₂, normiert ✓ |
| Umkreismittelpunkt | `yAxisCompute.js` Z. 6376 | Baryzentrische Gewichte ✓ |
| Mittelung Normalen | `yAxisCompute.js` Z. 153162 | Vorzeichen-Alignment + mean + renormieren ✓ |
| Mittelung Achspunkte | `yAxisCompute.js` Z. 164167 | Schwerpunkt der Cᵢ ✓ |
| Origin speichern | `calibration.js``setOriginBtn` sendet `axisPoint[1,2]` | `editRobot.js` schreibt `origin[1,2]` ✓ |
**Ergebnis: Implementierung entspricht der beschriebenen Mathematik (Verfahren B) vollständig.**
---
## [5] Arm-Marker — Spin-Verifikation und Korrektur
**Ziel:** Sicherstellen, dass die in `robot.json` eingetragenen Arm-Marker
(Position, Normale, Spin) mit dem realen Aufkleber übereinstimmen — insbesondere
die `spin`-Orientierung, die der Homing-Viewer korrekt darstellen muss.
**Ablauf:**
1. Arm-Marker physisch aufkleben und Position/Normale grob in `robot.json` eintragen
2. Homing-Run starten (Tab „Homing" oder `homing.html`)
3. Kalibrierung → Tab **„Marker"** öffnen:
- Tabelle zeigt alle Arm-Marker mit aktuellem `spin`-Wert
- Viewer zeigt 3D-Skeleton mit spin-orientiertem Marker-Quadrat und Orientierungszeiger (→ Ecke 0)
4. Visuelle Prüfung: zeigt die Viewer-Grafik dieselbe Orientierung wie der echte Aufkleber?
5. Falls nicht: Link und Marker-ID wählen → **90° / +90° / 180°** klicken → Viewer lädt neu
6. Wiederholen bis Modell und Realität übereinstimmen
**Aktionen im Marker-Tab:**
- **Link-Dropdown + ID-Dropdown**: Marker auswählen
- **Spin-Buttons** (90°, +90°, 180°): Spin in `robot.json` korrigieren und Viewer neu laden
- **Viewer** (boardViewer.html?mode=homing): zeigt Skeleton mit Marker-Quadraten und Orientierungszeigern
**Speichert:** `links.{Link}.markers[].spin` in `robot.json` (normalisiert auf [0, 360))
**Details und Roadmap:** [`doc/Kalibrierung_Marker.md`](Kalibrierung_Marker.md)
---
## Dateistruktur
```
data/
calibration/
{session}/
meta.json
{cam}_calibration.npz ← Kameraparameter
{cam}_aruco_detection.json ← Marker-Erkennung
{cam}_camera_pose.json ← Kamera-Pose
scripts/
robot_1781069752019.json ← robot.json (Haupt-Konfiguration)
links.Board.markers ← Board-Marker-Positionen
links.Base.markers ← fixe Basis-Marker (nach Y-Kalibrierung)
links.Arm1.jointToParent.origin ← Schultergelenk-Position (nach Y-Kalibrierung)
links.Arm1.markers ← Arm1-Marker (Nutzer eingetragen)
...
```

341
doc/Kalibrierung_Marker.md Normal file
View File

@@ -0,0 +1,341 @@
# Kalibrierung Arm-Marker
> Stand: 2026-06-15
> Ergänzung zu `doc/Kalibrierung.md` → Schritt „Arm-Marker eintragen und verifizieren"
> Dient als Programmier-Roadmap für den UI-Tab „Marker" und die Verifikations-Pipeline.
---
## Was ist ein Arm-Marker?
Ein ArUco-Aufkleber, der auf einem beweglichen Roboter-Glied (Arm1, Ellbow, Arm2, …)
befestigt ist. Arm-Marker unterscheiden sich von Board-Markern:
| | Board-Marker | Arm-Marker |
|--|--|--|
| Position | fest, kalibriert | bewegt sich mit dem Gelenk |
| Zweck | Referenz für Kamera-Pose | Gelenk-Winkel-Schätzung (Homing) |
| Position in robot.json | `links.Board.markers` | `links.{Link}.markers` |
| Koordinatensystem | Welt (Board) | lokal im Link-Frame |
---
## Marker-Daten in robot.json
```jsonc
{
"links": {
"Arm1": {
"markers": [
{
"id": 198, // ArUco-ID (eindeutig)
"name": "aruco_198", // optional, lesbar
"position": [0, -160, 35], // Mittelpunkt im lokalen Link-Frame [mm]
"normal": [0, 0, 1], // Normale der Marker-Fläche (Link-Frame)
"size": 25, // Kantenlänge mm
"spin": 0 // Drehung der Marker-Grafik um die Normale [°]
}
]
}
}
}
```
### Felder
| Feld | Typ | Bedeutung |
|------|-----|-----------|
| `id` | int | ArUco-ID — muss mit gedrucktem Marker übereinstimmen |
| `position` | `[x,y,z]` mm | Mittelpunkt im **lokalen Link-Frame** (nicht Welt) |
| `normal` | `[nx,ny,nz]` | Flächennormale des Markers im Link-Frame; `[0,0,1]` = Marker schaut in +Z |
| `size` | mm | Kantenlänge des ArUco-Quadrats |
| `spin` | ° | Drehung der Aufkleber-Grafik um die Normale — 0 / 90 / 180 / 270 |
### Spin-Semantik
Ein ArUco-Aufkleber kann in 4 Lagen aufgeklebt werden. Physisch ist das egal — der
Detektor findet die ID unabhängig von der Orientierung, und auch die gemessene
Mittelpunkt-Position ist spin-unabhängig.
`spin` beschreibt nur die visuelle Darstellung im 3D-Viewer, damit die angezeigte
Marker-Grafik mit dem echten Aufkleber übereinstimmt. Das hilft beim visuellen
Abgleich: wenn die Grafik im Viewer verdreht zum Aufkleber steht, ist spin falsch.
> **In der aktuellen Homing-Pipeline (3b, 4b) wird `spin` nicht verwendet.**
> Relevant wird es, wenn der Viewer spin korrekt darstellt (→ offenes Todo unten).
---
## Typischer Workflow
```
1. ArUco-Aufkleber auf Roboter-Glied kleben
2. Position messen / schätzen → in robot.json eintragen
(position = Mittelpunkt im lokalen Link-Frame, normal = Flächen-Richtung)
3. Homing-Run starten (homing.html)
4. Kalibrierung → Tab "Marker" öffnen
│ · Tabelle zeigt alle Marker und ihren aktuellen spin
│ · Viewer zeigt Modell-Marker (Vierecke) + beobachtete Punkte (Kugeln)
│ · Fehler-Linien: Modell-Marker → beobachteter Punkt
5. Prüfen:
· Linie kurz → Position stimmt gut
· Linie lang oder in falscher Richtung → position in robot.json korrigieren
· Viewer-Grafik verdreht → spin korrigieren (+90 / -90 / 180)
6. Spin / Normal korrigieren → Viewer lädt neu → erneut prüfen
```
---
## UI-Tab „Marker" (implementiert 2026-06-15)
### Datei: `public/calibration_marker.html`
Drei Abschnitte:
| Abschnitt | Inhalt |
|-----------|--------|
| Aktuelle Marker | Tabelle aller Arm-Marker (Link, ID, Name, Position, Normal, Size, **Spin**) aus robot.json |
| Aktionen | Link-Dropdown → Marker-ID-Dropdown → Spin-Buttons (90°, +90°, 180°) |
| Viewer | `boardViewer.html?mode=homing` — Modell + letzter Homing-Run |
### JS: `initMarker()` in `public/calibration.js`
| Funktion | Beschreibung |
|----------|-------------|
| `loadRobot()` | Holt `/api/robot`, rendert Tabelle, befüllt ID-Dropdown |
| `renderTable(robot)` | Tabelle aller Arm-Marker mit farbig hervorgehobenem Spin |
| `updateMarkerDropdown()` | Link-Dropdown-Wechsel → ID-Dropdown neu befüllen |
| `applySpin(delta)` | `POST /api/robot/set-arm-marker-spin` → Viewer `reload`-Message |
### Backend: `POST /api/robot/set-arm-marker-spin`
```
Body: { linkName: string, markerId: number, spin: number }
Return: { changed, linkName, markerId, oldSpin, newSpin }
```
Implementiert in `server/server.js` → delegiert an `setArmMarkerSpin()` in `server/editRobot.js`.
---
## Offene Punkte (Programmier-Roadmap)
### [P1] Spin-Rendering im boardViewer
**Status:** ✅ implementiert (2026-06-15)
**Datei:** `public/boardViewer.html``buildSkeletonFK()`, Abschnitt „4. Arm-Marker"
`spin` wird als zusätzliche Rotation um die Marker-Normale (in Welt-Koordinaten)
auf das orientierte Quadrat angewendet via `premultiply`:
```javascript
const normalW = new THREE.Vector3(nx, nz, -ny).transformDirection(childFrame).normalize();
const markerMesh = makeMarkerSquareOriented(posWorld, normalW, markerSizeM, col);
const spinRad = ((m.spin ?? 0) * Math.PI) / 180;
if (Math.abs(spinRad) > 1e-6) {
markerMesh.quaternion.premultiply(
new THREE.Quaternion().setFromAxisAngle(normalW, spinRad)
);
}
```
`premultiply(Q_spin)` setzt `quaternion = Q_spin * Q_normal` → zuerst Normal-Alignment,
dann Spin-Rotation in Welt-Koordinaten um `normalW`.
---
### [P2] Marker-Position verifizieren: Positions-Residuum
**Status:** ❌ noch nicht implementiert
Nach einem Homing-Run kennen wir für jeden erkannten Arm-Marker:
- `model_pos_world` = Modell-Mittelpunkt im Welt-Frame (via FK)
- `obs_pos_world` = triangulierter beobachteter Mittelpunkt
Das **Positions-Residuum** `|model_pos_world obs_pos_world|` zeigt, wie gut die
eingetragene `position` (Mittelpunkt im lokalen Frame) stimmt.
> **⚠ Wichtig: Spin-Fehler sind damit NICHT erkennbar.**
> Der Mittelpunkt eines Markers ist spin-unabhängig — egal wie ein Aufkleber gedreht
> ist, das Zentrum bleibt dasselbe. Ein falscher `spin`-Wert erzeugt daher kein
> Positions-Residuum. Spin-Fehler erfordern → P3 (visuell) oder P4 (automatisch).
**Erweiterung im Marker-Tab:** Tabelle um Spalten ergänzen:
- `Δ mm` = Positions-Residuum des letzten Homing-Runs
- `Status` = ✅ < 5 mm / ⚠ 520 mm / ❌ > 20 mm
**Datenquelle:** `/api/homing/run-data?run={ts}``finalState` + `measuredMarkers`
---
### [P3] Python: Ecken-Position in aruco_marker_poses.json ausgeben
**Status:** ❌ noch nicht implementiert
**Datei:** `scripts/3b_corner_marker_poses.py`
Voraussetzung für P3b. Aktuell enthält `aruco_marker_poses.json` pro Marker nur
`position_mm` (Mittelpunkt) und `normal`. Die Spin-Orientierung geht verloren.
`3b` trianguliert den Mittelpunkt aus den Ecken der Kamera-Beobachtungen.
Dieselbe Triangulierung auf **Ecke 0** anwenden und zusätzlich ausgeben:
```python
# Zusätzliches Feld pro Marker in aruco_marker_poses.json:
"corner0_mm": [x, y, z] # triangulierte Welt-Position von Ecke 0
```
Ecke 0 = top-left in OpenCVs ArUco-Konvention (Index 0 in `corners[0]`).
Zusammen mit `position_mm` (Mittelpunkt) definiert `corner0_mm` eindeutig die
Orientierung des Markers in 3D.
> Der Mittelpunkt bleibt spin-unabhängig. `corner0_mm` ist das einzige Feld,
> das den Spin kodiert.
---
### [P3b] boardViewer: Orientierungszeiger zeichnen
**Status:** ✅ Modell-Seite implementiert (2026-06-15) · Beobachtungs-Seite offen (→ P3)
**Datei:** `public/boardViewer.html``buildSkeletonFK()`
**Voraussetzungen:** P1 ✅, P3 (corner0_mm) für Beobachtungs-Zeiger noch offen
Für jeden Marker werden zwei kurze Linien vom Mittelpunkt zu Ecke 0 gezeichnet —
eine für das Modell, eine für die Beobachtung. Der Winkel zwischen beiden = Spin-Fehler.
**Modell-Zeiger** (implementiert — nutzt `markerMesh.quaternion` direkt):
`markerMesh.quaternion` kodiert bereits `Q_spin * Q_normal`, daher reicht:
```javascript
// Richtung zur Ecke 0: (1,1,0) normalisiert im lokalen Marker-Frame,
// transformiert durch die bereits berechnete Mesh-Rotation (Q_normal ∘ Q_spin)
const ptrDir = new THREE.Vector3(1, 1, 0).normalize().applyQuaternion(markerMesh.quaternion);
const corner0W = posWorld.clone().add(ptrDir.multiplyScalar(markerSizeM * Math.SQRT1_2));
gArmMarkers.add(makeLine(posWorld, corner0W, col, 0.9)); // Zeiger (Link-Farbe)
gArmMarkers.add(makeSphere(corner0W, 0.0008, col)); // Ecke 0 (Punkt)
```
`Math.SQRT1_2 = 1/√2` weil der Abstand Mittelpunkt→Ecke bei einem Quadrat mit
Kantenlänge `size` genau `size/√2` beträgt (`(size/2)·√2 = size/√2`).
**Beobachtungs-Zeiger** (aus `corner0_mm` in `aruco_marker_poses.json`,
sobald Python das Feld liefert → P3):
```javascript
// obs = beobachteter Marker aus _measuredMarkers
if (obs.corner0_mm) {
const corner0Obs = r2vArr(obs.corner0_mm); // robot→Three.js
gArmMarkers.add(makeLine(obsPosW, corner0Obs, 0xffffff, 0.6)); // Beobachtungs-Zeiger (dünn)
gArmMarkers.add(makeSphere(corner0Obs, 0.0006, 0xffffff)); // Beobachtungs-Ecke
}
```
**Ergebnis im Viewer:**
```
Modell-Mittelpunkt ●——▶ Modell-Ecke 0 (Link-Farbe, voll)
Obs-Mittelpunkt ●··▶ Obs-Ecke 0 (weiß, dünn)
```
Zeigen beide Zeiger in dieselbe Richtung → spin korrekt.
90°-Unterschied → spin um 90° falsch → +90° oder 90° klicken.
---
### [P3c] Homing-Check direkt im Marker-Tab starten
**Status:** ❌ noch nicht implementiert
Button „Homing-Check starten" im Marker-Tab triggert die vollständige Homing-Pipeline
(`POST /api/homing/run`) und zeigt:
- SSE-Log im Tab-internen Textfeld
- Fortschritt im Viewer via `postMessage({ type: 'homing-state', state })`
Kein struktureller Unterschied zu `homing.html` — Code-Duplizierung vermeiden
durch Extraktion eines gemeinsamen `runHomingStream(sendFn, frameFn)` aus `client.js`.
> Für Spin-Verifikation ist P3c nötig, damit frische Beobachtungsdaten im Viewer landen.
> Ohne P3 (corner0_mm) sieht man trotzdem nur Mittelpunkt-Fehlerlinien, keine Zeiger.
---
### [P4] Spin automatisch berechnen und korrigieren
**Status:** ❌ noch nicht implementiert
**Voraussetzungen:** P3 (corner0_mm), P3b (Zeiger im Viewer)
Sobald `corner0_mm` aus Python vorliegt und der Viewer die Zeiger anzeigt (P3b),
kann der tatsächliche Spin-Wert rechnerisch bestimmt werden — ohne manuelles Raten.
**Berechnung im Browser** (in `initMarker()` oder `buildSkeletonFK()`):
```javascript
// Modell-Richtung zu Ecke 0 bei spin=0 (im Welt-Frame, via FK):
const modelCorner0Dir = /* corner0World posWorld, normalisiert */;
// Beobachtete Richtung zu Ecke 0:
const obsCorner0Dir = r2vArr(obs.corner0_mm).sub(obsPosW).normalize();
// Winkel zwischen beiden (um die Marker-Normale):
const cross = new THREE.Vector3().crossVectors(modelCorner0Dir, obsCorner0Dir);
const sinA = cross.dot(normalWorld); // Vorzeichen = Drehrichtung
const cosA = modelCorner0Dir.dot(obsCorner0Dir);
const deltaDeg = Math.round(Math.atan2(sinA, cosA) * 180 / Math.PI / 90) * 90;
// → rundet auf nächste 90°: 0 / 90 / -90 / 180
```
**UI-Erweiterung im Marker-Tab:**
- Tabelle bekommt Spalte „Gemessener Spin" und „Soll-Spin (robot.json)"
- Unterschied → Badge „⚠ +90°" → Klick übernimmt die Korrektur direkt
---
### [P5] Marker-Position aus Homing übernehmen
**Status:** ❌ offen
Wenn ein Arm-Marker nur grob eingemessen wurde, kann die triangulierte Welt-Position
aus dem Homing-Run dazu genutzt werden, die `position` in robot.json zu verfeinern:
```
position_local = inverse_FK(obs_world, current_state)
```
Setzt voraus, dass der Gelenk-Winkel für diesen Link bereits korrekt bestimmt wurde.
Iterativ einsetzbar: grobe Startposition → erster Homing → verfeinerte Position → …
---
## Abhängigkeits-Kette Spin-Verifikation
```
P1 boardViewer rendert spin korrekt → Modell-Viereck zeigt echte Orientierung
P3 3b gibt corner0_mm aus → Beobachtungs-Orientierung verfügbar
P3b boardViewer zeichnet Orientierungszeiger → Spin-Fehler sichtbar als Winkel
P3c Marker-Tab: Homing-Check-Button → frische Daten ohne Tab-Wechsel
P4 JS berechnet Δspin, schlägt Korrektur vor → kein manuelles Raten mehr
```
---
## Dateiübersicht
| Datei | Rolle |
|-------|-------|
| `public/calibration.html` | Tab-Button „Marker" |
| `public/calibration_marker.html` | Panel-HTML (Tabelle, Aktionen, Viewer) |
| `public/calibration.js``initMarker()` | Frontend-Logik des Tabs |
| `server/server.js``POST /api/robot/set-arm-marker-spin` | Spin-Endpoint ✅ |
| `server/editRobot.js``setArmMarkerSpin()` | robot.json schreiben ✅ |
| `public/boardViewer.html``buildSkeletonFK()` | Spin-Rendering (→ P1) + Zeiger (→ P3b) |
| `scripts/3b_corner_marker_poses.py` | corner0_mm ausgeben (→ P3) |
| `scripts/robot_1781069752019.json``links.*.markers` | Marker-Daten |

View File

@@ -1,149 +0,0 @@
# appRobotBodyTrack
3D-Body-Tracking für Roboter aus Mehrkamera-ArUco-Bildern.
**Input**
- Bilder: `render_*.png`
- Intrinsics: `render_*.npz`
- Konfiguration: `robot.json`
**Output**
- Gelenke **R⁷**`{x, y, z, a, b, c, e}` (mm / Grad)
---
## Interfaces
Eine Logik, drei Zugänge:
- **Python**
- **CLI**
- **REST (FastAPI)**
---
## Quickstart
### Python
```python
from scripts import estimate_from_dir
result = estimate_from_dir("data/Scene8", robot_json="robot.json")
print(result.joints)
print(result.confidence)
```
---
### CLI
```bash
pip install -e .
python -m scripts data/Scene8 --robot robot.json
python -m scripts data/Scene8 --robot robot.json --cameras a,b,d
```
---
### REST API
```bash
docker compose up
```
**Request:**
```python
import requests
resp = requests.post(
"http://localhost:8446/v1/estimate",
files=[
("images", ("render_a.png", open("render_a.png", "rb"))),
("intrinsics", ("render_a.npz", open("render_a.npz", "rb"))),
],
)
print(resp.json()["joints"])
```
---
## API
| Endpoint | Methode | Zweck |
|----------|--------|------|
| `/v1/estimate` | POST | Bilder → Gelenke |
| `/v1/health` | GET | Status |
| `/v1/config` | GET | aktive Konfiguration |
**Response:**
```json
{
"joints": {"x": 50.2, "y": -2.1, "z": 94.8, "a": 20.1},
"confidence": {"x": "high", "b": "low"},
"residual_rms": 1.45,
"n_markers": 56,
"processing_ms": 1240
}
```
---
## Struktur
```
.
├── scripts/
├── config/robot.json
├── tests/
└── docker-compose.yaml
```
---
## Deployment (Docker / Portainer)
**Volume:**
```yaml
- /opt/approbot/config/robot.json:/config/robot.json:ro
```
**Healthcheck:**
```bash
curl http://<host>:8446/v1/health
```
---
## Konfiguration
Zentrale Datei: **`robot.json`**
Verwendete Bereiche:
- `links`
- `pose_estimation`
- `vision_config`
- `movements`
- `units`
---
## Stack (minimal)
- numpy
- scipy
- opencv (aruco)
- fastapi + uvicorn
---
## Naming
- **BodyTrack** → Tracking (dynamisch) ✅
- **BodyMap** → Modell / Repräsentation
- **BodySense** → Wahrnehmung (low-level)

270
doc/accessRobotAPI.md Normal file
View File

@@ -0,0 +1,270 @@
# robot.json Zugriff via appRobotDriver
> **Status: umgesetzt** (2026-06-17) — `server/robotConfig.js` ist aktiv.
> Dieses Dokument beschreibt Entwurf und Implementierung. Der Implementierungsplan
> (Schritte 13) ist vollständig abgearbeitet.
---
## Verhalten je Env-Variable
| 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:
```
ROBOT_JSON = process.env.ROBOT_JSON
|| 'scripts/robot_1781069752019.json'
```
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.
---
## Ziel-Zustand
```
Startup GET {ROBOT_URL}/api/robot/config → robot.json laden
Kalibrierung schreiben → lokal anpassen → POST {ROBOT_URL}/api/robot/config
Python-Skripte → weiterhin lokale Datei (Cache) (unverändert)
```
`appRobotDriver` ist die **Single Source of Truth**.
`appRobotHoming` hält eine **lokale Kopie** (Cache-Datei) nur für die Dauer eines
Laufs — Python-Skripte müssen nicht angepasst werden.
---
## appRobotDriver API (Platzhalter)
Die genaue API ist noch zu klären. Annahmen:
| Aktion | Endpoint | Body / Antwort |
|--------|----------|----------------|
| Konfiguration lesen | `GET {ROBOT_URL}/api/robot/config` | → JSON (robot.json-Inhalt) |
| Konfiguration schreiben | `POST {ROBOT_URL}/api/robot/config` | Body: JSON (robot.json-Inhalt), → `{ ok: true }` |
Sobald die echten Endpoints bekannt sind, diese Tabelle und die Implementierung
(`server/robotConfig.js`) entsprechend anpassen.
---
## Implementierungsplan
### Schritt 1 — `server/robotConfig.js` (neu)
Kapselt den gesamten robot.json-Zugriff. `server.js` und `editRobot.js` importieren
nur noch diese Funktionen — kein direktes `fsPromises.readFile` / `writeFile` mehr.
```javascript
// server/robotConfig.js (ESM)
import fsPromises from 'fs/promises';
import path from 'path';
import { fileURLToPath } from 'url';
const __dirname = path.dirname(fileURLToPath(import.meta.url));
const ROBOT_URL = process.env.ROBOT_URL || '';
// Lokale Cache-Datei: bleibt als Fallback und für Python-Skripte
const ROBOT_JSON = process.env.ROBOT_JSON
|| path.join(__dirname, '..', 'scripts', 'robot_1781069752019.json');
/**
* Lädt robot.json.
* Reihenfolge: (1) ROBOT_URL/api/robot/config, (2) lokale Datei als Fallback.
* Schreibt das Ergebnis immer in die lokale Cache-Datei (für Python-Skripte).
*/
export async function fetchRobot() {
if (ROBOT_URL) {
const res = await fetch(new URL('/api/robot/config', ROBOT_URL));
if (!res.ok) throw new Error(`Driver ${res.status}: ${await res.text()}`);
const data = await res.json();
// Cache für Python-Skripte aktualisieren
await fsPromises.writeFile(ROBOT_JSON, JSON.stringify(data, null, 2), 'utf8');
return data;
}
// Fallback: lokale Datei (Entwicklung ohne Driver)
return JSON.parse(await fsPromises.readFile(ROBOT_JSON, 'utf8'));
}
/**
* Speichert robot.json.
* Schreibt immer in lokale Cache-Datei; sendet zusätzlich an Driver wenn konfiguriert.
*/
export async function pushRobot(data) {
await fsPromises.writeFile(ROBOT_JSON, JSON.stringify(data, null, 2), 'utf8');
if (ROBOT_URL) {
const res = await fetch(new URL('/api/robot/config', ROBOT_URL), {
method: 'POST',
headers: { 'Content-Type': 'application/json' },
body: JSON.stringify(data),
});
if (!res.ok) throw new Error(`Driver ${res.status}: ${await res.text()}`);
}
}
/** Pfad zur lokalen Cache-Datei wird an Python-Skripte als -robot-Argument übergeben. */
export const robotCachePath = ROBOT_JSON;
```
---
### Schritt 2 — `server/editRobot.js` anpassen
`readRobot()` und `writeRobot()` sind die einzigen I/O-Primitiven in `editRobot.js`.
Sie müssen auf `fetchRobot()` / `pushRobot()` umgestellt werden.
**Aktuell:**
```javascript
async function readRobot(robotPath) {
return JSON.parse(await fsPromises.readFile(robotPath, 'utf8'));
}
async function writeRobot(robotPath, data) {
await fsPromises.writeFile(robotPath, JSON.stringify(data, null, 2), 'utf8');
}
```
**Neu:**
```javascript
import { fetchRobot, pushRobot } from './robotConfig.js';
async function readRobot(_robotPath) { // _robotPath ignoriert Quelle ist Driver
return fetchRobot();
}
async function writeRobot(_robotPath, data) {
return pushRobot(data);
}
```
Alle exportierten Funktionen (`assignByZRange`, `setArmMarkerSpin`, `adoptXAxis`,
`setJointOriginYZ`, …) bleiben **unverändert** — sie rufen intern `readRobot` /
`writeRobot` auf.
> Der `robotPath`-Parameter bleibt in den Signaturen erhalten (Kompatibilität),
> wird aber ignoriert. Alternativ: alle Aufrufer in `server.js` bereinigen und
> Parameter entfernen (Folgeschritt).
---
### Schritt 3 — `server/server.js` anpassen
#### 3a — Python-Skripte erhalten weiterhin die Cache-Datei
```javascript
// Vorher:
import { ROBOT_JSON } from './config.js'; // oder const direkt
// '-robot', ROBOT_JSON
// Nachher:
import { robotCachePath } from './robotConfig.js';
// '-robot', robotCachePath
```
Die Pipeline (`runBoardPipeline`, `runHoming`) fetcht robot.json **einmal vor dem
Lauf** via `fetchRobot()`, um den Cache zu aktualisieren:
```javascript
import { fetchRobot, robotCachePath } from './robotConfig.js';
async function runBoardPipeline(runDir, send, refSet) {
// Cache aktualisieren bevor Python startet
await fetchRobot();
// Python-Skripte erhalten robotCachePath wie bisher
const script1Args = [..., '-robot', robotCachePath, ...];
// …
}
```
#### 3b — `GET /api/robot` liest via `fetchRobot()`
```javascript
// Vorher:
app.get('/api/robot', async (req, res) => {
const robot = JSON.parse(await fsPromises.readFile(ROBOT_JSON, 'utf8'));
return res.json(robot);
});
// Nachher:
import { fetchRobot } from './robotConfig.js';
app.get('/api/robot', async (req, res) => {
try {
const robot = await fetchRobot();
return res.json(robot);
} catch (err) {
return res.status(502).json({ error: `Driver nicht erreichbar: ${err.message}` });
}
});
```
#### 3c — Kalibrierungs-Endpoints: kein Änderungsbedarf
Da `editRobot.js` intern `readRobot` / `writeRobot` verwendet und diese umgestellt
werden (Schritt 2), propagieren sich alle Kalibrierungs-Schreibvorgänge automatisch
zum Driver. Kein Änderungsbedarf in den einzelnen Endpoints.
---
## Startup-Verhalten
Beim Start von `server.js` einmalig robot.json laden und cachen:
```javascript
// server.js nach HTTPS-Server-Start
try {
await fetchRobot();
console.log('✅ robot.json vom Driver geladen und gecacht.');
} catch (err) {
console.warn(`⚠ Driver nicht erreichbar nutze lokale Datei: ${err.message}`);
}
```
---
## Fallback-Verhalten
| Szenario | Verhalten |
|----------|-----------|
| `ROBOT_URL` nicht gesetzt | Nur lokale Datei — Entwicklungsmodus, Driver nicht nötig |
| Driver beim Start nicht erreichbar | Warnung, lokale Cache-Datei wird verwendet |
| Driver während Lauf nicht erreichbar | `pushRobot()` wirft Fehler → Kalibrierungs-Endpoint antwortet 502 |
| Python-Skript schlägt fehl | Kein push nötig (Python schreibt nicht in robot.json) |
---
## Datei-Übersicht nach Umbau
| Datei | Rolle |
|-------|-------|
| `server/robotConfig.js` *(neu)* | `fetchRobot()`, `pushRobot()`, `robotCachePath` |
| `server/editRobot.js` | `readRobot` / `writeRobot` delegieren an `robotConfig.js` |
| `server/server.js` | importiert `robotCachePath` statt lokalem `ROBOT_JSON`; ruft `fetchRobot()` vor Pipelines |
| `scripts/robot_1781069752019.json` | Bleibt als lokale Cache-Datei; **nicht** mehr primäre Quelle der Wahrheit |
---
## Status: Umgesetzt (2026-06-17)
`server/robotConfig.js` erstellt. `server/editRobot.js` und `server/server.js` angepasst.
## Offene Fragen
- [ ] Genaue Endpoints des appRobotDriver für GET / POST robot.json bestätigen (aktuell: `/api/robot/config`)
- [ ] Soll der Driver eine Versions-/Konflikterkennung haben (z.B. ETag / `updatedAt`)?
- [ ] `pushRobot()` bei Driver-Fehler: aktuell hard fail → Kalibrierungs-Endpoint antwortet 502
- [ ] Authentifizierung zwischen appRobotHoming und appRobotDriver nötig?

467
doc/homingAPI.md Normal file
View File

@@ -0,0 +1,467 @@
# Homing Offline-API
> **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.
---
## Motivation
Die Live-Homing-Pipeline (`POST /api/homing/run`) setzt voraus, dass `WEBCAM_URL` auf
einen laufenden WebCam-Service zeigt, der Bilder auf Abruf liefert. Das ist für zwei
Szenarien unpraktisch:
1. **Simulations-Validierung**`appRobotRendering` rendert synthetische Bilder zu
bekannten Gelenkwinkeln und will prüfen, wie gut die aktuelle Pose-Erkennung die
Winkel zurückrechnet. Die Pipeline in `appRobotRendering` liegt lokal und braucht
keine Live-Kamera.
2. **Offline-Replay** — früher aufgenommene Bildsätze sollen mit dem *aktuellen*
Stand der Algorithmen neu ausgewertet werden (z. B. nach Verbesserungen an
`4b_revolute_angle.py` oder `5_pose_estimation.py`).
In beiden Fällen sind Bilder und Kalibrierungsdaten bereits vorhanden. Die API soll
diese entgegennehmen, die Pipeline identisch zum Live-Modus durchlaufen und die
Ergebnisdateien zurückgeben.
---
## Abgrenzung zum Live-Modus
| Aspekt | Live (`/api/homing/run`) | Offline (diese API) |
|---|---|---|
| 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 `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 |
Die Pipeline-Skripte (`1_detect_aruco_observations.py` bis `5_pose_estimation.py`)
werden **unverändert** aufgerufen — die Offline-API ist nur eine andere Eingangsschicht.
Neue Algorithmen erscheinen automatisch in beiden Pfaden.
---
## API-Beschreibung
### `POST /api/homing/run-offline`
**Content-Type:** `multipart/form-data`
#### Felder
| Feld | Typ | Pflicht | Beschreibung |
|---|---|---|---|
| `images` | Datei(en), `image/jpeg` | ja | Ein oder mehrere JPEG-Bilder. Dateiname **muss** `{cameraId}.jpg` sein (z. B. `cam0.jpg`, `cam1.jpg`). |
| `calibrations` | Datei(en), `application/octet-stream` | ja | Je eine `.npz`-Datei pro Kamera. Dateiname **muss** `{cameraId}_calibration.npz` sein (z. B. `cam0_calibration.npz`). |
| `robot` | Datei, `application/json` | ja | `robot.json` für diesen Lauf. Wird nur für diesen Aufruf verwendet, nicht dauerhaft gespeichert. |
| `refSet` | Text | nein | Optionaler Referenz-Set-Name für Script 2 (`--refSet`), z. B. `A0`. |
Das Pairing `{cameraId}.jpg``{cameraId}_calibration.npz` erfolgt rein über den
Dateinamen-Prefix vor dem ersten `_` bzw. vor `.jpg`. Kamera-IDs ohne passende NPZ
werden wie im Live-Modus übersprungen (Log-Eintrag, kein Fehler).
#### Antwort (Erfolg): `200 OK`
```json
{
"ok": true,
"runDir": "20260616_183042",
"state": {
"x": 312.4,
"y": 44.8,
"z": -12.1,
"a": 7.3,
"b": 0.0,
"c": null,
"e": null
},
"files": {
"aruco_marker_poses.json": { /* Inhalt */ },
"robot_state.json": { /* Inhalt */ },
"state_Arm1.json": { /* Inhalt */ },
"state_Ellbow.json": { /* Inhalt */ },
"state_Arm2.json": { /* Inhalt */ },
"state_Hand.json": { /* Inhalt */ },
"cam0_aruco_detection.json": { /* Inhalt */ },
"cam0_camera_pose.json": { /* Inhalt */ },
"cam1_aruco_detection.json": { /* Inhalt */ },
"cam1_camera_pose.json": { /* Inhalt */ }
},
"log": [
"▶ Homing-Run: 20260616_183042",
"▶ cam0: 14 Marker erkannt",
"…"
]
}
```
`state` entspricht dem `accumulated_state` aus der 4b-Kette (wenn erfolgreich) oder
den `movements`-Werten aus `robot_state.json` (wenn 4b abbricht und 5_pose_estimation
einspringt). `null`-Werte bedeuten: Gelenk nicht beobachtbar (wie im Live-Modus).
`files` enthält alle JSON-Ausgabedateien des Laufs als geparste Objekte — kein Base64,
da es sich ausschliesslich um JSON handelt.
#### Antwort (Fehler)
| Code | Bedeutung |
|---|---|
| `400` | Pflichtfelder fehlen, Dateinamen-Convention verletzt, robot.json ungültig |
| `422` | Pipeline abgebrochen: Script 3b konnte `aruco_marker_poses.json` nicht erzeugen (zu wenige Kameras o. ä.) |
| `500` | Unerwarteter Server-Fehler (Python nicht gefunden, Datei-I/O-Fehler, …) |
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 2060 s dauern. Client-Timeout auf **≥ 120 s** setzen
(curl: `--max-time 120`, requests: `timeout=120`, nginx/Caddy: `proxy_read_timeout 120s`).
---
## Datenfluss (detailliert)
```
multipart/form-data
images: cam0.jpg, cam1.jpg
calibrations: cam0_calibration.npz, cam1_calibration.npz
robot: robot_xxx.json
Server: Temp-Verzeichnis anlegen data/homing-offline/{timestamp}/
→ Bilder speichern: cam0.jpg, cam1.jpg
→ NPZ speichern: cam0_calibration.npz, cam1_calibration.npz
→ robot.json speichern: robot_run.json (nur für diesen Lauf)
▼ für jede Kamera:
1_detect_aruco_observations.py
-i cam0.jpg -npz cam0_calibration.npz -robot robot_run.json
-cameraId cam0 -outDir {runDir}
→ cam0_aruco_detection.json
2_estimate_camera_from_observations.py
-i cam0_aruco_detection.json -robot robot_run.json
-outDir {runDir} [--refSet A0]
→ cam0_camera_pose.json
▼ nach allen Kameras:
3b_corner_marker_poses.py
--evalDir {runDir} --robot robot_run.json
→ aruco_marker_poses.json
▼ X-Position schätzen (homingXEstimate)
x_mm ← estimateXFromMarkers(aruco_marker_poses.json, robot_run.json)
▼ 4b-Kette sequenziell Arm1→Ellbow→Arm2→Hand:
4b_revolute_angle.py
--robot robot_run.json --aruco aruco_marker_poses.json
--link Arm1 --x-mm {x_mm} --output state_Arm1.json
4b_revolute_angle.py --link Ellbow --from-state state_Arm1.json …
→ state_Arm1.json, state_Ellbow.json, state_Arm2.json, state_Hand.json
▼ (falls 4b-Kette vollständig)
accumulated_state → state in der Antwort
▼ (Verfeinerung / Fallback bei abgebrochener 4b-Kette)
5_pose_estimation.py
aruco_marker_poses.json -robot robot_run.json
--from-state state_Hand.json -out robot_state.json
→ robot_state.json
Antwort: { ok, state, files: { alle JSON-Dateien }, log }
```
Der Ablauf ist **identisch** zu `runHoming()` in `homingOrchestrator.js`, mit dem
einzigen Unterschied, dass `runBoardPipeline()` die Bilder nicht vom WebCam-Service
holt, sondern aus dem vorab befüllten `{runDir}`.
---
## Umsetzungsplan (abgeschlossen 2026-06-18)
### Schritt 1 — `runBoardPipelineOffline(runDir, send, opts)` (Kern)
**Was:** Neue Funktion in `server/server.js` oder `server/homingOrchestrator.js`, analog
zu `runBoardPipeline()`. Unterschied: statt `WEBCAM_URL` und `findLatestNpzForCamera()`
leitet sie die bereits-im-Verzeichnis-liegenden Dateien direkt weiter.
**Konkret:** Der einzige geänderte Teil in `runBoardPipeline()` ist die
Kamera-Schleife — statt Snapshot + NPZ-Suche wird einfach geprüft, ob
`{camId}.jpg` und `{camId}_calibration.npz` im `runDir` existieren. Danach
rufts Script 1 und 2 identisch auf.
**Risiko:** keines — die Script-Aufrufe selbst bleiben unverändert. Nur der Pfad zur
NPZ ändert sich von `data/calibration/{session}/` zu `{runDir}/`.
**Testbar:** Einzel-Test mit einer Kamera, geprüft, ob `cam0_aruco_detection.json`
korrekt erzeugt wird.
---
### Schritt 2 — Multipart-Upload-Handling
**Was:** Parsing von `multipart/form-data` für den neuen Endpoint. Express verarbeitet
`application/json` und `urlencoded` nativ, aber nicht `multipart`. Benötigt entweder:
- **Option A** — `multer` (npm-Package), etabliert, 0 Boilerplate
- **Option B** — manuell mit dem Node `busboy`-Parser (bereits in Node 18+, kein
Extra-Package)
Empfehlung: `multer` — es ist bereits `multer` in vielen Express-Projekten Standard,
klar dokumentiert, und `diskStorage` legt Dateien direkt in `{runDir}` ab. Vermeidet
Buffer-Accumulation für grosse NPZ-Dateien.
**Risiko:** Dateinamen-Sanitising — Multer übergibt den Originaldateinamen. Vor dem
Speichern: `path.basename()` und nur Zeichen `[a-zA-Z0-9_.-]` zulassen, sonst 400.
**Testbar:** curl-Upload-Test mit zwei kleinen Dummy-Dateien, geprüft, ob sie im
richtigen Verzeichnis landen.
---
### Schritt 3 — `runHomingOffline()` Orchestrator
**Was:** Analoge Funktion zu `runHoming()` in `homingOrchestrator.js`, jedoch:
- kein SSE-Stream, sondern Log-Akkumulation in ein Array
- `runBoardPipelineOffline()` statt `runBoardPipeline()`
- `robotJsonPath` zeigt auf die hochgeladene, temporäre `robot_run.json`
- Rückgabewert: `{ state, files, log }` statt SSE-Events
Die 4b-Kette und der 5_pose-Aufruf bleiben **unverändert** — gleiche Args, gleiche
Exit-Code-Logik, gleiche `accumulated_state`-Extraktion.
**Risiko:** 5_pose_estimation.py braucht `scipy`. In der lokalen Entwicklungsumgebung
muss `scipy` im Python-Umfeld vorhanden sein (in `docker-compose.yaml` ist es bereits
eingetragen, lokal muss `pip install scipy` geprüft werden).
---
### Schritt 4 — Endpoint `POST /api/homing/run-offline`
**Was:** Express-Route in `server.js`:
1. Multer-Middleware: Dateien in Temp-Verzeichnis
2. Validierung: mindestens 1 Kamera mit passendem `.jpg` + `.npz`-Pair; `robot`-Feld vorhanden
3. `runHomingOffline()` aufrufen
4. Alle JSON-Dateien aus `{runDir}` einlesen und in `files`-Objekt verpacken
5. Temp-Verzeichnis aufräumen (oder unter `data/homing-offline/` für Replay behalten?)
**Aufräumen vs. Behalten:** Empfehlung — Verzeichnis behalten, wie bei Live-Homing-Runs.
So ist Replay/Debug möglich. Ein periodischer Aufräum-Cron ist eine separate Aufgabe.
**Risiko:** Timeouts — bei vielen Kameras oder langsamer Maschine kann die Pipeline
2060 Sekunden dauern. Express hat kein Default-Timeout, aber ein vorgeschalteter
Reverse-Proxy (nginx, Caddy) schon. In der Doku festhalten: Client soll Timeout ≥ 120 s
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 `robotCachePath` (aus `robotConfig.js`)
und wird daher nie vom Live-Modus versehentlich gelesen. Kein Konflikt.
**Risiko:** keines — rein additive Datei in einem neuen Verzeichnis.
---
### Testplan
| Test | Was wird geprüft | Werkzeug |
|---|---|---|
| **Smoke-Test Upload** | Endpoint antwortet 200, `runDir` im Response vorhanden | curl mit zwei Dummy-JPEGs + NPZs + robot.json |
| **Kamera-Pairing** | `.jpg` ohne passende NPZ wird übersprungen (kein 500) | curl mit fehlendem NPZ |
| **Dateinamen-Sanitising** | `../../../evil.npz` → 400 | curl mit bösem Dateinamen |
| **Simulations-Roundtrip** | `appRobotRendering` rendert 10 Posen mit bekannten GT-Winkeln, API gibt `state` zurück, Abweichung < Toleranz | automatisiert aus `appRobotRendering`-Pipeline |
| **Identität Live vs. Offline** | Denselben Bildsatz einmal per Live-Run und einmal per Offline-API auswerten → `state`-Differenz ≈ 0 | manuell mit aufgezeichnetem Homing-Run |
| **Fallback-Pfad** | Script 3b schlägt fehl (< 2 Kameras) → 422 mit log | curl mit nur einem Bild |
| **Timeout-Robustheit** | 4b bricht ab → 5_pose_estimation greift ein, 200 wird trotzdem zurückgegeben | simulierter Abbruch durch fehlende Marker |
---
## Bekannte Risiken und Probleme
### Dateinamen-Convention ist implizit
Die Kamera-ID wird aus dem Dateinamen abgeleitet — kein explizites Metadaten-Feld.
Das funktioniert solange die Namenskonvention (`{cameraId}.jpg` / `{cameraId}_calibration.npz`)
eingehalten wird. Abweichungen (z. B. `frame_cam0_001.jpg`) führen zu einem ungematchten
Bild, das still übersprungen wird — kein Fehler, aber unerwartetes Verhalten.
**Mitigation:** Explizite Validierung und Fehlermeldung wenn kein Match gefunden wird.
Alternativ: Metadaten-Feld `cameraMappings: {"cam0": {"image": "frame_cam0_001.jpg", "npz": "cam0_calibration.npz"}}`.
Für den Simulations-Use-Case ist die einfache Konvention ausreichend.
### robot.json-Versionskonflikt
Simulation und Live-Homing teilen sich `robot.json` konzeptionell, aber die Datei
entwickelt sich weiter (Kalibrierung, neue Marker, geänderte Positionen). Eine veraltete
`robot.json` aus `appRobotRendering` kann zu systematisch falschen Posen führen, die
schwer von Algorithmus-Fehlern zu unterscheiden sind.
**Mitigation:** Im Simulations-Roundtrip-Test die `robot.json`-Version (z. B. Timestamp
im Dateinamen) protokollieren und mit dem API-Response abgleichen.
### Gleichzeitige Anfragen
Mehrere Offline-Runs gleichzeitig schreiben in separate `{timestamp}`-Verzeichnisse —
kein Konflikt. Aber: Python-Subprozesse multiplizieren sich. Bei parallelen Requests aus
der Simulations-Pipeline könnte die CPU-Last (scipy + ArUco) den Server blockieren.
**Mitigation (optional):** Request-Queue mit max. 12 parallelen Runs. Für den
Simulations-Use-Case wird sequenzieller Aufruf empfohlen.
### scipy-Abhängigkeit auf Deployment-Maschine
`5_pose_estimation.py` braucht `scipy`. In `docker-compose.yaml` ist es vorhanden.
Lokal (Entwicklung ohne Docker) muss `pip install scipy` sichergestellt sein, sonst
schlägt Schritt 5 stumm fehl (Exit 1, aber Schritt 4 hat bereits ein Ergebnis).
### Festplattenverbrauch
Jeder Offline-Run erzeugt ein Verzeichnis mit JPEGs + NPZs + ca. 10 JSON-Dateien.
Bei intensiver Simulations-Nutzung (100 Posen/Tag) kann das summieren.
**Mitigation:** TTL-basiertes Aufräumen als gelegentliche Wartungsaufgabe (kein Teil
dieser Implementierung).
---
## Offene Entscheidungen (getroffen)
- [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)
---
## Abhängigkeiten und Voraussetzungen
Vor der Implementierung müssen vorhanden sein:
- `multer` installiert (oder busboy-basierte Alternative entschieden)
- `scipy` im Python-Environment verfügbar (lokal + Docker)
- `scripts/5_pose_estimation.py` + `scripts/robot_fk.py` im Repo (beide vorhanden, ✅)
- `homingOrchestrator.js` für Orientierung (vorhanden, ✅)
- Test-Bilder + Test-NPZs für automatisierten Smoke-Test (aus `test/homing/` oder
`test/y-axis-finder-examples/`; die NPZs müssen dazu noch als Testfixtures bereitgestellt werden)
---
## Verweise
- [`Homing.md`](Homing.md) — Gesamtüberblick Homing-Ablauf
- [`Homing_0_Camera.md`](Homing_0_Camera.md) — Schritte 13b (Board-Pipeline)
- [`Homing_1_StepByStep.md`](Homing_1_StepByStep.md) — 4b-Kette (Gelenkwinkel-Schätzung)
- [`Homing_5_Pose.md`](Homing_5_Pose.md) — 5_pose_estimation.py (Bundle-Adjustment)
- `server/server.js` — bestehende Endpoints (`/api/homing/run`, `runBoardPipeline()`)
- `server/homingOrchestrator.js``runHoming()`, Vorlage für `runHomingOffline()`

232
doc/multilingual.md Normal file
View File

@@ -0,0 +1,232 @@
# Mehrsprachigkeit (DE/EN) Vorschlag
## Ausgangslage
Die App ist heute komplett deutschsprachig, und zwar an drei verschiedenen Stellen, die unterschiedlich behandelt werden müssen:
1. **Statisches HTML-Markup** Labels, Überschriften, `placeholder`/`title`-Attribute direkt in
`public/*.html` (`index.html`, `homing.html`, `calibration*.html`, …).
Beispiel: `<h2>Aktionen</h2>`, `<button title="Erst Homing ausführen">`.
2. **Dynamisch erzeugte Strings in Client-JS** Template-Literals in `public/client.js`,
`public/homing.js`, `public/calibration.js` etc., z. B.
`txt.textContent = text || \`Schritt ${step} / ${total}\`;` oder
`result.innerHTML = '<span>⚠ Bitte Set auswählen, das verschoben werden soll.</span>'`.
3. **Server-seitige Strings**, die als JSON an den Client durchgereicht und dort angezeigt werden,
z. B. `res.status(400).json({ error: '"y" und "z" müssen Zahlen sein.' })` in
[server/server.js](../server/server.js).
Es gibt kein Build-Tool (kein Webpack/Vite/React) alles ist Vanilla HTML/JS, das Express
als statische Dateien ausliefert ([server/server.js](../server/server.js)). Jede Lösung sollte
also **ohne Build-Schritt** funktionieren und sich inkrementell einführen lassen (nicht „big bang“,
sondern Seite für Seite).
## Zielbild
- Sprache wird primär aus `navigator.language` (Browser-Einstellung) bestimmt, mit manuellem
Override (Dropdown/Flag-Icon in der Topbar) und Persistenz in `localStorage`.
- Fallback-Sprache ist Deutsch (aktueller Stand), zweite Sprache Englisch.
- Übersetzungen liegen in einfachen JSON-Dateien, kein zusätzliches npm-Paket nötig
(es reicht ein paar Dutzend Zeilen eigener Lade-/Ersetzungslogik).
- Server-Fehlermeldungen werden nicht als fertiger Text geschickt, sondern als
**Fehlercode**, den der Client anhand der aktuellen Sprache übersetzt.
## 1. Struktur der Sprachdateien
```
public/
i18n/
de.json
en.json
i18n.js <- kleine Lade-/Helper-Bibliothek, von jeder Seite eingebunden
```
`de.json` (Auszug, Keys gruppiert nach Seite/Bereich, „flach mit Punktnotation“ ist für diese
Größenordnung einfacher zu pflegen als tief verschachteltes JSON):
```json
{
"common.back": "← Zurück",
"common.error": "Fehler",
"index.actions.title": "Aktionen",
"index.actions.runHoming": "📷 Foto & Homing berechnen",
"index.actions.sendToRobot": "✅ An Roboter senden",
"index.actions.sendDisabledTitle": "Erst Homing ausführen",
"index.status.idle": "○ Warte",
"index.status.running": "● Läuft …",
"calibration.move.selectSetWarning": "⚠ Bitte Set auswählen, das verschoben werden soll.",
"calibration.move.sameSetWarning": "⚠ \"Bleibt\" und \"verschoben\" dürfen nicht dasselbe Set sein.",
"server.error.yzMustBeNumbers": "\"y\" und \"z\" müssen Zahlen sein.",
"server.error.invalidAxisPayload": "Ungültige Nutzlast: axis.dir und axis.referencePoint erwartet"
}
```
`en.json` hat exakt dieselben Keys mit englischen Werten. Das ist die einzige Stelle, an der eine
fehlende Übersetzung sofort auffällt: Key in `de.json` ohne Pendant in `en.json` → Fallback auf
Deutsch (siehe unten), kein Crash.
**Warum nicht ein JSON pro HTML-Seite?** Weil viele Strings (Status-Badges, Fehlermeldungen,
Buttons wie „Zurück“) seitenübergreifend wiederverwendet werden. Eine flache `common.*`-Gruppe plus
eine Gruppe pro Seite (`index.*`, `homing.*`, `calibration.*`) hält das überschaubar, ohne dass man
Strings doppelt pflegt.
## 2. Statisches HTML markieren
Jedes übersetzbare Element bekommt ein `data-i18n`-Attribut mit dem Key. Für Attribute (z. B.
`title`, `placeholder`) gibt es ein zusätzliches `data-i18n-attr`:
```html
<h2 data-i18n="index.actions.title">Aktionen</h2>
<button id="btn-homing-send" disabled
data-i18n="index.actions.sendToRobot"
data-i18n-attr-title="index.actions.sendDisabledTitle"
title="Erst Homing ausführen">
✅ An Roboter senden
</button>
```
Wichtig: Der **deutsche Text bleibt im HTML stehen** (als Fallback/Default und damit die Seite ohne
JS oder bei Ladefehler nicht leer ist). `i18n.js` ersetzt den Inhalt nur, wenn die Zielsprache nicht
Deutsch ist bzw. wenn ein Override gesetzt wurde. Das macht die Migration risikoarm: Man kann Seite
für Seite Attribute ergänzen, ohne dass etwas kaputtgeht, falls eine Seite noch nicht migriert ist.
## 3. `i18n.js` minimale Laufzeit-Bibliothek
Kernfunktionen, ca. 6080 Zeilen, kein Tooling nötig:
```js
// public/i18n.js
const SUPPORTED = ['de', 'en'];
const FALLBACK = 'de';
function detectLang() {
const stored = localStorage.getItem('lang');
if (stored && SUPPORTED.includes(stored)) return stored;
const nav = (navigator.language || FALLBACK).slice(0, 2).toLowerCase();
return SUPPORTED.includes(nav) ? nav : FALLBACK;
}
let dict = {};
let fallbackDict = {};
export async function initI18n() {
const lang = detectLang();
[dict, fallbackDict] = await Promise.all([
fetch(`/i18n/${lang}.json`).then(r => r.json()),
lang === FALLBACK ? Promise.resolve({}) : fetch(`/i18n/${FALLBACK}.json`).then(r => r.json()),
]);
document.documentElement.lang = lang;
applyToDom();
return lang;
}
export function t(key, vars) {
let str = dict[key] ?? fallbackDict[key] ?? key; // Key selbst als letzter Fallback -> sichtbar im UI statt "undefined"
if (vars) for (const [k, v] of Object.entries(vars)) str = str.replaceAll(`{${k}}`, v);
return str;
}
function applyToDom(root = document) {
root.querySelectorAll('[data-i18n]').forEach(el => { el.textContent = t(el.dataset.i18n); });
root.querySelectorAll('[data-i18n-attr-title]').forEach(el => { el.title = t(el.dataset.i18nAttrTitle); });
root.querySelectorAll('[data-i18n-attr-placeholder]').forEach(el => { el.placeholder = t(el.dataset.i18nAttrPlaceholder); });
}
export async function setLang(lang) {
localStorage.setItem('lang', lang);
location.reload(); // einfach & robust, kein dynamisches Re-Rendering nötig bei dieser App-Größe
}
```
Einbindung pro Seite (vor dem bestehenden Seiten-Script):
```html
<script type="module" src="/i18n.js"></script>
<script type="module">
import { initI18n } from '/i18n.js';
await initI18n();
</script>
<script src="/client.js"></script>
```
Da `client.js`/`homing.js`/`calibration.js` aktuell keine ES-Module sind, reicht es, `initI18n()`
in einem kleinen Inline-`<script type="module">` vor dem normalen `<script>`-Tag auszuführen
kein Umbau der bestehenden Dateien zu Modulen nötig (DOM ist zu diesem Zeitpunkt geparst, weil
Module standardmäßig wie `defer` laufen).
## 4. Dynamische Strings in Client-JS
Für Strings, die per JS erzeugt werden (Status-Badges, Fehlermeldungen, generierte Tabellen),
wird `t()` global verfügbar gemacht (z. B. `window.t = t;` am Ende von `i18n.js`) und an den
Stellen, an denen heute literal Deutsch steht, durch einen Key-Aufruf ersetzt:
```js
// vorher (public/homing.js)
if (txt) txt.textContent = text || `Schritt ${step} / ${total}`;
// nachher
if (txt) txt.textContent = text || t('homing.progress.step', { step, total });
// "homing.progress.step": "Schritt {step} / {total}" / "Step {step} of {total}"
```
```js
// vorher (public/calibration.js)
result.innerHTML = '<span style="color:#f87171">⚠ Bitte Set auswählen, das verschoben werden soll.</span>';
// nachher
result.innerHTML = `<span style="color:#f87171">⚠ ${t('calibration.move.selectSetWarning')}</span>`;
```
Das ist die aufwändigste Stelle (laut grep aktuell **deutlich über 1000 Treffer** für deutsche
Wortfragmente über alle `public/*.js`-Dateien), aber rein mechanisch und gut inkrementell machbar:
eine Datei nach der anderen, jeweils committed und getestet.
## 5. Server-seitige Strings (Fehlermeldungen)
`server/server.js` schickt aktuell fertige deutsche Sätze im JSON zurück, z. B.
```js
return res.status(400).json({ error: '"y" und "z" müssen Zahlen sein.' });
```
Diese Strings landen direkt im UI (`infoEl.textContent = data.error` o. ä.) der Server kann aber
die Browsersprache des Clients nicht zuverlässig kennen (und sollte sie auch nicht raten müssen).
Saubere Lösung: Server schickt einen **Code**, Client übersetzt:
```js
// server/server.js
return res.status(400).json({ errorCode: 'yzMustBeNumbers' });
```
```js
// Client, z. B. client.js / homing.js
const msg = data.errorCode ? t(`server.error.${data.errorCode}`) : (data.error ?? t('common.unknownError'));
```
Migration ist optional und kann zuletzt erfolgen bis dahin einfach `error` (deutsch) weiter
durchreichen und im Client mit `data.error ?? t(...)` abfangen, damit nichts bricht, während man
Stück für Stück auf `errorCode` umstellt.
## 6. Sprachumschalter in der UI
Kleines Dropdown/Flag-Toggle in der Topbar (`calib-topbar` existiert schon in mehreren Seiten),
das `setLang('en')` / `setLang('de')` aufruft. Reicht als erster Schritt; ein automatisches
Re-Rendering ohne Reload ist bei dieser Seitenanzahl unnötiger Aufwand.
## 7. Reihenfolge der Umsetzung (Vorschlag)
1. `public/i18n/de.json` aus den **aktuellen** deutschen Strings extrahieren (1:1, keine
Textänderung) + `i18n.js` Lib bauen. Keine sichtbare Änderung im Verhalten.
2. `public/i18n/en.json` befüllen (Übersetzung).
3. Seite für Seite umstellen, anzufangen bei `index.html` + `client.js` (am meisten genutzt),
danach `homing.html`/`homing.js`, danach `calibration*.html`/`calibration.js`.
4. Sprachumschalter in der Topbar ergänzen.
5. Server-Fehlermeldungen optional zuletzt auf `errorCode` umstellen.
## Warum keine Library wie i18next?
Für die Größe dieser App (kein Build-Schritt, ~5 HTML-Seiten, kein Framework) wäre i18next/
FormatJS deutlich mehr Gewicht (zusätzliche Dependency, Build-Integration, ICU-Syntax) als Nutzen.
Die ~80 Zeilen oben decken Pluralisierung zwar nicht ab, aber dafür gibt es in der App aktuell
keinen Bedarf (keine zählerabhängigen Sätze wie „1 Marker“ vs. „3 Marker“ falls das später
gebraucht wird, reicht eine simple `count === 1 ? key + '.one' : key + '.many'`-Konvention).

View File

@@ -11,13 +11,22 @@ services:
- PYTHON_BIN=python3
- WEBCAM_URL=http://host.docker.internal:8444
- BODYTRACKER_URL=http://host.docker.internal:8446
# Driver-WebSocket (Plain-Text-G-Code, self-signed). Homing sendet G92
# hierhin (= Motorposition setzen ohne Bewegung).
# WICHTIG: Der Input-WS lauscht container-intern auf 2095 (startRobot.js:
# PORT||2095) und ist NICHT per host-port veröffentlicht. Die Driver-Ports
# 2081 (Node --inspect) und 2098 (Info/Status) sind NICHT dieser WS.
# Variante A (robust): beide Container im selben Netz (approbots), per Name:
- DRIVER_WS_URL=wss://appRobot_Driver:2095
# Variante B (über Host): im Driver `- "2095:2095"` veröffentlichen, dann
# DRIVER_WS_URL=wss://host.docker.internal:2095
extra_hosts:
# Macht host.docker.internal auf Linux verfügbar (Standard auf macOS/Windows)
- "host.docker.internal:host-gateway"
ports:
- "2093:2093"
command: >
/bin/bash -lc "apt-get update -qq && apt-get install -y --no-install-recommends python3-pip && pip3 install --quiet --no-cache-dir opencv-python-headless numpy && npm ci || npm install && node server/server.js"
/bin/bash -lc "apt-get update -qq && apt-get install -y --no-install-recommends python3-pip && pip3 install --quiet --no-cache-dir opencv-python-headless numpy scipy && npm ci || npm install && node server/server.js"
networks:
- approbots
restart: unless-stopped

98
package-lock.json generated
View File

@@ -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",

View File

@@ -15,7 +15,9 @@
},
"dependencies": {
"dotenv": "^16.4.5",
"express": "^4.19.2"
"express": "^4.19.2",
"multer": "^2.2.0",
"ws": "^8.20.0"
},
"devDependencies": {
"jest": "^29.7.0",

View File

@@ -204,11 +204,28 @@ import { OrbitControls } from 'three/addons/controls/OrbitControls.js';
const S = 1 / 1000; // mm → m
// ── Modus-Erkennung ──────────────────────────────────────────────────────────
const _urlParams = new URLSearchParams(window.location.search);
const IS_HOMING = _urlParams.get('mode') === 'homing';
if (IS_HOMING) {
document.getElementById('run-bar').style.display = 'none';
}
// robot (x=right, y=backward, z=up) → Three.js (x=right, y=up, z=toward viewer)
function r2v(rx, ry, rz) { return new THREE.Vector3(rx * S, rz * S, -ry * S); }
function r2vArr([rx, ry, rz]) { return r2v(rx, ry, rz); }
function r2dir([dx, dy, dz]) { return new THREE.Vector3(dx, dz, -dy).normalize(); }
/**
* True wenn `arr` ein nutzbares [x,y,z] ist (z. B. position_mm). Marker, die
* 3b nicht triangulieren konnte (z. B. nur 1 Kamera), haben dieses Feld nicht
* — solche Marker werden an allen Stellen, die diese Funktion nutzen, einfach
* ignoriert statt einen Crash auszulösen.
*/
function hasXYZ(arr) {
return Array.isArray(arr) && arr.length >= 3 && arr.slice(0, 3).every(Number.isFinite);
}
// ── Renderer ──────────────────────────────────────────────────────────────────
const canvas = document.getElementById('cv');
const renderer = new THREE.WebGLRenderer({ canvas, antialias: true });
@@ -243,13 +260,201 @@ const gCompare = new THREE.Group(); // Pos B Marker (nur fremd, orange)
const gCompareLines = new THREE.Group(); // Verbindungslinien Pos A↔Pos B
const gPositionC = new THREE.Group(); // Pos C Marker (nur fremd, cyan)
const gYAxis = new THREE.Group(); // Y-Achse Visualisierung (Kreismittelpunkte, Achse)
scene.add(gPaper, gMarkers, gMeasured, gCameras, gCompare, gCompareLines, gPositionC, gYAxis);
const gSkeleton = new THREE.Group(); // Roboter-Skeleton (FK, nur im Homing-Mode)
const gArmMarkers = new THREE.Group(); // Arm-Marker Modellpositionen + Fehlerlinien (FK)
scene.add(gPaper, gMarkers, gMeasured, gCameras, gCompare, gCompareLines, gPositionC, gYAxis, gSkeleton, gArmMarkers);
const LINK_COLORS = {
Board: 0x8b6528, Base: 0x888888,
Arm1: 0x3355cc, Ellbow: 0xaaaaaa, Arm2: 0xddcc88,
Hand: 0xcc8833, Palm: 0xcc3333,
FingerA: 0x33aa33, FingerB: 0x33aa33,
};
// ── Zustand für Positionen ────────────────────────────────────────────────────
let _primaryFremdMarkers = []; // Pos A [{marker_id, position_mm, num_cameras}]
let _compareFremdMarkers = []; // Pos B [{marker_id, position_mm, num_cameras}]
let _positionCFremdMarkers = []; // Pos C [{marker_id, position_mm, num_cameras}]
// ── Homing-Mode Zustand ───────────────────────────────────────────────────────
let _currentRobot = null; // robot.json nach loadData()
let _homingAngles = null; // { x, y, z, a, b, c, e } nach Homing-Run
let _measuredMarkers = null; // measuredMarkers aus letztem buildScene-Aufruf
// ── Roboter-Skeleton (Forward Kinematics) ─────────────────────────────────────
/**
* Zeichnet das Roboter-Skeleton mit vorwärts-kinematischer Berechnung.
* Nutzt skeleton.from/to aus robot.json (in lokalem Link-Frame).
* angles: { x_mm→x, y_deg→y, z_deg→z, a_deg→a, b_deg→b, c_deg→c, e_mm→e }
*/
function buildSkeletonFK(robot, angles) {
clearGroup(gSkeleton);
clearGroup(gArmMarkers);
if (!robot?.links) return;
const links = robot.links;
const order = ['Base', 'Arm1', 'Ellbow', 'Arm2', 'Hand', 'Palm', 'FingerA', 'FingerB'];
// frames: Link-Name → Matrix4 (link-lokal → Welt)
const frames = { Board: new THREE.Matrix4() }; // Board = Welt-Ursprung
for (const linkName of order) {
const link = links[linkName];
if (!link?.jointToParent) continue;
const parentName = link.parent ?? 'Board';
const parentFrame = frames[parentName] ?? new THREE.Matrix4();
const jtp = link.jointToParent;
// 1. Translation zum Gelenk-Ursprung (im Parent-Frame)
const [ox, oy, oz] = jtp.origin ?? [0, 0, 0];
const T_origin = new THREE.Matrix4().makeTranslation(ox * S, oz * S, -oy * S);
// 2. Gelenk-Transformation (Rotation/Translation je nach Typ)
const varName = jtp.variable;
const q = angles?.[varName] ?? 0;
let T_joint = new THREE.Matrix4(); // Einheitsmatrix bei q=0
if (jtp.type === 'revolute') {
const [ax, ay, az] = jtp.axis ?? [0, 1, 0];
// robot (x,y,z) → Three.js (x, z, -y)
const axisV = new THREE.Vector3(ax, az, -ay).normalize();
T_joint.makeRotationAxis(axisV, q * Math.PI / 180);
} else if (jtp.type === 'linear') {
const [ax, ay, az] = jtp.axis ?? [1, 0, 0];
T_joint.makeTranslation(ax * q * S, az * q * S, -ay * q * S);
}
// Child-Frame = Parent-Frame × T_origin × T_joint
const childFrame = parentFrame.clone().multiply(T_origin).multiply(T_joint);
frames[linkName] = childFrame;
// 3. Skeleton-Segment zeichnen
const skel = link.skeleton;
if (skel?.from && skel?.to) {
const [fx, fy, fz] = skel.from;
// Endpunkt folgt dem (kalibrierten) jointToParent.origin des einzigen
// Kind-Links, statt der statischen skeleton.to-Koordinate. Dadurch wandert
// z.B. die Basis-Linie mit, wenn der Arm1-Pivot in Y/Z kalibriert wurde.
let toCoord = skel.to;
const childNames = order.filter(cn => links[cn]?.parent === linkName);
if (childNames.length === 1) {
const childOrigin = links[childNames[0]]?.jointToParent?.origin;
if (Array.isArray(childOrigin) && childOrigin.length >= 3) toCoord = childOrigin;
}
const [tx, ty, tz] = toCoord;
const fromW = new THREE.Vector3(fx * S, fz * S, -fy * S).applyMatrix4(childFrame);
const toW = new THREE.Vector3(tx * S, tz * S, -ty * S).applyMatrix4(childFrame);
const [cr, cg, cb] = skel.color ?? [0.8, 0.2, 0.2];
const color = new THREE.Color(cr, cg, cb);
const rad = Math.max((skel.radius ?? 4) * S, 0.004);
gSkeleton.add(makeLine(fromW, toW, color, 0.9));
gSkeleton.add(makeSphere(fromW, rad, color));
gSkeleton.add(makeSphere(toW, rad, color));
// Gelenk-Mittelpunkt (Welt-Ursprung des Link-Frames)
const jointW = new THREE.Vector3().applyMatrix4(childFrame);
gSkeleton.add(makeSphere(jointW, 0.004, 0xc8cdd8));
}
// 4. Arm-Marker zeichnen (Modellposition via FK, orientiertes Quadrat + spin)
if (link.markers?.length > 0) {
const col = LINK_COLORS[linkName] ?? 0xffffff;
for (const m of link.markers) {
if (!m.position) continue;
const [lx, ly, lz] = m.position;
const posWorld = new THREE.Vector3(lx * S, lz * S, -ly * S).applyMatrix4(childFrame);
const markerSizeM = (m.size ?? 25) * S;
const [nx, ny, nz] = m.normal ?? [0, 0, 1];
// 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), 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(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
// Marker-Normale inkl. Link-Roll und Spin.
const markerMesh = makeMarkerSquareQuat(posWorld, qMarkerW, markerSizeM, col);
gArmMarkers.add(markerMesh);
gArmMarkers.add(makeSphere(posWorld, 0.0006, col));
// P3b (Modell-Seite): Orientierungszeiger zur Ecke 0 (top-left bei spin=0)
const ptrDir = new THREE.Vector3(1, 1, 0).normalize().applyQuaternion(qMarkerW);
const corner0W = posWorld.clone().add(ptrDir.multiplyScalar(markerSizeM * Math.SQRT1_2));
gArmMarkers.add(makeLine(posWorld, corner0W, col, 0.9));
gArmMarkers.add(makeSphere(corner0W, 0.0008, col));
// Modell-Normale (Pfeil, gleiche Link-Farbe, halbtransparent)
const modelNormalEnd = posWorld.clone().add(normalW.clone().multiplyScalar(markerSizeM * 1.5));
gArmMarkers.add(makeLine(posWorld, modelNormalEnd, col, 0.5));
gArmMarkers.add(makeSphere(modelNormalEnd, 0.0005, col));
}
}
}
// 5. Fehlerlinien: Modell-Marker → gemessene Position (aus 3b)
if (_measuredMarkers?.markers?.length > 0) {
// Fallback-Lookup: marker_id → Link-Name, falls obs.link fehlt oder 'Board' ist
const markerIdToLink = {};
for (const [lname, ldata] of Object.entries(links)) {
if (lname === 'Board') continue;
for (const m of (ldata.markers ?? [])) markerIdToLink[m.id] = lname;
}
for (const obs of _measuredMarkers.markers) {
if (!hasXYZ(obs.position_mm)) continue; // z.B. 1-Kamera-Marker, nicht trianguliert
const obsLink = (obs.link && obs.link !== 'Board')
? obs.link
: markerIdToLink[obs.marker_id];
if (!obsLink) continue;
const ldata = links[obsLink];
if (!ldata || !frames[obsLink]) continue;
const modelM = ldata.markers?.find(mm => mm.id === obs.marker_id);
if (!modelM?.position) continue;
const [lx, ly, lz] = modelM.position;
const modelPosW = new THREE.Vector3(lx * S, lz * S, -ly * S).applyMatrix4(frames[obsLink]);
const obsPosW = r2vArr(obs.position_mm);
const obsCol = LINK_COLORS[obsLink] ?? 0x3b82f6;
gArmMarkers.add(makeLine(modelPosW, obsPosW, 0xff8800, 0.85));
gArmMarkers.add(makeSphere(obsPosW, 0.007, obsCol));
// Beobachtungs-Ecke 0: Orientierungszeiger für Spin-Vergleich
// corners_m sind Weltkoordinaten in Metern → direkt in Three.js (Y↑=Z, Z↑=-Y)
const c0 = obs.corners_m?.[0];
if (c0) {
const obsCorner0W = new THREE.Vector3(c0[0], c0[2], -c0[1]);
gArmMarkers.add(makeLine(obsPosW, obsCorner0W, obsCol, 0.85));
gArmMarkers.add(makeSphere(obsCorner0W, 0.0012, obsCol));
}
// Beobachtungs-Normale (weißer Pfeil)
if (obs.normal) {
const [on0, on1, on2] = obs.normal;
const obsNormalW = new THREE.Vector3(on0, on2, -on1).normalize();
const arrowLen = (modelM.size ?? 25) * S * 1.5;
const obsNormalEnd = obsPosW.clone().add(obsNormalW.multiplyScalar(arrowLen));
gArmMarkers.add(makeLine(obsPosW, obsNormalEnd, 0xffffff, 0.55));
gArmMarkers.add(makeSphere(obsNormalEnd, 0.001, 0xffffff));
}
}
}
}
// ── Viewer-interner Logger ────────────────────────────────────────────────────
function vlog(msg, kind = '') {
const el = document.getElementById('viewer-log');
@@ -282,6 +487,18 @@ function makeMarkerSquare(pos, size, color) {
return m;
}
// Quadrat mit voll vorgegebener Orientierung (Quaternion). PlaneGeometry hat
// nativ die +Z-Normale; das Quaternion dreht den Marker komplett (Normale, Roll,
// Spin) nötig, damit der Roll bei achs-paralleler Normale nicht verloren geht.
function makeMarkerSquareQuat(pos, quat, size, color) {
const geo = new THREE.PlaneGeometry(size, size);
const mat = new THREE.MeshPhongMaterial({ color, side: THREE.DoubleSide, transparent: true, opacity: 0.85 });
const mesh = new THREE.Mesh(geo, mat);
mesh.position.copy(pos);
mesh.quaternion.copy(quat);
return mesh;
}
function makeEdgeBorder(pos, size, color) {
const geo = new THREE.EdgesGeometry(new THREE.PlaneGeometry(size, size));
const mat = new THREE.LineBasicMaterial({ color, transparent: true, opacity: 0.75 });
@@ -324,6 +541,7 @@ function buildScene(data) {
clearGroup(gPaper); clearGroup(gMarkers); clearGroup(gMeasured); clearGroup(gCameras);
const { robot, detections, cameraPoses, measuredMarkers } = data;
_measuredMarkers = measuredMarkers ?? null;
// Alle erkannten Marker-IDs (über alle Kameras)
const detectedIds = new Set();
@@ -399,6 +617,7 @@ function buildScene(data) {
);
for (const m of a0markers) {
if (!hasXYZ(m.position_mm)) continue; // z.B. 1-Kamera-Marker, nicht trianguliert
nTriangulated++;
// Kein künstlicher Offset Kugelmittelpunkt exakt an triangulierter Position
const mpos = r2vArr(m.position_mm);
@@ -422,6 +641,7 @@ function buildScene(data) {
!boardMarkers.some(bm => bm.id === m.marker_id)
);
for (const m of unknownTriangulated) {
if (!hasXYZ(m.position_mm)) continue; // z.B. 1-Kamera-Marker, nicht trianguliert
nUnknown++;
const mpos = r2vArr(m.position_mm);
gMeasured.add(makeSphere(mpos, 0.0055, 0x3b82f6));
@@ -530,9 +750,9 @@ function buildTable(data) {
let dist = null, dz = null, edge = null;
let state = 'none'; // 'tri', '1cam', 'unk'
if (meas) {
if (meas && hasXYZ(meas.position_mm)) {
[x, y, z] = meas.position_mm;
[nx, ny, nz] = meas.normal;
if (Array.isArray(meas.normal)) [nx, ny, nz] = meas.normal;
edge = meas.edge_length_mm;
state = model ? 'tri' : 'unk';
@@ -542,7 +762,10 @@ function buildTable(data) {
dist = Math.sqrt(dx*dx + dy*dy + ddz*ddz);
dz = ddz;
}
} else if (cameras.length > 0) {
} else if (cameras.length > 0 || meas) {
// meas ohne position_mm (z.B. 1-Kamera-Marker, noch nicht trianguliert)
// zählt wie "gesehen, aber nicht trianguliert" — gleicher Stand wie
// cameras.length>0, nur eben aus 3b statt aus den rohen Detektionen.
state = '1cam';
}
@@ -810,14 +1033,27 @@ function computeAndShowYAxis() {
// ── Daten laden ───────────────────────────────────────────────────────────────
/** Haupt-Run laden (Basis-Dropdown). Ohne Selektion → neuester Run. */
async function loadData() {
/**
* Haupt-Run laden.
* Im Homing-Mode: lädt spezifischen Run per runDir-Parameter (kein Dropdown).
* @param {string|null} specificRunDir Timestamp des Runs (nur im Homing-Mode genutzt)
*/
async function loadData(specificRunDir = null) {
const statusEl = document.getElementById('status');
statusEl.textContent = 'Laden …';
const selRun = document.getElementById('sel-run-primary')?.value ?? '';
const url = selRun
? `/api/board/latest?run=${encodeURIComponent(selRun)}`
: '/api/board/latest';
let url;
if (IS_HOMING) {
url = specificRunDir
? `/api/board/latest?from=homing&run=${encodeURIComponent(specificRunDir)}`
: '/api/board/latest?from=homing';
} else {
const selRun = document.getElementById('sel-run-primary')?.value ?? '';
url = selRun
? `/api/board/latest?run=${encodeURIComponent(selRun)}`
: '/api/board/latest';
}
try {
const r = await fetch(url);
if (!r.ok) throw new Error(`HTTP ${r.status}`);
@@ -826,19 +1062,28 @@ async function loadData() {
statusEl.textContent = 'Kein Board-Run vorhanden.';
document.getElementById('stats').textContent = '';
clearGroup(gPaper); clearGroup(gMarkers); clearGroup(gMeasured); clearGroup(gCameras);
if (IS_HOMING) clearGroup(gSkeleton);
return;
}
buildScene(data);
buildTable(data);
// Fremd-Marker für Verbindungslinien merken (Marker, die nicht in Board-Link stehen)
const bIds = new Set((data.robot?.links?.Board?.markers ?? []).map(m => m.id));
_primaryFremdMarkers = (data.measuredMarkers?.markers ?? []).filter(m => !bIds.has(m.marker_id));
_primaryFremdMarkers = (data.measuredMarkers?.markers ?? [])
.filter(m => !bIds.has(m.marker_id) && hasXYZ(m.position_mm));
const measTotal = data.measuredMarkers?.markers?.length ?? 0;
vlog(`Basis: run=${data.runDir} gesamt=${measTotal} fremd=${_primaryFremdMarkers.length} boardIDs=${bIds.size}` +
(_primaryFremdMarkers.length ? ` (${_primaryFremdMarkers.map(m => m.marker_id).join(' ')})` : ''));
buildCompareLines();
const robotLabel = data.robotFile ? ` • Robot: ${data.robotFile}` : '';
statusEl.textContent = `Run: ${data.runDir}${robotLabel}${new Date().toLocaleTimeString('de-CH')}`;
// Skeleton im Homing-Mode
if (IS_HOMING && data.robot) {
_currentRobot = data.robot;
const angles = _homingAngles ?? data.robot.defaultPosition ?? {};
buildSkeletonFK(data.robot, angles);
}
} catch (err) {
statusEl.textContent = `Fehler: ${err.message ?? err}`;
}
@@ -863,7 +1108,7 @@ async function loadCompareData() {
// Board-Marker-IDs aus Robot.json dieses Runs
const boardIds = new Set((data.robot?.links?.Board?.markers ?? []).map(m => m.id));
for (const m of markers) {
if (!boardIds.has(m.marker_id)) {
if (!boardIds.has(m.marker_id) && hasXYZ(m.position_mm)) {
_compareFremdMarkers.push(m); // für Linien
gCompare.add(makeSphere(r2vArr(m.position_mm), 0.006, 0xf97316)); // orange Kugel
}
@@ -896,7 +1141,7 @@ async function loadPositionC() {
const markers = data.measuredMarkers?.markers ?? [];
const boardIds = new Set((data.robot?.links?.Board?.markers ?? []).map(m => m.id));
for (const m of markers) {
if (!boardIds.has(m.marker_id)) {
if (!boardIds.has(m.marker_id) && hasXYZ(m.position_mm)) {
_positionCFremdMarkers.push(m);
gPositionC.add(makeSphere(r2vArr(m.position_mm), 0.006, 0x22d3ee)); // cyan
}
@@ -965,13 +1210,30 @@ async function initRunSelectors() {
/** Vollständige Initialisierung: Selektoren → Pos A → Pos B → Pos C (sequenziell!) */
async function initAll() {
if (IS_HOMING) {
// Homing-Mode: nur neuester Run laden, kein Dropdown, kein Vergleich
await loadData();
return;
}
await initRunSelectors();
await loadData(); // setzt _primaryFremdMarkers
await loadCompareData(); // setzt _compareFremdMarkers + baut Linien + Y-Achse (no-op)
await loadPositionC(); // setzt _positionCFremdMarkers + berechnet Y-Achse
}
initAll();
if (IS_HOMING) {
// Robot-Modell mit Defaultposition sofort laden (kein Board-Run nötig)
fetch('/api/robot').then(r => r.ok ? r.json() : null).then(robot => {
if (!robot) return;
_currentRobot = robot;
buildSkeletonFK(robot, robot.defaultPosition ?? {});
document.getElementById('status').textContent = '→ Homing-Run starten …';
}).catch(() => {
document.getElementById('status').textContent = '→ Homing-Run starten …';
});
} else {
initAll();
}
document.getElementById('btnReload').addEventListener('click', initAll);
document.getElementById('sel-run-primary')?.addEventListener('change', async () => {
await loadData();
@@ -984,6 +1246,16 @@ document.getElementById('sel-run-compare')?.addEventListener('change', async ()
document.getElementById('sel-run-c')?.addEventListener('change', loadPositionC);
window.addEventListener('message', async (e) => {
if (e.data?.type === 'reload') await initAll();
if (e.data?.type === 'load-homing-run' && IS_HOMING) {
await loadData(e.data.runDir);
}
if (e.data?.type === 'homing-state' && IS_HOMING) {
// Gefundene Winkel über Default-Position mergen, damit noch nicht erkannte
// Gelenke nicht auf 0 zusammenfallen, sondern sinnvoll stehen bleiben.
const base = _currentRobot?.defaultPosition ?? {};
_homingAngles = { ...base, ...e.data.state };
if (_currentRobot) buildSkeletonFK(_currentRobot, _homingAngles);
}
});
// ── Resize & Render-Loop ──────────────────────────────────────────────────────

View File

@@ -24,6 +24,7 @@
<button class="tab-btn" data-tab="board" data-src="/calibration_board.html">Board</button>
<button class="tab-btn" data-tab="robot-x-axis" data-src="/calibration_xaxis.html">Robot X Axis</button>
<button class="tab-btn" data-tab="arm1" data-src="/calibration_arm.html">Arm1 Y</button>
<button class="tab-btn" data-tab="marker" data-src="/calibration_marker.html">Marker</button>
</nav>
<!-- CONTENT (Panels werden lazy per fetch befüllt) -->
@@ -32,6 +33,7 @@
<div class="tab-panel" id="tab-board"></div>
<div class="tab-panel" id="tab-robot-x-axis"></div>
<div class="tab-panel" id="tab-arm1"></div>
<div class="tab-panel" id="tab-marker"></div>
</div>
</div><!-- /.calib-body -->

View File

@@ -22,6 +22,7 @@ async function loadPanel(tab, src) {
else if (tab === 'board') initBoard();
else if (tab === 'robot-x-axis') initXAxis();
else if (tab === 'arm1') initArm('arm1');
else if (tab === 'marker') initMarker();
} catch (err) {
document.getElementById('tab-' + tab).innerHTML =
@@ -950,3 +951,224 @@ function initBoard() {
}
});
}
// ── Tab: Marker ───────────────────────────────────────────────────────────────
function initMarker() {
const logEl = document.getElementById('log-marker');
const tableWrap = document.getElementById('marker-table-wrap');
const linkSel = document.getElementById('marker-action-link');
const idSel = document.getElementById('marker-action-id');
const spinLabel = document.getElementById('marker-spin-current');
const resultEl = document.getElementById('marker-action-result');
const frameEl = document.getElementById('marker-viewer-frame');
const ARM_LINKS = ['Arm1', 'Ellbow', 'Arm2', 'Hand', 'Palm', 'FingerA', 'FingerB'];
let _robot = null;
function logM(msg) {
const ts = new Date().toLocaleTimeString('de-CH');
logEl.value += `[${ts}] ${msg}\n`;
logEl.scrollTop = logEl.scrollHeight;
}
// ── Marker-Tabelle rendern ────────────────────────────────────────────────
function renderTable(robot) {
if (!tableWrap) return;
const links = robot?.links ?? {};
const th = (a) => `style="text-align:${a};padding:3px 8px;border-bottom:1px solid #2a2d35;white-space:nowrap;background:#1e293b;color:#555b6e;font-weight:normal"`;
const td = (a, x = '') => `style="padding:2px 8px;border-bottom:1px solid #111418;text-align:${a};white-space:nowrap;${x}"`;
let rows = '';
let total = 0;
for (const linkName of ARM_LINKS) {
const markers = links[linkName]?.markers ?? [];
for (const m of markers) {
total++;
const pos = m.position ? m.position.map(v => Number(v).toFixed(1)).join(', ') : '';
const norm = m.normal ? m.normal.map(v => Number(v).toFixed(2)).join(', ') : '';
rows += `<tr>
<td ${td('left', 'color:#4a9eff')}>${linkName}</td>
<td ${td('right')}>${m.id}</td>
<td ${td('left', 'color:#888')}>${m.name ?? ''}</td>
<td ${td('right')}>${pos}</td>
<td ${td('right', 'color:#aaa')}>${norm}</td>
<td ${td('right')}>${m.size ?? ''}</td>
<td ${td('right', 'color:#f0a500;font-weight:bold')}>${m.spin ?? 0}°</td>
</tr>`;
}
}
if (total === 0) {
tableWrap.innerHTML = '<p style="font-size:12px;color:var(--muted)">Keine Arm-Marker in robot.json eingetragen.</p>';
return;
}
tableWrap.innerHTML = `
<p style="font-size:10px;color:#555b6e;margin-bottom:4px">${total} Marker in Arm-Links</p>
<table style="border-collapse:collapse;font-size:11px;font-family:inherit;width:100%">
<thead><tr>
<th ${th('left')}>Link</th>
<th ${th('right')}>ID</th>
<th ${th('left')}>Name</th>
<th ${th('right')}>Position [x,y,z] mm</th>
<th ${th('right')}>Normal [nx,ny,nz]</th>
<th ${th('right')}>Size mm</th>
<th ${th('right')}>Spin</th>
</tr></thead>
<tbody>${rows}</tbody>
</table>`;
}
// ── Marker-Dropdown für gewählten Link befüllen ───────────────────────────
function updateMarkerDropdown() {
if (!idSel || !_robot) return;
const linkName = linkSel?.value;
const markers = _robot.links?.[linkName]?.markers ?? [];
const prev = idSel.value;
idSel.innerHTML = '<option value=""> wählen </option>' +
markers.map(m => `<option value="${m.id}">${m.id}${m.name ? ' ' + m.name : ''}</option>`).join('');
if (markers.some(m => String(m.id) === prev)) idSel.value = prev;
updateSpinLabel();
}
function updateSpinLabel() {
if (!spinLabel || !_robot) { if (spinLabel) spinLabel.textContent = ''; return; }
const linkName = linkSel?.value;
const markerId = idSel?.value;
if (!markerId) { spinLabel.textContent = ''; return; }
const markers = _robot.links?.[linkName]?.markers ?? [];
const m = markers.find(mm => String(mm.id) === String(markerId));
spinLabel.textContent = m ? `Aktuell: spin = ${m.spin ?? 0}°` : '';
}
// ── Robot laden ───────────────────────────────────────────────────────────
async function loadRobot() {
try {
const r = await fetch('/api/robot');
if (!r.ok) throw new Error(`HTTP ${r.status}`);
_robot = await r.json();
renderTable(_robot);
updateMarkerDropdown();
} catch (err) {
if (tableWrap) tableWrap.innerHTML = `<p style="color:#f87171;font-size:12px">Fehler: ${err}</p>`;
logM(`❌ robot.json konnte nicht geladen werden: ${err}`);
}
}
// ── Spin-Aktion ausführen ─────────────────────────────────────────────────
async function applySpin(delta) {
if (!resultEl) return;
const linkName = linkSel?.value;
const markerId = idSel?.value;
if (!markerId) {
resultEl.innerHTML = '<span style="color:#f87171">⚠ Bitte zuerst einen Marker wählen.</span>';
return;
}
const markers = _robot?.links?.[linkName]?.markers ?? [];
const current = markers.find(m => String(m.id) === String(markerId));
const oldSpin = current?.spin ?? 0;
const newSpin = ((oldSpin + delta) % 360 + 360) % 360;
resultEl.innerHTML = '<span style="color:#555b6e">Speichern …</span>';
try {
const r = await fetch('/api/robot/set-arm-marker-spin', {
method: 'POST',
headers: { 'Content-Type': 'application/json' },
body: JSON.stringify({ linkName, markerId: Number(markerId), spin: newSpin }),
});
const data = await r.json();
if (!r.ok || !data.changed) {
resultEl.innerHTML = `<span style="color:#f87171">❌ ${data.error ?? `HTTP ${r.status}`}</span>`;
return;
}
resultEl.innerHTML =
`<span style="color:#22c55e">✅ ${linkName} #${markerId}: spin ${data.oldSpin}° → ${data.newSpin}°</span>`;
logM(`Spin ${linkName}#${markerId}: ${data.oldSpin}° → ${data.newSpin}°`);
// Lokales Modell aktualisieren
if (current) current.spin = data.newSpin;
updateSpinLabel();
renderTable(_robot);
// Viewer neu laden
if (frameEl?.contentWindow) frameEl.contentWindow.postMessage({ type: 'reload' }, '*');
} catch (err) {
resultEl.innerHTML = `<span style="color:#f87171">❌ ${err}</span>`;
}
}
// ── Homing-Run starten ───────────────────────────────────────────────────
async function runMarkerHoming() {
const btn = document.getElementById('btn-marker-homing');
const statusEl = document.getElementById('marker-homing-status');
if (btn) btn.disabled = true;
if (statusEl) statusEl.textContent = '● Läuft …';
logM('▶ Homing gestartet …');
try {
const response = await fetch('/api/homing/run', { method: 'POST' });
if (!response.ok || !response.body) throw new Error(`HTTP ${response.status}`);
const reader = response.body.getReader();
const decoder = new TextDecoder();
let buf = '';
while (true) {
const { done, value } = await reader.read();
if (done) break;
buf += decoder.decode(value, { stream: true });
const lines = buf.split('\n');
buf = lines.pop();
for (const line of lines) {
if (!line.startsWith('data: ')) continue;
let evt;
try { evt = JSON.parse(line.slice(6)); } catch { continue; }
if (evt.type === 'log') { logM(evt.text ?? ''); }
if (evt.type === 'step') { logM(`[${evt.step}/${evt.total}] ${evt.text ?? ''}`); }
if (evt.type === 'analysis' && evt.key?.startsWith('state_') && evt.value) {
frameEl?.contentWindow?.postMessage({ type: 'homing-state', state: evt.value }, '*');
}
if (evt.type === 'error') {
logM(`${evt.text ?? ''}`);
if (statusEl) statusEl.textContent = '✗ Fehler';
}
if (evt.type === 'done') {
if (evt.state) {
if (statusEl) statusEl.textContent = '✓ Fertig';
logM('✅ Homing abgeschlossen');
} else {
if (statusEl) statusEl.textContent = '✗ Fehler';
}
if (evt.runDir && frameEl?.contentWindow) {
frameEl.contentWindow.postMessage({ type: 'load-homing-run', runDir: evt.runDir }, '*');
if (evt.state) {
frameEl.contentWindow.postMessage({ type: 'homing-state', state: evt.state }, '*');
}
}
}
}
}
} catch (err) {
logM(`${err}`);
if (statusEl) statusEl.textContent = '✗ Fehler';
} finally {
if (btn) btn.disabled = false;
}
}
// ── Event-Listener ────────────────────────────────────────────────────────
document.getElementById('btn-marker-reload')?.addEventListener('click', () => loadRobot());
document.getElementById('btn-marker-homing')?.addEventListener('click', runMarkerHoming);
linkSel?.addEventListener('change', () => updateMarkerDropdown());
idSel?.addEventListener('change', () => updateSpinLabel());
document.getElementById('btn-spin-minus90')?.addEventListener('click', () => applySpin(-90));
document.getElementById('btn-spin-plus90')?.addEventListener('click', () => applySpin(+90));
document.getElementById('btn-spin-180')?.addEventListener('click', () => applySpin(+180));
// Init
loadRobot();
}

View File

@@ -0,0 +1,111 @@
<div class="sections">
<!-- ── Aktuelle Marker ──────────────────────────────────────────────────── -->
<div class="section full">
<h2>Aktuelle Marker <span class="status-badge open">aus robot.json</span></h2>
<p style="font-size:12px;color:var(--muted);margin-top:8px;margin-bottom:10px">
Arm-Marker aller Links (Board-Marker ausgeblendet). Spin = Drehung um die Marker-Normale in Grad.
</p>
<div id="marker-table-wrap">
<p style="font-size:12px;color:var(--muted)">(wird geladen …)</p>
</div>
<div class="controls" style="margin-top:12px">
<button id="btn-marker-reload">Neu laden</button>
</div>
</div>
<!-- ── Aktionen ─────────────────────────────────────────────────────────── -->
<div class="section full">
<h2>Aktionen</h2>
<!-- Auswahl Link / Marker -->
<div style="margin-top:14px;display:flex;flex-wrap:wrap;align-items:center;gap:10px;font-size:12px;color:var(--text)">
<label>Link
<select id="marker-action-link"
style="margin-left:6px;background:#1e293b;border:1px solid #334155;color:#c8cdd8;border-radius:3px;padding:3px 8px;font:inherit;font-size:12px;cursor:pointer">
<option value="Arm1">Arm1</option>
<option value="Ellbow">Ellbow</option>
<option value="Arm2">Arm2</option>
<option value="Hand">Hand</option>
<option value="Palm">Palm</option>
<option value="FingerA">FingerA</option>
<option value="FingerB">FingerB</option>
</select>
</label>
<label>Marker-ID
<select id="marker-action-id"
style="margin-left:6px;background:#1e293b;border:1px solid #334155;color:#c8cdd8;border-radius:3px;padding:3px 8px;font:inherit;font-size:12px;cursor:pointer">
<option value=""> wählen </option>
</select>
</label>
<span id="marker-spin-current" style="color:var(--muted);font-size:11px"></span>
</div>
<!-- Spin-Aktionen -->
<div style="margin-top:12px">
<p style="font-size:10px;color:var(--muted);text-transform:uppercase;letter-spacing:.06em;margin-bottom:8px">
Spin (Drehung um Marker-Normale)
</p>
<div style="display:flex;flex-wrap:wrap;gap:8px;align-items:center">
<button id="btn-spin-minus90"
style="background:#1e293b;color:#c8cdd8;border:1px solid #334155;border-radius:3px;padding:5px 14px;cursor:pointer;font:inherit;font-size:12px">
90°
</button>
<button id="btn-spin-plus90"
style="background:#1e293b;color:#c8cdd8;border:1px solid #334155;border-radius:3px;padding:5px 14px;cursor:pointer;font:inherit;font-size:12px">
+90°
</button>
<button id="btn-spin-180"
style="background:#1e293b;color:#c8cdd8;border:1px solid #334155;border-radius:3px;padding:5px 14px;cursor:pointer;font:inherit;font-size:12px">
180°
</button>
<span style="font-size:10px;color:var(--muted)">→ wird sofort in robot.json gespeichert und im Viewer aktualisiert</span>
</div>
</div>
<!-- Ergebnis -->
<div id="marker-action-result" style="margin-top:10px;font-size:11px;min-height:18px;color:var(--muted)"></div>
</div>
<!-- ── Homing ───────────────────────────────────────────────────────────── -->
<div class="section full">
<h2>Homing</h2>
<p style="font-size:12px;color:var(--muted);margin-bottom:12px">
Startet einen vollständigen Homing-Run (Foto aller Kameras → ArUco-Erkennung → Gelenk-Winkel).
Der Viewer wird anschliessend mit den neuen Messwerten aktualisiert.
</p>
<div style="display:flex;align-items:center;gap:12px;flex-wrap:wrap">
<button id="btn-marker-homing"
style="background:#1d4ed8;color:#fff;border:none;border-radius:3px;padding:6px 16px;cursor:pointer;font:inherit;font-size:12px">
Foto &amp; Homing berechnen
</button>
<span id="marker-homing-status" style="font-size:11px;color:var(--muted)"></span>
</div>
</div>
<!-- ── Log ──────────────────────────────────────────────────────────────── -->
<div class="section full">
<h2>Log</h2>
<textarea id="log-marker" readonly placeholder="(Aktionen und Homing-Output erscheinen hier)"
style="height:140px"></textarea>
</div>
<!-- ── Viewer ───────────────────────────────────────────────────────────── -->
<div class="section full">
<h2>Viewer</h2>
<p style="font-size:12px;color:var(--muted);margin-bottom:10px">
Zeigt das Roboter-Modell mit den Arm-Markern (aus robot.json) und — nach einem Homing-Run —
die beobachteten Marker als Kugeln mit Abweichungs-Linien.
Nach einer Spin-Änderung oder einem Homing-Run wird der Viewer automatisch neu geladen.
</p>
<iframe
id="marker-viewer-frame"
src="/boardViewer.html?mode=homing"
style="width:100%;height:740px;border:1px solid #334155;border-radius:6px;background:#0d0f13;display:block"
title="Marker-Viewer"
></iframe>
</div>
</div>

View File

@@ -325,6 +325,314 @@ async function onFotoClick() {
display.appendChild(wrapper);
}
// ── Homing ────────────────────────────────────────────────────────────────────
let _homingState = null;
function setHomingStatus(label, cls) {
const el = document.getElementById('homing-status');
if (!el) return;
el.textContent = label;
el.className = `status-badge ${cls}`;
}
function setHomingProgress(step, total, text) {
const wrap = document.getElementById('homing-progress');
const bar = document.getElementById('homing-progress-bar');
const txt = document.getElementById('homing-progress-text');
if (!wrap) return;
wrap.style.display = 'block';
if (bar) bar.style.width = (total > 0 ? Math.round(step / total * 100) : 0) + '%';
if (txt) txt.textContent = text || `Schritt ${step} / ${total}`;
}
// Schreibt das G92-Kommando ins Eingabefeld — nur die tatsächlich bestimmten
// Achsen, identisch zu dem, was "An Roboter senden" via server/buildG92.cjs
// sendet (fehlende/unbeobachtbare Achsen werden weggelassen, nicht 0-gefüllt).
function writePartialGCode(state) {
const axisMap = { x: 'X', y: 'Y', z: 'Z', a: 'A', b: 'B', c: 'C', e: 'E' };
const parts = [];
for (const [key, axis] of Object.entries(axisMap)) {
const num = Number(state[key]);
if (state[key] != null && Number.isFinite(num)) {
parts.push(`${axis}${num.toFixed(2)}`);
}
}
if (!parts.length) return;
const el = document.getElementById('gcodePayload');
if (el) el.value = `G92 ${parts.join(' ')}`;
}
function showHomingResult(state) {
// Raw JSON
const jsonEl = document.getElementById('result-json');
if (jsonEl) jsonEl.value = JSON.stringify(state, null, 2);
// Tree View: Labels + Einheiten statt generischem renderTree
const treeEl = document.getElementById('result-tree');
if (treeEl) {
const LABELS = {
x_mm: 'x (Slider)', y_deg: 'y (Arm1)', z_deg: 'z (Ellbow)',
a_deg: 'a (Arm2)', b_deg: 'b (Hand)', c_deg: 'c (Palm)',
e_mm: 'e (Greifer)',
};
const UNITS = {
x_mm: 'mm', y_deg: '°', z_deg: '°',
a_deg: '°', b_deg: '°', c_deg: '°', e_mm: 'mm',
};
let html = '';
for (const [key, val] of Object.entries(state)) {
const label = LABELS[key] ?? key;
const unit = UNITS[key] ?? '';
const valStr = typeof val === 'number' ? val.toFixed(2) : String(val ?? '');
html += `<div style="display:flex;gap:8px;padding:3px 0;font-family:ui-monospace,monospace;font-size:13px">
<span style="min-width:130px;color:#94a3b8">${label}</span>
<span style="font-weight:600">${valStr}&thinsp;${unit}</span>
</div>`;
}
treeEl.innerHTML = html || '<span style="color:#64748b"></span>';
}
}
async function loadHomingImages(runDir) {
if (!runDir) return;
try {
const res = await fetch(`/api/homing/run-data?run=${encodeURIComponent(runDir)}`);
if (!res.ok) return;
const data = await res.json();
// ── Debug-Bilder ──────────────────────────────────────────────────────────
const display = document.getElementById('snapshot-info-picture');
if (display) {
const debugImages = (data.images ?? []).filter(img => /debug/i.test(img.filename));
let html = '<div style="display:flex;flex-direction:column;gap:12px;margin-top:8px">';
for (const img of debugImages) {
html += `<figure style="margin:0">
<img src="data:image/jpeg;base64,${img.contentBase64}"
style="width:100%;max-width:760px;height:auto;border:1px solid #1f2937;border-radius:4px;display:block"
alt="${img.filename}">
<figcaption style="font-size:11px;color:#64748b;margin-top:3px">
${img.filename}
</figcaption>
</figure>`;
}
html += '</div>';
display.innerHTML = html;
}
// ── Marker-CSV-Tabelle ────────────────────────────────────────────────────
if (data.csvContent) renderHomingCsv(data.csvContent, runDir);
} catch { /* nicht kritisch */ }
}
function renderHomingCsv(csvContent, runDir) {
const table = document.getElementById('snapshot-table');
const infoEl = document.getElementById('snapshot-info');
if (!table) return;
const lines = csvContent.trim().split(/\r?\n/).filter(Boolean);
if (lines.length < 2) return;
const headers = lines[0].split(',').map(h => h.trim());
const rows = lines.slice(1).map(line => {
const cells = line.split(',');
const obj = {};
headers.forEach((h, i) => {
const raw = (cells[i] ?? '').trim();
const num = Number(raw);
obj[h] = raw !== '' && Number.isFinite(num) ? num : raw;
});
return obj;
});
if (infoEl) infoEl.textContent = `aruco_marker_poses.csv · ${runDir} · ${rows.length} Marker`;
table.innerHTML = '';
const thead = document.createElement('thead');
const headerRow = document.createElement('tr');
headers.forEach(h => {
const th = document.createElement('th');
th.textContent = h;
headerRow.appendChild(th);
});
thead.appendChild(headerRow);
table.appendChild(thead);
const tbody = document.createElement('tbody');
rows.forEach(row => {
const tr = document.createElement('tr');
headers.forEach(h => {
const td = document.createElement('td');
let v = row[h];
if (typeof v === 'number') {
v = /^(marker_id|id|num_cameras|seen_by)$/.test(h) ? Math.round(v) : v.toFixed(1);
}
td.textContent = v ?? '';
tr.appendChild(td);
});
tbody.appendChild(tr);
});
table.appendChild(tbody);
}
async function runHoming() {
// UI zurücksetzen
clearTextarea('log');
clearTextarea('analysis-log');
clearTextarea('result-json');
clearElement('result-tree');
clearElement('snapshot-table');
clearElement('snapshot-info-picture');
const btnRun = document.getElementById('btn-homing-run');
const btnSend = document.getElementById('btn-homing-send');
_homingState = null;
if (btnRun) btnRun.disabled = true;
if (btnSend) {
btnSend.disabled = true;
btnSend.style.opacity = '.4';
btnSend.style.cursor = 'not-allowed';
}
setHomingStatus('● Läuft …', 'wip');
setHomingProgress(0, 6, 'Verbinde …');
try {
const response = await fetch('/api/homing/run', { method: 'POST' });
if (!response.ok || !response.body) throw new Error(`HTTP ${response.status}`);
const reader = response.body.getReader();
const decoder = new TextDecoder();
let buf = '';
while (true) {
const { done, value } = await reader.read();
if (done) break;
buf += decoder.decode(value, { stream: true });
const lines = buf.split('\n');
buf = lines.pop();
for (const line of lines) {
if (!line.startsWith('data: ')) continue;
let evt;
try { evt = JSON.parse(line.slice(6)); } catch { continue; }
switch (evt.type) {
case 'log':
appendLog(evt.text ?? '');
break;
case 'step':
setHomingProgress(evt.step, evt.total, evt.text);
appendLog(`[${evt.step}/${evt.total}] ${evt.text || ''}`);
break;
case 'analysis': {
const el = document.getElementById('analysis-log');
if (el) {
el.value += `${evt.key}:\n${JSON.stringify(evt.value, null, 2)}\n\n`;
el.scrollTop = el.scrollHeight;
}
// Progressiver Skeleton-Update: accumulated_state nach jedem Gelenk
if (evt.key?.startsWith('state_') && evt.value) {
const frame = document.getElementById('board-viewer-frame');
if (frame?.contentWindow) {
frame.contentWindow.postMessage({ type: 'homing-state', state: evt.value }, '*');
}
writePartialGCode(evt.value);
}
break;
}
case 'error':
appendLog(evt.text ?? '');
setHomingStatus('✗ Fehler', 'open');
break;
case 'done':
if (evt.state) {
_homingState = evt.state;
showHomingResult(evt.state);
// Finales G92 ins Feld — auch wenn der Lauf über den Fallback
// (5_pose_estimation → analysis 'robot_state' statt 'state_*')
// lief und progressiv kein G92 geschrieben wurde.
writePartialGCode(evt.state);
if (btnSend) {
btnSend.disabled = false;
btnSend.style.opacity = '';
btnSend.style.cursor = '';
}
setHomingStatus('✓ Fertig', 'done');
setHomingProgress(6, 6, 'Homing abgeschlossen');
} else {
setHomingStatus('✗ Fehler', 'open');
setHomingProgress(6, 6, 'Fehler aufgetreten');
}
if (evt.runDir) await loadHomingImages(evt.runDir);
const frame = document.getElementById('board-viewer-frame');
if (frame?.contentWindow) {
frame.contentWindow.postMessage({ type: 'load-homing-run', runDir: evt.runDir }, '*');
if (evt.state) {
frame.contentWindow.postMessage({ type: 'homing-state', state: evt.state }, '*');
}
}
break;
}
}
}
} catch (err) {
appendLog(`❌ Verbindungsfehler: ${err.message}`);
setHomingStatus('✗ Fehler', 'open');
} finally {
if (btnRun) btnRun.disabled = false;
}
}
async function sendHomingToRobot() {
if (!_homingState) return;
const btnSend = document.getElementById('btn-homing-send');
if (btnSend) btnSend.disabled = true;
try {
const res = await fetch('/api/homing/send-state', {
method: 'POST',
headers: { 'Content-Type': 'application/json' },
body: JSON.stringify({ state: _homingState }),
});
const data = await res.json();
if (res.ok) {
appendLog(`✅ An Roboter gesendet: ${data.gcode ?? ''}`);
if (data.note) appendLog(` ${data.note}`);
setHomingStatus('✓ Gesendet', 'done');
} else {
appendLog(`❌ Fehler beim Senden: ${data.error ?? JSON.stringify(data)}`);
if (btnSend) btnSend.disabled = false;
}
} catch (err) {
appendLog(`❌ Netzwerkfehler: ${err.message}`);
if (btnSend) btnSend.disabled = false;
}
}
// Transport für die G-Code-/Befehl-Buttons (data-cmd). Schickt eine rohe
// Zeile über das Backend an den Driver-WebSocket (POST /api/robot/gcode).
// Liegt ein Payload-Feld vor (z.B. das G92 aus #gcodePayload), wird dessen
// Inhalt gesendet, sonst der cmd-Name selbst. Ersetzt den toten WSS-Altpfad.
window.sendCommand = async function (cmd, payload) {
const line = (payload && payload.trim()) ? payload.trim() : String(cmd ?? '').trim();
if (!line) throw new Error('Leere Befehlszeile');
const res = await fetch('/api/robot/gcode', {
method: 'POST',
headers: { 'Content-Type': 'application/json' },
body: JSON.stringify({ line }),
});
const data = await res.json().catch(() => ({}));
if (!res.ok) throw new Error(data.error ?? `HTTP ${res.status}`);
return data;
};
async function onCommandClick(btn) {
const cmd = btn.dataset.cmd;
const payloadSelector = btn.dataset.payload;
@@ -346,6 +654,14 @@ async function onCommandClick(btn) {
}
function setupUi() {
// Homing-Buttons
const homingRunBtn = document.getElementById('btn-homing-run');
if (homingRunBtn) homingRunBtn.addEventListener('click', runHoming);
const homingSendBtn = document.getElementById('btn-homing-send');
if (homingSendBtn) homingSendBtn.addEventListener('click', sendHomingToRobot);
// Ältere Buttons (Fallback / Debug)
const calculateBtn = document.getElementById("btn-calculate");
if (calculateBtn) {
calculateBtn.addEventListener("click", onCalculateClick);

111
public/homing.html Normal file
View File

@@ -0,0 +1,111 @@
<!DOCTYPE html>
<html lang="de">
<head>
<meta charset="UTF-8" />
<meta name="viewport" content="width=device-width, initial-scale=1.0" />
<title>Homing appRobotHoming</title>
<link rel="stylesheet" href="/styles.css">
<link rel="stylesheet" href="/calibration.css">
</head>
<body>
<!-- HEADER (analog calibration.html) -->
<div class="calib-topbar">
<a href="/">← Zurück</a>
<h1>Homing</h1>
</div>
<div style="max-width:1200px;margin:0 auto;padding:0 16px">
<div class="sections">
<!-- AKTIONEN -->
<div class="section full">
<h2>Aktionen</h2>
<div class="controls" style="flex-wrap:wrap;gap:12px;align-items:center">
<button id="btn-homing-run">📷 Foto &amp; Homing berechnen</button>
<button id="btn-homing-send" disabled
style="opacity:.4;cursor:not-allowed"
title="Erst Homing ausführen">
✅ An Roboter senden
</button>
<span id="homing-status" class="status-badge open">○ Warte</span>
</div>
<!-- Fortschrittsbalken -->
<div id="homing-progress" style="display:none;margin-top:14px">
<div style="background:var(--border);border-radius:3px;height:5px;overflow:hidden">
<div id="homing-progress-bar"
style="height:100%;background:var(--accent);width:0%;transition:width .4s ease"></div>
</div>
<span id="homing-progress-text"
style="font-size:11px;color:var(--muted);display:block;margin-top:4px"></span>
</div>
</div>
<!-- AUSGABE / LOG -->
<div class="section full">
<h2>Ausgabe / Log</h2>
<textarea id="log-homing" readonly
placeholder="(Ausgabe erscheint hier sobald Homing gestartet wird)"
style="min-height:180px"></textarea>
</div>
<!-- ANALYSIS & REASONING -->
<div class="section full">
<h2>Analysis &amp; Reasoning</h2>
<textarea id="homing-analysis" readonly
placeholder="(Zwischenergebnisse je Script erscheinen hier)"
style="min-height:120px;font-size:12px"></textarea>
</div>
<!-- RESULT RAW JSON + TREE VIEW -->
<div class="section half">
<h2>Result Raw JSON</h2>
<div class="panel">
<textarea id="homing-result-json" readonly
placeholder="(Ergebnis erscheint hier)"
style="min-height:160px;font-size:12px"></textarea>
</div>
</div>
<div class="section half">
<h2>Result Tree View</h2>
<div class="panel" style="min-height:160px;padding:14px 16px">
<div id="homing-result-tree"
style="font-family:monospace;font-size:13px;line-height:2">
<span style="color:var(--muted);font-size:12px">(Ergebnis erscheint hier)</span>
</div>
</div>
</div>
<!-- SNAPSHOT CSV -->
<div class="section full">
<h2>Snapshot CSV (Marker)</h2>
<div id="homing-csv-info"
style="font-size:11px;color:var(--muted);margin-bottom:8px">
Marker-Positionen aus aruco_marker_poses.json werden nach dem Homing angezeigt.
</div>
<div style="overflow-x:auto">
<table id="homing-csv-table"
style="font-size:12px;border-collapse:collapse;width:100%;min-width:500px"></table>
</div>
</div>
<!-- SNAPSHOTS (Kamerabilder) -->
<div class="section full">
<h2>Snapshots (annotierte Kamerabilder)</h2>
<div id="homing-snapshots"
style="display:flex;gap:12px;flex-wrap:wrap">
<span style="color:var(--muted);font-size:12px">
(Bilder erscheinen nach dem Homing-Run)
</span>
</div>
</div>
</div><!-- /.sections -->
</div>
<script src="/homing.js"></script>
</body>
</html>

280
public/homing.js Normal file
View File

@@ -0,0 +1,280 @@
'use strict';
// ── DOM-Referenzen ────────────────────────────────────────────────────────────
const btnRun = document.getElementById('btn-homing-run');
const btnSend = document.getElementById('btn-homing-send');
const statusBadge = document.getElementById('homing-status');
const progressDiv = document.getElementById('homing-progress');
const progressBar = document.getElementById('homing-progress-bar');
const progressText = document.getElementById('homing-progress-text');
const logEl = document.getElementById('log-homing');
const analysisEl = document.getElementById('homing-analysis');
const resultJson = document.getElementById('homing-result-json');
const resultTree = document.getElementById('homing-result-tree');
const csvInfo = document.getElementById('homing-csv-info');
const csvTable = document.getElementById('homing-csv-table');
const snapshots = document.getElementById('homing-snapshots');
let _lastState = null; // letztes Homing-Ergebnis (zum Senden an Roboter)
// ── Hilfsfunktionen ───────────────────────────────────────────────────────────
function appendLog(text) {
logEl.value += (text ?? '') + '\n';
logEl.scrollTop = logEl.scrollHeight;
}
function appendAnalysis(key, value) {
const line = key + ':\n' + JSON.stringify(value, null, 2);
analysisEl.value += line + '\n\n';
analysisEl.scrollTop = analysisEl.scrollHeight;
}
function setStatus(label, cls) {
statusBadge.textContent = label;
statusBadge.className = `status-badge ${cls}`;
}
function setProgress(step, total, text) {
progressDiv.style.display = 'block';
const pct = total > 0 ? Math.round((step / total) * 100) : 0;
progressBar.style.width = pct + '%';
progressText.textContent = text || `Schritt ${step} / ${total}`;
}
/** Zeigt { x_mm, y_deg, z_deg, a_deg, b_deg, c_deg, e_mm } im Result-Bereich. */
function showResult(state) {
// Raw JSON
resultJson.value = JSON.stringify(state, null, 2);
// Tree View
const LABELS = {
x_mm: 'x (Slider)',
y_deg: 'y (Arm1)',
z_deg: 'z (Ellbow)',
a_deg: 'a (Arm2)',
b_deg: 'b (Hand)',
c_deg: 'c (Palm)',
e_mm: 'e (Greifer)',
};
const UNITS = {
x_mm: 'mm', y_deg: '°', z_deg: '°',
a_deg: '°', b_deg: '°', c_deg: '°', e_mm: 'mm',
};
let html = '';
for (const [key, val] of Object.entries(state)) {
const label = LABELS[key] ?? key;
const unit = UNITS[key] ?? '';
const valStr = typeof val === 'number' ? val.toFixed(2) : String(val ?? '');
html += `<div style="display:flex;gap:8px;padding:2px 0">
<span style="min-width:130px;color:var(--muted)">${label}</span>
<span style="font-weight:600">${valStr}&thinsp;${unit}</span>
</div>`;
}
resultTree.innerHTML = html || '<span style="color:var(--muted)"></span>';
}
/** Baut die CSV-Tabelle aus aruco_marker_poses.json-Daten. */
function showMarkerTable(markers) {
if (!markers || markers.length === 0) {
csvInfo.textContent = 'Keine Marker-Daten vorhanden.';
csvTable.innerHTML = '';
return;
}
csvInfo.textContent = `${markers.length} Marker trianguliert`;
const hdrs = ['ID', 'Link', 'Set', 'x mm', 'y mm', 'z mm', 'Kameras'];
const style = 'padding:4px 8px;border:1px solid var(--border);text-align:right';
const styleL = 'padding:4px 8px;border:1px solid var(--border);text-align:left';
let html = `<thead><tr>${hdrs.map(h =>
`<th style="${h==='Link'||h==='Set'||h==='ID'?styleL:style}">${h}</th>`).join('')}</tr></thead><tbody>`;
for (const m of markers) {
const pos = m.position_mm ?? [null, null, null];
const fmt = v => (v != null ? Number(v).toFixed(1) : '');
html += `<tr>
<td style="${styleL}">${m.marker_id}</td>
<td style="${styleL}">${m.link ?? ''}</td>
<td style="${styleL}">${m.set ?? ''}</td>
<td style="${style}">${fmt(pos[0])}</td>
<td style="${style}">${fmt(pos[1])}</td>
<td style="${style}">${fmt(pos[2])}</td>
<td style="${style}">${m.num_cameras ?? ''}</td>
</tr>`;
}
html += '</tbody>';
csvTable.innerHTML = html;
}
/** Lädt Debug-Bilder und Marker-Tabelle für einen Homing-Run. */
async function loadRunData(runDir) {
try {
const res = await fetch(`/api/homing/run-data?run=${encodeURIComponent(runDir)}`);
if (!res.ok) return;
const data = await res.json();
// Snapshots
snapshots.innerHTML = '';
for (const img of (data.images ?? [])) {
const figure = document.createElement('figure');
figure.style.cssText = 'margin:0;display:flex;flex-direction:column;gap:4px';
const el = document.createElement('img');
el.src = `data:image/jpeg;base64,${img.contentBase64}`;
el.style.cssText = 'max-width:400px;max-height:300px;border:1px solid var(--border);border-radius:4px';
el.alt = img.filename;
const cap = document.createElement('figcaption');
cap.textContent = img.filename;
cap.style.cssText = 'font-size:11px;color:var(--muted);text-align:center';
figure.appendChild(el);
figure.appendChild(cap);
snapshots.appendChild(figure);
}
if (!data.images?.length) {
snapshots.innerHTML = '<span style="color:var(--muted);font-size:12px">Keine Bilder vorhanden.</span>';
}
} catch { /* nicht kritisch */ }
}
/** Lädt aruco_marker_poses.json für den Run und zeigt die Marker-Tabelle. */
async function loadArucoData(runDir) {
try {
// Rohdaten über Homing-Run-Data-Endpoint nicht direkt verfügbar,
// daher holen wir uns die JSON über board/latest falls es der gleiche Run ist,
// oder wir parsen es aus den Analysis-Daten.
// Fürs erste: Marker aus dem finalState ableiten wenn vorhanden.
csvInfo.textContent = '(Marker-CSV wird nach dem nächsten Homing-Run geladen)';
} catch { /* ignorieren */ }
}
// ── Homing starten ────────────────────────────────────────────────────────────
async function runHoming() {
// UI zurücksetzen
logEl.value = '';
analysisEl.value = '';
resultJson.value = '';
resultTree.innerHTML = '<span style="color:var(--muted);font-size:12px">(Ergebnis erscheint hier)</span>';
csvInfo.textContent = '';
csvTable.innerHTML = '';
snapshots.innerHTML = '<span style="color:var(--muted);font-size:12px">…</span>';
_lastState = null;
btnRun.disabled = true;
btnSend.disabled = true;
btnSend.style.opacity = '0.4';
btnSend.style.cursor = 'not-allowed';
setStatus('● Läuft …', 'wip');
progressDiv.style.display = 'block';
progressBar.style.width = '2%';
progressText.textContent = 'Verbinde …';
try {
const response = await fetch('/api/homing/run', { method: 'POST' });
if (!response.ok || !response.body) throw new Error(`HTTP ${response.status}`);
const reader = response.body.getReader();
const decoder = new TextDecoder();
let buf = '';
let lastRunDir = null;
let hasError = false;
while (true) {
const { done, value } = await reader.read();
if (done) break;
buf += decoder.decode(value, { stream: true });
const lines = buf.split('\n');
buf = lines.pop(); // unvollständige letzte Zeile aufheben
for (const line of lines) {
if (!line.startsWith('data: ')) continue;
let evt;
try { evt = JSON.parse(line.slice(6)); } catch { continue; }
switch (evt.type) {
case 'log':
appendLog(evt.text);
break;
case 'step':
setProgress(evt.step, evt.total, evt.text);
appendLog(`[${evt.step}/${evt.total}] ${evt.text || ''}`);
break;
case 'analysis':
appendAnalysis(evt.key, evt.value);
break;
case 'error':
appendLog(evt.text);
hasError = true;
break;
case 'done':
if (evt.state) {
_lastState = evt.state;
showResult(evt.state);
btnSend.disabled = false;
btnSend.style.opacity = '';
btnSend.style.cursor = '';
setStatus('✓ Fertig', 'done');
progressBar.style.width = '100%';
progressText.textContent = 'Homing abgeschlossen';
} else {
setStatus('✗ Fehler', 'open');
progressBar.style.width = '100%';
progressText.textContent = hasError ? 'Fehler aufgetreten' : 'Abgebrochen';
}
if (evt.runDir) {
lastRunDir = evt.runDir;
await loadRunData(evt.runDir);
}
break;
}
}
}
} catch (err) {
appendLog(`❌ Verbindungsfehler: ${err.message}`);
setStatus('✗ Fehler', 'open');
progressBar.style.width = '100%';
progressText.textContent = 'Verbindungsfehler';
} finally {
btnRun.disabled = false;
}
}
// ── State an Roboter senden ───────────────────────────────────────────────────
async function sendToRobot() {
if (!_lastState) return;
btnSend.disabled = true;
try {
const res = await fetch('/api/homing/send-state', {
method: 'POST',
headers: { 'Content-Type': 'application/json' },
body: JSON.stringify({ state: _lastState }),
});
const data = await res.json();
if (res.ok) {
appendLog('✅ State erfolgreich an Roboter gesendet');
setStatus('✓ Gesendet', 'done');
} else {
appendLog(`❌ Fehler beim Senden: ${data.error ?? JSON.stringify(data)}`);
btnSend.disabled = false;
}
} catch (err) {
appendLog(`❌ Netzwerkfehler: ${err.message}`);
btnSend.disabled = false;
}
}
// ── Event-Listener ────────────────────────────────────────────────────────────
btnRun.addEventListener('click', runHoming);
btnSend.addEventListener('click', sendToRobot);

View File

@@ -15,26 +15,40 @@
<div class="section full">
<h2>Aktionen</h2>
<div class="controls">
<button data-cmd="HOME">HOME</button>
<button data-cmd="PING">PING</button>
<!-- Homing primäre Aktion -->
<div class="controls" style="flex-wrap:wrap;gap:10px;align-items:center">
<button id="btn-homing-run">📷 Foto &amp; Homing berechnen</button>
<button id="btn-homing-send" disabled
style="opacity:.4;cursor:not-allowed"
title="Erst Homing ausführen">
✅ An Roboter senden
</button>
<span id="homing-status" class="status-badge idle">○ Warte</span>
</div>
<!-- Fortschrittsbalken -->
<div id="homing-progress" style="display:none;margin-top:12px">
<div id="homing-progress-track">
<div id="homing-progress-bar"></div>
</div>
<span id="homing-progress-text"></span>
</div>
<!-- Sekundäre Aktionen -->
<div class="controls" style="margin-top:14px;flex-wrap:wrap;gap:8px;padding-top:12px;border-top:1px solid #1e293b">
<button data-cmd="HOME" style="font-size:12px;padding:4px 12px">HOME</button>
<button data-cmd="PING" style="font-size:12px;padding:4px 12px">PING</button>
<input
id="gcodePayload"
type="text"
placeholder="G-Code / Motorbefehl"
style="font-size:12px;padding:4px 8px"
/>
<button data-cmd="GCODEMOTOR" data-payload="#gcodePayload">
GCodeMotor
</button>
<button id="btn-calculate">Read Position from Markers</button>
<button id="btn-foto">Foto</button>
<button data-cmd="GCODEMOTOR" data-payload="#gcodePayload"
style="font-size:12px;padding:4px 12px">GCodeMotor</button>
<button id="btn-foto" style="font-size:12px;padding:4px 12px">Foto-Vorschau</button>
<a href="/calibration.html">
<button type="button">Calibration Page</button>
<button type="button" style="font-size:12px;padding:4px 12px">Kalibrierung</button>
</a>
</div>
</div>
@@ -51,36 +65,30 @@
<textarea id="analysis-log" readonly></textarea>
</div>
<!-- RESULT RAW JSON -->
<div class="section half">
<h2>Result Raw JSON</h2>
<div class="panel">
<label>Raw JSON</label>
<textarea id="result-json" readonly></textarea>
</div>
<!-- BOARD-VIEWER -->
<div class="section full">
<h2>Board-Viewer</h2>
<iframe
id="board-viewer-frame"
src="/boardViewer.html?mode=homing"
style="width:100%;height:1200px;border:1px solid #334155;border-radius:6px;background:#0d0f13;display:block;margin-top:12px"
title="Board-Viewer"
></iframe>
</div>
<!-- RESULT TREE VIEW -->
<div class="section half">
<h2>Result Tree View</h2>
<div class="panel">
<label>Tree View</label>
<div id="result-tree"></div>
</div>
<!-- SNAPSHOTS (nur Debug-Bilder) -->
<div class="section full">
<h2>Snapshots</h2>
<div id="snapshot-info-picture"></div>
</div>
<!-- SNAPSHOT -->
<!-- SNAPSHOT CSV -->
<div class="section full">
<h2>Snapshot CSV</h2>
<div id="snapshot-info"></div>
<table id="snapshot-table"></table>
</div>
<div class="section full">
<h2>Snapshots</h2>
<div id="snapshot-info-picture"></div>
</div>
</div>
<script src="/calculateAngles.js"></script>

View File

@@ -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);
@@ -858,6 +861,17 @@ function rebuild() {
}
}
// Debug: log model marker IDs and visibility state
try {
console.log('sceneViewer:model', {
tModel: document.getElementById('tModel')?.checked,
modelIds: Object.keys(modelPositions || {}),
gModelChildren: gModel.children.length
});
} catch (e) {
console.warn('sceneViewer: failed to log model info', e);
}
// ── observed markers + normals + error lines ──
const obs = {};
if (arucoData) {
@@ -875,6 +889,17 @@ function rebuild() {
}
}
// Debug: log observed markers and observed-toggle
try {
console.log('sceneViewer:observations', {
tObserved: document.getElementById('tObserved')?.checked,
arucoMarkers: (arucoData?.markers?.length ?? 0),
obsIds: Object.keys(obs || {})
});
} catch (e) {
console.warn('sceneViewer: failed to log observations', e);
}
const errors = [];
const normalErrors = [];
for (const [midStr, {pos: opos, nor: oNor}] of Object.entries(obs)) {

View File

@@ -63,7 +63,7 @@ body {
background: var(--panel);
border: 1px solid var(--border);
border-radius: 8px;
padding: 22px 20px;
padding: 20px;
}
.section h2 {
@@ -138,13 +138,18 @@ textarea {
font-size: 12px;
}
/* Ausgabe / Log: nur wenige Zeilen */
/* Ausgabe / Log */
#log {
min-height: 60px;
max-height: 150px;
min-height: 130px;
max-height: 220px;
resize: vertical;
}
/* Analysis & Reasoning */
#analysis-log {
min-height: 260px;
}
/* ===== PANEL (alte Struktur wiederhergestellt) ===== */
.panel {
@@ -224,4 +229,47 @@ textarea {
#snapshot-info-picture {
margin-top: 10px;
}
/* ===== STATUS-BADGE (Homing) ===== */
.status-badge {
display: inline-block;
padding: 2px 10px;
border-radius: 12px;
font-size: 12px;
font-weight: 600;
background: #1e293b;
color: #94a3b8;
}
.status-badge.idle { color: #60a5fa; }
.status-badge.wip { color: #93c5fd; }
.status-badge.open { color: #f59e0b; }
.status-badge.done { color: #34d399; background: #064e3b; }
/* ===== HOMING FORTSCHRITTSBALKEN ===== */
#homing-progress {
margin-top: 12px;
}
#homing-progress-track {
background: #1e293b;
border-radius: 3px;
height: 5px;
overflow: hidden;
}
#homing-progress-bar {
height: 100%;
background: var(--accent);
width: 0%;
transition: width .4s ease;
}
#homing-progress-text {
font-size: 11px;
color: var(--muted);
display: block;
margin-top: 4px;
}

View File

@@ -60,7 +60,7 @@
const n = cross.map(c => c / crossLen);
// Baryzentrische Gewichte → Umkreismittelpunkt (doc/04_y_achse.md)
// Baryzentrische Gewichte → Umkreismittelpunkt (doc/Kalibrierung.md → [4] Arm1, Verfahren B)
const a2 = dist2(P2, P3), b2 = dist2(P1, P3), c2 = dist2(P1, P2);
const w1 = a2 * (b2 + c2 - a2);
const w2 = b2 * (a2 + c2 - b2);
@@ -106,6 +106,13 @@
const mc = mapC.get(id);
if (!mb || !mc) continue;
// Fehlende position_mm (z.B. Einzelkamera-Marker, von 3b nicht
// trianguliert) ignorieren statt crashen — Marker bleibt im skipped-Log.
if (!Array.isArray(ma.position_mm) || !Array.isArray(mb.position_mm) || !Array.isArray(mc.position_mm)) {
skipped.push({ id, reason: 'fehlende position_mm (z.B. Einzelkamera-Marker, nicht trianguliert)' });
continue;
}
const P1 = ma.position_mm.map(Number);
const P2 = mb.position_mm.map(Number);
const P3 = mc.position_mm.map(Number);

View File

@@ -59,11 +59,14 @@ def load_cameras(eval_dir: str) -> Dict[str, dict]:
R = np.array(w2c["rotation_matrix"], dtype=float).reshape(3, 3)
t = np.array(w2c["translation_m"], dtype=float).reshape(3)
markers: Dict[int, np.ndarray] = {}
confidence: Dict[int, float] = {}
for d in det.get("detections", []):
pts = d.get("image_points_px")
if pts is not None:
markers[int(d["marker_id"])] = np.array(pts, dtype=float).reshape(4, 2)
cams[cam_id] = dict(K=K, D=D, R=R, t=t, markers=markers)
mid = int(d["marker_id"])
markers[mid] = np.array(pts, dtype=float).reshape(4, 2)
confidence[mid] = float(d.get("confidence", 1.0))
cams[cam_id] = dict(K=K, D=D, R=R, t=t, markers=markers, confidence=confidence)
return cams
@@ -156,6 +159,9 @@ def main() -> None:
normal, center = corner_plane_normal(corners3d)
edge_mm = float(np.mean([np.linalg.norm(corners3d[(i + 1) % 4] - corners3d[i]) for i in range(4)]) * 1000.0)
confidences = [cams[c]["confidence"].get(mid, 1.0) for c in cam_ids]
weight = float(np.mean(confidences))
markers_out.append({
"marker_id": int(mid),
"link": marker_info.get(mid, {}).get("link", "unknown"),
@@ -166,6 +172,7 @@ def main() -> None:
"corners_m": [[float(v) for v in c] for c in corners3d],
"num_cameras": len(cam_ids),
"edge_length_mm": edge_mm,
"weight": round(weight, 4),
})
# camera poses in world (for viewer frusta): centre C = -R^T t, view axis = R[2]

View File

@@ -10,7 +10,7 @@ Gegeben drei Messungen (Pos A, B, C) in denen dieselben fremd-Marker
- Referenzpunkt auf der Achse (axisPoint_mm)
- Residuen pro Punkt (Qualitätsmass)
Methode (doc/04_y_achse.md):
Methode (doc/Kalibrierung.md → [4] Arm1, Verfahren B):
Jeder Marker bewegt sich auf einem Kreisbogen. Die Ebene des Kreises
steht senkrecht zur Rotationsachse → Normalenvektor = Achsenrichtung.
Der Umkreismittelpunkt des Dreiecks P1-P2-P3 liegt auf der Achse.
@@ -161,10 +161,20 @@ def compute_rotation_axis(
for mid in common_ids:
# ── Mindest-Bewegungs-Filter ───────────────────────────────────────────
# Marker die sich kaum bewegen liefern degenerate Umkreismittelpunkte.
# Wir vergleichen die Zentren (position_mm) der drei Messungen.
cA = np.array(mA[mid].get('position_mm', [0, 0, 0]), dtype=float)
cB = np.array(mB[mid].get('position_mm', [0, 0, 0]), dtype=float)
cC = np.array(mC[mid].get('position_mm', [0, 0, 0]), dtype=float)
# Wir vergleichen die Zentren der drei Messungen.
# Fehlt position_mm in einer Messung (z.B. Einzelkamera-Marker) → überspringen.
cA_raw = mA[mid].get('position_mm')
cB_raw = mB[mid].get('position_mm')
cC_raw = mC[mid].get('position_mm')
if cA_raw is None or cB_raw is None or cC_raw is None:
skipped.append({
'marker_id': mid,
'reason': 'fehlende position_mm in mindestens einer Messung (z.B. Einzelkamera-Marker)',
})
continue
cA = np.array(cA_raw, dtype=float)
cB = np.array(cB_raw, dtype=float)
cC = np.array(cC_raw, dtype=float)
max_movement = max(
np.linalg.norm(cB - cA),
np.linalg.norm(cC - cB),

View File

@@ -5,11 +5,19 @@
Generic revolute-joint angle estimator.
For each movable link (Arm1, Ellbow, Arm2 …) whose joint type is 'revolute',
this script estimates the rotation angle using the pairwise-vector method:
this script estimates the rotation angle using one of three methods, tried
in order — each next TIER is a pure fallback, only used when the previous
one found NOT A SINGLE usable (non axis-degenerate) pair:
For every PAIR (m1, m2) of markers belonging to the target link:
v_model = local_pos_m2 - local_pos_m1 (in link's own frame)
v_obs = world_pos_m2 - world_pos_m1 (in world frame)
TIER 0 — PRIMARY: for every PAIR (m1, m2) of markers belonging to the
target link itself:
v_model = spoke_world(m2) - spoke_world(m1) (model, world-oriented)
v_obs = world_pos_m2 - world_pos_m1 (observed, world frame)
where spoke_world(m) rotates the marker's local `position` (from
robot.json) through the already-known PARENT joints via FK, so it is
expressed in world orientation at this joint's own angle = 0 — exactly
the frame v_obs lives in.
Both vectors are projected perpendicular to the joint axis (in world frame),
and the signed angle from v_model_perp to v_obs_perp is measured.
@@ -20,6 +28,29 @@ this script estimates the rotation angle using the pairwise-vector method:
Pair weights = baseline_model × baseline_obs (longer baselines → more reliable).
TIER 1 — FALLBACK-1 (child-axis): only entered when TIER 0 has nothing.
Uses a PAIR of markers on the DIRECT CHILD link instead of the target
link, picking only pairs whose LOCAL connecting vector is (nearly)
parallel to the CHILD's OWN joint axis. A rotation about an axis never
moves a vector parallel to that very axis, so such a pair is invariant
to the child's own (still-unknown) rotation and transforms purely under
the chain up to and including the TARGET joint — exactly like a TIER-0
pair, just sourced one link further down. Like TIER 0 (and unlike
TIER 2), this only needs the axis DIRECTION to be correct, not the
pivot's position, so it is preferred over TIER 2 whenever available.
Example: Ellbow (axis X) ← Arm2 markers 144/148 or 143/146 (Arm2's own
axis Y, ⟂ to X, both pairs exactly axis-aligned in Arm2's local frame).
TIER 2 — FALLBACK-2 (pivot): only entered when TIER 1 ALSO has nothing
(e.g. no markers visible at all besides one on the target link itself,
or no child link exists). The joint PIVOT itself stands in for a
missing second marker, i.e. the "pair" becomes (pivot, m1). This needs
only ONE matched marker on the target link, but — unlike TIER 0/1 —
its accuracy additionally depends on the already-estimated PARENT joint
*values* being correct (not just their axis direction), since the
pivot's world position comes from FK. See `PIVOT_FALLBACK_ID` /
`TIER_*` / `tier_used` in the code.
How to use sequentially
-----------------------
Run 4b once per revolute joint, from root to tip:
@@ -36,6 +67,7 @@ Output JSON
{
"link": "Arm1",
"joint": "y",
"method": "primary", // or "fallback_1_child_axis" / "fallback_2_pivot" — see TIERs above
"mean_angle_deg": 86.3,
"circular_std_deg": 0.7,
"num_pairs": 6,
@@ -62,6 +94,20 @@ from robot_fk import RobotFK
STATE_KEYS = ("x", "y", "z", "a", "b", "c", "e")
# Sentinel "marker id" used in `per_pair` reports for the joint pivot.
# Only ever appears in TIER_FALLBACK_2 entries (pivot vs. a single marker)
# — see the TIER_FALLBACK_2 block inside `estimate_revolute_angle()` below.
PIVOT_FALLBACK_ID = -1
# Tier labels — reported in `per_pair[].tier` and the top-level `method`
# field, so it's always traceable which method actually produced a given
# estimate. Tried in this order; each next one is a pure fallback (see
# module docstring above for what each tier means and why it's ordered
# this way).
TIER_PRIMARY = "primary" # pair of markers on the target link itself
TIER_FALLBACK_1 = "fallback_1_child_axis" # pair on a CHILD link, aligned with the child's OWN axis
TIER_FALLBACK_2 = "fallback_2_pivot" # single marker on the target link vs. the joint pivot
# ──────────────────────────────────────────────────────────────
# I/O
@@ -130,6 +176,112 @@ def _circular_mean_deg(angles_rad: np.ndarray,
return math.degrees(mean), c_var, math.degrees(c_std)
# ──────────────────────────────────────────────────────────────
# Shared spoke/pair math
# (used by BOTH the primary marker-pair method and the pivot FALLBACK)
# ──────────────────────────────────────────────────────────────
def _model_spoke_world(fk: RobotFK,
zero_transforms: Dict[str, np.ndarray],
link_name: str,
origin_world: np.ndarray,
local_pos: np.ndarray) -> np.ndarray:
"""
Vector from the joint PIVOT to a marker, in WORLD ORIENTATION, as it
would be if this joint's own angle were 0 (only the already-known
PARENT rotations applied).
This is what `v_model` must be expressed as before comparing it to
`axis_world` / an observed vector: the raw `position` field from
robot.json lives in the link's own pre-rotation local frame and is
NOT yet rotated into world orientation by the parent chain. Skipping
this rotation silently biases the estimated angle by (roughly) the
parent joint's own angle — invisible whenever the parent chain has
no rotation yet (e.g. Arm1, whose parent 'Base' is purely linear),
but wrong for anything further down the chain (Ellbow, Arm2, Hand …).
"""
p0 = fk.marker_world(zero_transforms, link_name, local_pos)
return p0 - origin_world
def _pair_estimate(v_model: np.ndarray,
v_obs: np.ndarray,
axis_world: np.ndarray,
marker_ids: Tuple[int, int],
min_baseline_mm: float,
tier: str,
source_link: str) -> Tuple[Optional[float], Optional[float], dict]:
"""
Project model/observed vectors perpendicular to the joint axis and
derive one angle estimate from them. Returns (angle_rad, weight,
per_pair_entry) — angle_rad/weight are None when skipped (baseline
too short).
`tier` (one of the TIER_* constants) and `source_link` (the link the
two marker_ids actually belong to — may differ from the target link
for TIER_FALLBACK_1) are purely descriptive, so callers/reports can
always tell where a given estimate came from.
"""
v_model_perp = _project_perp(v_model, axis_world)
v_obs_perp = _project_perp(v_obs, axis_world)
bl_model = float(np.linalg.norm(v_model_perp))
bl_obs = float(np.linalg.norm(v_obs_perp))
if bl_model < min_baseline_mm or bl_obs < min_baseline_mm:
return None, None, {
"marker_ids": list(marker_ids),
"link": source_link,
"tier": tier,
"skipped": True,
"reason": f"bl_model={bl_model:.1f} bl_obs={bl_obs:.1f} < {min_baseline_mm}",
}
angle = _wrap(_signed_angle_rad(v_model_perp, v_obs_perp, axis_world))
weight = bl_model * bl_obs
entry = {
"marker_ids": list(marker_ids),
"link": source_link,
"tier": tier,
"skipped": False,
"angle_deg": math.degrees(angle),
"baseline_model_mm": bl_model,
"baseline_obs_mm": bl_obs,
"weight": weight,
}
return angle, weight, entry
def _child_links(fk: RobotFK, link_name: str) -> List[str]:
"""Direct children of `link_name` in the kinematic tree (robot.json `parent` field)."""
return [n for n, d in fk.links.items() if d.get("parent") == link_name]
def _axis_aligned_pairs(local_positions: Dict[int, np.ndarray],
own_axis_local: np.ndarray,
tol_mm: float) -> List[Tuple[int, int]]:
"""
Among marker pairs on a CHILD link, return those whose LOCAL connecting
vector is (nearly) parallel to the CHILD's OWN joint axis — i.e. the
component perpendicular to that axis is within `tol_mm` of zero.
Such a pair is invariant to the child's own (still-unknown) rotation
(a rotation about an axis never moves a vector parallel to that same
axis), which is exactly what TIER_FALLBACK_1 relies on. Pairs that
fail this check are skipped here — using them would silently mix in
the child's unknown rotation and bias the result (see module
docstring / TIER 1).
"""
a_hat = own_axis_local / (np.linalg.norm(own_axis_local) + 1e-15)
good: List[Tuple[int, int]] = []
for id1, id2 in combinations(sorted(local_positions.keys()), 2):
v_local = local_positions[id2] - local_positions[id1]
v_radial = v_local - np.dot(v_local, a_hat) * a_hat
if float(np.linalg.norm(v_radial)) <= tol_mm:
good.append((id1, id2))
return good
# ──────────────────────────────────────────────────────────────
# Core estimator (generic — works for any revolute joint)
# ──────────────────────────────────────────────────────────────
@@ -140,6 +292,7 @@ def estimate_revolute_angle(
link_name: str,
known_state: Dict[str, float],
min_baseline_mm: float = 15.0,
child_axis_tol_mm: float = 1.0,
verbose: bool = True,
) -> dict:
"""
@@ -152,7 +305,10 @@ def estimate_revolute_angle(
link_name : e.g. "Arm1", "Ellbow", "Arm2"
known_state : already-estimated joint values (e.g. {"x": 180.0, "y": 86.0})
The target joint variable should NOT be in this dict.
min_baseline_mm : skip pairs with model or observed baseline shorter than this
min_baseline_mm : skip pairs with model or observed baseline shorter than this
child_axis_tol_mm : TIER_FALLBACK_1 only — max perpendicular component (mm)
a child-link marker pair may have relative to the
child's OWN axis to still count as "axis-aligned"
verbose : print report
Returns
@@ -179,8 +335,9 @@ def estimate_revolute_angle(
zero_state = dict(known_state)
zero_state[var] = 0.0
origin_world = fk.joint_origin_world(link_name, zero_state)
axis_world = fk.joint_axis_world(link_name, zero_state)
origin_world = fk.joint_origin_world(link_name, zero_state)
axis_world = fk.joint_axis_world(link_name, zero_state)
zero_transforms = fk.compute(zero_state) # needed to rotate model spokes into world orientation
# ── collect matched markers ───────────────────────────────
model_local: Dict[int, np.ndarray] = {}
@@ -192,15 +349,18 @@ def estimate_revolute_angle(
matched = {mid: (model_local[mid], observed_mm[mid])
for mid in model_local if mid in observed_mm}
if len(matched) < 2:
return {
"status": "failed",
"reason": (f"Need ≥2 matched markers, found {len(matched)}: "
f"{list(matched.keys())}. "
f"Model marker IDs: {list(model_local.keys())}"),
}
# No early return here even if `matched` is empty: TIER_FALLBACK_1
# below needs zero markers on the TARGET link itself — only on its
# child. Whether ANY tier found anything is checked once, at the end.
# ── pairwise estimation ───────────────────────────────────
def _spoke(local_pos: np.ndarray) -> np.ndarray:
return _model_spoke_world(fk, zero_transforms, link_name, origin_world, local_pos)
# ── TIER 0 — PRIMARY: marker-to-marker pairs within this link ──
# Preferred whenever ≥2 markers with a usable (non axis-parallel)
# baseline are visible. Only the AXIS DIRECTION needs to be correct
# for this — not the pivot's position — so it is the more robust
# source of truth and is always tried first.
ids = sorted(matched.keys())
angle_rad_list: List[float] = []
weight_list: List[float] = []
@@ -210,42 +370,102 @@ def estimate_revolute_angle(
l1, o1 = matched[id1]
l2, o2 = matched[id2]
v_model = l2 - l1 # local frame, both in same link
v_obs = o2 - o1 # world frame
v_model = _spoke(l2) - _spoke(l1) # model, world-oriented
v_obs = o2 - o1 # observed, world frame
v_model_perp = _project_perp(v_model, axis_world)
v_obs_perp = _project_perp(v_obs, axis_world)
angle, weight, entry = _pair_estimate(
v_model, v_obs, axis_world, (id1, id2), min_baseline_mm,
tier=TIER_PRIMARY, source_link=link_name)
per_pair.append(entry)
if angle is not None:
angle_rad_list.append(angle)
weight_list.append(weight)
bl_model = float(np.linalg.norm(v_model_perp))
bl_obs = float(np.linalg.norm(v_obs_perp))
tier_used = TIER_PRIMARY
children_tried: List[str] = [] # for the diagnostic message if everything fails
if bl_model < min_baseline_mm or bl_obs < min_baseline_mm:
per_pair.append({
"marker_ids": [id1, id2],
"skipped": True,
"reason": f"bl_model={bl_model:.1f} bl_obs={bl_obs:.1f} < {min_baseline_mm}",
})
continue
# ── TIER 1 — FALLBACK-1: axis-aligned pair on a CHILD link ────
# Only entered when TIER 0 produced NOT A SINGLE usable pair. Looks
# at every DIRECT child of this link and picks marker pairs whose
# local vector is parallel to the CHILD's OWN axis (see
# `_axis_aligned_pairs()`) — those are invariant to the child's own
# still-unknown rotation, so they can stand in for a TIER-0 pair.
# Like TIER 0, this needs only the axis DIRECTION, not the pivot's
# position, so it is preferred over TIER 2.
if not angle_rad_list:
tier_used = TIER_FALLBACK_1
children_tried = _child_links(fk, link_name)
angle = _wrap(_signed_angle_rad(v_model_perp, v_obs_perp, axis_world))
w = bl_model * bl_obs
for child_name in children_tried:
child = links[child_name]
child_ji = child.get("jointToParent", {}) or {}
child_axis_local = np.asarray(child_ji.get("axis", [1, 0, 0]), dtype=float)
angle_rad_list.append(angle)
weight_list.append(w)
per_pair.append({
"marker_ids": [id1, id2],
"skipped": False,
"angle_deg": math.degrees(angle),
"baseline_model_mm": bl_model,
"baseline_obs_mm": bl_obs,
"weight": w,
})
child_model_local: Dict[int, np.ndarray] = {}
for m in child.get("markers", []):
mid = int(m.get("id", -1))
if mid >= 0 and "position" in m:
child_model_local[mid] = np.array(m["position"], dtype=float)
child_matched = {mid: (child_model_local[mid], observed_mm[mid])
for mid in child_model_local if mid in observed_mm}
if len(child_matched) < 2:
continue
aligned_pairs = _axis_aligned_pairs(
{mid: l for mid, (l, _o) in child_matched.items()},
child_axis_local, child_axis_tol_mm)
for id1, id2 in aligned_pairs:
l1, o1 = child_matched[id1]
l2, o2 = child_matched[id2]
v_model = (_model_spoke_world(fk, zero_transforms, child_name, origin_world, l2)
- _model_spoke_world(fk, zero_transforms, child_name, origin_world, l1))
v_obs = o2 - o1
angle, weight, entry = _pair_estimate(
v_model, v_obs, axis_world, (id1, id2), min_baseline_mm,
tier=TIER_FALLBACK_1, source_link=child_name)
per_pair.append(entry)
if angle is not None:
angle_rad_list.append(angle)
weight_list.append(weight)
# ── TIER 2 — FALLBACK-2: pivot + single marker on the target link ──
# Only entered when TIER 1 ALSO produced nothing (e.g. no child
# link, or its markers aren't visible/aligned either). Each
# matched marker on the TARGET link is paired with the joint
# PIVOT instead of another marker, using the rotation axis
# already known from the predecessor joints. This is the last
# resort: unlike TIER 0/1 it additionally relies on the
# predecessor joints' *values* (not just their axis direction)
# being accurate, since the pivot's world position comes from FK
# rather than being observed directly.
if not angle_rad_list:
tier_used = TIER_FALLBACK_2
for mid in ids:
l, o = matched[mid]
v_model = _spoke(l) # pivot → marker, model, world-oriented
v_obs = o - origin_world # pivot → marker, observed
angle, weight, entry = _pair_estimate(
v_model, v_obs, axis_world,
(PIVOT_FALLBACK_ID, mid), min_baseline_mm,
tier=TIER_FALLBACK_2, source_link=link_name)
per_pair.append(entry)
if angle is not None:
angle_rad_list.append(angle)
weight_list.append(weight)
if not angle_rad_list:
return {
"status": "failed",
"reason": "All pairs below min_baseline_mm. "
"Try --min-baseline 5 or check step-3 output.",
"reason": (f"No usable pair at any tier: primary ({len(matched)} "
f"marker(s) on '{link_name}'), fallback-1 (children "
f"tried: {children_tried or 'none'}), fallback-2 "
f"(pivot, same {len(matched)} marker(s)). Try "
f"--min-baseline / --child-axis-tol, or check step-3 output."),
}
mean_deg, c_var, c_std_deg = _circular_mean_deg(
@@ -258,16 +478,30 @@ def estimate_revolute_angle(
print(f" Joint origin (world): [{', '.join(f'{v:.1f}' for v in origin_world)}] mm")
print(f" Joint axis (world): [{', '.join(f'{v:.3f}' for v in axis_world)}]")
print(f" Matched markers: {list(matched.keys())}")
print(f" Pairs used: {len(angle_rad_list)} / {len(list(combinations(ids, 2)))}")
if tier_used == TIER_FALLBACK_1:
print(f" [FALLBACK-1] No usable same-link pair — estimating from "
f"axis-aligned marker pair(s) on child link(s) "
f"{children_tried} instead.")
elif tier_used == TIER_FALLBACK_2:
print(f" [FALLBACK-2] No usable pair on this link or its children — "
f"estimating from pivot + predecessor axis instead "
f"(single-marker spokes).")
print(f" Pairs used: {len(angle_rad_list)} / {len(per_pair)}")
print(f" Angle: {mean_deg:+.2f} ° circular_σ {c_std_deg:.2f} °")
if c_std_deg > 5.0:
print(f" [WARN] high spread step-3 errors or marker overlap")
print(f"\n Pair detail:")
for pp in per_pair:
id0, id1_ = pp["marker_ids"]
m0 = "PIVOT" if id0 == PIVOT_FALLBACK_ID else f"M{id0}"
m1 = "PIVOT" if id1_ == PIVOT_FALLBACK_ID else f"M{id1_}"
link_prefix = f"{pp['link']}:" if pp["link"] != link_name else ""
tag = {TIER_PRIMARY: "", TIER_FALLBACK_1: " [fallback-1]",
TIER_FALLBACK_2: " [fallback-2]"}.get(pp.get("tier"), "")
if pp["skipped"]:
print(f" M{pp['marker_ids'][0]}M{pp['marker_ids'][1]}: SKIPPED {pp['reason']}")
print(f" {link_prefix}{m0}{link_prefix}{m1}{tag}: SKIPPED {pp['reason']}")
else:
print(f" M{pp['marker_ids'][0]}M{pp['marker_ids'][1]}: "
print(f" {link_prefix}{m0}{link_prefix}{m1}{tag}: "
f"{pp['angle_deg']:+7.2f}° "
f"bl_model={pp['baseline_model_mm']:.1f} "
f"bl_obs={pp['baseline_obs_mm']:.1f}")
@@ -280,6 +514,7 @@ def estimate_revolute_angle(
"status": "ok",
"link": link_name,
"joint": var,
"method": tier_used,
"joint_origin_world_mm": origin_world.tolist(),
"joint_axis_world": axis_world.tolist(),
"mean_angle_deg": mean_deg,
@@ -316,6 +551,10 @@ def main() -> int:
p.add_argument("--min-baseline", type=float, default=15.0,
help="Skip pairs with perpendicular baseline < this (mm)")
p.add_argument("--child-axis-tol", type=float, default=1.0,
help="FALLBACK-1 only: max perpendicular component (mm) a "
"child-link marker pair may have relative to the "
"child's own axis to still count as axis-aligned")
p.add_argument("--output", type=Path, default=None,
help="Save result JSON (readable by next 4b as --from-state)")
args = p.parse_args()
@@ -343,7 +582,8 @@ def main() -> int:
observed_mm = observed_mm,
link_name = args.link,
known_state = known_state,
min_baseline_mm = args.min_baseline,
min_baseline_mm = args.min_baseline,
child_axis_tol_mm = args.child_axis_tol,
verbose = True,
)

View File

@@ -0,0 +1,815 @@
#!/usr/bin/env python3
"""
pose_estimation.py
==================
Estimate robot joint angles (x, y, z, a, b, c, e) from triangulated marker
poses, using the kinematic model in robot.json (via robot_fk.py).
Design
------
The estimator is parametrised over JOINT VARIABLES, not links. This handles the
tricky cases of this robot family generically:
* Links with zero own markers (Base/x, Hand/b, Palm/c) — their variable is
observable only through descendant markers.
* A variable shared by several links (FingerA & FingerB share 'e').
* Occluded middle links — global BA reconstructs them from the fingers.
Four switchable methods (robot.json -> pose_estimation.method):
sequential_vector : analytic per joint from marker-pair / normal vectors (fast)
sequential_fk : block-wise least squares along the chain (robust, 1 marker ok)
global_ba : all variables jointly, position + normal residuals, robust loss
hybrid : sequential_fk init -> global_ba refine (default, most stable)
Observation input (robot.json -> pose_estimation.marker_observation):
"corner_pose" (default) -> aruco_marker_poses.json: 3 pos + 3 normal residuals/marker
"corner_points" -> aruco_marker_poses.json: 12 corner residuals for
robot-link markers (4 triangulated corners vs FK
corners; no separate normal), 1 center residual for
root-link (Board: floor/rail) markers whose spin is
uncalibrated. Robust loss acts per corner. Opt-in;
needs `corners_m`. Links via corner_point_links.
"center_point" -> aruco_positions_*.json: position only
Homing integration (appRobotHoming, see doc/Homing_5_Pose.md):
--from-state <json> seed/init state (flat {var: value}, or the
{"accumulated_state": {...}} shape written by
4b_revolute_angle.py) used as x0 for
global_ba/hybrid instead of the internal
estimate_sequential_fk() cold start. Missing
variables default to 0 and are estimated/flagged
normally. Without --from-state, behaviour is
unchanged (internal cold start, as before).
robot.json -> pose_estimation.fit_origin_link = "Arm1" one switch: the
named link's jointToParent.origin Y/Z is fit
TOGETHER WITH the normal pose in the same
global_ba solve (complements the geometric
multi-pose method in doc/Kalibrierung.md
Schritt [4]) and the result is adopted
automatically -- written back into robot.json
(surgical text patch, see patch_robot_json_origin()).
Off by default (key absent/null).
Unobservable joints (confidence "none") are written as value=null in the
output JSON — never a fabricated 0 (see movements.<var>.observable).
Both the engine (estimate_pose) and a CLI (main) live here.
"""
from __future__ import annotations
import argparse
import json
import math
import os
import re
import sys
import time
from collections import defaultdict
from pathlib import Path
from typing import Any, Dict, List, Optional, Tuple
import numpy as np
sys.path.insert(0, str(Path(__file__).parent))
from robot_fk import RobotFK, STATE_KEYS # noqa: E402
try:
from scipy.optimize import least_squares
HAVE_SCIPY = True
except ImportError:
HAVE_SCIPY = False
# ==================================================================
# Config
# ==================================================================
DEFAULT_CFG: Dict[str, Any] = {
"method": "hybrid",
"marker_observation": "corner_pose",
"use_normals": True,
"normal_weight": 100.0,
"use_marker_weight": False,
"robust_loss": "huber",
"huber_delta_mm": 8.0,
"max_iterations": 200,
"min_cameras_per_marker": 2,
"finger_block_joints": ["b", "c", "e"],
"per_link_method": {},
# Nur im marker_observation="corner_points"-Modus relevant: welche Links die
# 4 Eck-Residuen nutzen. None/absent = alle Nicht-Root-Links (= der Roboter);
# der Root-Link (Board mit Boden-/Rail-Markern) nutzt ein Center-Residuum.
# Hintergrund: nur die Roboter-Marker-Spins sind kalibriert/verifiziert; die
# Board/Rail-Spins nicht — deren Eckreihenfolge wäre unzuverlässig. Board ist
# zudem Root (Residuum konstant bzgl. der Gelenke). Explizite Liste möglich,
# z.B. ["Arm1","Ellbow","Arm2","Hand","Palm","FingerA","FingerB"].
"corner_point_links": None,
# One switch: if set to a link name (e.g. "Arm1"), that link's
# jointToParent.origin Y/Z is fit together with the normal pose (same
# global_ba solve) and the result is written back into robot.json
# automatically. None/absent = off, no behaviour change. See
# doc/Homing_5_Pose.md "Kalibrier-Switch".
"fit_origin_link": None,
}
def load_pose_cfg(robot_data: Dict[str, Any]) -> Dict[str, Any]:
cfg = dict(DEFAULT_CFG)
cfg.update(robot_data.get("pose_estimation", {}) or {})
return cfg
# ==================================================================
# Observations
# ==================================================================
def load_observations(path: str, use_normals: bool, min_cams: int = 2) -> Dict[int, Dict[str, Any]]:
"""
Load marker observations. Accepts aruco_marker_poses.json (with measured
normal + num_cameras + 4 triangulated corners) or aruco_positions_*.json
(position only).
Returns: marker_id -> {pos_mm:(3,), normal:(3,)|None, corners_mm:(4,3)|None,
link:str, n_cams:int, weight:float}
corners_mm (aus `corners_m`, m→mm) speist den marker_observation=
"corner_points"-Modus in residual_vector().
"""
data = json.load(open(path, "r", encoding="utf-8"))
out: Dict[int, Dict[str, Any]] = {}
for m in data.get("markers", []):
mid = int(m.get("marker_id", m.get("id", -1)))
if mid < 0:
continue
n_cams = int(m.get("num_cameras", 99))
if n_cams < min_cams:
continue
if "position_mm" in m:
pos = np.array(m["position_mm"], dtype=float)
elif "position_m" in m:
pos = np.array(m["position_m"], dtype=float) * 1000.0
else:
continue
nrm = None
if use_normals and m.get("normal") is not None:
nv = np.array(m["normal"], dtype=float)
nn = np.linalg.norm(nv)
if nn > 1e-9:
nrm = nv / nn
corners_mm = None
cm = m.get("corners_m")
if cm is not None:
arr = np.array(cm, dtype=float)
if arr.shape == (4, 3):
corners_mm = arr * 1000.0
out[mid] = {"pos_mm": pos, "normal": nrm, "corners_mm": corners_mm,
"link": m.get("link", "?"), "n_cams": n_cams,
"weight": float(m.get("weight", 1.0))}
return out
def load_seed_state(path: str) -> Dict[str, float]:
"""
Load a partial/full joint state to use as an optimisation seed (--from-state).
Accepts either a flat {variable: value} dict, or the
{"accumulated_state": {...}, ...} wrapper written by 4b_revolute_angle.py —
same unwrap rule as server/homingOrchestrator.js
(`stateData.accumulated_state ?? stateData`), so 4b's output files can be
passed in directly. Unknown keys are ignored; missing STATE_KEYS are simply
absent from the returned dict (caller defaults them, e.g. to 0.0).
"""
data = json.load(open(path, "r", encoding="utf-8"))
raw = data.get("accumulated_state", data) if isinstance(data, dict) else {}
return {k: float(v) for k, v in raw.items() if k in STATE_KEYS and v is not None}
# ==================================================================
# Kinematic chain analysis
# ==================================================================
def analyze_chain(fk: RobotFK) -> Dict[str, Any]:
"""
Derive, generically from the FK topology:
ordered_vars : movable joint variables, root->tip order, de-duplicated
var_links : variable -> list of links it drives
link_markers : link -> [model marker ids]
blocks : sequential estimation blocks; each block groups the
zero-marker ancestor variables with the next marker-
bearing joint variable, estimated from that link's own
markers (+ siblings sharing the same variable).
"""
links = fk.links
topo = fk._topo
link_markers: Dict[str, List[int]] = {}
for ln, ld in links.items():
ids = []
for mk in ld.get("markers", []) or []:
if "id" in mk and "position" in mk:
ids.append(int(mk["id"]))
link_markers[ln] = ids
link_var: Dict[str, str] = {}
for ln, ld in links.items():
j = ld.get("jointToParent", {}) or {}
if str(j.get("type", "")).lower() in ("revolute", "linear"):
v = str(j.get("variable", "")).lower()
if v:
link_var[ln] = v
var_type: Dict[str, str] = {}
var_links: Dict[str, List[str]] = defaultdict(list)
for ln, v in link_var.items():
var_links[v].append(ln)
var_type[v] = str(links[ln].get("jointToParent", {}).get("type", "")).lower()
ordered_vars: List[str] = []
for ln in topo:
if ln in link_var and link_var[ln] not in ordered_vars:
ordered_vars.append(link_var[ln])
# ---- build blocks ----
blocks: List[Dict[str, Any]] = []
var_block: Dict[str, int] = {}
pending: List[str] = []
for ln in topo:
if ln not in link_var:
continue
v = link_var[ln]
own = link_markers.get(ln, [])
if v in var_block:
# shared variable already in a block -> add this link's markers there
if own:
blocks[var_block[v]]["markers"].extend(own)
continue
if own:
bvars = []
for x in pending + [v]:
if x not in bvars and x not in var_block:
bvars.append(x)
blocks.append({"vars": bvars, "markers": list(own), "anchor": ln})
for x in bvars:
var_block[x] = len(blocks) - 1
pending = []
else:
if v not in pending:
pending.append(v)
if pending:
blocks.append({"vars": pending, "markers": [], "anchor": None})
for x in pending:
var_block[x] = len(blocks) - 1
# subtree_markers[L] = L's own markers + all descendants' markers. Lets
# observability() credit a block whose own link saw nothing this capture
# but whose CHILD link did (e.g. Ellbow has no visible markers, but Arm2's
# markers still constrain z through the chain — same idea as 4b's
# Fallback-1, just for confidence reporting here, not for the fit itself).
children: Dict[str, List[str]] = defaultdict(list)
for ln, ld in links.items():
p = ld.get("parent")
if p:
children[p].append(ln)
subtree_markers: Dict[str, List[int]] = {}
for ln in reversed(topo):
ids = list(link_markers.get(ln, []))
for c in children.get(ln, []):
ids.extend(subtree_markers.get(c, []))
subtree_markers[ln] = ids
return {
"ordered_vars": ordered_vars,
"var_type": var_type,
"var_links": dict(var_links),
"link_markers": link_markers,
"subtree_markers": subtree_markers,
"blocks": blocks,
}
# ==================================================================
# Residuals
# ==================================================================
def model_markers(fk: RobotFK, state: Dict[str, float]) -> Dict[int, Dict[str, np.ndarray]]:
T = fk.compute(state)
return fk.all_markers_world(T) # mid -> {world_mm, normal_world, link, local_mm}
def _resolve_corner_links(fk: RobotFK, cfg: Dict[str, Any]) -> set:
"""
Welche Links im "corner_points"-Modus die 4 Eck-Residuen nutzen.
Explizite Liste in cfg["corner_point_links"], sonst alle Nicht-Root-Links
(der Root-Link Board trägt die unkalibrierten Boden-/Rail-Marker und nutzt
ein Center-Residuum).
"""
explicit = cfg.get("corner_point_links")
if isinstance(explicit, list) and explicit:
return set(explicit)
links = fk.links
roots = {ln for ln, ld in links.items()
if not ld.get("parent") or ld.get("parent") not in links}
return set(links.keys()) - roots
def residual_vector(state: Dict[str, float], fk: RobotFK, obs: Dict[int, Dict[str, Any]],
marker_ids: List[int], cfg: Dict[str, Any]) -> np.ndarray:
"""
Residuen über die gegebenen Marker. Modus via pose_estimation.marker_observation:
"corner_pose" (Default): 3 Position (mm) + optional 3 Normale
(×normal_weight) je Marker — wie bisher.
"corner_points": 12 Eck-Residuen (4 Ecken × xyz, mm) NUR für Marker
auf den `corner_point_links` (z.B. Hand/Finger),
KEINE separate Normale (Orientierung steckt in den
Ecken). Alle übrigen Marker verhalten sich wie im
Default-Modus (Center + optionale Normale) — außer
dem Root-Link (Board: Boden-/Rail-Marker, Spin
unkalibriert), der nur Center bekommt ("ein Punkt
pro Marker"). So lassen sich Ecken gezielt für
Hand/Finger scharfschalten, ohne Arme/Board zu
verändern.
"""
model = model_markers(fk, state)
res: List[float] = []
use_mw = bool(cfg.get("use_marker_weight", False))
obs_mode = str(cfg.get("marker_observation", "corner_pose")).lower()
if obs_mode == "corner_points":
corner_links = _resolve_corner_links(fk, cfg)
roots = {ln for ln, ld in fk.links.items()
if not ld.get("parent") or ld.get("parent") not in fk.links}
w_n = float(cfg.get("normal_weight", 30.0))
use_n = bool(cfg.get("use_normals", True))
for mid in marker_ids:
if mid not in model or mid not in obs:
continue
mw = float(obs[mid].get("weight", 1.0)) if use_mw else 1.0
mm = model[mid]
link = mm.get("link")
oc = obs[mid].get("corners_mm")
mc = mm.get("corners_world")
if link in corner_links and oc is not None and mc is not None:
dc = (np.asarray(mc, float) - np.asarray(oc, float)) * mw # (4,3)
res.extend(dc.reshape(-1).tolist()) # 12 Werte
continue
# Nicht-Eck-Marker verhalten sich wie im Default-Modus: Center +
# optionale Normale — AUSSER auf dem Root-Link (Board: Boden-/Rail-
# Marker mit unkalibriertem Spin), der nur Center bekommt ("ein
# Punkt pro Marker"). So bleiben Arme/Board unverändert, wenn nur
# Hand/Finger über corner_point_links auf Ecken laufen.
dp = (np.asarray(mm["world_mm"], float) - obs[mid]["pos_mm"]) * mw
res.extend(dp.tolist())
if link not in roots and use_n and obs[mid]["normal"] is not None and "normal_world" in mm:
dn = (np.asarray(mm["normal_world"], float) - obs[mid]["normal"]) * w_n * mw
res.extend(dn.tolist())
return np.asarray(res, dtype=float)
# Default: Center (mm) + optionale Normale (skaliert)
w_n = float(cfg.get("normal_weight", 30.0))
use_n = bool(cfg.get("use_normals", True))
for mid in marker_ids:
if mid not in model or mid not in obs:
continue
mm = model[mid]
mw = float(obs[mid].get("weight", 1.0)) if use_mw else 1.0
dp = (np.asarray(mm["world_mm"], float) - obs[mid]["pos_mm"]) * mw
res.extend(dp.tolist())
if use_n and obs[mid]["normal"] is not None and "normal_world" in mm:
dn = (np.asarray(mm["normal_world"], float) - obs[mid]["normal"]) * w_n * mw
res.extend(dn.tolist())
return np.asarray(res, dtype=float)
def _state_from_vec(var_names: List[str], vec: np.ndarray, base: Dict[str, float]) -> Dict[str, float]:
s = dict(base)
for name, val in zip(var_names, vec):
s[name] = float(val)
return s
# ==================================================================
# Method: global bundle adjustment
# ==================================================================
def estimate_global_ba(fk: RobotFK, obs: Dict[int, Dict[str, Any]], var_names: List[str],
x0: Dict[str, float], cfg: Dict[str, Any]) -> Dict[str, float]:
if not HAVE_SCIPY:
print("[WARN] scipy missing — global_ba skipped, returning init")
return dict(x0)
marker_ids = list(obs.keys())
base = {k: 0.0 for k in STATE_KEYS}
base.update(x0)
# One switch (pose_estimation.fit_origin_link): also free that link's
# jointToParent.origin Y/Z as 2 extra parameters of THIS SAME solve, no
# separate pass. fk.links[...] is mutated in place -- compute() re-reads
# it fresh every call (robot_fk.py), so this takes effect immediately and
# is left adopted on success (main() writes it back into robot.json).
origin_link = cfg.get("fit_origin_link")
origin = fk.links.get(origin_link, {}).get("jointToParent", {}).get("origin") if origin_link else None
origin = origin if isinstance(origin, list) and len(origin) == 3 else None
origin_before = list(origin) if origin else None
n_state = len(var_names)
vec0 = np.array([base.get(v, 0.0) for v in var_names]
+ (origin_before[1:3] if origin else []), dtype=float)
def fun(vec):
st = _state_from_vec(var_names, vec[:n_state], base)
if origin:
origin[1], origin[2] = float(vec[n_state]), float(vec[n_state + 1])
return residual_vector(st, fk, obs, marker_ids, cfg)
loss = cfg.get("robust_loss", "huber")
f_scale = float(cfg.get("huber_delta_mm", 8.0))
try:
sol = least_squares(fun, vec0, loss=loss, f_scale=f_scale,
max_nfev=int(cfg.get("max_iterations", 200)) * max(1, len(vec0)))
state = _state_from_vec(var_names, sol.x[:n_state], base)
if origin:
origin[1], origin[2] = float(sol.x[n_state]), float(sol.x[n_state + 1])
print(f"[INFO] fit_origin_link={origin_link}: Y,Z {origin_before[1:3]} "
f"-> [{origin[1]:.3f}, {origin[2]:.3f}]")
return state
except Exception as exc:
print(f"[WARN] global_ba failed: {exc}")
if origin:
origin[1], origin[2] = origin_before[1], origin_before[2] # restore on failure
return dict(base)
# ==================================================================
# Method: sequential block-wise FK fit
# ==================================================================
def _multistart_values(vtype: str) -> List[float]:
# revolute: scan the circle to escape local minima at large angles
if vtype == "revolute":
return [0.0, 60.0, 120.0, 180.0, 240.0, 300.0]
return [0.0]
def estimate_sequential_fk(fk: RobotFK, obs: Dict[int, Dict[str, Any]], chain: Dict[str, Any],
cfg: Dict[str, Any], seed: Optional[Dict[str, float]] = None
) -> Dict[str, float]:
"""
Estimate block by block along the chain, freezing already-solved variables.
seed: optional partial/full state (e.g. from 4b_revolute_angle.py) to trust
as a starting point. A block is SKIPPED entirely (seed used as-is, no
re-fit) only if ALL of its variables are present in seed. Blocks with any
missing variable are still fit normally — including their own multi-start
— but using the seeded values of EARLIER blocks as fixed context instead
of 0. This keeps the local-minimum protection for whatever the seed does
NOT cover (see doc/Homing_5_Pose.md "Wichtige Einschraenkung"), while not
re-perturbing values the caller already trusts.
"""
state = {k: 0.0 for k in STATE_KEYS}
if seed:
state.update({k: v for k, v in seed.items() if k in STATE_KEYS})
var_type = chain["var_type"]
for block in chain["blocks"]:
bvars = block["vars"]
bmarkers = [m for m in block["markers"] if m in obs]
if not bvars:
continue
if seed and all(v in seed for v in bvars):
continue # fully seeded — trust it, don't re-fit
if not bmarkers:
# unobservable block: leave at seed/0, flag later
continue
if not HAVE_SCIPY:
continue
base = dict(state)
def fun(vec, _bvars=bvars, _bm=bmarkers, _base=base):
st = _state_from_vec(_bvars, vec, _base)
return residual_vector(st, fk, obs, _bm, cfg)
# multi-start over the first revolute variable in the block
starts = [[0.0] * len(bvars)]
lead_type = var_type.get(bvars[0], "linear")
if lead_type == "revolute":
starts = []
for a0 in _multistart_values("revolute"):
s = [0.0] * len(bvars)
s[0] = a0
starts.append(s)
best, best_cost = None, float("inf")
for s0 in starts:
try:
sol = least_squares(fun, np.array(s0, dtype=float),
loss=cfg.get("robust_loss", "huber"),
f_scale=float(cfg.get("huber_delta_mm", 8.0)),
max_nfev=200 * max(1, len(bvars)))
if sol.cost < best_cost:
best_cost, best = sol.cost, sol.x
except Exception:
continue
if best is not None:
for name, val in zip(bvars, best):
state[name] = float(val)
# wrap revolute angles to (-180, 180]
for v, vt in var_type.items():
if vt == "revolute":
state[v] = (state[v] + 180.0) % 360.0 - 180.0
return state
# ==================================================================
# Method: sequential analytic vector (per revolute joint)
# ==================================================================
def estimate_sequential_vector(fk: RobotFK, obs: Dict[int, Dict[str, Any]], chain: Dict[str, Any],
cfg: Dict[str, Any]) -> Dict[str, float]:
"""
Analytic angle from marker geometry where possible. For revolute joints with
>=2 markers on the link, use the perpendicular marker-pair vector. Falls back
to the FK block solver for linear / zero-marker / single-marker cases, so it
always returns a full state (still cheaper than full sequential_fk because
well-populated joints are solved in closed form).
"""
state = {k: 0.0 for k in STATE_KEYS}
var_type = chain["var_type"]
link_markers = chain["link_markers"]
var_links = chain["var_links"]
for block in chain["blocks"]:
bvars = block["vars"]
if len(bvars) == 1 and var_type.get(bvars[0]) == "revolute":
v = bvars[0]
ln = var_links[v][0]
mids = [m for m in link_markers.get(ln, []) if m in obs]
if len(mids) >= 2:
# model vectors must be expressed in the WORLD frame at angle=0
# (the link frame is already rotated by the parents y,z,...), so
# use FK marker world positions with this joint set to 0.
state_v0 = dict(state)
state_v0[v] = 0.0
model_v0 = model_markers(fk, state_v0)
axis_world = fk.joint_axis_world(ln, state_v0)
ang = _angle_from_pairs_world(mids, model_v0, obs, axis_world)
if ang is not None:
state[v] = ang
continue
# fallback: block FK fit for this single block
_fit_single_block(fk, obs, block, var_type, cfg, state)
for v, vt in var_type.items():
if vt == "revolute":
state[v] = (state[v] + 180.0) % 360.0 - 180.0
return state
def _angle_from_pairs_world(mids: List[int], model_v0: Dict[int, Dict[str, np.ndarray]],
obs: Dict[int, Dict[str, Any]], axis_world: np.ndarray) -> Optional[float]:
from itertools import combinations
a = np.asarray(axis_world, float)
a = a / (np.linalg.norm(a) + 1e-12)
angs, ws = [], []
for i, j in combinations(mids, 2):
if i not in model_v0 or j not in model_v0:
continue
vm = np.asarray(model_v0[j]["world_mm"], float) - np.asarray(model_v0[i]["world_mm"], float) # world @ angle 0
vo = obs[j]["pos_mm"] - obs[i]["pos_mm"] # observed vector (world, mm)
vm_p = vm - np.dot(vm, a) * a
vo_p = vo - np.dot(vo, a) * a
if np.linalg.norm(vm_p) < 5 or np.linalg.norm(vo_p) < 5:
continue
ang = math.atan2(float(np.dot(a, np.cross(vm_p, vo_p))), float(np.dot(vm_p, vo_p)))
angs.append(ang)
ws.append(np.linalg.norm(vm_p) * np.linalg.norm(vo_p))
if not angs:
return None
s = sum(w * math.sin(x) for w, x in zip(ws, angs))
c = sum(w * math.cos(x) for w, x in zip(ws, angs))
return math.degrees(math.atan2(s, c))
def _fit_single_block(fk, obs, block, var_type, cfg, state):
if not HAVE_SCIPY:
return
bvars = block["vars"]
bmarkers = [m for m in block["markers"] if m in obs]
if not bvars or not bmarkers:
return
base = dict(state)
def fun(vec):
return residual_vector(_state_from_vec(bvars, vec, base), fk, obs, bmarkers, cfg)
starts = [[0.0] * len(bvars)]
if var_type.get(bvars[0]) == "revolute":
starts = [[a0] + [0.0] * (len(bvars) - 1) for a0 in _multistart_values("revolute")]
best, best_cost = None, float("inf")
for s0 in starts:
try:
sol = least_squares(fun, np.array(s0, float), loss=cfg.get("robust_loss", "huber"),
f_scale=float(cfg.get("huber_delta_mm", 8.0)), max_nfev=200 * max(1, len(bvars)))
if sol.cost < best_cost:
best_cost, best = sol.cost, sol.x
except Exception:
continue
if best is not None:
for name, val in zip(bvars, best):
state[name] = float(val)
# ==================================================================
# Dispatch
# ==================================================================
def observability(chain: Dict[str, Any], obs: Dict[int, Dict[str, Any]]) -> Dict[str, Dict[str, Any]]:
"""
Per-variable confidence from how well its estimation block is determined.
A block groups coupled variables (e.g. b,c,e on the fingers); confidence is
driven by markers-per-variable in that block:
high : >= 2 markers per variable (well over-determined)
medium : >= 1 marker per variable
low : fewer markers than variables (under-determined — distrust!),
OR no own markers seen but a child link's markers were
(indirect evidence through the chain, e.g. Ellbow via Arm2)
none : no markers at all, not even indirectly (variable left at 0)
"""
info: Dict[str, Dict[str, Any]] = {}
subtree_markers = chain.get("subtree_markers", {})
for block in chain["blocks"]:
seen = [m for m in block["markers"] if m in obs]
indirect = False
if not seen and block["anchor"]:
seen = [m for m in subtree_markers.get(block["anchor"], []) if m in obs]
indirect = bool(seen)
nvars = max(1, len(block["vars"]))
ratio = len(seen) / nvars
if len(seen) == 0:
conf = "none"
elif indirect:
conf = "low" # indirect/coupled through a child link, not direct
elif ratio >= 2.0:
conf = "high"
elif ratio >= 1.0:
conf = "medium"
else:
conf = "low"
for v in block["vars"]:
info[v] = {"observable": len(seen) > 0, "n_markers": len(seen),
"block_vars": len(block["vars"]), "confidence": conf,
"block_anchor": block["anchor"], "indirect": indirect}
return info
def estimate_pose(fk: RobotFK, obs: Dict[int, Dict[str, Any]], cfg: Dict[str, Any],
seed: Optional[Dict[str, float]] = None) -> Dict[str, Any]:
"""
seed: optional partial/full joint state (e.g. from load_seed_state(), the
4b_revolute_angle.py chain) to trust as a starting point for global_ba/
hybrid. Passed through to estimate_sequential_fk(), which skips re-fitting
any block that is FULLY covered by seed and otherwise still applies its
normal per-block multi-start — so variables the seed does NOT cover keep
the existing local-minimum protection (see doc/Homing_5_Pose.md "Wichtige
Einschraenkung") instead of silently defaulting to an unprotected 0.
sequential_vector ignores seed (no x0 input; left untouched on purpose —
it is the cheap analytic method, not the one this seeding targets).
"""
chain = analyze_chain(fk)
var_names = chain["ordered_vars"]
method = str(cfg.get("method", "hybrid")).lower()
obsv = observability(chain, obs)
if method == "sequential_vector":
state = estimate_sequential_vector(fk, obs, chain, cfg)
elif method == "sequential_fk":
state = estimate_sequential_fk(fk, obs, chain, cfg, seed=seed)
else: # global_ba / hybrid (default) — both use the same init->refine path
init = estimate_sequential_fk(fk, obs, chain, cfg, seed=seed)
state = estimate_global_ba(fk, obs, var_names, init, cfg)
# final residual stats over all observed markers
final_res = residual_vector(state, fk, obs, list(obs.keys()), cfg)
rms = float(np.sqrt(np.mean(final_res ** 2))) if final_res.size else 0.0
return {"state": state, "method": method, "observability": obsv,
"residual_rms": rms, "num_markers": len(obs)}
# ==================================================================
# CLI
# ==================================================================
def patch_robot_json_origin(robot_path: str, link_name: str, yz: Tuple[float, float]) -> bool:
"""
Surgically rewrite links.<link_name>.jointToParent.origin[1],[2] (Y,Z) in
the robot.json TEXT in place. robot.json has a hand-curated, compact
format (markers etc. one per line) -- a full json.load()+json.dump()
round-trip would reformat the whole file, so this only touches the one
"origin": [...] array belonging to <link_name> (X left untouched).
Returns True if a match was found and patched.
"""
with open(robot_path, "r", encoding="utf-8") as f:
text = f.read()
link_m = re.search(r'"%s"\s*:\s*\{' % re.escape(link_name), text)
if not link_m:
return False
origin_m = re.search(r'"origin"\s*:\s*\[([^\]]*)\]', text[link_m.end():])
if not origin_m:
return False
parts = [p.strip() for p in origin_m.group(1).split(",")]
if len(parts) != 3:
return False
parts[1] = f"{float(yz[0]):.4f}".rstrip("0").rstrip(".")
parts[2] = f"{float(yz[1]):.4f}".rstrip("0").rstrip(".")
new_array = f"[{parts[0]}, {parts[1]}, {parts[2]}]"
start = link_m.end() + origin_m.start()
end = link_m.end() + origin_m.end()
with open(robot_path, "w", encoding="utf-8") as f:
f.write(text[:start] + '"origin": ' + new_array + text[end:])
return True
def main() -> None:
ap = argparse.ArgumentParser(description="Estimate robot joint angles from marker poses")
ap.add_argument("markers", help="aruco_marker_poses.json (corner_pose) or aruco_positions_*.json (center)")
ap.add_argument("-robot", "--robot", required=True)
ap.add_argument("-out", "--out", default=None)
ap.add_argument("--method", default=None, help="override robot.json method")
ap.add_argument("--marker-observation", default=None, dest="marker_observation",
help="override robot.json marker_observation "
"(corner_pose | corner_points | center_point)")
ap.add_argument("--from-state", default=None, metavar="JSON",
help="Seed/init state (flat {var:value} or {accumulated_state:{...}} as "
"written by 4b_revolute_angle.py). Used as x0 for global_ba/hybrid "
"instead of the internal cold start. See doc/Homing_5_Pose.md.")
args = ap.parse_args()
robot_data = json.load(open(args.robot, "r", encoding="utf-8"))
cfg = load_pose_cfg(robot_data)
if args.method:
cfg["method"] = args.method
if args.marker_observation:
cfg["marker_observation"] = args.marker_observation
fk = RobotFK(robot_data)
obs = load_observations(args.markers, cfg.get("use_normals", True),
int(cfg.get("min_cameras_per_marker", 2)))
seed = load_seed_state(args.from_state) if args.from_state else None
print(f"[INFO] method={cfg['method']} | observed markers={len(obs)} | use_normals={cfg.get('use_normals')}"
+ (f" | seed={seed}" if seed else ""))
if cfg.get("fit_origin_link"):
print(f"[INFO] fit_origin_link={cfg['fit_origin_link']} -> robot.json wird bei Erfolg aktualisiert")
result = estimate_pose(fk, obs, cfg, seed=seed)
st = result["state"]
print("\nEstimated joint values:")
for v in ["x", "y", "z", "a", "b", "c", "e"]:
ob = result["observability"].get(v, {})
unit = "mm" if v in ("x", "e") else "deg"
conf = ob.get("confidence", "?")
tag = "" if ob.get("observable", False) else " [UNOBSERVABLE -> null]"
print(f" {v}: {st.get(v, 0.0):8.2f} {unit} (markers={ob.get('n_markers','?')}, conf={conf}){tag}")
print(f"\n[INFO] residual RMS over {result['num_markers']} markers: {result['residual_rms']:.3f}")
movements = {}
for v in ["x", "y", "z", "a", "b", "c", "e"]:
ob = result["observability"].get(v, {})
observable = ob.get("observable", False)
movements[v] = {
# Unobservable -> null, never a fabricated 0 (see module docstring).
"value": st.get(v, 0.0) if observable else None,
"unit": "mm" if v in ("x", "e") else "deg",
"observable": observable,
"confidence": ob.get("confidence", "none"),
"n_markers": ob.get("n_markers", 0),
}
out = {
"schema_version": "1.0",
"created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
"method": result["method"],
"seeded": seed is not None,
"movements": movements,
"residual_rms": result["residual_rms"],
"num_markers": result["num_markers"],
}
out_path = args.out or os.path.join(os.path.dirname(args.markers), "robot_state.json")
json.dump(out, open(out_path, "w", encoding="utf-8"), indent=2)
print(f"[INFO] wrote {out_path}")
# "Uebernehmen": fit_origin_link's Y/Z is already adopted on `fk` (see
# estimate_global_ba) -- write it back into robot.json itself.
origin_link = cfg.get("fit_origin_link")
if origin_link:
origin = fk.links.get(origin_link, {}).get("jointToParent", {}).get("origin")
if isinstance(origin, list) and len(origin) == 3:
if patch_robot_json_origin(args.robot, origin_link, (origin[1], origin[2])):
print(f"[INFO] robot.json aktualisiert: {origin_link}.jointToParent.origin "
f"= [{origin[0]}, {origin[1]:.3f}, {origin[2]:.3f}]")
else:
print(f"[WARN] {origin_link}.jointToParent.origin in {args.robot} nicht gefunden — "
f"nicht aktualisiert (Wert nur in {out_path} sichtbar)")
if __name__ == "__main__":
main()

Binary file not shown.

Binary file not shown.

Binary file not shown.

File diff suppressed because it is too large Load Diff

409
scripts/robot_fk.py Normal file
View File

@@ -0,0 +1,409 @@
#!/usr/bin/env python3
"""
robot_fk.py
-----------
Minimal forward kinematics engine for the robot.json format.
Matches the Blender hierarchy used by render_robot.py exactly:
world_T_link = world_T_parent
@ Translate(mountPosition) @ Rotate_xyz(mountRotation)
@ Translate(jointOrigin) @ Rotate_xyz(joint.rotation)
@ T_motion
T_motion = Rotate(axis, value_deg) for revolute joints
Translate(axis * value_mm) for linear joints
Units throughout: millimetres, degrees.
Public API
----------
fk = RobotFK.from_file("robot.json")
transforms = fk.compute({"x": 180, "y": 86, "z": -120,
"a": -60, "b": 22, "c": 91, "e": 10})
# → dict link_name -> 4×4 np.ndarray (world frame, mm)
p_world = fk.marker_world(transforms, "Arm1", [0, -160, 35])
# → np.ndarray shape (3,), in mm
all_m = fk.all_markers_world(transforms)
# → dict marker_id -> {"world_mm", "link", "local_mm"}
# Cumulative x-offset for a link at all-zero state
# (useful for 4a: x_slider = world_x - local_x - link_x_at_zero)
x0 = fk.link_x_at_zero_state("Arm1") # → float mm
"""
from __future__ import annotations
import json
import math
from pathlib import Path
from typing import Any, Dict, List, Optional, Sequence, Tuple
import numpy as np
STATE_KEYS = ("x", "y", "z", "a", "b", "c", "e")
# ──────────────────────────────────────────────────────────────
# Low-level matrix helpers
# ──────────────────────────────────────────────────────────────
def _rot_axis_angle(axis: Sequence[float], angle_deg: float) -> np.ndarray:
"""3×3 rotation matrix via Rodrigues (axis need not be normalised)."""
ax = np.asarray(axis, dtype=float)
n = float(np.linalg.norm(ax))
if n < 1e-12:
return np.eye(3)
ax = ax / n
c = math.cos(math.radians(angle_deg))
s = math.sin(math.radians(angle_deg))
t = 1.0 - c
x, y, z = ax
return np.array([
[t*x*x + c, t*x*y - s*z, t*x*z + s*y],
[t*x*y + s*z, t*y*y + c, t*y*z - s*x],
[t*x*z - s*y, t*y*z + s*x, t*z*z + c ],
])
def _rot_xyz_euler(rx: float, ry: float, rz: float) -> np.ndarray:
"""XYZ Euler angles (degrees) → 3×3 — matches Blender XYZ Euler mode."""
return (_rot_axis_angle([0, 0, 1], rz)
@ _rot_axis_angle([0, 1, 0], ry)
@ _rot_axis_angle([1, 0, 0], rx))
def make_T(R: np.ndarray, t: Sequence[float]) -> np.ndarray:
"""4×4 homogeneous transform."""
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = np.asarray(t, dtype=float)
return T
def transform_point(T: np.ndarray, p: Sequence[float]) -> np.ndarray:
"""Apply 4×4 transform to a 3-D point."""
h = np.array([p[0], p[1], p[2], 1.0])
return (T @ h)[:3]
# ──────────────────────────────────────────────────────────────
# FK engine
# ──────────────────────────────────────────────────────────────
class RobotFK:
"""Forward kinematics for the robot.json format."""
def __init__(self, robot_data: Dict[str, Any]):
self.robot = robot_data
self.links: Dict[str, Any] = robot_data.get("links", {})
self._topo: List[str] = self._topological_sort()
# ── construction ─────────────────────────────────────────
@classmethod
def from_file(cls, path) -> "RobotFK":
with open(path, "r", encoding="utf-8") as f:
return cls(json.load(f))
def _topological_sort(self) -> List[str]:
parent_map = {n: d.get("parent") for n, d in self.links.items()}
visited, order = set(), []
def visit(name: str) -> None:
if name in visited:
return
visited.add(name)
p = parent_map.get(name)
if p and p in self.links:
visit(p)
order.append(name)
for name in self.links:
visit(name)
return order
# ── core computation ──────────────────────────────────────
def compute(self, joint_values: Dict[str, float]) -> Dict[str, np.ndarray]:
"""
Compute link world transforms for the given joint state.
Parameters
----------
joint_values : dict variable_name -> value
Linear joints (x, e): mm
Revolute joints (y, z, a, b, c): degrees
Returns
-------
dict link_name -> 4×4 np.ndarray (world frame, mm)
"""
state = {k: 0.0 for k in STATE_KEYS}
for k, v in joint_values.items():
if k in state:
state[k] = float(v)
transforms: Dict[str, np.ndarray] = {}
for link_name in self._topo:
d = self.links[link_name]
parent = d.get("parent")
T_parent = transforms.get(parent, np.eye(4)) if parent else np.eye(4)
# 1 · Mount (static in parent frame)
mp = d.get("mountPosition", [0, 0, 0])
mr = d.get("mountRotation", [0, 0, 0])
T_m = make_T(_rot_xyz_euler(*mr), mp)
# 2 · Joint origin (pivot point in mount frame)
ji = d.get("jointToParent", {}) or {}
jp = ji.get("origin", [0, 0, 0])
jr = ji.get("rotation", [0, 0, 0])
T_j = make_T(_rot_xyz_euler(*jr), jp)
# 3 · Motion
jtype = str(ji.get("type", "fixed")).lower()
var = str(ji.get("variable", "")).lower()
axis = np.asarray(ji.get("axis", [1, 0, 0]), dtype=float)
val = state.get(var, 0.0)
if jtype == "revolute":
T_mot = make_T(_rot_axis_angle(axis, val), [0, 0, 0])
elif jtype == "linear":
n = float(np.linalg.norm(axis))
u = axis / n if n > 1e-12 else np.array([1.0, 0, 0])
T_mot = make_T(np.eye(3), u * val)
else:
T_mot = np.eye(4)
transforms[link_name] = T_parent @ T_m @ T_j @ T_mot
return transforms
# ── marker helpers ────────────────────────────────────────
@staticmethod
def marker_world(transforms: Dict[str, np.ndarray],
link_name: str,
local_pos: Sequence[float]) -> np.ndarray:
"""Transform a local marker position → world (mm)."""
return transform_point(transforms.get(link_name, np.eye(4)), local_pos)
# ── Marker-Eckpunkte (Mehrpunkt-Residuen, doc/Homing_5_Pose_MultiPoint_Weighted.md Schritt 3) ──
@staticmethod
def _shortest_arc_R(normal: Sequence[float]) -> np.ndarray:
"""
Rotationsmatrix [0,0,1] → normal (kürzester Bogen).
Repliziert THREE.Quaternion.setFromUnitVectors(vFrom=[0,0,1], vTo=n)
exakt (inkl. antiparallelem Sonderfall), damit die hier erzeugten
Ecken zur visuell verifizierten Orientierungs-Konvention in
boardViewer.html passen (qNormalLoc dort).
"""
n = np.asarray(normal, dtype=float)
nn = float(np.linalg.norm(n))
n = n / nn if nn > 1e-12 else np.array([0.0, 0.0, 1.0])
# three.js: quat = (cross([0,0,1], n), dot([0,0,1], n) + 1), dann normalisieren.
r = float(n[2]) + 1.0
if r < 1e-12:
# antiparallel (n == [0,0,-1]): three.js else-Zweig für vFrom=[0,0,1] → (0,-1,0,0)
qx, qy, qz, qw = 0.0, -1.0, 0.0, 0.0
else:
qx, qy, qz, qw = -float(n[1]), float(n[0]), 0.0, r # cross([0,0,1], n) = (-ny, nx, 0)
ql = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw)
qx, qy, qz, qw = qx / ql, qy / ql, qz / ql, qw / ql
return np.array([
[1 - 2 * (qy * qy + qz * qz), 2 * (qx * qy - qz * qw), 2 * (qx * qz + qy * qw)],
[2 * (qx * qy + qz * qw), 1 - 2 * (qx * qx + qz * qz), 2 * (qy * qz - qx * qw)],
[2 * (qx * qz - qy * qw), 2 * (qy * qz + qx * qw), 1 - 2 * (qx * qx + qy * qy)],
])
@staticmethod
def _marker_plane_corners(half: float) -> np.ndarray:
"""
Die 4 Eckpunkte in der Marker-Ebene (+Z = Normale), in DERSELBEN
Reihenfolge wie die von 3b_corner_marker_poses.py triangulierten
`corners_m` (ArUco 0..3, im Uhrzeigersinn von der Vorderseite gesehen).
Ecke 0 zeigt in Richtung (+h, +h) — dieselbe Konvention wie der visuell
kalibrierte Orientierungszeiger (1,1,0) in boardViewer.html. Damit passt
sie zu den manuell/visuell gesetzten `spin`-Werten der ARM-Marker, die
im Homing die Gelenkwinkel bestimmen (gegen echte corners_m am
Seed-Pose verifiziert, test_robot_fk_corners.py, RMS ~1 mm).
Hinweis: Die Board-Referenzmarker (Set A0) sind ~90° anders kalibriert,
ihre Eckreihenfolge passt unter dieser Konvention NICHT — egal, weil
Board der Root-Link ist: ihr Eck-Residuum ist konstant bzgl. der
Gelenkvariablen und beeinflusst die Schätzung nicht (der robuste
Huber-Verlust dämpft es als Ausreißer). Siehe Doc-Notiz.
Die Drehrichtung 0→1→2→3 ist so, dass 3b's `corner_plane_normal()`
(outward = -cross(e01,e02)) wieder +Z liefert — identisch zur
Beobachtungs-Konvention.
"""
h = float(half)
return np.array([
[ h, h, 0.0], # 0
[ h, -h, 0.0], # 1
[-h, -h, 0.0], # 2
[-h, h, 0.0], # 3
])
@classmethod
def marker_corners_local(cls,
position: Sequence[float],
normal: Sequence[float],
size_mm: float,
spin_deg: float = 0.0) -> np.ndarray:
"""
Die 4 Marker-Ecken im LINK-lokalen Frame (mm), Reihenfolge wie `corners_m`.
Orientierung = Spin um die Normale ∘ Minimal-Rotation [0,0,1]→Normale,
exakt wie boardViewer.html (qSpinLoc.multiply(qNormalLoc)).
"""
n = np.asarray(normal, dtype=float)
R = _rot_axis_angle(n, float(spin_deg)) @ cls._shortest_arc_R(n)
plane = cls._marker_plane_corners(float(size_mm) / 2.0) # (4,3)
return np.asarray(position, dtype=float) + (R @ plane.T).T # (4,3)
@classmethod
def marker_corners_world(cls,
transforms: Dict[str, np.ndarray],
link_name: str,
position: Sequence[float],
normal: Sequence[float],
size_mm: float,
spin_deg: float = 0.0) -> np.ndarray:
"""Die 4 Marker-Ecken im Weltframe (mm), Reihenfolge wie `corners_m`."""
T = transforms.get(link_name, np.eye(4))
local = cls.marker_corners_local(position, normal, size_mm, spin_deg)
return np.array([transform_point(T, c) for c in local])
def all_markers_world(self,
transforms: Dict[str, np.ndarray]
) -> Dict[int, Dict[str, Any]]:
"""
Returns
-------
dict marker_id -> {world_mm, local_mm, link, normal_world, corners_world}
corners_world: (4,3) Welt-mm in `corners_m`-Reihenfolge (für den
marker_observation="corner_points"-Modus in 5_pose_estimation.py).
"""
default_size = float((self.robot.get("markerDefaults", {}) or {}).get("size", 25.0))
result: Dict[int, Dict[str, Any]] = {}
for lname, ldata in self.links.items():
T = transforms.get(lname, np.eye(4))
R = T[:3, :3]
for m in ldata.get("markers", []):
mid = int(m.get("id", -1))
if mid < 0 or "position" not in m:
continue
loc = np.array(m["position"], dtype=float)
nor = np.array(m.get("normal", [0, 0, 1]), dtype=float)
size_mm = float(m.get("size", default_size))
spin_deg = float(m.get("spin", 0.0))
local_corners = self.marker_corners_local(loc, nor, size_mm, spin_deg)
result[mid] = {
"world_mm": transform_point(T, loc),
"local_mm": loc,
"link": lname,
"normal_world": (R @ nor) / max(np.linalg.norm(R @ nor), 1e-12),
"corners_world": np.array([transform_point(T, c) for c in local_corners]),
}
return result
# ── x-axis invariant helpers (used by 4a) ────────────────
def link_x_at_zero_state(self, link_name: str) -> float:
"""
Return the world x-coordinate of the link-frame origin
when ALL joint variables are zero.
For any link reachable via only x-axis rotations (Arm1, Ellbow, Arm2),
this value is constant regardless of the actual revolute angles.
Adding the slider value x_mm gives the true link origin x:
link_origin_world_x = x_mm + link_x_at_zero_state(link_name)
And for any marker in that link:
marker_world_x = x_mm + link_x_at_zero_state(link_name) + marker_local_x
"""
T = self.compute({k: 0.0 for k in STATE_KEYS})
return float(T[link_name][0, 3])
def joint_origin_world(self,
link_name: str,
joint_state: Dict[str, float]) -> np.ndarray:
"""
World position of the pivot that link_name rotates / slides around.
"""
d = self.links[link_name]
parent = d.get("parent")
T_all = self.compute(joint_state)
T_parent = T_all.get(parent, np.eye(4)) if parent else np.eye(4)
mp = d.get("mountPosition", [0, 0, 0])
mr = d.get("mountRotation", [0, 0, 0])
T_m = make_T(_rot_xyz_euler(*mr), mp)
ji = d.get("jointToParent", {}) or {}
jp = ji.get("origin", [0, 0, 0])
jr = ji.get("rotation", [0, 0, 0])
T_j = make_T(_rot_xyz_euler(*jr), jp)
return transform_point(T_parent @ T_m @ T_j, [0, 0, 0])
def joint_axis_world(self,
link_name: str,
joint_state: Dict[str, float]) -> np.ndarray:
"""
Joint axis of link_name expressed in world frame.
"""
d = self.links[link_name]
parent = d.get("parent")
T_all = self.compute(joint_state)
T_parent = T_all.get(parent, np.eye(4)) if parent else np.eye(4)
mp = d.get("mountPosition", [0, 0, 0])
mr = d.get("mountRotation", [0, 0, 0])
T_m = make_T(_rot_xyz_euler(*mr), mp)
ji = d.get("jointToParent", {}) or {}
jp = ji.get("origin", [0, 0, 0])
jr = ji.get("rotation", [0, 0, 0])
T_j = make_T(_rot_xyz_euler(*jr), jp)
R_to_pivot = (T_parent @ T_m @ T_j)[:3, :3]
axis_local = np.asarray(ji.get("axis", [1, 0, 0]), dtype=float)
world = R_to_pivot @ axis_local
n = float(np.linalg.norm(world))
return world / n if n > 1e-12 else world
# ── utility ───────────────────────────────────────────────
def chain(self, link_name: str) -> List[str]:
"""Return chain from root to link_name (inclusive)."""
out, cur = [], link_name
while cur:
out.append(cur)
cur = self.links.get(cur, {}).get("parent")
return list(reversed(out))
def board_marker_world_positions(self, length_scale: float = 1.0) -> Dict[int, np.ndarray]:
"""
Return known world positions for all Board markers (in mm).
Board is the root, so its marker positions ARE world positions.
length_scale: 1/1000 if robot.json uses mm.
"""
board = self.links.get("Board", {})
result: Dict[int, np.ndarray] = {}
for m in board.get("markers", []):
mid = int(m.get("id", -1))
if mid >= 0 and "position" in m:
p = np.array(m["position"], dtype=float) * length_scale
result[mid] = p
return result

View File

@@ -0,0 +1,502 @@
{
"_label": "todo3_2026-06-11",
"coordinateSystem": {"handedness": "right", "x": "right", "y": "backward", "z": "up"},
"units": {"_owner": "appRobotDriver", "length": "mm", "rotation": "degree"},
"kinematics": {
"_owner": "appRobotDriver",
"type": "arm3segmentlinearx"
},
"motion": {
"_owner": "appRobotDriver",
"defaultFeedrate": 2300,
"speedMode": "legacy",
"speedModeOptions": ["legacy", "correct"]
},
"controllers": {
"_owner": "appRobotDriver",
"base": { "ip": "fluidNcBase.local", "port": 2300, "protocol": "telnet", "axes": ["x", "y", "z"] },
"elbow": { "ip": "fluidNcEllbow.local", "port": 5000, "protocol": "telnet", "axes": ["a", null, null] },
"hand": { "ip": "fluidNcHand.local", "port": 5000, "protocol": "telnet", "axes": ["c", "e", "b"] }
},
"vision_config": {"MarkerType": "DICT_4X4_250", "MarkerSize": 0.025},
"renderingInfo": {
"width": 1280,
"height": 720,
"renderDefaults": {"width": 1280, "height": 720, "dofFStop": 11},
"cameraPosition__1": [-10, -800, 500],
"cameraPosition__2": [-500, 300, 1200],
"cameraPosition__3": [-200, -900, 200],
"cameraPosition__4": [1200, 200, 300],
"cameraPosition_a": [-300, -800, 500],
"cameraPosition": [-200, 200, 1400],
"cameraPosition_c": [600, -500, 600],
"cameraTarget": [200, -200, 180],
"cameraUpVector": [0, 0, 1],
"lightPosition": [-500, -500, 500],
"lightTarget": [0, 0, 0],
"lightUpVector": [0, 0, 1],
"metric": "mm",
"showSkeleton": true,
"showMarkers": true,
"backgroundColor": [0.7, 0.85, 1.0],
"backgroundStrength": 0.2,
"sunEnergy": 0.35,
"areaEnergy": 120,
"exposure": -1.5,
"lensDirt": true,
"lensDirtStrength": 0.08,
"dofEnabled": true,
"dofFStop": 11.0,
"arucoDust": true,
"arucoDustStrength": 1.6,
"markerOffsetMaxMm": 4.0,
"markerOffsetSeed": 0,
"markerRotationMaxDeg": 3,
"motionBlur": true,
"motionBlurMaxPx": 5.5,
"focalErrorPct": 0.5,
"principalErrorPx": 3.0,
"residualDistortion": [0.02, -0.01],
"localizedBlur": false,
"localizedBlurStrength": 0.15,
"vignette": true,
"vignetteStrength": 0.08,
"sensorNoise": true,
"sensorNoiseStrength": 0.01,
"lensDistortion": true,
"lensDistortionStrength": 0.002,
"materials": {
"wood": {"baseColor": [0.72, 0.52, 0.33], "roughness": 0.85, "metallic": 0.0},
"plaWhite": {"baseColor": [0.95, 0.95, 0.95], "roughness": 0.45, "metallic": 0.0},
"steel": {"baseColor": [0.72, 0.72, 0.75], "roughness": 0.25, "metallic": 1.0},
"powderCoatBlue": {"baseColor": [0.15, 0.25, 0.7], "roughness": 0.55, "metallic": 0.0},
"defaultPlastic": {"baseColor": [0.95, 0.95, 0.95], "roughness": 0.4, "metallic": 0.0},
"skeletonRed": {"baseColor": [0.85, 0.2, 0.2], "roughness": 0.35, "metallic": 0.0},
"markerBlack": {"baseColor": [0.04, 0.04, 0.04], "roughness": 0.8, "metallic": 0.0}
},
"skeletonDefaults": {"radius": 4, "color": [0.85, 0.2, 0.2]},
"markerDefaults": {"size": 25, "thickness": 1, "color": [0.04, 0.04, 0.04]},
"defaultPosition": {"x": 80, "y": 20, "z": 80, "a": -120, "b": 23, "c": 9, "e": 3}
},
"defaultPosition__": {"x": 10, "y": 4, "z": 20, "a": 10, "b": 2, "c": 9, "e": 1},
"defaultPosition": {"x": 50, "y": 4, "z": 176, "a": 20, "b": 60, "c": 9, "e": 5},
"recognized": {"x": null, "y": null, "z": null, "a": null, "b": null, "c": null, "e": null},
"constraint_rules": {
"rigid_distance": {"enabled": true, "mode": "mst", "weight": 1.0},
"joint_axis_projection": {"enabled": true, "max_pairs": 2, "weight": 0.35},
"chain_axis_projection": {"enabled": false, "max_depth": 3, "max_pairs": 2, "weight": 0.15},
"axis_alignment_threshold": 0.95
},
"observation_weighting": {"enabled": true, "distance_weight": true, "marker_size_weight": true, "view_angle_weight": true},
"multiview_calculation": {
"combine_mode": "mean",
"size_ref_px": 50.0,
"border_ref_px": 120.0,
"center_ref_norm": 0.01,
"sharpness_ref": 2500.0,
"homography_ref": 0.18,
"size_factor": 0.3,
"aspect_factor": 0.3,
"border_factor": 0.01,
"center_factor": 0.01,
"sharpness_factor": 0.5,
"homography_factor": 0.2,
"normal_visibility_factor": 0.01,
"spin_factor": 0.3,
"weight_floor": 0.3
},
"pose_estimation": {
"method": "hybrid",
"marker_observation": "corner_pose",
"use_normals": true,
"normal_weight": 100.0,
"robust_loss": "huber",
"huber_delta_mm": 8.0,
"max_iterations": 200,
"min_cameras_per_marker": 2,
"finger_block_joints": ["b", "c", "e"],
"per_link_method": {}
},
"robot_test_poses": {
"4": {"x": 70, "y": 50, "z": -70, "a": 120, "b": 50, "c": 30, "e": 20},
"5": {"x": 180, "y": 86, "z": -120, "a": -60, "b": 22, "c": 91, "e": 10},
"6": {"x": 80, "y": 20, "z": 80, "a": -120, "b": 23, "c": 9, "e": 3},
"7": {"x": 30, "y": -2, "z": 95, "a": 20, "b": 23, "c": 9, "e": 9},
"8": {"x": 50, "y": -2, "z": 95, "a": 20, "b": 60, "c": 9, "e": 3},
"9": {"x": 60, "y": -2, "z": 95, "a": 200, "b": 60, "c": 9, "e": 8},
"9a": {
"x": 60,
"y": -2,
"z": 95,
"a": 200,
"b": 60,
"c": 9,
"e": 8,
"rendering": {"width": 1440, "height": 1080, "dofFStop": 11}
},
"9b": {
"x": 60,
"y": -2,
"z": 95,
"a": 200,
"b": 60,
"c": 9,
"e": 8,
"rendering": {"width": 4896, "height": 3264, "dofFStop": 5.6}
},
"10": {"x": 120, "y": 60, "z": -110, "a": 20, "b": 30, "c": 180, "e": 4},
"11": {"x": 50, "y": 4, "z": 176, "a": 20, "b": 60, "c": 9, "e": 5},
"12": {"x": 50, "y": 0, "z": 178, "a": 210, "b": 80, "c": 90, "e": 6}
},
"test_camera_positions": {
"a": [-300, -800, 800],
"b": [300, -900, 1200],
"c": [300, -900, 400],
"d": [700, -800, 400],
"e": [1200, -900, 400],
"f": [500, -300, 1400],
"g": [-200, 200, 1400]
},
"test_camera_targets": {
"a": [210, -100, 180],
"b": [310, -80, 180],
"c": [210, -100, 150],
"d": [210, -100, 150],
"e": [210, -100, 50],
"f": [200, -200, 180],
"g": [200, -200, 180]
},
"movements": {"x": null, "y": null, "z": null, "a": null, "b": null, "c": null, "e": null},
"state_pose_params": {
"numbers_of_Elements_to_consider_start": 3,
"numbers_of_Elements_to_consider_final": 5,
"solver_in_between_geometrical": false,
"solver_after_geometrical": false,
"geometric_passes_per_stage": 2,
"revolute_search_coarse_deg": 5.0,
"revolute_search_fine_deg": 1.0,
"root_pose_min_markers": 3,
"use_marker_normals_flip_tiebreak": true,
"normal_flip_weight": 0.05
},
"links": {
"_owner": "appRobotDriver",
"Board": {
"parent": null,
"size": [1000, 200, 25],
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"skeleton": {"from": [0, 0, 16], "to": [1000, 0, 16], "radius": 4, "color": [0.85, 0.2, 0.2]},
"markers": [
{"id": 210, "set": "Brett", "position": [20, -20, 0.3], "normal": [0, 0, 1]},
{"id": 211, "set": "Brett", "position": [250, -10, 0.3], "normal": [0, 0, 1]},
{"id": 215, "set": "Brett", "position": [250, -90, 0.3], "normal": [0, 0, 1]},
{"id": 214, "set": "Brett", "position": [350, -10, 0.3], "normal": [0, 0, 1]},
{"id": 208, "set": "Brett", "position": [350, -90, 0.3], "normal": [0, 0, 1]},
{"id": 206, "set": "Brett", "position": [650, -10, 0.3], "normal": [0, 0, 1]},
{"id": 205, "set": "Brett", "position": [750, -90, 0.3], "normal": [0, 0, 1]},
{"id": 207, "set": "Brett", "position": [750, -10, 0.3], "normal": [0, 0, 1]},
{"id": 217, "set": "Brett", "position": [650, -90, 0.3], "normal": [0, 0, 1]},
{
"id": 46,
"set": "A0",
"position": [536.71, 185.44, -27.3],
"normal": [0, 0, 1],
"spin": 90,
"info": "is placed on a white paper, A0_60Arucos_25mm_Seet223.pdf, with the following marker placements:"
},
{"id": 47, "set": "A0", "position": [344.23, -286.54, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 48, "set": "A0", "position": [688.69, -320.72, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 49, "set": "A0", "position": [1006.0, 158.33, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 50, "set": "A0", "position": [573.41, 211.86, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 51, "set": "A0", "position": [167.8, -172.08, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 52, "set": "A0", "position": [94.68, 208.66, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 53, "set": "A0", "position": [486.25, 212.24, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 54, "set": "A0", "position": [342.27, -330.59, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 55, "set": "A0", "position": [283.72, -262.58, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 56, "set": "A0", "position": [498.68, 168.67, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 57, "set": "A0", "position": [602.86, -364.05, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 58, "set": "A0", "position": [50.09, -218.11, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 59, "set": "A0", "position": [626.21, -278.75, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 60, "set": "A0", "position": [434.36, 283.81, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 61, "set": "A0", "position": [-22.42, 335.83, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 62, "set": "A0", "position": [404.7, -175.1, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 63, "set": "A0", "position": [777.4, -236.15, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 64, "set": "A0", "position": [-21.27, -188.23, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 65, "set": "A0", "position": [803.39, -297.37, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 66, "set": "A0", "position": [209.75, -363.23, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 67, "set": "A0", "position": [523.07, 267.04, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 68, "set": "A0", "position": [573.73, 170.64, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 69, "set": "A0", "position": [7.61, -281.21, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 70, "set": "A0", "position": [601.87, 300.33, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 71, "set": "A0", "position": [749.75, -284.01, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 72, "set": "A0", "position": [440.99, 194.32, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 73, "set": "A0", "position": [221.73, 333.11, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 74, "set": "A0", "position": [93.78, 144.5, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 75, "set": "A0", "position": [-25.7, 194.58, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 76, "set": "A0", "position": [685.21, 166.8, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 77, "set": "A0", "position": [18.19, 191.57, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 78, "set": "A0", "position": [823.11, -344.38, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 79, "set": "A0", "position": [312.3, -159.11, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 80, "set": "A0", "position": [863.59, -335.92, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 81, "set": "A0", "position": [132.14, 169.03, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 82, "set": "A0", "position": [219.16, 297.24, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 83, "set": "A0", "position": [44.16, 339.22, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 84, "set": "A0", "position": [407.49, 258.42, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 85, "set": "A0", "position": [504.58, -312.75, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 86, "set": "A0", "position": [362.89, 292.01, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 87, "set": "A0", "position": [943.63, -245.76, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 88, "set": "A0", "position": [765.87, 316.04, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 89, "set": "A0", "position": [988.02, -369.14, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 90, "set": "A0", "position": [643.17, 316.43, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 91, "set": "A0", "position": [723.35, 328.05, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 92, "set": "A0", "position": [645.09, -184.84, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 93, "set": "A0", "position": [934.88, 143.6, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 94, "set": "A0", "position": [875.7, 173.65, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 95, "set": "A0", "position": [186.04, -274.07, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 96, "set": "A0", "position": [369.77, -186.49, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 97, "set": "A0", "position": [304.35, -359.67, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 98, "set": "A0", "position": [575.27, 315.06, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 99, "set": "A0", "position": [959.16, -321.55, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 100, "set": "A0", "position": [803.25, 172.36, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 101, "set": "A0", "position": [117.7, 298.66, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 102, "set": "A0", "position": [649.69, -223.0, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 103, "set": "A0", "position": [105.71, -187.71, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 104, "set": "A0", "position": [826.71, 239.16, -27.3], "normal": [0, 0, 1], "spin": 90},
{"id": 105, "set": "A0", "position": [524.84, -266.25, -27.3], "normal": [0, 0, 1], "spin": 90}
],
"model": [
{
"stlFile": "surfaces/Board.stl",
"originOfModel": [0, 0, 0],
"rotationOfModelDegree": [0, 0, -90],
"material": "wood"
},
{
"stlFile": "surfaces/BoardRail.stl",
"originOfModel": [0, 0, 0],
"rotationOfModelDegree": [0, 0, -90],
"material": "steel"
}
]
},
"Base": {
"parent": "Board",
"size": [150, 200, 150],
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Slider",
"type": "linear",
"axis": [1, 0, 0],
"origin": [0, 0, 16],
"rotation": [0, 0, 0],
"variable": "x",
"feedrate": 2000,
"controller": "base"
},
"skeleton": {"from": [0, 108, 45], "to": [110, 108, 45], "radius": 4, "color": [0.2, 0.8, 0.2]},
"markers": [],
"model": [
{
"stlFile": "surfaces/Base.stl",
"originOfModel": [-30, 0, -35],
"rotationOfModelDegree": [0, 0, 0],
"material": "plaWhite"
}
]
},
"Arm1": {
"parent": "Base",
"size": [70, 250, 70],
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Joint1",
"type": "revolute",
"axis": [-1, 0, 0],
"origin": [110, 108, 45],
"rotation": [0, 0, 0],
"variable": "y",
"feedrate": 2300,
"controller": "base"
},
"skeleton": {"from": [0, 0, 0], "to": [0, -250, 0], "radius": 4, "color": [0.2, 0.2, 0.9]},
"markers": [
{"id": 198, "name": "aruco_198", "position": [0, -160, 35], "normal": [0, 0, 1], "size": 25, "spin": 0},
{"id": 229, "name": "aruco_229", "position": [0, -250, 35], "normal": [0, 0, 1], "size": 25, "spin": 0},
{"id": 242, "name": "aruco_242", "position": [0, -250, -35], "normal": [0, 0, -1], "size": 25, "spin": 0},
{"id": 243, "name": "aruco_243", "position": [0, -285, 0], "normal": [0, -1, 0], "size": 25, "spin": 0}
],
"model": [
{
"stlFile": "surfaces/Holm.stl",
"originOfModel__": [-25, 29, -28.5],
"originOfModel": [-29, 25, 28.5],
"rotationOfModelDegree__": [0, 0, 0],
"rotationOfModelDegree": [180, 0, -90],
"material": "powderCoatBlue"
}
]
},
"Ellbow": {
"parent": "Arm1",
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Joint2",
"type": "revolute",
"axis": [-1, 0, 0],
"origin": [0, -250, 0],
"rotation": [0, 0, 0],
"variable": "z",
"feedrate": 2300,
"controller": "base"
},
"skeleton": {"from": [0, 0, 0], "to": [90, 0, 0], "radius": 4, "color": [0.9, 0.2, 0.2]},
"model": [
{
"stlFile": "surfaces/Ellebogen.stl",
"originOfModel": [90, 0, 0],
"rotationOfModelDegree": [0, -90, -90],
"material": "defaultPlastic"
}
],
"markers": [
{"id": 244, "name": "aruco_244", "position": [125, 0, 0], "normal": [1, 0, 0], "size": 25, "spin": 0},
{"id": 245, "name": "aruco_245", "position": [90, 0, -35], "normal": [0, 0, -1], "size": 25, "spin": 0},
{"id": 246, "name": "aruco_246", "position": [90, 0, 35], "normal": [0, 0, 1], "size": 25},
{"id": 247, "name": "aruco_247", "position": [52.5, 0, 35], "normal": [0, 0, 1], "size": 25},
{"id": 248, "name": "aruco_248", "position": [52.5, 0, -35], "normal": [0, 0, -1], "size": 25},
{"id": 232, "name": "aruco_232", "position": [90, 24.75, -24.75], "normal": [0, 1, -1], "size": 25},
{"id": 231, "name": "aruco_231", "position": [90, 24.75, 24.75], "normal": [0, 1, 1], "size": 25}
]
},
"Arm2": {
"parent": "Ellbow",
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Joint3",
"type": "revolute",
"axis": [0, -1, 0],
"origin": [90, 0, 0],
"rotation": [0, 0, 0],
"variable": "a",
"feedrate": 2300,
"controller": "elbow"
},
"skeleton": {"from": [0, 0, 0], "to": [0, -250, 0], "radius": 4, "color": [0.95, 0.85, 0.2]},
"model": [
{
"stlFile": "surfaces/Unterarm.stl",
"originOfModel": [0, -250, 0],
"rotationOfModelDegree": [180, 0, -90],
"material": "defaultPlastic"
}
],
"markers": [
{"id": 120, "position": [24.75, -112, -24.75], "normal": [1, 0, -1]},
{"id": 122, "name": "aruco_122", "position": [-35, -112, 0], "normal": [-1, 0, 0]},
{"id": 218, "name": "aruco_218", "position": [35, -112, 0], "normal": [1, 0, 0]},
{"id": 113, "name": "aruco_113", "position": [0, -182, 30], "normal": [0, 0, 1]},
{"id": 114, "name": "aruco_114", "position": [24.75, -182, -24.75], "normal": [1, 0, -1]},
{"id": 115, "name": "aruco_115", "position": [-24.75, -182, -24.75], "normal": [-1, 0, -1]},
{"id": 124, "name": "aruco_124", "position": [-35, -219, 0], "normal": [-1, 0, 0]},
{"id": 219, "name": "aruco_219", "position": [35, -219, 0], "normal": [1, 0, 0]}
]
},
"Hand": {
"parent": "Arm2",
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Joint4",
"type": "revolute",
"axis": [1, 0, 0],
"origin": [0, -250, 0],
"rotation": [0, 0, 0],
"variable": "b",
"feedrate": 2300,
"controller": "hand"
},
"skeleton": {"from": [0, 0, 0], "to": [0, -35, 0], "radius": 4, "color": [0.95, 0.55, 0.15]}
},
"Palm": {
"parent": "Hand",
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Joint3",
"type": "revolute",
"axis": [0, -1, 0],
"origin": [0, 0, 0],
"rotation": [0, 0, 0],
"variable": "c",
"feedrate": 2300,
"controller": "hand"
},
"skeleton": {"from": [-50, -35, 0], "to": [50, -35, 0], "radius": 7, "color": [0.95, 0.2, 0.2]}
},
"FingerA": {
"parent": "Palm",
"size": [80, 60, 20],
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Slider",
"type": "linear",
"axis": [1, 0, 0],
"origin": [4, -35, 0],
"rotation": [0, 0, 0],
"variable": "e",
"feedrate": 2000,
"controller": "hand"
},
"skeleton": {"from": [0, 0, 0], "to": [0, -60, 0], "radius": 4, "color": [0.2, 0.8, 0.2]},
"markers": [
{"id": 40, "position": [12, -24, -17.1], "normal": [-10.98, 0, -23.56]},
{"id": 41, "position": [1.5, -2.2, 25.8], "normal": [0, -25.6, 9.5]},
{"id": 42, "position": [13.9, -40, 0], "normal": [1, -0.35, 0.4], "spin": 27}
],
"model": [
{
"stlFile": "surfaces/Finger.stl",
"originOfModel": [24, 0, -9.1],
"rotationOfModelDegree": [90, -90, 0],
"material": "defaultPlastic"
}
]
},
"FingerB": {
"parent": "Palm",
"size": [80, 60, 20],
"mountPosition": [0, 0, 0],
"mountRotation": [0, 0, 0],
"jointToParent": {
"name": "Slider",
"type": "linear",
"axis": [-1, 0, 0],
"origin": [-4, -35, 0],
"rotation": [0, 0, 0],
"variable": "e",
"feedrate": 2000,
"controller": "hand"
},
"skeleton": {"from": [0, 0, 0], "to": [0, -60, 0], "radius": 4, "color": [0.2, 0.8, 0.2]},
"markers": [
{"id": 43, "position": [-12, -24, 17.1], "normal": [10.98, 0, 23.56], "spin": 90},
{"id": 44, "position": [-1.5, -2.2, -25.8], "normal": [0, -25.6, -9.5], "spin": 90},
{"id": 45, "position": [-13.9, -40, 0], "normal": [-1, -0.35, -0.4], "spin": -27}
],
"model": [
{
"stlFile": "surfaces/Finger.stl",
"originOfModel": [-24, 0, 9.1],
"rotationOfModelDegree": [90, 90, 0],
"material": "defaultPlastic"
}
]
}
}
}

48
server/buildG92.cjs Normal file
View File

@@ -0,0 +1,48 @@
/**
* buildG92.cjs
* Baut aus einem Homing-State {x,y,z,a,b,c,e} einen G92-G-Code-String.
*
* G92 setzt am appRobotDriver die Motorposition OHNE Bewegung (intern als M92
* verarbeitet, siehe appRobotDriver/doc/API.md + robot/RobotController.js) —
* exakt die Homing-Semantik. Die Achsbuchstaben bilden 1:1 auf die Motorachsen
* ab: X→xMotor, Y→alpha, Z→beta, A→a, B→b, C→c, E→e.
*
* Bekannte Achsen werden immer mit ihrem realen Wert gesendet. Welche Achsen
* bekannt sind, hängt vom Pfad ab:
* - 5_pose_estimation.py (Fallback) liefert alle 7 (x,y,z,a,b,c,e),
* - die 4b-Primärkette (Arm1→y … Hand→b) liefert nur x,y,z,a,b.
* Eine Achse, die wirklich fehlt oder als unbeobachtbar `null` markiert ist,
* wird per Default WEGGELASSEN — der Driver lässt nicht genannte Achsen
* unverändert (M92 setzt nur Achsen mit endlichem Zahlenwert), statt eine
* unbekannte Position fälschlich als 0 zu behaupten. `fillMissingWithZero`
* erzwingt bei Bedarf das alte 0-Auffüllen.
*
* CommonJS, damit Jest (CJS) und der ESM-Server dieselbe Funktion nutzen
* (gleiches Muster wie spinNormalize.cjs / homingXEstimate.cjs).
*/
// Reihenfolge + Achsbuchstaben wie vom Driver erwartet.
const AXES = [
['x', 'X'], ['y', 'Y'], ['z', 'Z'],
['a', 'A'], ['b', 'B'], ['c', 'C'], ['e', 'E'],
];
/**
* @param {Record<string, number|null>} state flacher Joint-State (accumulated_state)
* @param {{decimals?: number, fillMissingWithZero?: boolean}} [opts]
* @returns {string} z.B. "G92 X164.57 Y-2.09 Z60.58 A86.75 B-46.97 C-64.91 E22.59"
*/
function buildG92(state = {}, { decimals = 2, fillMissingWithZero = false } = {}) {
const parts = [];
for (const [key, axis] of AXES) {
const num = Number(state?.[key]);
if (state?.[key] != null && Number.isFinite(num)) {
parts.push(`${axis}${num.toFixed(decimals)}`);
} else if (fillMissingWithZero) {
parts.push(`${axis}${(0).toFixed(decimals)}`);
}
}
return `G92 ${parts.join(' ')}`;
}
module.exports = { buildG92, AXES };

77
server/driverClient.js Normal file
View File

@@ -0,0 +1,77 @@
/**
* driverClient.js WebSocket-Transport zum appRobotDriver
*
* Der Driver nimmt Steuerbefehle als Plain-Text-G-Code über einen WebSocket
* entgegen (wss://…:2096, self-signed), NICHT über HTTP — siehe
* appRobotDriver/doc/API.md. Ein früher angenommenes `POST /api/state` existiert
* dort nicht (war Platzhalter, vgl. doc/accessRobotAPI.md). G92 setzt am Driver
* die Motorposition ohne Bewegung (intern M92) = exakt die Homing-Semantik.
*
* DRIVER_WS_URL nicht gesetzt → kein Kontakt, klarer 501-Fehler (analog zum
* früheren ROBOT_URL-Verhalten).
*/
import { WebSocket } from 'ws';
const DRIVER_WS_URL = process.env.DRIVER_WS_URL || '';
/** true, wenn ein Driver-WebSocket konfiguriert ist. */
export function isDriverConfigured() {
return Boolean(DRIVER_WS_URL);
}
/**
* Öffnet eine kurzlebige WS-Verbindung zum Driver, sendet eine G-Code-Zeile und
* wartet auf die erste Antwort (Positions-JSON bzw. Fehler-Envelope). Der Driver
* broadcastet nach jedem G-Code das aktuelle Positions-JSON an alle Clients —
* der Sender ist selbst Client und bekommt es zurück.
*
* @param {string} line z.B. "G92 X1 Y2 …"
* @param {{timeoutMs?: number}} [opts]
* @returns {Promise<{ok:boolean, sent:string, response?:any, error?:string, note?:string}>}
*/
export function sendGcode(line, { timeoutMs = 4000 } = {}) {
const text = String(line ?? '').trim();
if (!text) {
return Promise.reject(Object.assign(new Error('Leere G-Code-Zeile'), { statusCode: 400 }));
}
if (!DRIVER_WS_URL) {
return Promise.reject(Object.assign(
new Error('DRIVER_WS_URL ist nicht konfiguriert'), { statusCode: 501 }));
}
return new Promise((resolve, reject) => {
// Self-signed Cert am Driver → Zertifikatsprüfung deaktivieren (interner Hop).
const ws = new WebSocket(DRIVER_WS_URL, { rejectUnauthorized: false });
let settled = false;
const finish = (fn, arg) => {
if (settled) return;
settled = true;
clearTimeout(timer);
try { ws.close(); } catch { /* egal */ }
fn(arg);
};
// Gesendet, aber keine Antwort rechtzeitig: kein harter Fehler — der Befehl
// ist raus, der Driver antwortet nur evtl. nicht broadcastfähig.
const timer = setTimeout(() => {
finish(resolve, { ok: true, sent: text, response: null, note: 'keine Antwort (Timeout)' });
}, timeoutMs);
ws.on('open', () => ws.send(text));
ws.on('message', (data) => {
const raw = data.toString();
let parsed;
try { parsed = JSON.parse(raw); } catch { parsed = raw; }
if (parsed && typeof parsed === 'object' && parsed.type === 'error') {
finish(resolve, { ok: false, sent: text, error: parsed.message || raw, response: parsed });
} else {
finish(resolve, { ok: true, sent: text, response: parsed });
}
});
ws.on('error', (err) => finish(reject, Object.assign(
new Error(`Driver-WS-Fehler: ${err.message}`), { statusCode: 502 })));
});
}

View File

@@ -5,16 +5,18 @@
* atomisches Write per Temp-Datei ist hier nicht nötig die Datei wird direkt
* überschrieben; bei Bedarf Backup-Strategie ergänzen).
*/
import fsPromises from 'fs/promises';
import { createRequire } from 'module';
import { fetchRobot, pushRobot } from './robotConfig.js';
const { normalizeSpinDeg } = createRequire(import.meta.url)('./spinNormalize.cjs');
// ── I/O ───────────────────────────────────────────────────────────────────────
async function readRobot(robotPath) {
return JSON.parse(await fsPromises.readFile(robotPath, 'utf8'));
async function readRobot(_robotPath) {
return fetchRobot();
}
async function writeRobot(robotPath, data) {
await fsPromises.writeFile(robotPath, JSON.stringify(data, null, 2), 'utf8');
async function writeRobot(_robotPath, data) {
return pushRobot(data);
}
// ── Aktion 1: Marker nach Z-Bereich zuordnen ─────────────────────────────────
@@ -372,6 +374,12 @@ export async function assignMarkerId(robotPath, { markerId, set, link, extraMark
return { changed: false, error: 'Link muss angegeben werden, um einen neuen Marker hinzuzufügen.' };
}
if (!Array.isArray(em.position_mm)) {
return {
changed: false,
error: `Marker ${id} hat keine triangulierte Position (position_mm fehlt z.B. Einzelkamera-Marker).`,
};
}
const newMarker = {
id,
position: em.position_mm.map(v => Math.round(Number(v) * 100) / 100),
@@ -609,3 +617,33 @@ export async function setJointOriginYZ(robotPath, { linkName, y, z }) {
newOrigin: [...joint.origin],
};
}
// ── Aktion 8: Arm-Marker Spin setzen ─────────────────────────────────────────
/**
* Setzt den `spin`-Wert eines Arm-Markers in robot.json.
* Body: { linkName, markerId, spin }
* spin: absolute Gradzahl (0 / 90 / 180 / 270) oder relativer Delta (+90, -90, +180)
* — hier wird immer die *neue absolute* Gradzahl erwartet.
*
* @returns {{ changed, linkName, markerId, oldSpin, newSpin } | { changed: false, error }}
*/
export async function setArmMarkerSpin(robotPath, { linkName, markerId, spin }) {
const robot = await readRobot(robotPath);
const linkData = robot.links?.[linkName];
if (!linkData) return { changed: false, error: `Link '${linkName}' nicht gefunden.` };
const markers = linkData.markers ?? [];
const idx = markers.findIndex(m => Number(m.id) === Number(markerId));
if (idx === -1) {
return { changed: false, error: `Marker ${markerId} in Link '${linkName}' nicht gefunden.` };
}
const oldSpin = markers[idx].spin ?? 0;
const newSpin = normalizeSpinDeg(spin);
markers[idx].spin = newSpin;
await writeRobot(robotPath, robot);
return { changed: true, linkName, markerId: Number(markerId), oldSpin, newSpin };
}

View File

@@ -0,0 +1,262 @@
/**
* homingOrchestrator.js
* Vollständiger Homing-Ablauf: Board-Pipeline (1→2→3b) + 4b-Schleife.
*
* Abhängigkeiten werden von server.js per Parameter übergeben
* (kein Circular-Import-Problem).
*/
import path from 'path';
import fs from 'fs';
import fsPromises from 'fs/promises';
import { estimateXFromParsed } from './homingXEstimate.cjs';
/**
* Schätzt die Slider-X-Position aus den triangulierten Marker-Positionen.
* Dünner I/O-Wrapper: liest die Dateien und delegiert die reine Geometrie an
* `estimateXFromParsed()` (`server/homingXEstimate.cjs`, unit-getestet).
*
* @param {string} arucoJsonPath Pfad zu aruco_marker_poses.json
* @param {string} [robotJsonPath] Pfad zu robot.json (für den Gelenk-Offset)
* @returns {number} x_mm
*/
export function estimateXFromMarkers(arucoJsonPath, robotJsonPath) {
try {
const arucoData = JSON.parse(fs.readFileSync(arucoJsonPath, 'utf8'));
const links = robotJsonPath
? (JSON.parse(fs.readFileSync(robotJsonPath, 'utf8')).links ?? {})
: {};
return estimateXFromParsed(arucoData, links);
} catch {
return 0.0;
}
}
/**
* Führt den vollständigen Homing-Ablauf als SSE-Stream aus.
*
* @param {{
* robotJsonPath: string,
* homingDir: string,
* send: (obj: object) => void,
* runScript: (args: string[], send: Function) => Promise<number>,
* runBoardPipeline: (runDir: string, send: Function) => Promise<void>,
* SCRIPT_4B: string,
* SCRIPT_5POSE: string,
* }} opts
*/
export async function runHoming({
robotJsonPath,
homingDir,
send,
runScript,
runBoardPipeline,
SCRIPT_4B,
SCRIPT_5POSE,
}) {
// Lauf-Verzeichnis anlegen
const ts = makeTimestamp();
const runDir = path.join(homingDir, ts);
await fsPromises.mkdir(runDir, { recursive: true });
send({ type: 'log', text: `▶ Homing-Run: ${ts}` });
send({ type: 'log', text: `▶ Ordner: ${runDir}` });
send({ type: 'log', text: `▶ Robot-JSON: ${robotJsonPath}` });
send({ type: 'log', text: '' });
// ── Schritt 13b: Board-Pipeline ─────────────────────────────────────────
send({ type: 'step', step: 1, total: 6, text: 'Foto + Marker-Triangulierung …' });
await runBoardPipeline(runDir, send);
// Prüfen ob aruco_marker_poses.json erzeugt wurde
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, runDir: ts });
return;
}
// ── Schritt 2: X-Position bestimmen ─────────────────────────────────────
send({ type: 'step', step: 2, total: 6, text: 'X-Position bestimmen …' });
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 });
// ── Schritt 36: 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: 3 + i, total: 6, 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;
// Zwischenergebnis an Analysis-Sektion
try {
const stateData = JSON.parse(await fsPromises.readFile(outputPath, 'utf8'));
const acc = stateData.accumulated_state ?? stateData;
send({ type: 'analysis', key: `state_${link}`, value: acc });
} catch { /* ignorieren */ }
}
// ── Endergebnis ──────────────────────────────────────────────────────────
try {
let finalState;
if (chainComplete) {
const finalData = JSON.parse(await fsPromises.readFile(fromState, 'utf8'));
finalState = finalData.accumulated_state ?? finalData;
} else {
// 4b vorzeitig abgebrochen -> 5_pose_estimation.py mit dem letzten
// erfolgreichen Zwischenstand (falls vorhanden) als Startwert.
send({ type: 'step', step: 6, total: 6, 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: 'analysis', key: 'robot_state', value: poseData });
}
send({ type: 'log', text: '' });
send({ type: 'log', text: `✅ Homing abgeschlossen: ${ts}` });
send({ type: 'done', exitCode: 0, state: finalState, runDir: ts });
} catch (err) {
send({ type: 'error', text: `❌ Endzustand konnte nicht gelesen werden: ${err.message}` });
send({ type: 'done', exitCode: -1, runDir: ts });
}
}
/**
* 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<number>,
* runBoardPipelineOffline:(runDir: string, send: Function) => Promise<void>,
* 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 35 (24): 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();
const p = (n, l = 2) => String(n).padStart(l, '0');
return `${now.getFullYear()}${p(now.getMonth() + 1)}${p(now.getDate())}`
+ `_${p(now.getHours())}${p(now.getMinutes())}${p(now.getSeconds())}`;
}

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

44
server/robotConfig.js Normal file
View File

@@ -0,0 +1,44 @@
import fsPromises from 'fs/promises';
import path from 'path';
import { fileURLToPath } from 'url';
const __dirname = path.dirname(fileURLToPath(import.meta.url));
const ROBOT_URL = process.env.ROBOT_URL || '';
const ROBOT_JSON = process.env.ROBOT_JSON
|| path.join(__dirname, '..', 'scripts', 'robot_1781069752019.json');
/**
* Lädt robot.json.
* Reihenfolge: (1) ROBOT_URL/api/robot/config, (2) lokale Datei als Fallback.
* Schreibt das Ergebnis immer in die lokale Cache-Datei (für Python-Skripte).
*/
export async function fetchRobot() {
if (ROBOT_URL) {
const res = await fetch(new URL('/api/robot/config', ROBOT_URL));
if (!res.ok) throw new Error(`Driver ${res.status}: ${await res.text()}`);
const data = await res.json();
await fsPromises.writeFile(ROBOT_JSON, JSON.stringify(data, null, 2), 'utf8');
return data;
}
return JSON.parse(await fsPromises.readFile(ROBOT_JSON, 'utf8'));
}
/**
* Speichert robot.json.
* Schreibt immer in lokale Cache-Datei; sendet zusätzlich an Driver wenn konfiguriert.
*/
export async function pushRobot(data) {
await fsPromises.writeFile(ROBOT_JSON, JSON.stringify(data, null, 2), 'utf8');
if (ROBOT_URL) {
const res = await fetch(new URL('/api/robot/config', ROBOT_URL), {
method: 'POST',
headers: { 'Content-Type': 'application/json' },
body: JSON.stringify(data),
});
if (!res.ok) throw new Error(`Driver ${res.status}: ${await res.text()}`);
}
}
/** Pfad zur lokalen Cache-Datei wird an Python-Skripte als -robot-Argument übergeben. */
export const robotCachePath = ROBOT_JSON;

View File

@@ -8,7 +8,12 @@ import { fileURLToPath } from 'url';
import process from 'process';
import { spawn } from 'child_process';
import { WebcamClient } from './webcamClient.js';
import { assignByZRange, removeMarkerAssignment, alignSetToMeasured, assignMarkerId, adoptXAxis, assignFixedMarkersToLink, setJointOriginYZ } from './editRobot.js';
import { assignByZRange, removeMarkerAssignment, alignSetToMeasured, assignMarkerId, adoptXAxis, assignFixedMarkersToLink, setJointOriginYZ, setArmMarkerSpin } from './editRobot.js';
import multer from 'multer';
import { runHoming, runHomingOffline } from './homingOrchestrator.js';
import { fetchRobot, robotCachePath } from './robotConfig.js';
import { sendGcode, isDriverConfigured } from './driverClient.js';
import { buildG92 } from './buildG92.cjs';
const __filename = fileURLToPath(import.meta.url);
const __dirname = path.dirname(__filename);
@@ -19,12 +24,20 @@ app.use(express.json({ limit: '20mb' }));
const PORT = parseInt(process.env.PORT || process.env.HTTPS_PORT || '2093', 10);
const publicDir = path.join(__dirname, '..', 'public');
const snapshotsDir = path.join(publicDir, 'snapshots');
const WEBCAM_URL = process.env.WEBCAM_URL || '';
const WEBCAM_URL = process.env.WEBCAM_URL || '';
const BODYTRACKER_URL = process.env.BODYTRACKER_URL || '';
// Roboter-Transport läuft über den Driver-WebSocket (DRIVER_WS_URL,
// server/driverClient.js), nicht mehr über HTTP ROBOT_URL.
const HTTPS_KEY_PATH = process.env.HTTPS_KEY_PATH || path.join(__dirname, '..', 'https', 'localhost.key');
const HTTPS_CERT_PATH = process.env.HTTPS_CERT_PATH || path.join(__dirname, '..', 'https', 'localhost.pem');
const HTTPS_PASSPHRASE = process.env.HTTPS_PASSPHRASE || 'abcd';
// .html/.js immer revalidieren lassen (kein stilles Stale-Caching durch Browser/Proxy
// nach Code-Änderungen, z.B. boardViewer.html) Bilder/STL etc. bleiben normal cachebar.
app.use((req, res, next) => {
if (/\.(html|js)$/.test(req.path)) res.setHeader('Cache-Control', 'no-cache');
next();
});
app.use(express.static(publicDir));
app.get('/api/health', (req, res) => {
@@ -429,12 +442,14 @@ app.post('/api/calibration/compute', async (req, res) => {
// ── Board-Erkennung ───────────────────────────────────────────────────────────
const boardDataDir = path.join(__dirname, '..', 'data', 'board');
const ROBOT_JSON = process.env.ROBOT_JSON
|| path.join(__dirname, '..', 'scripts', 'robot_1781069752019.json');
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');
const SCRIPT_4B = path.join(__dirname, '..', 'scripts', '4b_revolute_angle.py');
const SCRIPT_5POSE = path.join(__dirname, '..', 'scripts', '5_pose_estimation.py');
/**
* Führt ein Python-Script aus und leitet stdout/stderr zeilenweise an `send` weiter.
@@ -442,6 +457,8 @@ const SCRIPT_3B = path.join(__dirname, '..', 'scripts', '3b_corner_marker_po
*/
function runScript(args, send) {
return new Promise((resolve) => {
const cmd = [PYTHON_BIN, '-u', ...args].join(' ');
console.log(`[runScript] ${cmd}`);
const proc = spawn(PYTHON_BIN, ['-u', ...args]);
let outBuf = '';
@@ -473,6 +490,230 @@ function runScript(args, send) {
});
}
/**
* Board-Pipeline: Snapshot + Script 1 + Script 2 (pro Kamera) + Script 3b.
* Schreibt Ergebnisse nach runDir (muss bereits existieren).
* Wird von /api/board/run UND /api/homing/run genutzt.
*
* @param {string} runDir Zielverzeichnis (bereits erstellt)
* @param {Function} send SSE-Send-Funktion (obj => void)
* @param {{ refSet?: string }} [opts]
*/
async function runBoardPipeline(runDir, send, { refSet } = {}) {
try {
await fetchRobot();
} catch (err) {
send({ type: 'log', text: `⚠ robot.json-Cache: Driver nicht erreichbar nutze lokale Datei (${err.message})` });
}
// Kameras ermitteln
if (!WEBCAM_URL) throw new Error('WEBCAM_URL nicht konfiguriert');
const camData = await new WebcamClient(WEBCAM_URL).getCameras();
const cameraIds = (camData.cameras ?? []).map(c => c.id);
send({ type: 'log', text: `▶ Kameras: ${cameraIds.join(', ')}` });
send({ type: 'log', text: '' });
// 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)}` });
if (error) {
send({ type: 'log', text: `${error} Kamera übersprungen` });
continue;
}
send({ type: 'log', text: `✅ Foto: ${camId}.jpg` });
// NPZ suchen neueste Session, die eine NPZ für diese Kamera enthält
const npzInfo = await findLatestNpzForCamera(camId);
if (!npzInfo) {
send({ type: 'log', text: `⚠ Keine NPZ für ${camId} gefunden übersprungen` });
continue;
}
const npzPath = npzInfo.npzPath;
send({ type: 'log', text: `▶ NPZ: data/calibration/${npzInfo.session}/${camId}_calibration.npz` });
// Script 1 ArUco-Erkennung
send({ type: 'log', text: '\n▷ 1_detect_aruco_observations' });
const exit1 = await runScript([
SCRIPT_1,
'-i', imgPath,
'-npz', npzPath,
'-robot', robotCachePath,
'-cameraId', camId,
'-outDir', runDir,
'--saveDebugImage',
], send);
if (exit1 !== 0) {
send({ type: 'log', text: `❌ Script 1 Exit ${exit1}` });
continue;
}
// Script 2 Kamera-Pose schätzen
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', robotCachePath, '-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: '' });
}
// Script 3b: Marker-Triangulierung (benötigt ≥2 Kamera-Posen)
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', robotCachePath,
], 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: '' });
}
/**
* 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}/
@@ -502,105 +743,19 @@ app.post('/api/board/run', async (req, res) => {
// Robot-JSON laden und Marker-Anzahl loggen
let robotData = null;
try { robotData = JSON.parse(await fsPromises.readFile(ROBOT_JSON, 'utf8')); } catch {}
try { robotData = JSON.parse(await fsPromises.readFile(robotCachePath, 'utf8')); } catch {}
const boardMarkers = robotData?.links?.Board?.markers ?? [];
const boardMarkerCount = boardMarkers.length;
const refMarkerCount = refSet
? boardMarkers.filter(m => m.set === refSet).length
: boardMarkerCount;
send({ type: 'log', text: `▶ Robot-JSON: ${ROBOT_JSON}` });
send({ type: 'log', text: `▶ Robot-JSON: ${robotCachePath}` });
send({ type: 'log', text: `▶ Board-Marker: ${boardMarkerCount} (links.Board.markers)` });
send({ type: 'log', text: `▶ Referenz-Set: ${refSet ? `"${refSet}" (${refMarkerCount} Marker)` : 'alle'}` });
send({ type: 'log', text: '' });
// 2. Kameras ermitteln
if (!WEBCAM_URL) throw new Error('WEBCAM_URL nicht konfiguriert');
const camData = await new WebcamClient(WEBCAM_URL).getCameras();
const cameraIds = (camData.cameras ?? []).map(c => c.id);
send({ type: 'log', text: `▶ Kameras: ${cameraIds.join(', ')}` });
send({ type: 'log', text: '' });
// 3. Pro Kamera: Foto → Script 1 → Script 2
for (const camId of cameraIds) {
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` });
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
const npzInfo = await findLatestNpzForCamera(camId);
if (!npzInfo) {
send({ type: 'log', text: `⚠ Keine NPZ für ${camId} gefunden (in keiner Kalibrierungs-Session) übersprungen` });
continue;
}
const npzPath = npzInfo.npzPath;
send({ type: 'log', text: `▶ NPZ: data/calibration/${npzInfo.session}/${camId}_calibration.npz` });
// Script 1 ArUco-Erkennung
send({ type: 'log', text: '\n▷ 1_detect_aruco_observations' });
const exit1 = await runScript([
SCRIPT_1,
'-i', imgPath,
'-npz', npzPath,
'-robot', ROBOT_JSON,
'-cameraId', camId,
'-outDir', runDir,
'--saveDebugImage',
], send);
if (exit1 !== 0) {
send({ type: 'log', text: `❌ Script 1 Exit ${exit1}` });
continue;
}
// Script 2 Kamera-Pose schätzen
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', ROBOT_JSON, '-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: '' });
}
// ── Script 3b: Marker-Triangulierung (benötigt ≥2 Kamera-Posen) ──
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', ROBOT_JSON,
], send);
if (exit3b !== 0) send({ type: 'log', text: `❌ Script 3b Exit ${exit3b}` });
} else {
send({ type: 'log', text: `⚠ Nur ${numPoses} Kamera-Pose(n) vorhanden Script 3b braucht ≥2 Kameras für Triangulierung, wird übersprungen.` });
}
send({ type: 'log', text: '' });
// 23b: Board-Pipeline (Foto + Scripts 1, 2, 3b)
await runBoardPipeline(runDir, send, { refSet });
send({ type: 'log', text: `✅ Board-Run abgeschlossen: ${ts}` });
send({ type: 'done', exitCode: 0, runDir: ts });
@@ -652,20 +807,34 @@ app.get('/api/board/runs', async (req, res) => {
});
/**
* GET /api/board/latest?run=<timestamp>
* GET /api/board/latest?run=<timestamp>&from=homing
* Gibt Daten eines Board-Runs zurück: robot.json + Detection-Ergebnisse + Kamera-Posen.
* Ohne ?run → neuester Run. Mit ?run=<timestamp> → genau dieser Run.
* ?from=homing → liest aus data/homing/ statt data/board/ (für boardViewer im Homing-Mode).
* Wird vom Board-Viewer (boardViewer.html) abgefragt.
*/
app.get('/api/board/latest', async (req, res) => {
try {
const runName = req.query.run || await findLatestBoardRun();
const fromHoming = req.query.from === 'homing';
const dataDir = fromHoming ? homingDataDir : boardDataDir;
let runName = req.query.run;
if (!runName) {
if (fromHoming) {
try {
const entries = await fsPromises.readdir(dataDir, { withFileTypes: true });
runName = entries.filter(e => e.isDirectory()).map(e => e.name).sort().reverse()[0] ?? null;
} catch { runName = null; }
} else {
runName = await findLatestBoardRun();
}
}
if (!runName) return res.json({ runDir: null, robot: null, detections: [], cameraPoses: [] });
const runDir = path.join(boardDataDir, runName);
const runDir = path.join(dataDir, runName);
let robot = null;
try { robot = JSON.parse(await fsPromises.readFile(ROBOT_JSON, 'utf8')); } catch {}
try { robot = JSON.parse(await fsPromises.readFile(robotCachePath, 'utf8')); } catch {}
let files = [];
try { files = await fsPromises.readdir(runDir); } catch {}
@@ -708,12 +877,246 @@ app.get('/api/board/latest', async (req, res) => {
measuredMarkers = JSON.parse(raw);
} catch {}
return res.json({ runDir: runName, robotFile: path.basename(ROBOT_JSON), robot, detections, cameraPoses, measuredMarkers });
return res.json({ runDir: runName, robotFile: path.basename(robotCachePath), robot, detections, cameraPoses, measuredMarkers });
} catch (err) {
return res.status(500).json({ error: String(err) });
}
});
// ── Homing ───────────────────────────────────────────────────────────────────
/**
* POST /api/homing/run
* Vollständiger Homing-Ablauf: Board-Pipeline + 4b-Kette (SSE-Stream).
*/
app.post('/api/homing/run', async (req, res) => {
res.setHeader('Content-Type', 'text/event-stream');
res.setHeader('Cache-Control', 'no-cache');
res.setHeader('Connection', 'keep-alive');
res.flushHeaders();
const send = (obj) => {
if (!res.writableEnded) res.write(`data: ${JSON.stringify(obj)}\n\n`);
};
try {
await fsPromises.mkdir(homingDataDir, { recursive: true });
await runHoming({
robotJsonPath: robotCachePath,
homingDir: homingDataDir,
send,
runScript,
runBoardPipeline,
SCRIPT_4B,
SCRIPT_5POSE,
});
} catch (err) {
console.error('homing/run error:', err);
try {
send({ type: 'error', text: String(err) });
send({ type: 'done', exitCode: -1 });
} catch {}
}
if (!res.writableEnded) res.end();
});
/**
* Konvertiert den FK-State (von 4b_revolute_angle.py / 5_pose_estimation.py)
* in die G92-Driver-Konvention (appRobotDriver/doc/Info_G92.md).
*
* Unterschiede:
* b: FK b=0 = gerade Hand; Driver B=180° = gerade Hand → B = 180 b
* c: FK c=0 = neutral Roll; Driver C=90° = neutral → C = c + 90
* z: 4b misst Ellbogen RELATIV zu Arm1; Driver braucht absoluten Winkel → Z = y + z
*/
function fkStateToDriverG92(s) {
const d = { ...s };
if (d.b != null) d.b = 180 - d.b;
if (d.c != null) d.c = d.c + 90;
if (d.z != null && d.y != null) d.z = d.y + d.z;
return d;
}
/**
* POST /api/homing/send-state
* Baut aus { state: { x, y, z, a, b[, c, e] } } ein G92 und sendet es als
* Plain-Text-G-Code über den Driver-WebSocket (DRIVER_WS_URL). G92 setzt am
* Driver die Motorposition ohne Bewegung (intern M92) = Homing.
* Bekannte Achsen werden real gesendet; wirklich fehlende/unbeobachtbare
* Achsen (z.B. c/Palm, e/Greifer in der 4b-Kette) werden weggelassen — der
* Driver lässt sie unverändert (siehe server/buildG92.cjs).
*/
app.post('/api/homing/send-state', async (req, res) => {
try {
const { state } = req.body ?? {};
if (!state) return res.status(400).json({ error: '"state" fehlt' });
if (!isDriverConfigured())
return res.status(501).json({ error: 'DRIVER_WS_URL ist nicht konfiguriert' });
const gcode = buildG92(fkStateToDriverG92(state));
const result = await sendGcode(gcode);
if (!result.ok)
return res.status(502).json({ error: `Robot-Fehler: ${result.error}`, gcode });
return res.json({ ok: true, gcode, result: result.response, note: result.note });
} catch (err) {
console.error('homing/send-state error:', err);
return res.status(err.statusCode || 500).json({ error: String(err.message || err) });
}
});
/**
* POST /api/robot/gcode { line: "G92 X… Y…" }
* Sendet eine beliebige G-Code-Zeile über den Driver-WebSocket. Transport für
* die G-Code-/Befehl-Buttons im Frontend (window.sendCommand) — ersetzt den
* toten WSS-Altpfad.
*/
app.post('/api/robot/gcode', async (req, res) => {
try {
const line = (req.body?.line ?? '').toString().trim();
if (!line) return res.status(400).json({ error: '"line" fehlt' });
if (!isDriverConfigured())
return res.status(501).json({ error: 'DRIVER_WS_URL ist nicht konfiguriert' });
const result = await sendGcode(line);
if (!result.ok)
return res.status(502).json({ error: `Robot-Fehler: ${result.error}`, line });
return res.json({ ok: true, line, result: result.response, note: result.note });
} catch (err) {
console.error('robot/gcode error:', err);
return res.status(err.statusCode || 500).json({ error: String(err.message || err) });
}
});
/**
* GET /api/homing/run-data?run=<timestamp>
* Gibt Bilder (base64) und JSON-Dateien eines Homing-Runs zurück.
*/
app.get('/api/homing/run-data', async (req, res) => {
try {
const runName = req.query.run;
if (!runName) return res.status(400).json({ error: '"run" parameter fehlt' });
const runDir = path.join(homingDataDir, runName);
let files = [];
try { files = await fsPromises.readdir(runDir); } catch {}
const images = [];
for (const f of files.sort()) {
if (/\.(jpg|jpeg|png)$/i.test(f)) {
try {
const buf = await fsPromises.readFile(path.join(runDir, f));
images.push({ filename: f, contentBase64: buf.toString('base64'), mimeType: 'image/jpeg' });
} catch {}
}
}
// Letzten accumulated_state zurückgeben
let finalState = null;
const stateFiles = files.filter(f => f.startsWith('state_') && f.endsWith('.json')).sort();
if (stateFiles.length > 0) {
try {
const raw = await fsPromises.readFile(path.join(runDir, stateFiles[stateFiles.length - 1]), 'utf8');
finalState = JSON.parse(raw).accumulated_state ?? null;
} catch {}
}
// aruco_marker_poses.csv für Snapshot-CSV-Tabelle
let csvContent = null;
try {
csvContent = await fsPromises.readFile(path.join(runDir, 'aruco_marker_poses.csv'), 'utf8');
} catch {}
return res.json({ runDir: runName, images, finalState, csvContent });
} catch (err) {
return res.status(500).json({ error: String(err) });
}
});
/**
* 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 ─────────────────────────────────────────────────────
/**
@@ -743,7 +1146,7 @@ app.post('/api/robot/assign-by-z', async (req, res) => {
}
} catch { /* kein 3b-Output vorhanden nur bestehende robot.json-Marker bearbeiten */ }
const result = await assignByZRange(ROBOT_JSON, { zMin, zMax, set, link, extraMarkers });
const result = await assignByZRange(robotCachePath, { zMin, zMax, set, link, extraMarkers });
const added = result.changes.filter(c => c.action === 'added').length;
const updated = result.changes.filter(c => c.action === 'updated').length;
console.log(`robot/assign-by-z z=[${zMin}..${zMax}] set="${set}" link="${link}" → ${updated} aktualisiert, ${added} neu (von ${extraMarkers.length} 3b-Markern)`);
@@ -768,7 +1171,7 @@ app.post('/api/robot/remove-marker', async (req, res) => {
if (!['set', 'link'].includes(removeFrom)) {
return res.status(400).json({ error: 'removeFrom muss "set" oder "link" sein' });
}
const result = await removeMarkerAssignment(ROBOT_JSON, { markerId, removeFrom });
const result = await removeMarkerAssignment(robotCachePath, { markerId, removeFrom });
console.log(`robot/remove-marker id=${markerId} from=${removeFrom} → changed=${result.changed}`);
return res.json(result);
} catch (err) {
@@ -777,6 +1180,19 @@ app.post('/api/robot/remove-marker', async (req, res) => {
}
});
/**
* GET /api/robot
* Gibt robot.json zurück (ohne Board-Run-Daten).
*/
app.get('/api/robot', async (req, res) => {
try {
const robot = await fetchRobot();
return res.json(robot);
} catch (err) {
return res.status(500).json({ error: String(err) });
}
});
/**
* GET /api/robot/board-sets
* Gibt die einzigartigen "set"-Werte aller Marker in links.Board zurück.
@@ -784,7 +1200,7 @@ app.post('/api/robot/remove-marker', async (req, res) => {
*/
app.get('/api/robot/board-sets', async (req, res) => {
try {
const robot = JSON.parse(await fsPromises.readFile(ROBOT_JSON, 'utf8'));
const robot = await fetchRobot();
const markers = robot?.links?.Board?.markers ?? [];
const sets = [...new Set(markers.map(m => m.set).filter(Boolean))].sort();
return res.json({ sets });
@@ -815,7 +1231,7 @@ app.post('/api/robot/align-sets', async (req, res) => {
}
} catch { /* kein 3b-Output vorhanden */ }
const result = await alignSetToMeasured(ROBOT_JSON, { setToMove, extraMarkers });
const result = await alignSetToMeasured(robotCachePath, { setToMove, extraMarkers });
if (result.error) return res.status(400).json(result);
console.log(
@@ -850,7 +1266,7 @@ app.post('/api/robot/assign-id', async (req, res) => {
}
} catch { /* kein 3b-Output vorhanden */ }
const result = await assignMarkerId(ROBOT_JSON, { markerId, set, link, extraMarkers });
const result = await assignMarkerId(robotCachePath, { markerId, set, link, extraMarkers });
if (!result.changed && result.error) return res.status(400).json(result);
console.log(
@@ -876,7 +1292,7 @@ app.post('/api/robot/adopt-x-axis', async (req, res) => {
if (!Array.isArray(direction) || direction.length < 3) {
return res.status(400).json({ error: '"direction" muss ein Array [vx,vy,vz] sein.' });
}
const result = await adoptXAxis(ROBOT_JSON, { direction });
const result = await adoptXAxis(robotCachePath, { direction });
console.log(
`robot/adopt-x-axis dir=[${direction.map(v => Number(v).toFixed(4)).join(', ')}]` +
`${result.numChanged} Marker, Ursprung=[${result.origin.join(', ')}]` +
@@ -904,7 +1320,7 @@ app.post('/api/robot/assign-fixed-markers', async (req, res) => {
if (!targetLink) {
return res.status(400).json({ error: '"targetLink" muss angegeben werden.' });
}
const result = await assignFixedMarkersToLink(ROBOT_JSON, { markerIds, targetLink, measuredPositions });
const result = await assignFixedMarkersToLink(robotCachePath, { markerIds, targetLink, measuredPositions });
console.log(
`robot/assign-fixed-markers [${markerIds.join(',')}] → ${targetLink}` +
` added=${result.numAdded} alreadyPresent=${result.numAlreadyPresent}`,
@@ -929,7 +1345,7 @@ app.post('/api/robot/set-joint-origin', async (req, res) => {
if (!Number.isFinite(Number(y)) || !Number.isFinite(Number(z))) {
return res.status(400).json({ error: '"y" und "z" müssen Zahlen sein.' });
}
const result = await setJointOriginYZ(ROBOT_JSON, { linkName, y: Number(y), z: Number(z) });
const result = await setJointOriginYZ(robotCachePath, { linkName, y: Number(y), z: Number(z) });
if (!result.changed) {
return res.status(400).json({ error: result.error });
}
@@ -944,6 +1360,27 @@ app.post('/api/robot/set-joint-origin', async (req, res) => {
}
});
/**
* POST /api/robot/set-arm-marker-spin
* Setzt den `spin`-Wert eines Arm-Markers in robot.json.
* Body: { linkName: string, markerId: number, spin: number }
*/
app.post('/api/robot/set-arm-marker-spin', async (req, res) => {
try {
const { linkName, markerId, spin } = req.body ?? {};
if (!linkName) return res.status(400).json({ error: '"linkName" muss angegeben werden.' });
if (markerId == null) return res.status(400).json({ error: '"markerId" muss angegeben werden.' });
if (!Number.isFinite(Number(spin))) return res.status(400).json({ error: '"spin" muss eine Zahl sein.' });
const result = await setArmMarkerSpin(robotCachePath, { linkName, markerId, spin: Number(spin) });
if (!result.changed) return res.status(400).json({ error: result.error });
console.log(`robot/set-arm-marker-spin ${linkName}#${markerId}: ${result.oldSpin}° → ${result.newSpin}°`);
return res.json(result);
} catch (err) {
console.error('robot/set-arm-marker-spin error:', err);
return res.status(500).json({ error: String(err) });
}
});
/**
* POST /api/calibration/upload-npz
* Liest {camera}_calibration.npz aus der aktuellen Session und
@@ -1087,6 +1524,13 @@ async function startServer() {
await checkServiceReachability('BODYTRACKER_URL', new URL('/v1/health', BODYTRACKER_URL).toString());
}
try {
await fetchRobot();
console.log('✅ robot.json geladen und gecacht.');
} catch (err) {
console.warn(`⚠ robot.json: Driver nicht erreichbar nutze lokale Datei: ${err.message}`);
}
const server = await createHttpsServer();
const isHttps = Boolean(server);
const listenServer = server || app;

18
server/spinNormalize.cjs Normal file
View File

@@ -0,0 +1,18 @@
/**
* spinNormalize.cjs
* Reine Spin-Normalisierung: [0, 360) ohne I/O.
* CommonJS damit Jest (CJS) und ESM-Server dieselbe Funktion nutzen.
*/
/**
* Normalisiert einen Spin-Winkel auf [0, 360).
* Negative Werte und Werte ≥ 360 werden korrekt behandelt.
*
* @param {number|string} spin Spin in Grad (kann negativ oder > 360 sein)
* @returns {number} Spin in Grad, 0 ≤ result < 360
*/
function normalizeSpinDeg(spin) {
return ((Number(spin) % 360) + 360) % 360;
}
module.exports = { normalizeSpinDeg };

215
test/assignMarkerId.test.js Normal file
View File

@@ -0,0 +1,215 @@
/**
* assignMarkerId.test.js
* ======================
* Integration-Test für server/editRobot.js → assignMarkerId().
*
* Testet insbesondere den Guard für fehlende position_mm (Marker ohne
* triangulierte Position, z.B. Einzelkamera-Marker nach Schritt 5).
*
* Technisch: editRobot.js ist ein ES-Modul — es wird über den dünnen Runner
* test/fixtures/runAssignMarkerId.mjs per spawnSync aufgerufen (gleiche
* Strategie wie yAxisRotation.test.js für das Python-Skript).
* Datei-I/O läuft gegen echte Temp-Dateien (os.tmpdir()).
*/
const { spawnSync } = require('child_process');
const os = require('os');
const fs = require('fs');
const path = require('path');
const RUNNER = path.join(__dirname, 'fixtures', 'runAssignMarkerId.mjs');
// ── Hilfsfunktionen ───────────────────────────────────────────────────────────
function callAssignMarkerId(robotPath, params) {
const proc = spawnSync('node', [RUNNER, robotPath, JSON.stringify(params)], {
encoding: 'utf-8',
});
if (proc.error) throw proc.error;
const stdout = (proc.stdout ?? '').trim();
if (!stdout) throw new Error(`Kein Output.\nstderr: ${proc.stderr}`);
return JSON.parse(stdout);
}
function makeTempRobot(content) {
const p = path.join(
os.tmpdir(),
`robot_test_${Date.now()}_${Math.random().toString(36).slice(2)}.json`
);
fs.writeFileSync(p, JSON.stringify(content, null, 2), 'utf8');
return p;
}
const ROBOT_WITH_42 = () => ({
links: {
Arm1: { markers: [{ id: 42, set: 'A0', position: [100, 200, 300] }] },
},
});
const ROBOT_EMPTY = () => ({
links: { Arm1: { markers: [] } },
});
// ── Eingabe-Validierung ───────────────────────────────────────────────────────
describe('assignMarkerId Eingabe-Validierung', () => {
test('ungültige Marker-ID → changed: false', () => {
const p = makeTempRobot(ROBOT_EMPTY());
try {
const r = callAssignMarkerId(p, { markerId: -1, link: 'Arm1' });
expect(r.changed).toBe(false);
expect(r.error).toMatch(/Ungültige Marker-ID/);
} finally { fs.unlinkSync(p); }
});
test('kein link für neuen Marker → changed: false', () => {
const p = makeTempRobot(ROBOT_EMPTY());
try {
const r = callAssignMarkerId(p, {
markerId: 99,
extraMarkers: [{ marker_id: 99, position_mm: [10, 20, 30] }],
});
expect(r.changed).toBe(false);
expect(r.error).toMatch(/Link/);
} finally { fs.unlinkSync(p); }
});
});
// ── Guard: fehlende position_mm ───────────────────────────────────────────────
describe('assignMarkerId Guard: fehlende position_mm (z.B. Einzelkamera-Marker)', () => {
test('extraMarker ohne position_mm → changed: false, kein Crash', () => {
const p = makeTempRobot(ROBOT_EMPTY());
try {
const r = callAssignMarkerId(p, {
markerId: 55,
link: 'Arm1',
extraMarkers: [{ marker_id: 55 }], // kein position_mm
});
expect(r.changed).toBe(false);
expect(r.error).toMatch(/position_mm fehlt/);
} finally { fs.unlinkSync(p); }
});
test('extraMarker mit position_mm: null → changed: false, kein Crash', () => {
const p = makeTempRobot(ROBOT_EMPTY());
try {
const r = callAssignMarkerId(p, {
markerId: 56,
link: 'Arm1',
extraMarkers: [{ marker_id: 56, position_mm: null }],
});
expect(r.changed).toBe(false);
expect(r.error).toMatch(/position_mm fehlt/);
} finally { fs.unlinkSync(p); }
});
test('extraMarker mit position_mm als String → changed: false, kein Crash', () => {
const p = makeTempRobot(ROBOT_EMPTY());
try {
const r = callAssignMarkerId(p, {
markerId: 57,
link: 'Arm1',
extraMarkers: [{ marker_id: 57, position_mm: '[1,2,3]' }],
});
expect(r.changed).toBe(false);
expect(r.error).toMatch(/position_mm fehlt/);
} finally { fs.unlinkSync(p); }
});
});
// ── Normalfall: Marker hinzufügen ─────────────────────────────────────────────
describe('assignMarkerId Normalfall: neuen Marker hinzufügen', () => {
test('gültige position_mm → changed: true, action: added, Datei geschrieben', () => {
const p = makeTempRobot(ROBOT_EMPTY());
try {
const r = callAssignMarkerId(p, {
markerId: 77,
link: 'Arm1',
extraMarkers: [{ marker_id: 77, position_mm: [10.1, 20.22, 30.333] }],
});
expect(r.changed).toBe(true);
expect(r.change.action).toBe('added');
expect(r.change.markerId).toBe(77);
expect(r.change.newLink).toBe('Arm1');
const saved = JSON.parse(fs.readFileSync(p, 'utf8'));
const added = saved.links.Arm1.markers.find(m => m.id === 77);
expect(added).toBeDefined();
expect(Array.isArray(added.position)).toBe(true);
expect(added.position).toHaveLength(3);
} finally { fs.unlinkSync(p); }
});
test('Marker nicht in extraMarkers → changed: false', () => {
const p = makeTempRobot(ROBOT_EMPTY());
try {
const r = callAssignMarkerId(p, { markerId: 99, link: 'Arm1', extraMarkers: [] });
expect(r.changed).toBe(false);
expect(r.error).toMatch(/nicht.*vorhanden/i);
} finally { fs.unlinkSync(p); }
});
test('mit set → Marker hat set-Wert in der gespeicherten Datei', () => {
const p = makeTempRobot(ROBOT_EMPTY());
try {
const r = callAssignMarkerId(p, {
markerId: 78,
link: 'Arm1',
set: 'A0',
extraMarkers: [{ marker_id: 78, position_mm: [1, 2, 3] }],
});
expect(r.changed).toBe(true);
const saved = JSON.parse(fs.readFileSync(p, 'utf8'));
const added = saved.links.Arm1.markers.find(m => m.id === 78);
expect(added.set).toBe('A0');
} finally { fs.unlinkSync(p); }
});
});
// ── Normalfall: bestehenden Marker aktualisieren ──────────────────────────────
describe('assignMarkerId Normalfall: bestehenden Marker aktualisieren', () => {
test('Set ändern → changed: true, action: updated, Datei geändert', () => {
const p = makeTempRobot(ROBOT_WITH_42());
try {
const r = callAssignMarkerId(p, { markerId: 42, set: 'B0' });
expect(r.changed).toBe(true);
expect(r.change.action).toBe('updated');
expect(r.change.oldSet).toBe('A0');
expect(r.change.newSet).toBe('B0');
const saved = JSON.parse(fs.readFileSync(p, 'utf8'));
expect(saved.links.Arm1.markers.find(m => m.id === 42).set).toBe('B0');
} finally { fs.unlinkSync(p); }
});
test('in anderen Link verschieben → oldLink / newLink korrekt, Datei geändert', () => {
const p = makeTempRobot(ROBOT_WITH_42());
try {
const r = callAssignMarkerId(p, { markerId: 42, link: 'Arm2' });
expect(r.changed).toBe(true);
expect(r.change.oldLink).toBe('Arm1');
expect(r.change.newLink).toBe('Arm2');
const saved = JSON.parse(fs.readFileSync(p, 'utf8'));
expect(saved.links.Arm1.markers.find(m => m.id === 42)).toBeUndefined();
expect(saved.links.Arm2.markers.find(m => m.id === 42)).toBeDefined();
} finally { fs.unlinkSync(p); }
});
test('bestehender Marker: fehlende position_mm in extraMarkers ist irrelevant', () => {
const p = makeTempRobot(ROBOT_WITH_42());
try {
// Marker 42 ist in robot.json → position_mm-Guard darf nicht zuschlagen
const r = callAssignMarkerId(p, {
markerId: 42,
set: 'C0',
extraMarkers: [{ marker_id: 42 }], // kein position_mm aber irrelevant
});
expect(r.changed).toBe(true);
expect(r.change.action).toBe('updated');
} finally { fs.unlinkSync(p); }
});
});

View File

@@ -0,0 +1,59 @@
/**
* boardViewerHasXYZ.test.js
* =========================
* Unit-Test für die reine Hilfsfunktion `hasXYZ()` aus public/boardViewer.html.
*
* boardViewer.html ist kein ladbares JS-Modul (Inline-<script type="module">
* mit THREE.js/DOM/fetch-Abhängigkeiten) — ein normales require() ist daher
* nicht möglich, ohne die Datei in Module aufzuteilen (nicht Teil dieser
* Änderung). Stattdessen wird die `hasXYZ`-Funktionsdefinition per Regex aus
* der Datei extrahiert und isoliert ausgewertet — testet exakt die Guard-Logik,
* die an allen position_mm-Zugriffsstellen in boardViewer.html verwendet wird,
* ohne Three.js/DOM laden zu müssen.
*/
const fs = require('fs');
const path = require('path');
const SRC = fs.readFileSync(path.join(__dirname, '../public/boardViewer.html'), 'utf8');
const match = SRC.match(/function hasXYZ\(arr\)\s*\{[^}]*\}/);
if (!match) {
throw new Error('hasXYZ() nicht in boardViewer.html gefunden — Guard wurde entfernt/umbenannt?');
}
// eslint-disable-next-line no-eval
const hasXYZ = eval(`(${match[0]})`);
describe('boardViewer.html: hasXYZ() (Guard für fehlende position_mm)', () => {
test('gültiges [x,y,z] → true', () => {
expect(hasXYZ([1, 2, 3])).toBe(true);
expect(hasXYZ([0, 0, 0])).toBe(true);
expect(hasXYZ([-1.5, 200.25, 0])).toBe(true);
});
test('undefined/null (fehlendes position_mm) → false, kein Crash', () => {
expect(() => hasXYZ(undefined)).not.toThrow();
expect(hasXYZ(undefined)).toBe(false);
expect(hasXYZ(null)).toBe(false);
});
test('zu kurzes Array → false', () => {
expect(hasXYZ([1, 2])).toBe(false);
expect(hasXYZ([])).toBe(false);
});
test('nicht-numerische/NaN-Werte → false', () => {
expect(hasXYZ([1, NaN, 3])).toBe(false);
expect(hasXYZ(['a', 'b', 'c'])).toBe(false);
expect(hasXYZ([1, Infinity, 3])).toBe(false);
});
test('kein Array (z.B. Objekt oder String) → false', () => {
expect(hasXYZ({ x: 1, y: 2, z: 3 })).toBe(false);
expect(hasXYZ('1,2,3')).toBe(false);
});
test('Array mit mehr als 3 Werten (z.B. mit Zusatzfeld) → true (erste 3 zählen)', () => {
expect(hasXYZ([1, 2, 3, 4])).toBe(true);
});
});

Some files were not shown because too many files have changed in this diff Show More