Axis automatisch

2026-06-16 17:36:46 +02:00
parent 5f6d28673a
commit eae6b6098a
6 changed files with 158 additions and 286 deletions
--- a/doc/Homing_5_Pose.md
+++ b/doc/Homing_5_Pose.md
@@ -10,10 +10,12 @@
 > 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).
+> `scipy` in `docker-compose.yaml` ergänzt, sowie der **eine Schalter**
+> `pose_estimation.fit_origin_link` (siehe eigener Abschnitt unten), der einen
+> Gelenk-Drehpunkt automatisch mitbestimmt und in `robot.json` übernimmt. **Noch
+> offen:** Anbindung in `homingOrchestrator.js`/`server.js`/Frontend (siehe
+> Integrationsschritte — Umfang/Fehlerfall/Robotersteuerung-Politik dafür sind
+> noch nicht festgelegt).

 ---

@@ -296,82 +298,44 @@ bzw. `state_Arm1.json` für die unvollständige erste Fixture):

 ---

-## Kalibrier-Switch: Gelenk-Origin (`--calibrate-origin`)
+## Kalibrier-Switch: Gelenk-Origin (`pose_estimation.fit_origin_link`)

 **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 denselben Drehpunkt
-**aus den ohnehin vorhandenen Homing-Aufnahmen** zu verfeinern, statt eine eigene
-Aufnahme-Session zu brauchen.
+aus einer dedizierten 3-Pose-Aufnahme (Marker-Mittelpunkte, keine Normalen). Diese
+Y/Z-Werte sind laut Nutzer „etwas ungenau gemessen" — `5_pose_estimation.py` hat mit
+Position+Normale und einem robusten Least-Squares-Löser bereits die Bausteine, um
+denselben Drehpunkt aus den ohnehin vorhandenen Homing-Aufnahmen zu verfeinern.

-### Ansatz
+**Ein Schalter, eine Stelle:** `robot.json` → `pose_estimation.fit_origin_link`
+(aktuell `"Arm1"`). Wenn gesetzt, gibt `estimate_global_ba()` für diesen Link
+`jointToParent.origin[1,2]` (Y,Z) als **2 zusätzliche Variablen derselben
+Optimierung** frei (kein separater Lauf, keine Restore-Logik, keine eigene
+Funktion) — die Gelenkvariable und der Drehpunkt werden **gemeinsam** bestimmt.
+Bei Erfolg übernimmt `main()` das Ergebnis automatisch: `patch_robot_json_origin()`
+schreibt nur die eine `"origin": [...]`-Zeile des Links in `robot.json` zurück
+(Text-Patch, nicht `json.dump()` — der Rest der handgepflegten, kompakten Datei
+bleibt unverändert). `null`/Feld weglassen = aus, keine Verhaltensänderung.

-Statt nur die Gelenkvariable `q` zu fitten, werden für **einen** angegebenen Link
-zusätzlich 2 Komponenten seines `jointToParent.origin` freigegeben:
+### Befund an echten Daten (drei reale Captures, sequenziell, 2026-06-16)

-```
-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)‖)
-```
+| Lauf | Fixture | Origin Y / Z danach | Δ zum Vorlauf |
+|---|---|---|---|
+| 1 | `20260616_133151` | 108,28 / 34,81 | +7,18 / −20,39 (ggü. ursprünglich 101,1 / 55,2) |
+| 2 | `20260616_135403` | 108,84 / 34,89 | +0,57 / +0,08 |
+| 3 | `20260616_120456` (unvollst. Seed) | 107,42 / 35,33 | −1,43 / +0,45 |

-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.
+Drei unabhängige Aufnahmen (unterschiedliche Marker, unterschiedliche Posen) landen
+im selben Bereich, und die Schritte werden **kleiner statt größer** — spricht für
+Konvergenz, nicht für Rauschen/Drift. (Für die Doku danach wieder auf den
+Ursprungswert `101.1, 55.2` zurückgesetzt — die Tabelle zeigt nur den Testlauf.)
+**Konsequenz des „bei jedem Lauf automatisch":** der Wert wandert mit jeder neuen
+Aufnahme leicht weiter, statt einmalig fixiert zu werden — gewünscht laut Nutzer,
+aber gut zu wissen. Schalter auf `null` setzen, um das einzufrieren.

