MultiPose

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**
+> Status (2026-06-16): **Code-Hooks umgesetzt und gegen drei echte Homing-Captures
-> in `homingOrchestrator.js`/`server.js` verdrahtet — und braucht noch einen
+> verifiziert** — `--from-state` (Startwert aus 4b, mit Multi-Start-Schutz für alles,
-> kleinen Code-Hook, um den 4b-Startwert überhaupt entgegennehmen zu können
+> was der Startwert nicht abdeckt), `null` statt `0` für unbeobachtbare Gelenke
-> (siehe Offene Punkte).
+> (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**
+**Ursprünglicher Befund (vor dem Code-Hook vom 2026-06-16):** `estimate_pose()`
-`estimate_sequential_fk()` als „billigen, robusten Init" auf
+rief für `global_ba`/`hybrid` immer selbst `estimate_sequential_fk()` als
-(`scripts/5_pose_estimation.py:471-476`) — es gibt aktuell **keinen** Parameter,
+„billigen, robusten Init" auf — es gab keinen Parameter, um stattdessen einen
-um stattdessen einen extern vorgegebenen Startwert (z. B. den `accumulated_state`
+extern vorgegebenen Startwert einzuspeisen, obwohl `estimate_global_ba()`
-aus 4b) einzuspeisen, obwohl `estimate_global_ba()` selbst intern bereits ein
+selbst intern bereits ein `x0`-Dict entgegennahm (`:272-273`). **Das ist jetzt
-`x0`-Dict entgegennimmt (`:236-237`).
+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
+`estimate_sequential_fk()` initialisiert jede nicht geseedete Variable bei
-Multi-Start `{0,60,120,180,240,300}°` **nur über die erste Variable eines
+`0.0` und rastert den Multi-Start `{0,60,120,180,240,300}°` **nur über die
-Blocks** (`bvars[0]`) — und auch das **nur, wenn diese selbst `revolute`
+erste Variable eines Blocks** (`bvars[0]`) — und auch das **nur, wenn diese
-ist** (`:296-304`). Für dieses Robotermodell heißt das konkret:
+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**
+**Konsequenz / Status:** `5_pose_estimation.py` sollte in appRobotHoming **nicht
-laufen, sondern mit dem `accumulated_state` der 4b-Kette als Startwert (Details
+kalt** laufen, sondern mit dem `accumulated_state` der 4b-Kette als Startwert.
-und der dafür nötige Code-Hook: Abschnitt „Integrationsschritte").
+**Umgesetzt:** `--from-state <json>` 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`
+- ~~`scipy` fehlt im appRobotHoming-Container~~ — **behoben (2026-06-16):**
-  installiert nur `opencv-python-headless numpy`
+  `docker-compose.yaml` installierte nur `opencv-python-headless numpy`; ohne
-  (`pip3 install --quiet --no-cache-dir opencv-python-headless numpy`). Ohne
+  `scipy` greift `HAVE_SCIPY=False` und `estimate_sequential_fk`/`estimate_global_ba`
-  `scipy` greift `HAVE_SCIPY=False`: `estimate_sequential_fk` lässt jeden Block
+  fallen lautlos auf den Nullzustand zurück (nur eine `[WARN]`-Zeile, kein
-  auf `0.0` stehen, `estimate_global_ba` gibt den (dann ebenfalls nullwertigen)
+  Fehler-Exit) — ein stiller Fehlmodus. `scipy` ist jetzt in der
-  Startwert unverändert zurück — **kein Fehler, nur eine `[WARN]`-Zeile auf
+  `pip3 install`-Zeile ergänzt (kein separater Image-Build nötig — `pip3
-  stdout.** Das ist ein stiller Fehlmodus: muss vor dem ersten Einsatz behoben
+  install` läuft laut `command:` bei jedem Containerstart neu). **Noch nicht**
-  werden (scipy zur `pip3 install`-Zeile ergänzen).
+  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.<var>.value`). Eine kleine Adapterfunktion
   ist nötig, kein Drop-in-Ersatz.
-- **Unbeobachtbare Gelenke werden als `0.0` ausgegeben**, nicht als `null`
+- ~~Unbeobachtbare Gelenke werden als `0.0` ausgegeben, nicht als `null`~~ —
-  (Konfidenz `none`/`observable:false` steht nur als Metadatum daneben). Das
+  **behoben (2026-06-16):** `main()`s Output-Writer schreibt jetzt `"value":
-  widerspricht der sonst im Projektverbund befolgten Konvention „Unbekannt bleibt
+  null`, wenn `observable:false`, statt der internen `0.0` (die intern bleibt,
-  `null`, nie erfundene `0`". Eine Integration muss `observable:false` aktiv auf
+  weil die FK für die *anderen* Gelenke einen Zahlenwert braucht — nur der
-  `null` ummappen, bevor der Zustand weitergereicht wird — sonst wandert eine
+  *Output*-Vertrag ändert sich). Verifiziert an allen drei Fixtures (`Hand`,
-  stille `0°`/`0mm` in Richtung Robotersteuerung.
+  `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
+  `--from-state` und 4b-Startwert — als Regressionstest für genau diese
-  diese Einschränkung.
+  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
+**Motivation:** `doc/Kalibrierung.md` Schritt [4] bestimmt
-existiert noch nicht, s. Integrationsschritte). Geeignet, um das Kaltstart-/
+`links.Arm1.jointToParent.origin[1,2]` (Y/Z des Schultergelenk-Drehpunkts) geometrisch
-Lokales-Minimum-Verhalten aus „Wichtige Einschränkung" zu beobachten und zu
+aus einer **dedizierten 3-Pose-Aufnahme** (Verfahren B: Kreis-Umkreismittelpunkt
-reproduzieren — **nicht** der vorgesehene Produktionspfad.
+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[<Link>]["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/<run>/aruco_marker_poses.json \
        -robot scripts/robot_1781069752019.json \
+       --from-state data/homing/<run>/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/<run>/aruco_marker_poses.json \
+       -robot scripts/robot_1781069752019.json \
+       --from-state data/homing/<run>/state_Arm2.json \
        -out   data/homing/<run>/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/<run>/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/<run>/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) —
+**Erledigt (2026-06-16):**
-  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
+- [x] **`scipy` in `docker-compose.yaml` ergänzt** (`pip3 install … numpy scipy`).
-  Weg, einen externen Startwert hineinzugeben. Vorschlag: CLI-Flag analog zu 4b
+- [x] **Code-Hook `--from-state`:** `load_seed_state()` (akzeptiert flach oder
-  (`--from-state accumulated_state.json`), das den 4b-Zustand als `x0` direkt an
+  `{accumulated_state:{...}}`) + `estimate_sequential_fk(..., seed=...)`
-  `estimate_global_ba()` durchreicht (Parameter existiert dort bereits,
+  überspringt nur vollständig geseedete Blöcke, alles andere bleibt
-  `:236-237`) und so den internen `estimate_sequential_fk()`-Kaltstart in
+  Multi-Start-geschützt. `estimate_pose(..., seed=...)` reicht das durch.
-  `estimate_pose()` (`:471-476`) umgeht bzw. nur als Fallback nutzt, falls kein
+  Verifiziert an 3 echten Fixtures (s. „Validierung an echten
-  externer Startwert übergeben wird.
+  appRobotHoming-Daten").
-- [ ] Adapter `movements.<var>.value` → flaches `{x,…,e}`-State-Objekt für
+- [x] **Robustheit gegen fehlende Marker:** `Hand`/`Palm`/`FingerA`/`FingerB`
-  `POST /api/homing/send-state`; dabei `observable:false → null` ummappen.
+  (aktuell `"markers": []`) laufen ohne Crash durch, Output zeigt `null`/
-- [ ] Anbindung in `homingOrchestrator.js` (neuer Schritt, analog `runBoardPipeline`/
+  `confidence:"none"` statt erfundener `0`. `main()`s Output-Writer mappt
-  4b-Loop) + SSE-Event(s) für Fortschritt (auch ohne echtes Zwischenergebnis,
+  `observable:false → value:null` (intern bleibt `0.0` für die FK-Rechnung
-  z. B. ein `step`-Event „läuft" / „fertig").
+  der anderen Gelenke — nur der Output-Vertrag ändert sich).
-- [ ] Erste echte Messung: `hybrid`-Ergebnis gegen 4b-Kette auf demselben
+- [x] **Kalibrier-Switch `--calibrate-origin <Link>`** umgesetzt
-  `data/homing/<run>/aruco_marker_poses.json` vergleichen (insbesondere am
+  (`estimate_origin_calibration()`) — generisch für jeden Link mit eigenen
-  Ellbow-Fall aus `Homing_1_StepByStep.md`).
+  Markern, getestet an `Arm1` und `Ellbow`. Schreibt nie `robot.json`, nur
+  einen `*_origin_calibration.json`-Report. Details: eigener Abschnitt oben.
+
+**Noch offen:**
+
+- [ ] **Adapter** `movements.<var>.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.
 
 ---
 

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 <evalDir>/aruco_marker_poses.json \
+       -robot scripts/robot_1781069752019.json \
+       --from-state <evalDir>/state_Arm2.json \
+       --calibrate-origin Arm1
+```
+
 ---
 
 ### Mathematik: Bestimmung der Rotationsachse

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

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 <json>        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 <Link>  special mode: instead of estimating the full
+                              pose, fit <Link>'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.<var>.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]:
+                           cfg: Dict[str, Any], seed: Optional[Dict[str, float]] = None
-    """Estimate block by block along the chain, freezing already-solved variables."""
+                           ) -> 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)
+        state = estimate_sequential_fk(fk, obs, chain, cfg, seed=seed)
-    elif method == "global_ba":
+    else:  # global_ba / hybrid (default) — both use the same init->refine path
-        init = estimate_sequential_fk(fk, obs, chain, cfg)  # cheap robust init
+        init = estimate_sequential_fk(fk, obs, chain, cfg, seed=seed)
-        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
@@ -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),
+        "seeded": seed is not None,
-                          "unit": "mm" if v in ("x", "e") else "deg",
+        "movements": movements,
-                          "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"],
     }