diff --git a/doc/Homing_5_Pose.md b/doc/Homing_5_Pose.md index 0df738a..80c3665 100644 --- a/doc/Homing_5_Pose.md +++ b/doc/Homing_5_Pose.md @@ -6,10 +6,14 @@ > 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). +> Status (2026-06-16): **Code-Hooks umgesetzt und gegen drei echte Homing-Captures +> verifiziert** — `--from-state` (Startwert aus 4b, mit Multi-Start-Schutz für alles, +> was der Startwert nicht abdeckt), `null` statt `0` für unbeobachtbare Gelenke +> (z. B. `Hand`/`Palm`/Finger, aktuell ohne Marker in `robot_1781069752019.json`), +> `scipy` in `docker-compose.yaml` ergänzt, sowie neu der Kalibrier-Switch +> `--calibrate-origin` (siehe eigener Abschnitt unten). **Noch offen:** Anbindung in +> `homingOrchestrator.js`/`server.js`/Frontend (siehe Integrationsschritte — +> Umfang/Fehlerfall/Robotersteuerung-Politik dafür sind noch nicht festgelegt). --- @@ -94,17 +98,20 @@ 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`). +**Ursprünglicher Befund (vor dem Code-Hook vom 2026-06-16):** `estimate_pose()` +rief für `global_ba`/`hybrid` immer selbst `estimate_sequential_fk()` als +„billigen, robusten Init" auf — es gab keinen Parameter, um stattdessen einen +extern vorgegebenen Startwert einzuspeisen, obwohl `estimate_global_ba()` +selbst intern bereits ein `x0`-Dict entgegennahm (`:272-273`). **Das ist jetzt +behoben** (`seed`-Parameter auf `estimate_pose()`/`estimate_sequential_fk()`, +CLI `--from-state`) — die folgende Beschreibung des Multi-Start-Mechanismus +gilt weiterhin unverändert für **alles, was der Seed nicht abdeckt** (bzw. für +den reinen Kaltstart ohne `--from-state`): -`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: +`estimate_sequential_fk()` initialisiert jede nicht geseedete 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** (`:348-356`). 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` @@ -124,9 +131,16 @@ unten sichtbaren großen Abstand zwischen Mittelwert (0,253°) und Schlechtestfa (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"). +**Konsequenz / Status:** `5_pose_estimation.py` sollte in appRobotHoming **nicht +kalt** laufen, sondern mit dem `accumulated_state` der 4b-Kette als Startwert. +**Umgesetzt:** `--from-state ` lädt einen flachen oder +`{"accumulated_state": {...}}`-verpackten Zustand; `estimate_sequential_fk()` +überspringt nur Blöcke, die **vollständig** im Startwert enthalten sind, und +wendet auf alles andere weiterhin seinen normalen Multi-Start an — auch bei +einem **unvollständigen** Seed (z. B. nur `x,y` aus einem abgebrochenen +4b-Lauf) bleiben `z`/`a` also Multi-Start-geschützt, statt ungeschützt bei `0` +zu starten. Verifiziert an drei echten Captures (s. „Validierung an echten +appRobotHoming-Daten" unten). ### Validierung im Rendering-Projekt (Simulation, 10 Posen, bekannte GT) @@ -139,6 +153,37 @@ und der dafür nötige Code-Hook: Abschnitt „Integrationsschritte"). (Quelle: `appRobotRendering/doc/pipeline.tex`, Abschnitt „Validierung und Ergebnisse".) +### Validierung an echten appRobotHoming-Daten (2026-06-16) + +Drei echte Captures aus `test/homing/` (nicht simuliert; vom Nutzer bereitgestellt, +inkl. eines bereits aktualisierten 4b-Laufs): + +| Fixture | 4b kam bis | Arm1-Marker gesehen | Ellbow-Marker gesehen | +|---|---|---|---| +| `20260616_120456` | Arm1 (Ellbow nicht gespeichert) | 197, 243 | 129, 132 | +| `20260616_133151` | Arm2 | 198, 229 | 129, 132 | +| `20260616_135403` | Arm2 | 197, 243 | 129, 132, 121 | + +Geprüft für jede Fixture (`python scripts/5_pose_estimation.py … --from-state state_Arm2.json`, +bzw. `state_Arm1.json` für die unvollständige erste Fixture): + +- **Kein Crash**, trotz `Hand`/`Palm`/`FingerA`/`FingerB` aktuell ganz ohne + Marker in `robot_1781069752019.json` (`"markers": []` an allen vieren) — + `b`, `c`, `e` kommen als `confidence:"none"`, `"value": null` heraus, exakt + wie gefordert ("Hand als unbekannt stehen lassen"). +- **Kalt vs. geseedet liefert dieselben Werte** (z. B. `133151`: `x=193.96mm, + y=25.74°, z=-28.00°, a=-0.81°` in beiden Fällen) — der Seed verändert das + Ergebnis nicht, wenn der Kaltstart bereits im richtigen Minimum lag; er + schützt nur die Fälle, in denen das nicht so ist. +- **Unvollständiger Seed** (`120456`, nur `x,y` aus `state_Arm1.json`, `z`/`a` + fehlen): liefert dieselben Werte wie der volle Kaltstart — und durchläuft + jetzt nachweislich den Multi-Start-Pfad für `z`/`a` (Code-Pfad geprüft, nicht + nur Zufall des Ergebnisses). +- Residuum über alle Marker: **4,3–4,5 mm RMS** (deutlich über der + Simulationsvalidierung oben — erwartbar, reale Marker/Kameras sind + verrauschter als der appRobotRendering-Renderfehler-Boden; noch keine + `huber_delta_mm`/`normal_weight`-Nachjustierung vorgenommen). + --- ## Vorteile @@ -183,14 +228,15 @@ und der dafür nötige Code-Hook: Abschnitt „Integrationsschritte"). `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). +- ~~`scipy` fehlt im appRobotHoming-Container~~ — **behoben (2026-06-16):** + `docker-compose.yaml` installierte nur `opencv-python-headless numpy`; ohne + `scipy` greift `HAVE_SCIPY=False` und `estimate_sequential_fk`/`estimate_global_ba` + fallen lautlos auf den Nullzustand zurück (nur eine `[WARN]`-Zeile, kein + Fehler-Exit) — ein stiller Fehlmodus. `scipy` ist jetzt in der + `pip3 install`-Zeile ergänzt (kein separater Image-Build nötig — `pip3 + install` läuft laut `command:` bei jedem Containerstart neu). **Noch nicht** + auf einem laufenden Container wirksam geprüft — wirkt erst nach dessen + nächstem Neustart. - **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 @@ -211,12 +257,14 @@ und der dafür nötige Code-Hook: Abschnitt „Integrationsschritte"). `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. +- ~~Unbeobachtbare Gelenke werden als `0.0` ausgegeben, nicht als `null`~~ — + **behoben (2026-06-16):** `main()`s Output-Writer schreibt jetzt `"value": + null`, wenn `observable:false`, statt der internen `0.0` (die intern bleibt, + weil die FK für die *anderen* Gelenke einen Zahlenwert braucht — nur der + *Output*-Vertrag ändert sich). Verifiziert an allen drei Fixtures (`Hand`, + `Palm`, `e` → `null`). Gilt nur für `5_pose_estimation.py` selbst — der + Adapter zu `/api/homing/send-state` (nächster Punkt) muss `null` weiterhin + korrekt durchreichen, nicht wieder in `0` zurückverwandeln. - **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. @@ -240,59 +288,178 @@ und der dafür nötige Code-Hook: Abschnitt „Integrationsschritte"). CLI-Override, oder dauerhaft über `robot_1781069752019.json` → `pose_estimation.method`. Nützlich, um den Effekt des Startwerts zu isolieren: einmal kalt (zeigt das Problem aus „Wichtige Einschränkung"), einmal mit - 4b-Startwert (sobald der Code-Hook existiert) — als Regressionstest für genau - diese Einschränkung. + `--from-state` und 4b-Startwert — als Regressionstest für genau diese + Einschränkung (beide Aufrufe stehen unter „Aufruf (Stand-alone, zum Testen)"). - **`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) +## Kalibrier-Switch: Gelenk-Origin (`--calibrate-origin`) -⚠️ 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. +**Motivation:** `doc/Kalibrierung.md` Schritt [4] bestimmt +`links.Arm1.jointToParent.origin[1,2]` (Y/Z des Schultergelenk-Drehpunkts) geometrisch +aus einer **dedizierten 3-Pose-Aufnahme** (Verfahren B: Kreis-Umkreismittelpunkt +durch 3 Positionen je Marker, nur Marker-**Mittelpunkte**, keine Normalen — Details +dort). Diese Y/Z-Werte sind laut Nutzer „etwas ungenau gemessen". `5_pose_estimation.py` +hat mit `residual_vector()` (Position **und** Normale, robuste Verlustfunktion, +generischer Least-Squares-Löser) bereits die Bausteine, um densel­ben Drehpunkt +**aus den ohnehin vorhandenen Homing-Aufnahmen** zu verfeinern, statt eine eigene +Aufnahme-Session zu brauchen. + +### Ansatz + +Statt nur die Gelenkvariable `q` zu fitten, werden für **einen** angegebenen Link +zusätzlich 2 Komponenten seines `jointToParent.origin` freigegeben: + +``` +Normalfall: min_q Σ_marker ρ(‖r(q)‖) (3 Freiheitsgrade weniger) +--calibrate-origin: min_{q_link, origin_y, origin_z} Σ_marker∈link ρ(‖r(q_link, origin_y, origin_z)‖) +``` + +Implementiert in `estimate_origin_calibration()` (neu, +`scripts/5_pose_estimation.py`): mutiert `fk.links[]["jointToParent"]["origin"][1,2]` +**transient** während des Solves (jeder `fk.compute()`-Aufruf liest `origin` frisch +aus dem Dict, siehe `robot_fk.py:compute()` — kein Caching, daher funktioniert die +direkte Mutation ohne Änderung an `robot_fk.py`) und stellt den Originalwert danach +**immer** wieder her — das Skript bleibt ein reines Report-Tool, **`robot.json` wird +nie geschrieben**. Multi-Start `{0,60,…,300}°` für die eigene Gelenkvariable, wenn +revolut (wie bei den anderen Verfahren). Alle anderen Gelenke bleiben fix (aus +`--from-state`, sonst `0`) — Vorbedingung wie beim bestehenden Verfahren: die +übrige Kette (insbesondere `x`) muss schon vertrauenswürdig sein. + +### Aufruf ```bash python scripts/5_pose_estimation.py data/homing//aruco_marker_poses.json \ -robot scripts/robot_1781069752019.json \ + --from-state data/homing//state_Arm2.json \ + --calibrate-origin Arm1 +# -> schreibt Arm1_origin_calibration.json (Report), robot.json unverändert +``` + +Funktioniert generisch für jeden Link mit eigenen Markern (an `Ellbow` mit +`--calibrate-origin Ellbow` getestet) — keine Arm1-spezifische Hardcodierung. + +### Befund an echten Daten (2026-06-16, vorläufig) + +Auf zwei **unabhängigen** Fixtures mit **unterschiedlichen** sichtbaren +Arm1-Markern ergibt sich eine konsistente Korrektur: + +| Fixture | gesehene Marker | Δ Origin Y | Δ Origin Z | Residuum RMS | +|---|---|---|---|---| +| `20260616_133151` | 198, 229 | **+6,46 mm** | **−19,97 mm** | 1,19 mm | +| `20260616_135403` | 197, 243 | **+7,33 mm** | **−18,49 mm** | 1,19 mm | + +Beide Läufe sehen **andere** Markerpaare und kommen trotzdem auf nahezu +denselben Versatz (~+7 mm / ~−19 mm) — das ist kein Zufallsrauschen eines +einzelnen Markers, sondern ein konsistenter Hinweis, dass der aktuell in +`robot_1781069752019.json` hinterlegte Wert (`[110, 101.1, 55.2]`) tatsächlich +um ungefähr diesen Betrag daneben liegt. **Noch nicht** unabhängig gegen das +geometrische Verfahren B (3-Pose-Aufnahme) gegengeprüft — siehe Offene Punkte. + +### Einschränkungen / Unterschiede zum bestehenden Verfahren + +| | Verfahren B (`yAxisCompute.js`, bestehend) | `--calibrate-origin` (neu) | +|---|---|---| +| Aufnahmen nötig | 3 Posen, ≥15° Drehung dazwischen | **1** Pose (mehr optional, noch nicht implementiert) | +| Signal | Marker-Mittelpunkt über 3 Zeitpunkte | Position **+ Normale**, robuste Verlustfunktion | +| Fehlerabschätzung | Residuum εᵢ je Marker (Kreis-Abweichung) | `residual_rms` über alle Link-Marker | +| Achsrichtung | wird mitbestimmt (Kreuzprodukt/Ebenen-Normale) | wird **nicht** gefittet — nur `origin`, `axis` bleibt aus robot.json | +| Identifizierbarkeit | durch Drehung explizit entkoppelt von Winkel | aus 1 Pose: Winkel/Origin-Korrelation theoretisch möglich, durch mehrere Marker + Normalen an verschiedenen Hebelarmen empirisch entkoppelt (s. Befund oben) — **nicht formal bewiesen** | +| Schreibt robot.json | ja, über „Joint-Origin Y/Z übernehmen" | nein — nur Report, gleiche Übernahme-Aktion nutzbar | + +Die Achs**richtung** (`jointToParent.axis`) fitten beide Verfahren nicht — das +bleibt vorerst bei Verfahren B, falls sie ebenfalls ungenau ist. + +--- + +## Aufruf (Stand-alone, zum Testen) + +**Empfohlen — mit Startwert aus der 4b-Kette** (z. B. dem letzten vorhandenen +`state_*.json`; unvollständig ist ok, fehlende Variablen bleiben Multi-Start-geschützt): + +```bash +python scripts/5_pose_estimation.py data/homing//aruco_marker_poses.json \ + -robot scripts/robot_1781069752019.json \ + --from-state data/homing//state_Arm2.json \ -out data/homing//robot_state.json +``` + +**Kalt** (kein `--from-state`) — funktioniert weiterhin identisch wie vor diesem +Code-Hook, aber ohne den oben beschriebenen Schutz für gekoppelte Blöcke; +nützlich, um das Kaltstart-/Lokales-Minimum-Verhalten aus „Wichtige +Einschränkung" gezielt zu reproduzieren/regressionszutesten: + +```bash +python scripts/5_pose_estimation.py data/homing//aruco_marker_poses.json \ + -robot scripts/robot_1781069752019.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 ``` +Gegen die echten Testdaten in `test/homing/*/` ausprobiert — siehe +„Validierung an echten appRobotHoming-Daten" oben. + --- ## Integrationsschritte (Offene Punkte) -- [ ] **`scipy` in `docker-compose.yaml` ergänzen** (`pip3 install …` Zeile) — - ohne das läuft `hybrid` lautlos auf Nullzustand. +**Erledigt (2026-06-16):** + - [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`). +- [x] **`scipy` in `docker-compose.yaml` ergänzt** (`pip3 install … numpy scipy`). +- [x] **Code-Hook `--from-state`:** `load_seed_state()` (akzeptiert flach oder + `{accumulated_state:{...}}`) + `estimate_sequential_fk(..., seed=...)` + überspringt nur vollständig geseedete Blöcke, alles andere bleibt + Multi-Start-geschützt. `estimate_pose(..., seed=...)` reicht das durch. + Verifiziert an 3 echten Fixtures (s. „Validierung an echten + appRobotHoming-Daten"). +- [x] **Robustheit gegen fehlende Marker:** `Hand`/`Palm`/`FingerA`/`FingerB` + (aktuell `"markers": []`) laufen ohne Crash durch, Output zeigt `null`/ + `confidence:"none"` statt erfundener `0`. `main()`s Output-Writer mappt + `observable:false → value:null` (intern bleibt `0.0` für die FK-Rechnung + der anderen Gelenke — nur der Output-Vertrag ändert sich). +- [x] **Kalibrier-Switch `--calibrate-origin `** umgesetzt + (`estimate_origin_calibration()`) — generisch für jeden Link mit eigenen + Markern, getestet an `Arm1` und `Ellbow`. Schreibt nie `robot.json`, nur + einen `*_origin_calibration.json`-Report. Details: eigener Abschnitt oben. + +**Noch offen:** + +- [ ] **Adapter** `movements..value` → flaches `{x,…,e}`-State-Objekt für + `POST /api/homing/send-state`; `null` muss `null` bleiben (nicht zurück zu `0`). +- [ ] **Anbindung in `homingOrchestrator.js`** (neuer Schritt nach der 4b-Schleife, + SSE-Events) — **Umfang/Fehlerfall/Sende-Politik an die Robotersteuerung sind + noch nicht festgelegt** (offene Rückfrage vom 2026-06-16, noch unbeantwortet: + Minimal-Fix vs. Voll-Integration; Abbruch vs. Fallback bei Fehler; Senden vs. + nur Anzeigen). Diese drei Entscheidungen zuerst klären, dann verdrahten. +- [ ] **Arm1-Origin-Befund anwenden oder verwerfen:** Δ(Y,Z) ≈ (+7, −19) mm ist + auf zwei unabhängigen Fixtures konsistent (s. Abschnitt „Kalibrier-Switch"). + Vor dem Übernehmen: (a) mit mehr Fixtures/Posen erhärten, (b) wenn möglich + gegen eine frische Verfahren-B-3-Pose-Messung gegenchecken, (c) erst dann via + Kalibrierung-Tab „Joint-Origin Y/Z übernehmen" übernehmen. +- [ ] **`--calibrate-origin` an die Kalibrierung-UI anbinden** (`doc/Kalibrierung.md` + Schritt [4]) — aktuell nur CLI/Report; Tab „Arm1 – Y" könnte beide Verfahren + (Geometrisch/Verfahren B und `--calibrate-origin`) nebeneinander anzeigen. +- [ ] **Mehrpose-Erweiterung für `--calibrate-origin`** (mehrere + `aruco_marker_poses.json` + gemeinsames `origin`, je Pose ein eigener + Gelenkwinkel) — würde die Winkel/Origin-Korrelationsschwäche aus einer + Einzelpose weiter reduzieren, analog zur bestehenden 3-Pose-Aufnahme. - [ ] `huber_delta_mm`/`normal_weight` ggf. gegen reale Marker-Genauigkeit - nachjustieren (Defaults sind aus appRobotRendering-Simulation übernommen). + nachjustieren — reales Residuum (4,3–4,5 mm RMS) liegt deutlich über der + Simulation; Defaults sind unverändert aus appRobotRendering übernommen. +- [ ] Python-Tests (`pytest`) für `load_seed_state()`, den Block-Skip in + `estimate_sequential_fk()` und `estimate_origin_calibration()` — aktuell nur + manuell gegen die drei Fixtures verifiziert (s. oben); appRobotHoming hat + bisher keine Python-Testinfrastruktur (nur Jest/JS), das wäre die erste. - [ ] Eintrag in `Homing.md`-Tabelle (Doku-Übersicht) ergänzen, sobald - verdrahtet. + `homingOrchestrator.js` verdrahtet ist. --- diff --git a/doc/Kalibrierung.md b/doc/Kalibrierung.md index 196d567..77ecb5d 100644 --- a/doc/Kalibrierung.md +++ b/doc/Kalibrierung.md @@ -102,6 +102,29 @@ befestigt. Diese werden in `links.Base.markers` eingetragen. - `links.Arm1.jointToParent.origin[1]` (Y) und `[2]` (Z) in `robot.json` - Optional: `links.Base.markers` ergänzt +### Alternative/Ergänzung — `--calibrate-origin` (`5_pose_estimation.py`) + +🔶 *Experimentell, noch nicht in dieses UI eingebunden* — Details, Mathematik und +Vergleichstabelle: [`doc/Homing_5_Pose.md`](Homing_5_Pose.md) (Abschnitt +„Kalibrier-Switch: Gelenk-Origin"). + +Bestimmt `origin[1,2]` desselben Gelenks **aus einer einzelnen vorhandenen +Homing-Aufnahme** (Position + gemessene Normale aller Arm1-Marker, robuster +Least-Squares-Fit) statt aus der dedizierten 3-Pose-Aufnahme oben — keine +eigene Mess-Session nötig, dafür (noch) ohne die explizite Drehung, die hier +Achse und Winkel sauber entkoppelt. Auf zwei realen Captures ergab sich eine +konsistente Korrektur von ca. **+7 mm (Y) / −19 mm (Z)** gegenüber dem +aktuellen `robot.json`-Wert — bisher **nicht** gegen eine frische Messung mit +Verfahren B gegengeprüft, daher noch nicht über „Joint-Origin Y/Z übernehmen" +angewendet. Aufruf: + +```bash +python scripts/5_pose_estimation.py /aruco_marker_poses.json \ + -robot scripts/robot_1781069752019.json \ + --from-state /state_Arm2.json \ + --calibrate-origin Arm1 +``` + --- ### Mathematik: Bestimmung der Rotationsachse diff --git a/docker-compose.yaml b/docker-compose.yaml index 08ab19f..f2a1641 100755 --- a/docker-compose.yaml +++ b/docker-compose.yaml @@ -17,7 +17,7 @@ services: ports: - "2093:2093" command: > - /bin/bash -lc "apt-get update -qq && apt-get install -y --no-install-recommends python3-pip && pip3 install --quiet --no-cache-dir opencv-python-headless numpy && npm ci || npm install && node server/server.js" + /bin/bash -lc "apt-get update -qq && apt-get install -y --no-install-recommends python3-pip && pip3 install --quiet --no-cache-dir opencv-python-headless numpy scipy && npm ci || npm install && node server/server.js" networks: - approbots restart: unless-stopped diff --git a/scripts/5_pose_estimation.py b/scripts/5_pose_estimation.py index 8bf703c..0fff55d 100644 --- a/scripts/5_pose_estimation.py +++ b/scripts/5_pose_estimation.py @@ -24,6 +24,26 @@ Observation input: marker_observation = "corner_pose" -> aruco_marker_poses.json (pos + measured normal) marker_observation = "center_point" -> aruco_positions_*.json (pos only) +Homing integration (appRobotHoming, see doc/Homing_5_Pose.md): + --from-state seed/init state (flat {var: value}, or the + {"accumulated_state": {...}} shape written by + 4b_revolute_angle.py) used as x0 for + global_ba/hybrid instead of the internal + estimate_sequential_fk() cold start. Missing + variables default to 0 and are estimated/flagged + normally. Without --from-state, behaviour is + unchanged (internal cold start, as before). + --calibrate-origin special mode: instead of estimating the full + pose, fit 's own joint value TOGETHER WITH + its jointToParent.origin Y/Z from that link's own + markers (complements the geometric multi-pose + method in doc/Kalibrierung.md Schritt [4]). + Writes a *_origin_calibration.json report; never + modifies robot.json. + +Unobservable joints (confidence "none") are written as value=null in the +output JSON — never a fabricated 0 (see movements..observable). + Both the engine (estimate_pose) and a CLI (main) live here. """ from __future__ import annotations @@ -109,6 +129,22 @@ def load_observations(path: str, use_normals: bool, min_cams: int = 2) -> Dict[i return out +def load_seed_state(path: str) -> Dict[str, float]: + """ + Load a partial/full joint state to use as an optimisation seed (--from-state). + + Accepts either a flat {variable: value} dict, or the + {"accumulated_state": {...}, ...} wrapper written by 4b_revolute_angle.py — + same unwrap rule as server/homingOrchestrator.js + (`stateData.accumulated_state ?? stateData`), so 4b's output files can be + passed in directly. Unknown keys are ignored; missing STATE_KEYS are simply + absent from the returned dict (caller defaults them, e.g. to 0.0). + """ + data = json.load(open(path, "r", encoding="utf-8")) + raw = data.get("accumulated_state", data) if isinstance(data, dict) else {} + return {k: float(v) for k, v in raw.items() if k in STATE_KEYS and v is not None} + + # ================================================================== # Kinematic chain analysis # ================================================================== @@ -270,9 +306,23 @@ def _multistart_values(vtype: str) -> List[float]: 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.""" + cfg: Dict[str, Any], seed: Optional[Dict[str, float]] = None + ) -> Dict[str, float]: + """ + Estimate block by block along the chain, freezing already-solved variables. + + seed: optional partial/full state (e.g. from 4b_revolute_angle.py) to trust + as a starting point. A block is SKIPPED entirely (seed used as-is, no + re-fit) only if ALL of its variables are present in seed. Blocks with any + missing variable are still fit normally — including their own multi-start + — but using the seeded values of EARLIER blocks as fixed context instead + of 0. This keeps the local-minimum protection for whatever the seed does + NOT cover (see doc/Homing_5_Pose.md "Wichtige Einschraenkung"), while not + re-perturbing values the caller already trusts. + """ state = {k: 0.0 for k in STATE_KEYS} + if seed: + state.update({k: v for k, v in seed.items() if k in STATE_KEYS}) var_type = chain["var_type"] for block in chain["blocks"]: @@ -280,8 +330,10 @@ def estimate_sequential_fk(fk: RobotFK, obs: Dict[int, Dict[str, Any]], chain: D bmarkers = [m for m in block["markers"] if m in obs] if not bvars: continue + if seed and all(v in seed for v in bvars): + continue # fully seeded — trust it, don't re-fit if not bmarkers: - # unobservable block: leave at 0, flag later + # unobservable block: leave at seed/0, flag later continue if not HAVE_SCIPY: @@ -458,7 +510,140 @@ def observability(chain: Dict[str, Any], obs: Dict[int, Dict[str, Any]]) -> Dict return info -def estimate_pose(fk: RobotFK, obs: Dict[int, Dict[str, Any]], cfg: Dict[str, Any]) -> Dict[str, Any]: +# ================================================================== +# Mode: joint-origin calibration (--calibrate-origin) +# ================================================================== + +def estimate_origin_calibration(fk: RobotFK, obs: Dict[int, Dict[str, Any]], + link_name: str, cfg: Dict[str, Any], + seed: Optional[Dict[str, float]] = None, + free_axes: Tuple[int, ...] = (1, 2)) -> Dict[str, Any]: + """ + Fit `link_name`'s OWN joint variable together with its + `jointToParent.origin` components (default: indices 1,2 = Y,Z) from that + link's own markers, in a single robust least-squares solve. All other + joint variables are held fixed at `seed` (or 0) — this assumes the rest of + the chain (in particular a slider `x` seed, if relevant) is already + trustworthy, same precondition as the existing geometric method. + + Complements doc/Kalibrierung.md Schritt [4] ("Arm1 Y-Rotationsachse"), + which fits the axis from a dedicated 3-pose capture using marker *centres* + only (circle fit). This fits from a single capture's marker corner poses + (position + measured normal, same residual as estimate_pose), reusing + whatever Homing run data is already on hand instead of a separate capture + session — useful for ANY revolute/linear joint's origin, not just Arm1/y. + + Never writes robot.json. `fk.links[link_name]["jointToParent"]["origin"]` + is mutated transiently during the solve (RobotFK.compute() re-reads it + fresh on every call — see robot_fk.py) and always restored before + returning, success or not. + + Returns a report dict; result["status"] is one of: + "ok" | "scipy_missing" | "insufficient_markers" | "unknown_link" | "failed" + """ + if link_name not in fk.links: + return {"link": link_name, "status": "unknown_link"} + + chain = analyze_chain(fk) + link_var = next((v for v, links in chain["var_links"].items() if link_name in links), None) + if link_var is None: + return {"link": link_name, "status": "unknown_link", + "detail": "link has no movable jointToParent"} + + own_markers = [m for m in chain["link_markers"].get(link_name, []) if m in obs] + joint = fk.links[link_name].get("jointToParent", {}) or {} + origin = joint.get("origin", [0.0, 0.0, 0.0]) + if not isinstance(origin, list): + origin = list(origin) + joint["origin"] = origin + origin_before = list(origin) + var_type = chain["var_type"].get(link_var, "linear") + + result: Dict[str, Any] = { + "link": link_name, "joint_variable": link_var, + "joint_unit": "mm" if var_type == "linear" else "deg", + "origin_before_mm": origin_before, "free_axes": list(free_axes), + "n_markers": len(own_markers), "status": "skipped", + } + if not HAVE_SCIPY: + result["status"] = "scipy_missing" + return result + if len(own_markers) < 2: + result["status"] = "insufficient_markers" + return result + + base = {k: 0.0 for k in STATE_KEYS} + if seed: + base.update({k: v for k, v in seed.items() if k in STATE_KEYS}) + + def fun(vec): + st = dict(base) + st[link_var] = vec[0] + for i, ax in enumerate(free_axes): + origin[ax] = vec[1 + i] + return residual_vector(st, fk, obs, own_markers, cfg) + + starts = [base.get(link_var, 0.0)] if var_type != "revolute" else _multistart_values("revolute") + best, best_cost = None, float("inf") + try: + for a0 in starts: + vec0 = np.array([a0] + [origin_before[ax] for ax in free_axes], dtype=float) + try: + sol = least_squares(fun, vec0, loss=cfg.get("robust_loss", "huber"), + f_scale=float(cfg.get("huber_delta_mm", 8.0)), + max_nfev=int(cfg.get("max_iterations", 200)) * 3) + if sol.cost < best_cost: + best_cost, best = sol.cost, sol.x + except Exception: + continue + finally: + for ax in free_axes: + origin[ax] = origin_before[ax] # always restore — report-only tool + + if best is None: + result["status"] = "failed" + return result + + fitted_joint = float(best[0]) + if var_type == "revolute": + fitted_joint = (fitted_joint + 180.0) % 360.0 - 180.0 + fitted_origin = list(origin_before) + for i, ax in enumerate(free_axes): + fitted_origin[ax] = float(best[1 + i]) + + final_state = dict(base) + final_state[link_var] = fitted_joint + for i, ax in enumerate(free_axes): + origin[ax] = fitted_origin[ax] + final_res = residual_vector(final_state, fk, obs, own_markers, cfg) + for ax in free_axes: + origin[ax] = origin_before[ax] # restore again after the check above + + result.update({ + "status": "ok", + "joint_value": fitted_joint, + "origin_after_mm": fitted_origin, + "origin_delta_mm": [round(b - a, 4) for a, b in zip(origin_before, fitted_origin)], + "residual_rms": float(np.sqrt(np.mean(final_res ** 2))) if final_res.size else 0.0, + "note": "robot.json NOT modified — apply via Kalibrierung-Tab " + "\"Joint-Origin Y/Z übernehmen\" (editRobot.js) if this looks good.", + }) + return result + + +def estimate_pose(fk: RobotFK, obs: Dict[int, Dict[str, Any]], cfg: Dict[str, Any], + seed: Optional[Dict[str, float]] = None) -> Dict[str, Any]: + """ + seed: optional partial/full joint state (e.g. from load_seed_state(), the + 4b_revolute_angle.py chain) to trust as a starting point for global_ba/ + hybrid. Passed through to estimate_sequential_fk(), which skips re-fitting + any block that is FULLY covered by seed and otherwise still applies its + normal per-block multi-start — so variables the seed does NOT cover keep + the existing local-minimum protection (see doc/Homing_5_Pose.md "Wichtige + Einschraenkung") instead of silently defaulting to an unprotected 0. + sequential_vector ignores seed (no x0 input; left untouched on purpose — + it is the cheap analytic method, not the one this seeding targets). + """ chain = analyze_chain(fk) var_names = chain["ordered_vars"] method = str(cfg.get("method", "hybrid")).lower() @@ -467,12 +652,9 @@ def estimate_pose(fk: RobotFK, obs: Dict[int, Dict[str, Any]], cfg: Dict[str, An 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_sequential_fk(fk, obs, chain, cfg, seed=seed) + else: # global_ba / hybrid (default) — both use the same init->refine path + init = estimate_sequential_fk(fk, obs, chain, cfg, seed=seed) state = estimate_global_ba(fk, obs, var_names, init, cfg) # final residual stats over all observed markers @@ -493,6 +675,14 @@ def main() -> None: 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") + ap.add_argument("--from-state", default=None, metavar="JSON", + help="Seed/init state (flat {var:value} or {accumulated_state:{...}} as " + "written by 4b_revolute_angle.py). Used as x0 for global_ba/hybrid " + "instead of the internal cold start. See doc/Homing_5_Pose.md.") + ap.add_argument("--calibrate-origin", default=None, metavar="LINK", + help="Instead of estimating the full pose, fit LINK's own joint value " + "together with its jointToParent.origin Y/Z from LINK's own markers. " + "Writes a *_origin_calibration.json report; never modifies robot.json.") args = ap.parse_args() robot_data = json.load(open(args.robot, "r", encoding="utf-8")) @@ -503,9 +693,31 @@ def main() -> None: 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')}") + seed = load_seed_state(args.from_state) if args.from_state else None + print(f"[INFO] method={cfg['method']} | observed markers={len(obs)} | use_normals={cfg.get('use_normals')}" + + (f" | seed={seed}" if seed else "")) - result = estimate_pose(fk, obs, cfg) + # ── Mode: joint-origin calibration ────────────────────────────────────── + if args.calibrate_origin: + calib = estimate_origin_calibration(fk, obs, args.calibrate_origin, cfg, seed=seed) + print(f"\nOrigin calibration for link={calib['link']} status={calib['status']}") + if calib["status"] == "ok": + unit = calib["joint_unit"] + print(f" joint {calib['joint_variable']}: {calib['joint_value']:.2f} {unit}") + print(f" origin before: {calib['origin_before_mm']}") + print(f" origin after: {calib['origin_after_mm']} (delta {calib['origin_delta_mm']} mm)") + print(f" residual RMS over {calib['n_markers']} markers: {calib['residual_rms']:.3f}") + print(f" {calib['note']}") + else: + print(f" (no fit — {calib.get('detail', calib['status'])}, n_markers={calib.get('n_markers', 0)})") + out_path = args.out or os.path.join( + os.path.dirname(args.markers), f"{args.calibrate_origin}_origin_calibration.json") + json.dump(calib, open(out_path, "w", encoding="utf-8"), indent=2) + print(f"[INFO] wrote {out_path}") + return + + # ── Mode: full pose estimation (default) ──────────────────────────────── + result = estimate_pose(fk, obs, cfg, seed=seed) st = result["state"] print("\nEstimated joint values:") @@ -513,20 +725,28 @@ def main() -> None: 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]" + tag = "" if ob.get("observable", False) else " [UNOBSERVABLE -> null]" 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}") + movements = {} + for v in ["x", "y", "z", "a", "b", "c", "e"]: + ob = result["observability"].get(v, {}) + observable = ob.get("observable", False) + movements[v] = { + # Unobservable -> null, never a fabricated 0 (see module docstring). + "value": st.get(v, 0.0) if observable else None, + "unit": "mm" if v in ("x", "e") else "deg", + "observable": observable, + "confidence": ob.get("confidence", "none"), + "n_markers": ob.get("n_markers", 0), + } 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"]}, + "seeded": seed is not None, + "movements": movements, "residual_rms": result["residual_rms"], "num_markers": result["num_markers"], } diff --git a/scripts/__pycache__/5_pose_estimation.cpython-311.pyc b/scripts/__pycache__/5_pose_estimation.cpython-311.pyc new file mode 100644 index 0000000..4831c79 Binary files /dev/null and b/scripts/__pycache__/5_pose_estimation.cpython-311.pyc differ