diff --git a/doc/Homing.md b/doc/Homing.md index d575bae..3e1fea9 100644 --- a/doc/Homing.md +++ b/doc/Homing.md @@ -14,6 +14,14 @@ 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 1–3b) | +| [`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 | + --- ## Kinematik-Kette diff --git a/doc/Homing_0_Camera.md b/doc/Homing_0_Camera.md new file mode 100644 index 0000000..2c1f0f4 --- /dev/null +++ b/doc/Homing_0_Camera.md @@ -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 1–3b 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` diff --git a/doc/Homing_1_StepByStep.md b/doc/Homing_1_StepByStep.md new file mode 100644 index 0000000..4212b85 --- /dev/null +++ b/doc/Homing_1_StepByStep.md @@ -0,0 +1,77 @@ +# 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** — 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** *(Konzept 2026-06-16, noch nicht implementiert)* — zwei Marker auf + dem **direkten Kind-Link**, deren Verbindungsvektor (im Kind-Lokalframe) parallel + zur **eigenen** Achse des Kind-Links liegt → 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 ist nicht + nötig, das ergibt sich automatisch aus der bestehenden Mindest-Baseline-Prüfung nach + der Projektion. +3. **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). + +→ Code: `scripts/4b_revolute_angle.py` (`estimate_revolute_angle()`), Konstante +`PIVOT_FALLBACK_ID`, Feld `"method"` im Ergebnis-JSON. + +### Befund 2026-06-16 (wichtig für Priorisierung) + +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, Fallback-2 (Pivot) springt ein und meldet `z ≈ -4.33°` (intern konsistent, +Exit 0). Eine unabhängige Gegenrechnung (Least-Squares über alle Ellbow- *und* +Arm2-Marker, `z`+`a` frei) zeigt aber ein Minimum bei `z ≈ -38°` — die Fallback-2-Schätzung +liegt hier ca. 35–40° daneben. Die (noch nicht implementierte) Fallback-1-Rechnung mit +Arm2-Marker 144↔148/143↔146 hätte `z ≈ -44°` ergeben, sehr nah am Least-Squares-Minimum, +weil sie (wie Primär) nicht von der Pivot-Position abhängt. → Fallback-1 ist nicht nur +„nice to have", sondern in diesem Fall klar genauer als Fallback-2. +Detaillierte Aufarbeitung/Entscheidung: siehe `Homing_2_improvement.md` (geplant). + +## robot.json-Struktur (Kurzreferenz) + +- `links..parent` — Name des Eltern-Links (Kette/Baum) +- `links..jointToParent` — `{type, axis, origin, rotation, variable}` +- `links..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` Tuning / Auswirkung +- [ ] Fallback-1 Implementierung (Aufwandsschätzung: klein-mittel, ~2-3h — Kern-Mathematik + `_model_spoke_world()`/`_pair_estimate()` bereits vorhanden und wiederverwendbar; + neu: Kind-Link-Suche, Achsparallelitäts-Filter, 3-stufige Tier-Logik) + Tests +- [ ] 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) +- Bekannte Probleme / Ideen: `Homing_2_improvement.md` (geplant) diff --git a/doc/Homing_5_Pose.md b/doc/Homing_5_Pose.md new file mode 100644 index 0000000..2dcd4ef --- /dev/null +++ b/doc/Homing_5_Pose.md @@ -0,0 +1,307 @@ +# 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: Skript liegt bereits in `scripts/5_pose_estimation.py`, **noch nicht** +> in `homingOrchestrator.js`/`server.js` verdrahtet — und braucht noch einen +> kleinen Code-Hook, um den 4b-Startwert überhaupt entgegennehmen zu können +> (siehe Offene Punkte). + +--- + +## 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..{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 + +`estimate_pose()` ruft für `global_ba`/`hybrid` **immer zuerst selbst** +`estimate_sequential_fk()` als „billigen, robusten Init" auf +(`scripts/5_pose_estimation.py:471-476`) — es gibt aktuell **keinen** Parameter, +um stattdessen einen extern vorgegebenen Startwert (z. B. den `accumulated_state` +aus 4b) einzuspeisen, obwohl `estimate_global_ba()` selbst intern bereits ein +`x0`-Dict entgegennimmt (`:236-237`). + +`estimate_sequential_fk()` initialisiert jede 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** (`:296-304`). 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:** `5_pose_estimation.py` sollte in appRobotHoming **nicht kalt** +laufen, sondern mit dem `accumulated_state` der 4b-Kette als Startwert (Details +und der dafür nötige Code-Hook: Abschnitt „Integrationsschritte"). + +### 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".) + +--- + +## 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 + 35–40° 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 aktuell im appRobotHoming-Container.** `docker-compose.yaml` + installiert nur `opencv-python-headless numpy` + (`pip3 install --quiet --no-cache-dir opencv-python-headless numpy`). Ohne + `scipy` greift `HAVE_SCIPY=False`: `estimate_sequential_fk` lässt jeden Block + auf `0.0` stehen, `estimate_global_ba` gibt den (dann ebenfalls nullwertigen) + Startwert unverändert zurück — **kein Fehler, nur eine `[WARN]`-Zeile auf + stdout.** Das ist ein stiller Fehlmodus: muss vor dem ersten Einsatz behoben + werden (scipy zur `pip3 install`-Zeile ergänzen). +- **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..value`). Eine kleine Adapterfunktion + ist nötig, kein Drop-in-Ersatz. +- **Unbeobachtbare Gelenke werden als `0.0` ausgegeben**, nicht als `null` + (Konfidenz `none`/`observable:false` steht nur als Metadatum daneben). Das + widerspricht der sonst im Projektverbund befolgten Konvention „Unbekannt bleibt + `null`, nie erfundene `0`". Eine Integration muss `observable:false` aktiv auf + `null` ummappen, bevor der Zustand weitergereicht wird — sonst wandert eine + stille `0°`/`0mm` in Richtung Robotersteuerung. +- **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 z. B. `hybrid` parallel zur bestehenden + 4b-Kette laufen zu lassen und beide Ergebnisse zu vergleichen, bevor irgendetwas + ersetzt wird. +- **`finger_block_joints`/`per_link_method`** stehen schon (leer) in der + robot.json — vorbereitete, aber im Skript bisher ungenutzte Erweiterungspunkte + aus appRobotRendering. + +--- + +## Aufruf (Stand-alone, zum Testen) + +⚠️ Diese Aufrufe laufen **kalt** (kein externer Startwert — der Code-Hook dafür +existiert noch nicht, s. Integrationsschritte). Geeignet, um das Kaltstart-/ +Lokales-Minimum-Verhalten aus „Wichtige Einschränkung" zu beobachten und zu +reproduzieren — **nicht** der vorgesehene Produktionspfad. + +```bash +python scripts/5_pose_estimation.py data/homing//aruco_marker_poses.json \ + -robot scripts/robot_1781069752019.json \ + -out data/homing//robot_state.json +# Verfahren erzwingen, z.B. zum gezielten Vergleich einzelner Methoden: +python scripts/5_pose_estimation.py data/homing//aruco_marker_poses.json \ + -robot scripts/robot_1781069752019.json --method global_ba +``` + +--- + +## Integrationsschritte (Offene Punkte) + +- [ ] **`scipy` in `docker-compose.yaml` ergänzen** (`pip3 install …` Zeile) — + ohne das läuft `hybrid` lautlos auf Nullzustand. +- [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. +- [ ] **Code-Hook in `5_pose_estimation.py` ergänzen:** aktuell gibt es keinen + Weg, einen externen Startwert hineinzugeben. Vorschlag: CLI-Flag analog zu 4b + (`--from-state accumulated_state.json`), das den 4b-Zustand als `x0` direkt an + `estimate_global_ba()` durchreicht (Parameter existiert dort bereits, + `:236-237`) und so den internen `estimate_sequential_fk()`-Kaltstart in + `estimate_pose()` (`:471-476`) umgeht bzw. nur als Fallback nutzt, falls kein + externer Startwert übergeben wird. +- [ ] Adapter `movements..value` → flaches `{x,…,e}`-State-Objekt für + `POST /api/homing/send-state`; dabei `observable:false → null` ummappen. +- [ ] Anbindung in `homingOrchestrator.js` (neuer Schritt, analog `runBoardPipeline`/ + 4b-Loop) + SSE-Event(s) für Fortschritt (auch ohne echtes Zwischenergebnis, + z. B. ein `step`-Event „läuft" / „fertig"). +- [ ] Erste echte Messung: `hybrid`-Ergebnis gegen 4b-Kette auf demselben + `data/homing//aruco_marker_poses.json` vergleichen (insbesondere am + Ellbow-Fall aus `Homing_1_StepByStep.md`). +- [ ] `huber_delta_mm`/`normal_weight` ggf. gegen reale Marker-Genauigkeit + nachjustieren (Defaults sind aus appRobotRendering-Simulation übernommen). +- [ ] Eintrag in `Homing.md`-Tabelle (Doku-Übersicht) ergänzen, sobald + verdrahtet. + +--- + +## Verweise + +- Allgemeiner Ablauf: [`Homing.md`](Homing.md) +- Vorheriger Schritt (Kamera/Triangulation, liefert den gemeinsamen Input): + [`Homing_0_Camera.md`](Homing_0_Camera.md) +- Alternative/Ist-Zustand (4b-Kette, dieselbe Aufgabe anders gelöst): + [`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"). diff --git a/scripts/4b_revolute_angle.py b/scripts/4b_revolute_angle.py index e371fe9..4ac41ab 100644 --- a/scripts/4b_revolute_angle.py +++ b/scripts/4b_revolute_angle.py @@ -5,10 +5,12 @@ 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 -(PRIMARY), with a single-marker pivot method as FALLBACK: +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: - PRIMARY — for every PAIR (m1, m2) of markers belonging to the target link: + 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) @@ -26,15 +28,28 @@ this script estimates the rotation angle using the pairwise-vector method Pair weights = baseline_model × baseline_obs (longer baselines → more reliable). - FALLBACK — only used when the PRIMARY method has no usable pair at all - (e.g. just one marker visible, or every visible pair happens to lie - parallel to the joint axis, as for two markers spaced along a forearm): - the joint PIVOT itself stands in for the missing second marker, i.e. the - "pair" becomes (pivot, m1). This needs only ONE matched marker, but — - unlike the primary method — 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` / `used_fallback` in the code. + 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 ----------------------- @@ -52,7 +67,7 @@ Output JSON { "link": "Arm1", "joint": "y", - "method": "marker_pair", // or "pivot_fallback" — see FALLBACK above + "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, @@ -80,11 +95,19 @@ 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 when the FALLBACK path (pivot vs. a single marker) -# was used instead of a real marker-to-marker pair — see the -# `used_fallback` block inside `estimate_revolute_angle()` below. +# 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 @@ -186,16 +209,18 @@ def _pair_estimate(v_model: np.ndarray, axis_world: np.ndarray, marker_ids: Tuple[int, int], min_baseline_mm: float, - fallback: bool) -> Tuple[Optional[float], Optional[float], dict]: + 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). - `fallback=True` marks entries produced by the pivot FALLBACK (one of - the two "markers" is actually the joint pivot, see PIVOT_FALLBACK_ID) - so callers/reports can always tell primary and fallback data apart. + `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) @@ -206,7 +231,8 @@ def _pair_estimate(v_model: np.ndarray, if bl_model < min_baseline_mm or bl_obs < min_baseline_mm: return None, None, { "marker_ids": list(marker_ids), - "fallback": fallback, + "link": source_link, + "tier": tier, "skipped": True, "reason": f"bl_model={bl_model:.1f} bl_obs={bl_obs:.1f} < {min_baseline_mm}", } @@ -215,7 +241,8 @@ def _pair_estimate(v_model: np.ndarray, weight = bl_model * bl_obs entry = { "marker_ids": list(marker_ids), - "fallback": fallback, + "link": source_link, + "tier": tier, "skipped": False, "angle_deg": math.degrees(angle), "baseline_model_mm": bl_model, @@ -225,6 +252,36 @@ def _pair_estimate(v_model: np.ndarray, 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) # ────────────────────────────────────────────────────────────── @@ -235,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: """ @@ -247,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 @@ -288,20 +349,14 @@ def estimate_revolute_angle( matched = {mid: (model_local[mid], observed_mm[mid]) for mid in model_local if mid in observed_mm} - # Only 1 matched marker is enough to *attempt* an estimate — the - # PIVOT FALLBACK below can work with a single marker. With 0 there - # is nothing to go on at all. - if len(matched) < 1: - return { - "status": "failed", - "reason": (f"Need ≥1 matched marker, found {len(matched)}. " - 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. def _spoke(local_pos: np.ndarray) -> np.ndarray: return _model_spoke_world(fk, zero_transforms, link_name, origin_world, local_pos) - # ── PRIMARY: marker-to-marker pairs within this link ────── + # ── 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 @@ -319,45 +374,98 @@ def estimate_revolute_angle( v_obs = o2 - o1 # observed, world frame angle, weight, entry = _pair_estimate( - v_model, v_obs, axis_world, (id1, id2), min_baseline_mm, fallback=False) + 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) - # ── FALLBACK: pivot + single marker, axis from predecessor ──── - # Only entered when the PRIMARY method above produced NOT A SINGLE - # usable pair (e.g. only one marker visible at all, or every visible - # pair happens to lie parallel to the joint axis — as for two - # markers spaced along a forearm). Each matched marker is paired - # with the joint PIVOT instead of another marker, using the - # rotation axis already known from the predecessor joints. - # This is strictly a fallback: compared to a real 2-marker baseline - # it additionally relies on the predecessor joints' *values* (not - # just their axis direction) being accurate, since the pivot's - # world position is computed via FK rather than observed directly. - used_fallback = False - if not angle_rad_list: - used_fallback = True - 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 + tier_used = TIER_PRIMARY + children_tried: List[str] = [] # for the diagnostic message if everything fails - angle, weight, entry = _pair_estimate( - v_model, v_obs, axis_world, - (PIVOT_FALLBACK_ID, mid), min_baseline_mm, fallback=True) - per_pair.append(entry) - if angle is not None: - angle_rad_list.append(angle) - weight_list.append(weight) + # ── 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) + + 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) + + 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, including the " - "pivot fallback. 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( @@ -370,9 +478,14 @@ 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())}") - if used_fallback: - print(f" [FALLBACK] No usable marker-marker pair — estimating from " - f"pivot + predecessor axis instead (single-marker spokes).") + 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: @@ -382,11 +495,13 @@ def estimate_revolute_angle( 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_}" - tag = " [fallback]" if pp.get("fallback") else "" + 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" {m0}↔{m1}{tag}: SKIPPED – {pp['reason']}") + print(f" {link_prefix}{m0}↔{link_prefix}{m1}{tag}: SKIPPED – {pp['reason']}") else: - print(f" {m0}↔{m1}{tag}: " + 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}") @@ -399,7 +514,7 @@ def estimate_revolute_angle( "status": "ok", "link": link_name, "joint": var, - "method": "pivot_fallback" if used_fallback else "marker_pair", + "method": tier_used, "joint_origin_world_mm": origin_world.tolist(), "joint_axis_world": axis_world.tolist(), "mean_angle_deg": mean_deg, @@ -436,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() @@ -463,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, ) diff --git a/scripts/5_pose_estimation.py b/scripts/5_pose_estimation.py new file mode 100644 index 0000000..8bf703c --- /dev/null +++ b/scripts/5_pose_estimation.py @@ -0,0 +1,539 @@ +#!/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: + marker_observation = "corner_pose" -> aruco_marker_poses.json (pos + measured normal) + marker_observation = "center_point" -> aruco_positions_*.json (pos only) + +Both the engine (estimate_pose) and a CLI (main) live here. +""" +from __future__ import annotations + +import argparse +import json +import math +import os +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, + "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": {}, +} + + +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) or aruco_positions_*.json (position only). + Returns: marker_id -> {pos_mm:(3,), normal:(3,)|None, link:str, n_cams:int} + """ + 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 + out[mid] = {"pos_mm": pos, "normal": nrm, "link": m.get("link", "?"), "n_cams": n_cams} + return out + + +# ================================================================== +# 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 + + return { + "ordered_vars": ordered_vars, + "var_type": var_type, + "var_links": dict(var_links), + "link_markers": link_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 residual_vector(state: Dict[str, float], fk: RobotFK, obs: Dict[int, Dict[str, Any]], + marker_ids: List[int], cfg: Dict[str, Any]) -> np.ndarray: + """Position (mm) + optional normal (scaled) residuals over the given markers.""" + model = model_markers(fk, state) + res: List[float] = [] + 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] + dp = np.asarray(mm["world_mm"], float) - obs[mid]["pos_mm"] + 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 + 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) + vec0 = np.array([base.get(v, 0.0) for v in var_names], dtype=float) + + def fun(vec): + st = _state_from_vec(var_names, vec, base) + 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(var_names))) + return _state_from_vec(var_names, sol.x, base) + except Exception as exc: + print(f"[WARN] global_ba failed: {exc}") + 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]) -> Dict[str, float]: + """Estimate block by block along the chain, freezing already-solved variables.""" + state = {k: 0.0 for 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 not bmarkers: + # unobservable block: leave at 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!) + none : no markers at all (variable left at 0) + """ + info: Dict[str, Dict[str, Any]] = {} + for block in chain["blocks"]: + seen = [m for m in block["markers"] if m in obs] + nvars = max(1, len(block["vars"])) + ratio = len(seen) / nvars + if len(seen) == 0: + conf = "none" + 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"]} + return info + + +def estimate_pose(fk: RobotFK, obs: Dict[int, Dict[str, Any]], cfg: Dict[str, Any]) -> Dict[str, Any]: + 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) + elif method == "global_ba": + init = estimate_sequential_fk(fk, obs, chain, cfg) # cheap robust init + state = estimate_global_ba(fk, obs, var_names, init, cfg) + else: # hybrid (default) + init = estimate_sequential_fk(fk, obs, chain, cfg) + 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 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") + 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 + + fk = RobotFK(robot_data) + obs = load_observations(args.markers, cfg.get("use_normals", True), + int(cfg.get("min_cameras_per_marker", 2))) + print(f"[INFO] method={cfg['method']} | observed markers={len(obs)} | use_normals={cfg.get('use_normals')}") + + result = estimate_pose(fk, obs, cfg) + 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 -> 0]" + 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}") + + out = { + "schema_version": "1.0", + "created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()), + "method": result["method"], + "movements": {v: {"value": st.get(v, 0.0), + "unit": "mm" if v in ("x", "e") else "deg", + "observable": result["observability"].get(v, {}).get("observable", False), + "confidence": result["observability"].get(v, {}).get("confidence", "none"), + "n_markers": result["observability"].get(v, {}).get("n_markers", 0)} + for v in ["x", "y", "z", "a", "b", "c", "e"]}, + "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}") + + +if __name__ == "__main__": + main() diff --git a/scripts/__pycache__/4b_revolute_angle.cpython-311.pyc b/scripts/__pycache__/4b_revolute_angle.cpython-311.pyc new file mode 100644 index 0000000..f729e5a Binary files /dev/null and b/scripts/__pycache__/4b_revolute_angle.cpython-311.pyc differ