-### 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.
+Ergänzt, ersetzt nicht: `doc/Kalibrierung.md` Schritt [4] (3-Pose-Kreis-Fit, nur
+Marker-Mittelpunkte) bleibt die unabhängige Gegenmessung. Die Achs**richtung**
+(`jointToParent.axis`) fitten beide Verfahren nicht.

 ---

@@ -425,10 +389,11 @@ Gegen die echten Testdaten in `test/homing/*/` ausprobiert — siehe
  `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 <Link>`** 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.
+- [x] **Kalibrier-Switch `pose_estimation.fit_origin_link`** umgesetzt — ein
+  Konfig-Feld, direkt in `estimate_global_ba()` integriert (keine separate
+  Funktion/Report/CLI-Flag mehr), übernimmt das Ergebnis automatisch in
+  `robot.json` (`patch_robot_json_origin()`, Text-Patch). Generisch für jeden
+  Link, aktuell für `Arm1` aktiviert. Details: eigener Abschnitt oben.

 **Noch offen:**

@@ -439,25 +404,24 @@ Gegen die echten Testdaten in `test/homing/*/` ausprobiert — siehe
  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.
+- [ ] **Arm1-Origin-Wert beobachten:** wandert bei jedem Lauf mit `fit_origin_link`
+  leicht weiter (s. Befund-Tabelle oben, wird kleiner/konvergiert bisher). Falls
+  das nicht erwünscht ist: Schalter nach dem Einschwingen auf `null` setzen, oder
+  gegen eine frische Verfahren-B-3-Pose-Messung gegenchecken.
+- [ ] **`fit_origin_link` in der Kalibrierung-UI sichtbar machen** (`doc/Kalibrierung.md`
+  Schritt [4]) — aktuell nur in `robot.json` umschaltbar; Tab „Arm1 – Y" könnte
+  den aktuellen Wert/letzte Änderung neben Verfahren B anzeigen.
+- [ ] **Mehrpose-Erweiterung** (mehrere `aruco_marker_poses.json` mit
+  gemeinsamem `origin`, je Pose ein eigener Gelenkwinkel, ein gemeinsamer Solve)
+  — würde die Winkel/Origin-Korrelation aus einer Einzelpose weiter reduzieren,
+  analog zur bestehenden 3-Pose-Aufnahme.
 - [ ] `huber_delta_mm`/`normal_weight` ggf. gegen reale Marker-Genauigkeit
-  nachjustieren — reales Residuum (4,3–4,5 mm RMS) liegt deutlich über der
+  nachjustieren — reales Residuum (2,4–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.
+  `estimate_sequential_fk()` und die Origin-Erweiterung in `estimate_global_ba()`
+  — aktuell nur manuell gegen die drei Fixtures verifiziert (s. oben);
+  appRobotHoming hat bisher keine Python-Testinfrastruktur (nur Jest/JS).
 - [ ] Eintrag in `Homing.md`-Tabelle (Doku-Übersicht) ergänzen, sobald
  `homingOrchestrator.js` verdrahtet ist.

--- a/doc/Kalibrierung.md
+++ b/doc/Kalibrierung.md
@@ -102,28 +102,22 @@ 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`)
+### Alternative/Ergänzung — `pose_estimation.fit_origin_link` (`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
+🔶 *Experimentell, noch nicht in dieses UI eingebunden* — Details, Befund und
+Vergleich zu Verfahren B: [`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
-```
+Ein Schalter in `robot.json` (`pose_estimation.fit_origin_link: "Arm1"`, aktuell
+gesetzt): bestimmt `origin[1,2]` (Y,Z) **zusammen mit** der Gelenkvariable in
+derselben Pose-Schätzung — aus einer einzelnen vorhandenen Homing-Aufnahme
+(Position + gemessene Normale aller Arm1-Marker), keine eigene Mess-Session
+nötig. Bei Erfolg **automatisch übernommen**: jeder Lauf schreibt den neuen
+Wert direkt in `robot.json` zurück (wie der „Joint-Origin Y/Z übernehmen"-Button,
+nur automatisch statt per Klick). Auf drei realen Captures ergab sich eine
+konsistente, sich einschwingende Korrektur von ca. **+6 bis +7 mm (Y) / −19 bis
+−20 mm (Z)** gegenüber dem ursprünglichen Wert — bisher **nicht** gegen eine
+frische Messung mit Verfahren B gegengeprüft.

 ---