Callibration endet bei Y-Axis
This commit is contained in:
chk
381
doc/Homing_ROADMAP.md
Normal file
381
doc/Homing_ROADMAP.md
Normal file
@@ -0,0 +1,381 @@
|
||||
# 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 |
|
||||
Reference in New Issue
Block a user