From 578b9555081d3abe9621c107ef86b832d68e4d1c Mon Sep 17 00:00:00 2001 From: chk <79915315+ChKendel@users.noreply.github.com> Date: Tue, 16 Jun 2026 15:28:14 +0200 Subject: [PATCH] =?UTF-8?q?4b=20kind-marker=20f=C3=BCr=20winkel=20beachten?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- doc/Homing.md | 8 + doc/Homing_0_Camera.md | 34 ++ doc/Homing_1_StepByStep.md | 77 +++ doc/Homing_5_Pose.md | 307 ++++++++++ scripts/4b_revolute_angle.py | 260 ++++++--- scripts/5_pose_estimation.py | 539 ++++++++++++++++++ .../4b_revolute_angle.cpython-311.pyc | Bin 0 -> 29863 bytes 7 files changed, 1155 insertions(+), 70 deletions(-) create mode 100644 doc/Homing_0_Camera.md create mode 100644 doc/Homing_1_StepByStep.md create mode 100644 doc/Homing_5_Pose.md create mode 100644 scripts/5_pose_estimation.py create mode 100644 scripts/__pycache__/4b_revolute_angle.cpython-311.pyc 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 0000000000000000000000000000000000000000..f729e5a1bea998b594257238201bf3ae4ed2931e GIT binary patch literal 29863 zcmdUYYj9h~mFC5Z1Of0(ks`&%CHMj;zD2!gQ5N;IUX(0Kax7V)KweOSL;~~$NQp4G zVH|HvYLzkU$R#+DCUC}a=vW?eW|Jw-)@0T@Ge102wG~{|O&NtM&Zs=&%Fdr%x{_46 z^k?=v-FRI{ik+F+s@cnnzJ2?4pMIS_efpf!%^w#QS_E9W6lc3XN`1Rs>E}XG%VHNSu358>hY|TPD&!4_0qFaGoB67DX9}r zQ5uo9Bv<*wS!!)7u;?5 zwY-bB_j%L6eHU%>K7Raq8ErBmIG>;S<03(@^pB0o?pfdDOu#+r@?M&BcTLY(Iv;ac z4!ON<*)uMtqIX{LdAtFUB@*5KfM?1T@X1{k%R!$kx?SU!#VOycYi!ajPI|moMaPgl z)!QK+n4BE*UF#6>)+c_t@UBz5cG>55iyT;BZrUw+{9;=wleP{^;4%XIJq1CWVblsLK$7-S33Y}a!#D_yCoFT?PYmRqhuc6jNd}}O!#KJ zk~n;Pi3Dm%PXtm__sr{umw4d)8Do#M31BM(?CBG0jh731Wo!^ekC zJwE_oXtr6mJSUzUI($lWP-{)~IjJ&JF8Qik_KRcgNuT!;)iU6t(g0GxC0}v}coTa9 ze)r_W076A^c67=oxhLTp6#dh_tM1WjK6z4d5X4SxNUxI;I9U7+kwxqDp}o8Tx8zI$ z^Nsl_5`eMqqtibBC_1n+`?Z(FV2%Uv-SVtkO2QK-WY?72NmWNnqK!30a`K)`_A0Mz zn_rytjk_kri^$4Dy?jx0Ovt_|fRKG-zChO%zt8KWe)i2=y3G24&WFnKP~&N;1wB9 zyX9%OSMrR{OuFP`rzVF2A&=-tmy9=H2f(?2a+IInbIA)3yhzkCsXiYis056ag}(vT zl-uP8dP}TKrvV^2ekmVqHZ&H-eN)q*2~x7*fY%J|kWACcC=UlS>zbTFm01^w{jLtN zf2@~$s3uq zjpUh_ph3Z^jt23_NsOVdtdb`&+{9~c&!x)&KQMR9h2i1xx|1X0uWx5UsLvcUwm=(F z(fs14-+lu$3>_vT9Sv(IZ#yCvy{za5hmIZFJGAd;XRqiOzwDWmIw^DK0H+)@g3#cc zcrYT>f-%}PaW8ITHN={;bZ%gL)?p&aY>xQ zxs#(GaEQcqh|`|&tDFXcZV|CbHQuok`-YC8(Y;q4VNFD9)x?S?*91ZkomkiPe zqQR*P&li|~;<@20Wa$F2%TV4i-we$GjMj->qF(?6!q2Le%tZ^=6+rHwa@0}`G5j-_ zD?odxw-_$Yx@3>b8vvkWL0Ho!hoA#9&E#a~jF*k_BxcTkQqUIGRfEU?dsiHS%In`gXkx5Vc) zhTgpI51$yOMi@ik`H1Nt?4jCFK|Hg*0GcI5A+v@Vh8_k7As@OzPG-jtk-A89&AL33 zgwce92d=xOrYGG4$+=l{u>8N~1pbHlJe`_-Y3}%YMZ6gBxaCpe;V#lFha3P%0^VDm@m(Vv#V674esKoG+Cx=-Eb|L~ z*m7#di%AVFfr4U&P4mH=GRL=-&>OGvWl7nzR#m8bCvk%W_96{`-`@j~K!y zWn!UGqxm>7>AHlbb)`rluyP8_QJNj#MHG*WfG~oWoAb?p)_U6l81S=h3tE2K<@YD~ zXb?dVXExmtcalP4Akq{OW8wvX2Pb9%;B*+o?7p|WgeI_457-ApJ9UDc2!Ne2B@{Hr z-}w@Q0;Xjwfn)?T7MMHT-9$3%sU^i|?gux0TUZ}Xc!-!zqDMf~ z0+?p`Og1qC;!2by)rUzmW2+YsGAzC@q&(hPu=+#^EaPA$sP~CUCzd@MH@YnUOe@+2e`#s}ds~6fG$Z9(Cp?bk`HQxr8p5(76)2v4n0sp>zN9zu-#bCp$Q?;j!JBC`kt= z`!ZpiMmb~?3Z2N4-GLd|n=pde4~(Z5y5V`@nocJ?FX-?m z;fvEaoY2V+O*4Us&K>XR5@xp-q!Yu*X-F7-m=_X8T7xBwlRlT^)XP;UMnX5?r$UJ0 z!-~fdy@X|yVvkZnxfu}y`1xOfvmnF^3SXEJSJDU;(o1>OsbC>s zPJ4xW`Py7Ux{zTPKHv06W(KkCi4e_S4Wd*KM48fX-wf%1pqAl8ev&s$6mZX|M@r~D zl3b45Wx~XSHN`=I&;g505($|SmQ3g+R*yZe?E%FwK$wy+pomv`wiw%wvBMx0Ws(|1}CO zJS<`8?+;+{!x}`3`T2;#FnAdTQmN`*Yrj)CG4P16|V0Ra~&aXDr5cVvU-V{xL={!#8v#O0y<8%){VU z5+?T0PzNIQKuq1Q55jX>@POD~sDA+^p4ZLmLpq{`scTmFum~aPu?O7OYf1Wd^*0+v zg!creK4BR-J#_lO=+OhukH}j90R&p#)sxU&P3UI*1R{l=4=uac`~%_6?qCB)RaVTd zU2OT~-`$moa03bbPB;rf%zA*|Z|#o^MP6Ka_UG*@qbs9}L*JPRKf84O=5=~o8I^ai z%o$;qyWvlmJOTHV-)Z2JgF$v(lZh^93YD;C6-1_{`8Gv44rf95%u*3PtypSQOU=T* zxUKlLLpKh^D%zIIKdkym)k@W>U2zVo&OybtTea<8I1pzAjUM>%(H|at`}jM@QEJ6{ zKt-tW04trm7hoS+GW0uPBo1dh8*PaAF;spT9?Zy^j=7RX+{dF@?#wIcf`AgVqLcL@ z&9KNKtkk(EL1tzT>51G0F`WQ8^}&`?Zjz}amoHP(Sf@ht4uR1um@bH|*n+C-SCwaf z^@krC?(YJr>P4oBJg~2>$x8{N7b1~_LGlIA6Vw3mGvpM*(e^~)Xi_%Gq=xb^BD_wK zcEVW@9++)!>Tea?E_$OV5>QH-)sp7uj8f8}m^&AS;#S*tEau|Sm+Rcv2dY_GaQ1j2Tb!R#7XFQ2t!>4`j>`q_{TYoC8YsCFXHv`$Dz>ydX6?|o`dc7eWqrRi5PTxId)3e5;h z!^xfKz!Im&lOSZRakSy-v7O6qP;Y6c&aVWIW&w76IlOt}ydgkqhvao7LoE!U9Cm!( zq=}hkqC;TC8H;NS85w>7mbTPit##Q%K%A+}G zKL7D$4s1x}fw7V&X=9}+Y<4WGZSjR&zm(RAzWow$fS)I(v~F?&#a9% zL8@?baN8-iI%i5UFUe3k#I#g}Ya_uyC;!-rN>$d~@v9MC6=O=};>jN|{5kI#sLlgT8y)RvLdc_S1>gbIP`7 z)NRiwea|SH538FGFTA)gv}P;0c_jQ|q*1ZesZ*PBRyJBmngr#rVZ?#1lBhN;4N|{|Pvn#eH)z*Z<+KR)CONSN?Fj#Mn zN17r5P|eF9_*Z&Y-0#ibn^#%~)z(3!Avw1wwx?Cw(}<6$XjjCjSnE`4U1SDnCRes9 ztvl7$oiXdqm~kg3wEBswGU0imIOvvrqZ##6QdYngOBr7CLZW~KilbgA!_nsfIbq9| z#NjKcyh*t}o~Ay4PJTv_2jF?Kgb0gcJ*bVACYwSg&=o!->1KY7p#@>0D4vDn9fC== ztRa~=U+Zwq1#u1;wS-rE?!+mmA5WY*40Za@X=tQ6L?|&mq@RO2-#0TULE0~R$mRmt z2?i;ttK+)qYoze+5gkllL7Anr2uy>=pd_^jDYYWRp3~Elu-M=VSE$PfGVW`ap|H7_ zv~9R3PLU1{d1o~FDA<@Anii7sPeJcU!e59I13s3`#f&hMbOVqzrq=06h3Ej|u(3+X zHanQ`xKTxFDh4EDeE$RBfp4lFV%xYu~mK!V{6e*B7 z_#R`0FgC}4qbkJlGYuwmA6Gro)2JFN4{P;wZhi=2y)a4;$2?GXb17^_Tf;j9FdfWl z0%~7^&cGydc(nUSZ3GEzQej4hFieF=9hqG!(zr1|(2u1{Llxw@Hxi)37=+pSB$Ku6 zMG4!eTgmJOMMeuk7wshBv&b}7*JN7TFvXb{8V~BsYy~T88LrfIWZM;Wh|iFGpR9{W zzbT54@Y<5%A*NJv=E*N&D9JK8#Kg(7N*GDAm9TOZG^P>n)rXxtQA*OydA$oA;v)eh z1h_Ds=2B3DG}reVkbW)KhZRrG>J8h>v*wLvg{Hm&R=#$A z*1pXb{Hx*S%=9W^wA%iw$C@*A^AbJWFNXG9Q!7o|Z zGcRQPzHk>D(H#?U7}-2afL7hfE0iCCcvMTVweaVyD4B)0qIuhwmctw%9w2#Lp*f#j zA*&W9K@8cXf;;B&~H(7_MJpwS>XPz`*hX z1QH8UkbE*NOq=he=bfo8F~xboJ_1o_$pSLLY<4&uKxRyZH43sOgI*CiN=NPHF_~4> zFy+JS^!!8~K~f#5;lfH2@cBglWuF{KO)5+^468Cq$1Dc9&TX`vbXbszd7;;56#HV< z7(;xbd^FtBq%&|WWmo{|GilNTGfa8T&GkqfOog6uL7mQQmeSSFw%%i9BvQw8*J+hD zjPsbu7^jf1QDBCUXjW$I0d8RuHc;KNdm0sk)t1Z4;J7BQx#mFp-B7=AgFBaa35mR! zE*J+7!V1Dk6Q`5QndXnRm~`DOCvCmWhX z;w~t34nnFxapYIYp(yeq#Vg?b1r9S&n(C4yMFkxzP6r_`B5ZlQQ=Jwjf8w%;UIKQM zoMQwLg#hJ?W^S5GHfthX=DjK6z%k4_} zR<(TV!m+h-F}flBdgQa>iZ@T)dj9s;-}rj;n6jZ)-O#HP_o>Bw3(u@&eLA9S=vFs$ zE5$u(anHiR2l>U}?)&+TvHZs9*=67TmcdxdV7$0Ie0r^>J`z-F+SQu&cuh;Zrk>p| zL@&o`Uf}oY!L;||r@43CnpbOC7r0YGz5>f{ON7F*UzRmRkFB(?UQx;pt7V53+Y!}v z1mf1h4Us&huzum-XLXIycBQUUt?P`}IpTGV@jCljUE`e>mRpp%9<{C~Ue^+@Yr?%= zT2Pp`4na`f!h!V)!B!e>jkLrvlEQ}tq^~`X{=J})%7U-uTrf!`U)9|f7IhP#&aaq5 z!WD$@E1ESgrNX{~_+KA%KSw1LiQK9$Km0B`zOYaZs zL%P@XHyg2d)g|(H#*iR&BPm36d+N&w_q&e7x{fRk!QnS!cc7av zG1&5cDw>|NUBq#5E%8M@bZpthkniI2zfcs?Js{r)seF-p#D2%|gRbv&al}S|Vm+$j zyYVPzuZq(vC}zXtPZYrrk6Bwl#{g=X$P4&D<)$8PU`Uq3WL?mRq=^dz(&>Kv4jSR# zQ4L_YB+MJL#nOTFR1F45h-NSSPy~Lxi^uz^mmI8sAP(!gjPd|_WX-L?vnbf_RN;%`Pw}Iieaapkk@a*=!8;XX?s`2%n%H`JIedNPm@;MXm}N zqiNAt3N}5loXwla5%=qan{y!}#)4sH6b->>l8ADg&meF%a*nSh9+c^ zfoYsJAn>J0uAwZ0^{BBdt$b+e{Ss4IHt(kl`xq!1cHlR#lzR*lMaGC&4X}l-hn73A zUSrsLk*E<#7?@q8D2_bAVwPE+fjjvs_Oh%v4p(Na21}z{Q{FE%HceY zusE$33OC&yyW{zR_j}&uuPgPt)cRekGfMq_#dZL*xV;(DwhHk_mCMDSICkB4>{1-N zRmX0n@oBa3>5od4ihXLuzIDN9YG%%odC{;q5-)3vHmPN;%MGjLq=79PT+EM`*W5jI z=WO)Q@)@OWi(0p3#jVsmrIbIlXkN3Hyjgau`gZ*r^-;4@>QqaeinU#}wl7cIxAw=Z z{c&5#Ye#P!jg&{mBNi^ei`klBfKa@^iTkfm7hGL?oZ!{NG?{_9|G5SM{%MeRbQb+h5&rTUg3sF6Q%> z^r8G&om@c5k0t&01<7z1_BMC)BUrdD=`)n|@ob1>%C%vb*1p=?QZpdc&%1lN;L+H$8)8x5uM#=WU3aEdohL&s{Lz*x?awZ2L+*T=W>5u95k| z?1g!#@CiL!ER|?$qwV4OqEL}kdX;epvO_8h6~a=hIa|?l1!7))g$kvz3^oQTQ{jSe zuUwnQ(zZ|$weii#=9iytFOXLT0{LIKC^!?wBx28(*=_mjp5y2%+L= zmijA`n->3a>Qf3ns0_by{3-%iHVI5Gn~(Yx8AcRoXDd(`*XWgf=o?ZJ;5mG@C-T5Rz;NRZc*>9uPBMmh|^(-USA| zpT?pWFt+CF=IiGhLKRV>E00{E25rtdAyoIx_E6aqV%I$qyZi|;>vLn?{9blh=S8$~ zW2h{w_g@M$XNom12HG-jOTtnPFNrjb`-QOX!e7kW!4cXsZ9Lx;YI*`DKc3EMakV0q zYpD4NP--s;Qr!=8j4gf_F3-1MoNj$$ss8^Pn_JUkb1NI0^%$F59~qmifz7G51b^JB z#RIAOD*FiQ&VLMaX5vW=u$F6$R%JpmKJBQ@%`emfWh?MJOYz?pYQqS9FFitAA3H)@ zA2&h+#6cx5VBe0AeQ7&JEU4W3>2=U!<5X$m{2n7X3;RR%|Ij1&Ct=;qzYLZ6{~*wp zY9r7WF;t4yeg*SEai}OW9$D)<=ADebIkZ?XzbWKg+WsWk7U<7F4;3*gmzCdta4Pqo z(LP<^Wkk&=DAv{={7oa{b?l+WX`R%B_vSB&-*Oised6o_+cT~6gxR)Co7)p&m1|>Z zXE-)}=mbq%r_>(JnoF~wY<@*p+MYX_ zwPgbQj=MRWSPuNoC&KRnMX`M;{O&J--xC^ANV zHKO)?kCx6`gVb5$A(^^ZlX{D`9dp{OUxRBo*n(WQYQ9%!VWB=oZAcFEZJ~B)JCsj5 zLLJghJO>`d5PW&+DlJZ7^r~7&BdH$c@xhCrmwTJro znoJtOipmlam$pZM~okGX~&(3s# zL1EIG14(sEQq;pwI!WzjXG%%Zxn)WElOnd05b$AZACzkhOy)oo_BwGjt^9#!Kum5Q zW8bo*66_n_#+s5uCJ(bnZe+z~{%LUwOP1C(FvtTU+p(P7cPQGqc>}fH5qdcZq<`Int?%l$$lOC_^^w#3)}dxhmq|g?3w51e6XyuoYN4ZCASf$Kw)P- z?VE-omF#}#=o7O2At5S+c4St-g=9I#3aor2$V-Z1uR*-Ywac_UGSy<~qZhcI7JFU}w zq~WFGM8uTJocWW8$^6v(B5T?t3Shq@64GuZzqlC(A4qH;?IT-NSmHz;318h(Fz*;u zye(MX#*P|5-)59t(C6gQevui!yo9lyC}e1H+K15snKn#PXls>_Fk_nxJ5nfUIl>MJ zg7~^ESj}vGXeW5;Gj!=lFxVEcALW@riIk}uLc95Q9)Te-}sO-R^91KnX-e?QE&AYd!Y*YSa226HqO z|9^`QNdfb}H&%R)T4XP85gS>i9?JW4lk{24d7V#+v$9M%1RAt51F8 zSD!i>vmcAIkND17PSYa9{xMV#;Yp4UD_b<`n5)NTP*{*+9N3;X4s??V{irXcP!?Ig z|Nh8NoR;5?Qb4C%vX|d)9gFOHyDHikJrynwyVAj|8o`!fI;Vk6JfzBFr*?Q@7zqP7 zMjtV`!lW@+oFvx{*zJJwwFSjC=%iA+veReukanM)2R0zm&M2=txG6hY>WC2-*pP`# z@Ms1qoxp*dIy*7Fb*9W#pdae&WTc)Q{?iGC15OT;lNv0V;XAj8P_5yRfvy>nl3G5! zwSXIY2nSHcg0`e_lw)Q;KEl9>ef_t%!D>-V(p|RCUvzM*N9^AZ>S4x?=1*VXlY}j# z@``~fYtoWcE3@5+lSM^=vTw``ZAh4F$oHI=C>n__X+ zOS-cE`ice(U0Wu2MdgoCs_Yude~WwZb$h705G)tPFQ z`~%{Q2bqiF`ILQFXYU0uGdKugIp4yD39&&bPJm2j>5Q;!ej}T^NsbwY$IQBo0)ho# z?bv}+T{ExdWZajXG0P4^j&XsEal!zgQ)L)D8gP#*3p?-vj0w!8C7jgZhu$=)08gHB z|hHPiAlvVo$E3n!al8=$|?>~Y`hycclAxolu?QQY6T2adZ)uL zzJnXZCOpbpydd@qC6FUC_MtUC0>B1&t`P z$jbQXw{Ojm!P$T~a`f=YlLz*rA-^j!=Ifz;K$LIbThO$5JJpHevFfI5*aqeQf*>|% zsdbXpeDX~MCiK3m*pl*R^qqP(VPc$IB99+ulGwmrbI%D$l<;6eB7P=FqKIuC$ujUu zw{>dYOrj(ml-xIyC`zA8!cOK%l+!zJQ&<3UoKKijmY?!dC?MD|#)llD5)+%l7#Vq%TqW$oWNV3r@nsczztO z0p?l$1ImremSDBz+{qa+VnMm#`U?5JNzR{;^QYuclO*(4Jm^%$kQ-;*dEH`#}0 zUzP#pS&H)=a=uFu3({vXC5lpn0efb!&m&=B?|fs7iTM*|9)xO?a38}uZV^$)LsF1cXqyhW;U3+kGzPaiN)1;h-iKCGwk3AMJ$^61{ zVYg6NeDfKYZRD3kOtJhr-0>QFyk;o66K6f}8^@%S?9+jJQaZW9${-mJp zenDNtw=63KeQH5ptYF8=*uvg5?3L2r&U+&-JR6af61!TG+(z|@&3WJERBW47+h)qu z7Pj2auZiW?uv||^=VCRdaes`HCg_QqazD#*XKuk+9k!BLix$UTo47F%J{K)hikj7; zX2sH?T3SA_Y`$;VtXMi#OXtGA-!&Ghmvf(edUcS(Hf<)L#^ysIDwkCcP!x8 zi?Z-ewXp7!!p8fBjZx?FDW$MSE$mqr45s}kEVWUOciq{$%?_6KjEw|q*{P~$!!#<^9pW3i*@o2<=4QyYm8#Rl%tmR!z3!0~R z5Qo+j4`RD=@!-Ot^&*|=Jhj*F<-#!6S{kmpUHeAu>-D$l6^p1^L>!V}Z(bNC=O{VP zd{!Yw%cCx(!l70;)`db-8*`F2Jf#~W<+o~=@}Sz^8_#}uP__|vxn-}n-)fJ{MrNa9 zZw2oJl`n6Rp?Q;RnZ^%*1 zwTq_3+3*Z){w{s3_(pNWq?qefbA8NQAGemothI4l%_p|T`?khti(+%AHb>0nh})_@ zvDM$V)kkI(TdQhojoDhi_^hOM;c#5EFAS66aLwn!9-XOst)gkMV6D0(I=1Xos{7UI z{>7pPHK?k+p!;)j7LUX$sw3@cMa$xWcvbD~xi{vb4J&q~YO7kcHCFZ1YT4p5)S1C$ z$A=w1=~$Uvoufac{jl18_?OR~jWs-{G(4v^JQuUoKG?o9W~+;DsJZ?88_!4PmhH-h zPIW^ku&0m~YvhJ?Yn9cJ#vinNuO(_kyLI2`Rw~=o%J#4UOZ!OU8#}@~qLyV>v^ZM4 z*4cgUYb&l*%TK5N=hj%~vr6Z)YUi`Dik9fOQqfLGl(PoZtOuO~z$8QfD!~OV!CgH5 zS#eo-P$_Oyv7{clU)&NaZi$y{{H(S+&emGX$3N+K`hLgLO2?4eF@%MdQnFVq*&8nr zWA-hdoAh;C(QS2G7mu%-g*qHdQ&iZ^oY&1^LwF=!-5T|))tg_3k}AAc`}WzU&gHR{ z?vDmO4#h5vDw{5Z zRC>>PzQHks(v$le=4MK7{x_HYfwg*FFq04!sjv&(8!Wp_uY%4*++t5ui-4_pg~PH_hRC z#Vo33@kjP|9LrBB_U)>D`{xF|JrCVv&r^)`3(qXN!_Ti<1atA{LW5~=&0H8O3PtAN z;9l9npKulPylS40nde!I^UKX2I)CEiE4`0xcq&`YsBqiQsO@KBMd$ggm|sxMFT~6* zutaB5iD(t|u?3E@WkiMBKBBgd#EQ=HTQNVUnxBiApJRyzezbqN>ciTf)V^1LuU={1 zp*HV`6%F84%ulK2r())(;?|10#rLgkF>4z(7_STZ^6m#4nt+q#K#!CI=I->>bD)%J(0!d0uz z)p7ish*vBAvU%(M+O4tLt?bLdYG8F>bpT(wdR8j$U0FSRzw1D(>p;Izu<1%IIF?H)P zNVf`09u${`oj2#P38ipwtz<*2ay)V#&axd2?v+h&R>$Cc)Wkz#XP%3F?V@_d#b1>Y zNiC6L?Eb7|1E_P=uE=@zShlkVb5=I7$Lbh+eAJZr60hDAIeModR=IWEfVAwx1AB9< zWru3t8LJ#xH>E;Bj~bgGjZ?*~vC5|~^pmkM{7W&(!$IPt^IMxxUKM^xDTDEB6DH}Z zhJ~YHgJP@z5PNI9u|3|{%-qgT8vE}z_A8Cs)W&V`#?7BJ_T6vnQyTl##{PJtGu}v% z*BYCnU*}=ueeUhrcWT+kj`dANHte(Ejwz3c71)Aj94u@4DuB;G@*ffbYsJGEQ+`Qz zFGr<5Pa75VX((A9$q*pfYx2t8=77^Cz|6jnAHVM9m=VDg?nyB7o(MB%zv%A))0};P zom3!M?pfc(itl||^|9-Bl*g7`9-OOKc&{ioZ2&xb>VnezDRvOxV_OW8wqlra=1C?HDGS57VSGzP5tD%aAo8-9X)TYBZs+IuBLz35(O(`Gkm6qD? zZPxZ3osass(^S3I$DgLE!MTp?GLpfIw(n4f$G47N>cmLzf*ihkIM_wXgOuDLqfcc5 zucSU9d29j`=3Ne8!I;HN%hpUnWR9m@qO;umMI)nLTQb zkgXjEnmaqW+A6qhgquB(B>_Kr+d;B%rjWy7BB^tnVb74=i_Tz$DAK8#ZYgxYiBZ=1>b93xfaZ!Ij3OK6ch7%=i~NfQY?8MMLtXnJJ`zJp=xKO~Qt=@JI2L zg|Ty-nUGn2n(~0wOu#X+6qDlk6Beo--+?3_q!|B1G4gSutSb%^Tj6BMGr6D=M+8D1-GpIP2}Z_G zG!dG6bBSthh*#_fM_+J|WD#&7i-5Z@^uTC(b@=7s@XimKe{NiTLD_Lc-Em}LSTP<| zjkIYup83AEsUarrQ8o>!n})#to66}_=Y0eTE{VqW=~B>ke9*Mo9N)Mr9c?$o+D(ym zr{e7Xpn0_-zVX>~oDl|Ugn=5-LCjdRkv6l@nY-jJ>f+{t*NScwMP17SQD4*-ekn2( zeu@9B?)pf#x{LnSY8#jPF7utXnq;1y$hw z3y#p@A1?U+jB620dDJek@}>`3sF<|i;WZv+#T;hE98T3ZR^IZ#fz>VRdVMt@@u&vV zQ34GYpvg5SfrbZo)yYJF!(_4{6Eh}&`@m{@ZP$%mOS^CGUf7Q{huQYZH>eG&*D4#M z&F?^1)u&eWEn3;4@j&$SJEO6p1GrbM{Hd4^s^)_+c0($=XvJcdw`D7VX+=xc)qZ)IquhW-|fYY-Xkk2FV(imgSpwIH_Dwm7&}XOH&2^|d=+gZxytH2lS9 zWz|?6MfbmRL@Dc5%eo<+E~|~4zH<(ziK%6dg(GYDn)-J1vqGRlO?+chd}HmM0(#bp zYHcTTyAiftY%a?CT!8Z%auyCke28klMCI**Jn6Dp)wbM!zp6V{)g3q6Zdz4y%_nB@ zzFAzZST)AXqGBFY&4V%XV7##O=J`(w>+ToUMaGv8#R}_`!hW@|AGlYrc~!@cESre9 zmUS^(J5DE4ZSC;T(AdnPFo(v-W*Q<{VVSQPA3v}BkyxLRFr_0BxbqSoxN-cdYVM4g zJ0Fx(hOet7Vs!ug5=X4WvDmxVyX=Y?yZto%{q5k!BaMc?Ycw8d$^Sct4sOCc3T+#X z6&i(hjo2+t;)va13N(Z=+CY31*SWRA_h?+wf#66bMp2l4?iosuqp36V6GU0)$@KS} zm_$Am^Lpu(G{OJ_h?+X&)=kD|k&EF~!Q0DFyrfCVe;aUBJ|$d@C@^ zF-otQn!imk4UFC3n_itIS=D4(Kn5U_0a19FIwY&0Of$%UGWFnprLUPz)^mRkW#vfZ zkB(yJ^XMoJ>K~n$!Txyn=%@@r4;Bh^Ulz%C3QnQ`Cp*hHQ)kl8cRgcL4qTq}j3xiw z0ThqW-OG6tKc5_0s>vj6kZBo}FhS)zJ;%>PG_#p-GCf z=g5QP8z$!rInR?bP0mZ?+#=^LIUkUdM+K)@i}0P8Q>xv_9F+P~2d;Yp+~jWu#cYD} z(7cQ90^conAq{T9zX{VF#;{JeZaksW8P^5oj_CzmQ%taD-w%Z11@;#gtPAWf%cF^j zRB>TrOnb+L9kH)=KM=}dsXH#z#JHNUnCn&=bRuRQjbk(EaPY3yDbee8Qa;&^#ikp8d|l%> zDs*-0g4XHkB=B12qE4?nh5|j}S}JuV>w?zVuM_I+@dkUmV&i(T*Pw$e>