405 lines
16 KiB
Markdown
405 lines
16 KiB
Markdown
# 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: schnell, robust, vollautomatisch.
|
||
Kalibrierung hingegen läuft nur nach mechanischen Änderungen (≈ einmalig).
|
||
|
||
---
|
||
|
||
## Kinematik-Kette (aus `robot.json → links`)
|
||
|
||
```
|
||
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)
|
||
```
|
||
|
||
**Resultat-State:** `{ x_mm, y_deg, z_deg, a_deg, b_deg, c_deg, e_mm }`
|
||
|
||
---
|
||
|
||
## Voraussetzungen (Kalibrierung)
|
||
|
||
| Was | Mechanismus | Status |
|
||
|-----|-------------|--------|
|
||
| Kamera-Intrinsik (NPZ) | `calibration.html` → Tab Camera NPZ | ✅ fertig |
|
||
| Board-Marker-Positionen | `calibration.html` → Tab Board | ✅ fertig |
|
||
| X-Achsen-Richtung | `calibration.html` → Tab Robot X-Axis | ✅ fertig |
|
||
| **Arm1 Joint-Origin Y/Z** | `calibration.html` → Tab Arm1 → Button „Joint-Origin Y/Z übernehmen" | ✅ **Button vorhanden, ausführbar** |
|
||
| Arm-Marker in robot.json | Manuell eintragen (links.Arm1/Ellbow/Arm2/Hand.markers) | 🔶 Nutzer trägt ein |
|
||
|
||
> **Kalibrierung gilt als abgeschlossen** sobald der Arm1-Button geklickt und
|
||
> die Arm-Marker eingetragen sind.
|
||
|
||
---
|
||
|
||
## robot.json – Ladestrategie
|
||
|
||
### Aktuell: lokale Datei
|
||
```
|
||
ROBOT_JSON = process.env.ROBOT_JSON || 'scripts/robot_1781069752019.json'
|
||
```
|
||
|
||
### Geplant: vom Driver per API
|
||
Der Driver-Service (ROBOT_URL) kennt die aktuelle Roboter-Konfiguration.
|
||
Lademechanismus (bereits implementiertes Muster aus `ROBOT_URL`/`BODYTRACKER_URL`):
|
||
|
||
```
|
||
GET ROBOT_URL/api/robot/config → robot.json Inhalt
|
||
```
|
||
|
||
**Implementierung (Backend `server.js`):**
|
||
```javascript
|
||
async function loadRobotConfig() {
|
||
if (ROBOT_URL) {
|
||
// Vom Driver holen
|
||
const res = await fetch(new URL('/api/robot/config', ROBOT_URL));
|
||
return res.json();
|
||
}
|
||
// Fallback: lokale Datei
|
||
return JSON.parse(await fs.readFile(ROBOT_JSON, 'utf8'));
|
||
}
|
||
```
|
||
|
||
**Konsequenz für Homing:** Das Homing-Script bekommt robot.json als
|
||
temporäre Datei (bereits vorhandenes Muster: `ROBOT_JSON` als Pfad an Python).
|
||
Falls ROBOT_URL konfiguriert: zuerst fetch → temp-Datei schreiben → Script aufrufen.
|
||
|
||
**Priorität:** Kann nach dem restlichen Homing implementiert werden.
|
||
Solange ROBOT_URL nicht konfiguriert, läuft alles mit der lokalen Datei.
|
||
|
||
---
|
||
|
||
## X-Position (Slider) – Bestimmung
|
||
|
||
Die Slider-Position `x` wird **nicht** manuell eingegeben, sondern aus den
|
||
triangulierten Marker-Positionen berechnet (nach Schritt 3b).
|
||
|
||
**Ansatz:** Die absolute X-Position eines bekannten Arm-Markers im
|
||
Board-Koordinatensystem enthält direkt die Slider-Information —
|
||
alle anderen Gelenke sind rotatorisch und verschieben den Marker nicht
|
||
entlang der X-Achse des Boards.
|
||
|
||
Alternativ: Schwerpunkt der Board-nahen A0-Marker projiziert auf die X-Achse
|
||
(robust, braucht keine Arm-Marker).
|
||
|
||
**In `4b_revolute_angle.py`:** `--x-mm` wird aus `aruco_marker_poses.json`
|
||
berechnet und als erstes Argument übergeben. Alle weiteren 4b-Aufrufe
|
||
nutzen `--from-state` des vorherigen Schritts.
|
||
|
||
---
|
||
|
||
## Homing-Ablauf (Script-Kette)
|
||
|
||
```
|
||
[Foto] → [1_detect] → [2_camera] → [3b_poses]
|
||
│
|
||
▼
|
||
[4b Arm1]
|
||
[4b Ellbow]
|
||
[4b Arm2]
|
||
[4b Hand]
|
||
│
|
||
▼
|
||
State-JSON
|
||
{x,y,z,a,b,c,e}
|
||
│
|
||
▼
|
||
POST ROBOT_URL/api/state
|
||
```
|
||
|
||
**Strategie:** `4b_revolute_angle.py` sequenziell, Link für Link von root nach tip.
|
||
X-Position (`--x-mm`) wird aus den triangulierten Board-Marker-Positionen bestimmt.
|
||
|
||
---
|
||
|
||
## Implementierungsplan: Homing-UI
|
||
|
||
### ⚠ Wichtig: Schritte 1–3b existieren bereits
|
||
|
||
Die Kette **Foto → 1_detect → 2_camera → 3b_poses** ist bereits vollständig
|
||
implementiert und produktiv in `POST /api/board/run` (`server/server.js`,
|
||
ca. Zeile 520–606). Sie erzeugt `<runDir>/aruco_marker_poses.json`.
|
||
|
||
**Die echten, funktionierenden Aufrufe (NICHT neu erfinden):**
|
||
|
||
```javascript
|
||
// Pro Kamera geloopt (jede Kamera hat eigene NPZ):
|
||
runScript([SCRIPT_1, '-i', imgPath, '-npz', npzPath, '-robot', ROBOT_JSON,
|
||
'-cameraId', camId, '-outDir', runDir, '--saveDebugImage']);
|
||
|
||
runScript([SCRIPT_2, '-i', detJson, '-robot', ROBOT_JSON, '-outDir', runDir]);
|
||
|
||
// Einmal, nach allen Kameras (braucht ≥2 _camera_pose.json):
|
||
runScript([SCRIPT_3B, '--evalDir', runDir, '--robot', ROBOT_JSON]);
|
||
// → schreibt <runDir>/aruco_marker_poses.json
|
||
```
|
||
|
||
**Empfehlung:** Die Snapshot+1+2+3b-Logik aus `/api/board/run` in eine
|
||
gemeinsame Funktion `runBoardPipeline(runDir, send)` auslagern. Homing ruft
|
||
sie auf und hängt nur den 4b-Teil an. So gibt es keine Duplikate und keine
|
||
abweichenden Argumente.
|
||
|
||
---
|
||
|
||
### Phase 1 — Backend-Route `POST /api/homing/run`
|
||
|
||
**Datei:** `server/server.js` (neue Route) + `server/homingOrchestrator.js` (neue Datei)
|
||
|
||
**Ablauf als SSE-Stream:**
|
||
|
||
```javascript
|
||
// server/homingOrchestrator.js
|
||
export async function runHoming({ robotJsonPath, send }) {
|
||
|
||
// 1–3b: bestehende Board-Pipeline wiederverwenden
|
||
// (Foto + 1_detect + 2_camera + 3b_poses, pro Kamera geloopt)
|
||
send({ type: 'step', step: 1, text: 'Snapshot + Marker-Triangulation …' });
|
||
const runDir = await runBoardPipeline(robotJsonPath, send); // aus server.js ausgelagert
|
||
const arucoJson = path.join(runDir, 'aruco_marker_poses.json');
|
||
|
||
// 4. X-Position aus triangulierten Markern bestimmen
|
||
const xMm = estimateXFromMarkers(arucoJson); // siehe Abschnitt „X-Position"
|
||
|
||
// 4b. Gelenkwinkel sequenziell, Link für Link
|
||
const links = ['Arm1', 'Ellbow', 'Arm2', 'Hand'];
|
||
let fromState = null;
|
||
for (const link of links) {
|
||
send({ type: 'step', text: `Gelenkwinkel ${link} …` });
|
||
const args = [SCRIPT_4B,
|
||
'--robot', robotJsonPath, '--aruco', arucoJson,
|
||
'--link', link,
|
||
'--output', path.join(runDir, `state_${link}.json`),
|
||
];
|
||
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: 'error', text: `4b ${link} Exit ${exit}` }); return; }
|
||
fromState = path.join(runDir, `state_${link}.json`);
|
||
}
|
||
|
||
// Ergebnis: letztes state_*.json enthält den vollständigen accumulated_state
|
||
const finalState = JSON.parse(fs.readFileSync(fromState, 'utf8'));
|
||
send({ type: 'done', state: finalState.accumulated_state, runDir });
|
||
}
|
||
```
|
||
|
||
> **Hinweis:** `runScript()` gibt den Exit-Code zurück (nicht den Pfad).
|
||
> Der State-Pfad wird separat gemerkt (`fromState`).
|
||
|
||
**Route:**
|
||
```javascript
|
||
app.post('/api/homing/run', async (req, res) => {
|
||
res.setHeader('Content-Type', 'text/event-stream');
|
||
const send = (data) => res.write(`data: ${JSON.stringify(data)}\n\n`);
|
||
try {
|
||
await runHoming({ robotJsonPath: ROBOT_JSON, send });
|
||
} catch (err) {
|
||
send({ type: 'error', text: String(err) });
|
||
}
|
||
res.end();
|
||
});
|
||
|
||
app.post('/api/homing/send-state', async (req, res) => {
|
||
// Sendet { x, y, z, a, b, c, e } an ROBOT_URL/api/state
|
||
});
|
||
```
|
||
|
||
---
|
||
|
||
### Phase 2 — Frontend `public/homing.html`
|
||
|
||
Neue Seite (zugänglich von `index.html` via Link-Button, wie `calibration.html`).
|
||
|
||
**Sektionen (identisches Muster wie `index.html`):**
|
||
|
||
```
|
||
┌──────────────────────────────────────────────────────┐
|
||
│ AKTIONEN │
|
||
│ [📷 Foto & Homing berechnen] │
|
||
│ [✅ An Roboter senden] (disabled bis Ergebnis) │
|
||
│ Status-Badge: ○ Warte ● Läuft ✓ Fertig ✗ Fehler │
|
||
├──────────────────────────────────────────────────────┤
|
||
│ AUSGABE / LOG │
|
||
│ Schritt-für-Schritt Log aller Scripts (SSE-Stream) │
|
||
│ Fortschritt: ──── Schritt 3/6 ──── │
|
||
├──────────────────────────────────────────────────────┤
|
||
│ ANALYSIS & REASONING │
|
||
│ Zwischenergebnisse je Script als JSON │
|
||
│ { camera_reprojection_px, arm1_std_deg, … } │
|
||
├──────────────────┬───────────────────────────────────┤
|
||
│ RESULT RAW JSON │ RESULT TREE VIEW │
|
||
│ { │ x (Slider): 180.0 mm │
|
||
│ "x": 180.0, │ y (Arm1): +23.4° │
|
||
│ "y": 23.4, │ z (Ellbow): -12.1° │
|
||
│ ... │ a (Arm2): +5.0° │
|
||
│ } │ b (Hand): 0.0° │
|
||
│ │ c (Palm): 0.0° │
|
||
│ │ e (Greifer): 0.0 mm │
|
||
├──────────────────┴───────────────────────────────────┤
|
||
│ SNAPSHOT CSV (Marker-Tabelle) │
|
||
│ ID │ Link │ x mm │ y mm │ z mm │ Residual │ │
|
||
│ 218 │ Arm2 │ 229.1 │ 118.5 │ 48.3 │ 2.1 mm │ │
|
||
├──────────────────────────────────────────────────────┤
|
||
│ SNAPSHOTS (annotierte Kamerabilder) │
|
||
│ [cam0] [cam1] [cam2] │
|
||
└──────────────────────────────────────────────────────┘
|
||
```
|
||
|
||
**Schlüssel-Implementierungsdetails:**
|
||
|
||
```javascript
|
||
// homing.js (client)
|
||
|
||
// SSE-Stream vom Backend empfangen
|
||
async function runHoming() {
|
||
const response = await fetch('/api/homing/run', { method: 'POST' });
|
||
await readSseStream(response, appendLog, (evt) => {
|
||
if (evt.type === 'step') { updateProgress(evt); }
|
||
if (evt.type === 'analysis') { showAnalysis(evt.data); }
|
||
if (evt.type === 'done') {
|
||
showResult(evt.state);
|
||
enableSendButton(evt.state);
|
||
}
|
||
});
|
||
}
|
||
|
||
// Ergebnis an Roboter senden
|
||
async function sendToRobot(state) {
|
||
await fetch('/api/homing/send-state', {
|
||
method: 'POST',
|
||
body: JSON.stringify({ state }),
|
||
});
|
||
}
|
||
```
|
||
|
||
---
|
||
|
||
### Phase 3 — robot.json via Driver-API
|
||
|
||
**Voraussetzung:** ROBOT_URL ist konfiguriert und der Driver hat `GET /api/robot/config`.
|
||
|
||
**Implementierung in `server.js`:**
|
||
```javascript
|
||
// Beim Start oder on-demand: robot.json vom Driver laden
|
||
async function fetchRobotConfig() {
|
||
if (!ROBOT_URL) return; // lokale Datei reicht
|
||
const res = await fetch(new URL('/api/robot/config', ROBOT_URL));
|
||
if (!res.ok) return; // Fallback auf lokale Datei
|
||
const data = await res.json();
|
||
// Temporär in data/robot/robot_live.json cachen
|
||
await fs.writeFile(ROBOT_JSON_LIVE, JSON.stringify(data, null, 2));
|
||
}
|
||
```
|
||
|
||
**Auswirkung:** Nur `ROBOT_JSON` Variable ändern — alle Scripts bekommen
|
||
automatisch die aktuelle Konfiguration.
|
||
|
||
---
|
||
|
||
## To-Do / Fortschritt
|
||
|
||
### Voraussetzungen (Kalibrierung abschliessen)
|
||
|
||
- [x] Kamera-Intrinsik (NPZ) kalibriert
|
||
- [x] Board-Marker-Positionen kalibriert
|
||
- [x] X-Achsen-Richtung kalibriert
|
||
- [x] Arm1 Joint-Origin Y/Z — Button in `calibration_arm.html` ausführbar
|
||
- [ ] **Arm-Marker eintragen** (Nutzer): `links.Arm1.markers`, `links.Ellbow.markers`, `links.Arm2.markers`, `links.Hand.markers` in `scripts/robot_*.json`
|
||
- [ ] Arm1 Joint-Origin Y/Z Button klicken + in robot.json gespeichert
|
||
|
||
---
|
||
|
||
### Phase 0 — Refactor: Board-Pipeline auslagern
|
||
|
||
- [ ] Funktion `runBoardPipeline(runDir, send)` in `server/server.js` extrahieren
|
||
- [ ] Bestehende Logik aus `POST /api/board/run` (Zeile ~520–606) in Funktion verschieben
|
||
- [ ] `POST /api/board/run` ruft `runBoardPipeline()` auf (Verhalten unverändert)
|
||
- [ ] Rückgabewert: `runDir` (String, Pfad zum Lauf-Verzeichnis)
|
||
- [ ] Test: Board-Run funktioniert weiterhin identisch
|
||
|
||
---
|
||
|
||
### Phase 1 — Backend `POST /api/homing/run`
|
||
|
||
- [ ] Konstante `SCRIPT_4B` in `server/server.js` ergänzen (analog zu `SCRIPT_1`, `SCRIPT_3B` etc.)
|
||
- [ ] `server/homingOrchestrator.js` erstellen
|
||
- [ ] `estimateXFromMarkers(arucoJsonPath)` implementieren (X aus triangulierten Marker-Positionen)
|
||
- [ ] `runHoming({ robotJsonPath, send })` implementieren
|
||
- [ ] `runBoardPipeline()` aufrufen → `runDir` + `aruco_marker_poses.json`
|
||
- [ ] X-Position berechnen
|
||
- [ ] 4b-Schleife: Arm1 → Ellbow → Arm2 → Hand (sequenziell, `--from-state`)
|
||
- [ ] `send({ type: 'done', state: accumulated_state, runDir })`
|
||
- [ ] Route `POST /api/homing/run` in `server/server.js` registrieren (SSE-Stream)
|
||
- [ ] Minimale Test-UI (temporär): Fetch + Log im Browser-Konsole genügt
|
||
|
||
---
|
||
|
||
### Phase 2 — Frontend `public/homing.html`
|
||
|
||
- [ ] Datei `public/homing.html` erstellen
|
||
- [ ] Sektion **Aktionen**: Button „Foto & Homing berechnen", Button „An Roboter senden" (disabled)
|
||
- [ ] Sektion **Ausgabe / Log**: SSE-Stream-Ausgabe, Schritt-Fortschritt
|
||
- [ ] Sektion **Analysis & Reasoning**: Zwischenergebnisse je Script als JSON
|
||
- [ ] Sektion **Result Raw JSON** + **Result Tree View**: `{ x, y, z, a, b, c, e }`
|
||
- [ ] Sektion **Snapshot CSV**: Marker-Tabelle (ID, Link, x, y, z, Residual)
|
||
- [ ] Sektion **Snapshots**: annotierte Kamerabilder (cam0, cam1, …)
|
||
- [ ] `public/homing.js` erstellen
|
||
- [ ] `runHoming()`: POST + SSE-Stream lesen, Log befüllen
|
||
- [ ] `showResult(state)`: Tree View + Raw JSON befüllen, Send-Button aktivieren
|
||
- [ ] `sendToRobot(state)`: POST `/api/homing/send-state`
|
||
- [ ] Link-Button von `public/index.html` zu `homing.html` ergänzen
|
||
|
||
---
|
||
|
||
### Phase 3 — State an Roboter senden
|
||
|
||
- [ ] Route `POST /api/homing/send-state` in `server/server.js` registrieren
|
||
- [ ] Body: `{ state: { x, y, z, a, b, c, e } }`
|
||
- [ ] Weiterleitung an `ROBOT_URL/api/state` (analog zu `robotActions.js`)
|
||
- [ ] Fehler wenn `ROBOT_URL` nicht konfiguriert: JSON-Fehler zurückgeben
|
||
- [ ] Frontend: Fehler-Feedback wenn kein ROBOT_URL
|
||
|
||
---
|
||
|
||
### Phase 4 — robot.json via Driver-API *(nach allem anderen)*
|
||
|
||
- [ ] `loadRobotConfig()` Funktion in `server/server.js`
|
||
- [ ] Wenn `ROBOT_URL` gesetzt: `GET ROBOT_URL/api/robot/config` → temp-Datei cachen
|
||
- [ ] Fallback: lokale Datei (Verhalten unverändert)
|
||
- [ ] Homing und Board-Run nutzen `loadRobotConfig()` statt direkt `ROBOT_JSON`
|
||
- [ ] *(Voraussetzung: Driver implementiert `GET /api/robot/config`)*
|
||
|
||
---
|
||
|
||
## Status-Übersicht
|
||
|
||
| Bereich | Status |
|
||
|---------|--------|
|
||
| Python-Scripts (1, 2, 3b, 4b) | ✅ vorhanden |
|
||
| Kalibrierung (Kamera, Board, X-Achse) | ✅ fertig |
|
||
| Arm1 Joint-Origin Button | ✅ ausführbar |
|
||
| Arm-Marker in robot.json | 🔶 Nutzer |
|
||
| Phase 0 – runBoardPipeline() | ❌ offen |
|
||
| Phase 1 – /api/homing/run | ❌ offen |
|
||
| Phase 2 – homing.html | ❌ offen |
|
||
| Phase 3 – /api/homing/send-state | ❌ offen |
|
||
| Phase 4 – robot.json via Driver-API | ⏳ später |
|