ChK/appRobotHoming
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

4b kind-marker für winkel beachten

Browse Source
This commit is contained in:
chk
2026-06-16 15:28:14 +02:00
parent 0234c1ef1d
commit 578b955508
7 changed files with 1155 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
doc/Homing.md
View File

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

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 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`

77
doc/Homing_1_StepByStep.md Normal file
View File

@@ -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.<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` 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)

307
doc/Homing_5_Pose.md Normal file
View File

@@ -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.<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
`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.<var>.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/<run>/aruco_marker_poses.json \
-robot scripts/robot_1781069752019.json \
-out data/homing/<run>/robot_state.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
```
---
## 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.<var>.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/<run>/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").

234
scripts/4b_revolute_angle.py
View File

@@ -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:
"""
@@ -248,6 +306,9 @@ def estimate_revolute_angle(
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
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,26 +374,76 @@ 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
tier_used = TIER_PRIMARY
children_tried: List[str] = [] # for the diagnostic message if everything fails
# ── 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:
used_fallback = True
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
@@ -346,7 +451,8 @@ def estimate_revolute_angle(
angle, weight, entry = _pair_estimate(
v_model, v_obs, axis_world,
(PIVOT_FALLBACK_ID, mid), min_baseline_mm, fallback=True)
(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)
@@ -355,9 +461,11 @@ def estimate_revolute_angle(
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()
@@ -464,6 +583,7 @@ def main() -> int:
link_name = args.link,
known_state = known_state,
min_baseline_mm = args.min_baseline,
child_axis_tol_mm = args.child_axis_tol,
verbose = True,
)

539
scripts/5_pose_estimation.py Normal file
View File

@@ -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()

BIN
scripts/__pycache__/4b_revolute_angle.cpython-311.pyc Normal file
View File

Binary file not shown.