Axis automatisch

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
+> `scipy` in `docker-compose.yaml` ergänzt, sowie der **eine Schalter**
-> `--calibrate-origin` (siehe eigener Abschnitt unten). **Noch offen:** Anbindung in
+> `pose_estimation.fit_origin_link` (siehe eigener Abschnitt unten), der einen
-> `homingOrchestrator.js`/`server.js`/Frontend (siehe Integrationsschritte —
+> Gelenk-Drehpunkt automatisch mitbestimmt und in `robot.json` übernimmt. **Noch
-> Umfang/Fehlerfall/Robotersteuerung-Politik dafür sind noch nicht festgelegt).
+> 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
+aus einer dedizierten 3-Pose-Aufnahme (Marker-Mittelpunkte, keine Normalen). Diese
-durch 3 Positionen je Marker, nur Marker-**Mittelpunkte**, keine Normalen — Details
+Y/Z-Werte sind laut Nutzer „etwas ungenau gemessen" — `5_pose_estimation.py` hat mit
-dort). Diese Y/Z-Werte sind laut Nutzer „etwas ungenau gemessen". `5_pose_estimation.py`
+Position+Normale und einem robusten Least-Squares-Löser bereits die Bausteine, um
-hat mit `residual_vector()` (Position **und** Normale, robuste Verlustfunktion,
+denselben Drehpunkt aus den ohnehin vorhandenen Homing-Aufnahmen zu verfeinern.
-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
+**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
+### Befund an echten Daten (drei reale Captures, sequenziell, 2026-06-16)
-zusätzlich 2 Komponenten seines `jointToParent.origin` freigegeben:
 
-```
+| Lauf | Fixture | Origin Y / Z danach | Δ zum Vorlauf |
-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)‖)
+| 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,
+Drei unabhängige Aufnahmen (unterschiedliche Marker, unterschiedliche Posen) landen
-`scripts/5_pose_estimation.py`): mutiert `fk.links[<Link>]["jointToParent"]["origin"][1,2]`
+im selben Bereich, und die Schritte werden **kleiner statt größer** — spricht für
-**transient** während des Solves (jeder `fk.compute()`-Aufruf liest `origin` frisch
+Konvergenz, nicht für Rauschen/Drift. (Für die Doku danach wieder auf den
-aus dem Dict, siehe `robot_fk.py:compute()` — kein Caching, daher funktioniert die
+Ursprungswert `101.1, 55.2` zurückgesetzt — die Tabelle zeigt nur den Testlauf.)
-direkte Mutation ohne Änderung an `robot_fk.py`) und stellt den Originalwert danach
+**Konsequenz des „bei jedem Lauf automatisch":** der Wert wandert mit jeder neuen
-**immer** wieder her — das Skript bleibt ein reines Report-Tool, **`robot.json` wird
+Aufnahme leicht weiter, statt einmalig fixiert zu werden — gewünscht laut Nutzer,
-nie geschrieben**. Multi-Start `{0,60,…,300}°` für die eigene Gelenkvariable, wenn
+aber gut zu wissen. Schalter auf `null` setzen, um das einzufrieren.
-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
+Ergänzt, ersetzt nicht: `doc/Kalibrierung.md` Schritt [4] (3-Pose-Kreis-Fit, nur
-
+Marker-Mittelpunkte) bleibt die unabhängige Gegenmessung. Die Achs**richtung**
-```bash
+(`jointToParent.axis`) fitten beide Verfahren nicht.
-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.
 
 ---
 
@@ -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
+- [x] **Kalibrier-Switch `pose_estimation.fit_origin_link`** umgesetzt — ein
-  (`estimate_origin_calibration()`) — generisch für jeden Link mit eigenen
+  Konfig-Feld, direkt in `estimate_global_ba()` integriert (keine separate
-  Markern, getestet an `Arm1` und `Ellbow`. Schreibt nie `robot.json`, nur
+  Funktion/Report/CLI-Flag mehr), übernimmt das Ergebnis automatisch in
-  einen `*_origin_calibration.json`-Report. Details: eigener Abschnitt oben.
+  `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
+- [ ] **Arm1-Origin-Wert beobachten:** wandert bei jedem Lauf mit `fit_origin_link`
-  auf zwei unabhängigen Fixtures konsistent (s. Abschnitt „Kalibrier-Switch").
+  leicht weiter (s. Befund-Tabelle oben, wird kleiner/konvergiert bisher). Falls
-  Vor dem Übernehmen: (a) mit mehr Fixtures/Posen erhärten, (b) wenn möglich
+  das nicht erwünscht ist: Schalter nach dem Einschwingen auf `null` setzen, oder
-  gegen eine frische Verfahren-B-3-Pose-Messung gegenchecken, (c) erst dann via
+  gegen eine frische Verfahren-B-3-Pose-Messung gegenchecken.
-  Kalibrierung-Tab „Joint-Origin Y/Z übernehmen" übernehmen.
+- [ ] **`fit_origin_link` in der Kalibrierung-UI sichtbar machen** (`doc/Kalibrierung.md`
-- [ ] **`--calibrate-origin` an die Kalibrierung-UI anbinden** (`doc/Kalibrierung.md`
+  Schritt [4]) — aktuell nur in `robot.json` umschaltbar; Tab „Arm1 – Y" könnte
-  Schritt [4]) — aktuell nur CLI/Report; Tab „Arm1 – Y" könnte beide Verfahren
+  den aktuellen Wert/letzte Änderung neben Verfahren B anzeigen.
-  (Geometrisch/Verfahren B und `--calibrate-origin`) nebeneinander anzeigen.
+- [ ] **Mehrpose-Erweiterung** (mehrere `aruco_marker_poses.json` mit
-- [ ] **Mehrpose-Erweiterung für `--calibrate-origin`** (mehrere
+  gemeinsamem `origin`, je Pose ein eigener Gelenkwinkel, ein gemeinsamer Solve)
-  `aruco_marker_poses.json` + gemeinsames `origin`, je Pose ein eigener
+  — würde die Winkel/Origin-Korrelation aus einer Einzelpose weiter reduzieren,
-  Gelenkwinkel) — würde die Winkel/Origin-Korrelationsschwäche aus einer
+  analog zur bestehenden 3-Pose-Aufnahme.
-  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
+  `estimate_sequential_fk()` und die Origin-Erweiterung in `estimate_global_ba()`
-  manuell gegen die drei Fixtures verifiziert (s. oben); appRobotHoming hat
+  — aktuell nur manuell gegen die drei Fixtures verifiziert (s. oben);
-  bisher keine Python-Testinfrastruktur (nur Jest/JS), das wäre die erste.
+  appRobotHoming hat bisher keine Python-Testinfrastruktur (nur Jest/JS).
 - [ ] Eintrag in `Homing.md`-Tabelle (Doku-Übersicht) ergänzen, sobald
   `homingOrchestrator.js` verdrahtet ist.
 

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
+🔶 *Experimentell, noch nicht in dieses UI eingebunden* — Details, Befund und
-Vergleichstabelle: [`doc/Homing_5_Pose.md`](Homing_5_Pose.md) (Abschnitt
+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
+Ein Schalter in `robot.json` (`pose_estimation.fit_origin_link: "Arm1"`, aktuell
-Homing-Aufnahme** (Position + gemessene Normale aller Arm1-Marker, robuster
+gesetzt): bestimmt `origin[1,2]` (Y,Z) **zusammen mit** der Gelenkvariable in
-Least-Squares-Fit) statt aus der dedizierten 3-Pose-Aufnahme oben — keine
+derselben Pose-Schätzung — aus einer einzelnen vorhandenen Homing-Aufnahme
-eigene Mess-Session nötig, dafür (noch) ohne die explizite Drehung, die hier
+(Position + gemessene Normale aller Arm1-Marker), keine eigene Mess-Session
-Achse und Winkel sauber entkoppelt. Auf zwei realen Captures ergab sich eine
+nötig. Bei Erfolg **automatisch übernommen**: jeder Lauf schreibt den neuen
-konsistente Korrektur von ca. **+7 mm (Y) / −19 mm (Z)** gegenüber dem
+Wert direkt in `robot.json` zurück (wie der „Joint-Origin Y/Z übernehmen"-Button,
-aktuellen `robot.json`-Wert — bisher **nicht** gegen eine frische Messung mit
+nur automatisch statt per Klick). Auf drei realen Captures ergab sich eine
-Verfahren B gegengeprüft, daher noch nicht über „Joint-Origin Y/Z übernehmen"
+konsistente, sich einschwingende Korrektur von ca. **+6 bis +7 mm (Y) / −19 bis
-angewendet. Aufruf:
+−20 mm (Z)** gegenüber dem ursprünglichen Wert — bisher **nicht** gegen eine
-
+frische Messung mit Verfahren B gegengeprüft.
-```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
-```
 
 ---
 

public/index.html

@@ -65,24 +65,6 @@
       <textarea id="analysis-log" readonly></textarea>
     </div>
 
-    <!-- RESULT RAW JSON -->
-    <div class="section half">
-      <h2>Result – Raw JSON</h2>
-      <div class="panel">
-        <label>Raw JSON</label>
-        <textarea id="result-json" readonly></textarea>
-      </div>
-    </div>
-
-    <!-- RESULT TREE VIEW -->
-    <div class="section half">
-      <h2>Result – Tree View</h2>
-      <div class="panel">
-        <label>Tree View</label>
-        <div id="result-tree"></div>
-      </div>
-    </div>
-
     <!-- BOARD-VIEWER -->
     <div class="section full">
       <h2>Board-Viewer</h2>

scripts/5_pose_estimation.py

@@ -33,13 +33,15 @@ Homing integration (appRobotHoming, see doc/Homing_5_Pose.md):
                               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
+  robot.json -> pose_estimation.fit_origin_link = "Arm1"   one switch: the
-                              pose, fit <Link>'s own joint value TOGETHER WITH
+                              named link's jointToParent.origin Y/Z is fit
-                              its jointToParent.origin Y/Z from that link's own
+                              TOGETHER WITH the normal pose in the same
-                              markers (complements the geometric multi-pose
+                              global_ba solve (complements the geometric
-                              method in doc/Kalibrierung.md Schritt [4]).
+                              multi-pose method in doc/Kalibrierung.md
-                              Writes a *_origin_calibration.json report; never
+                              Schritt [4]) and the result is adopted
-                              modifies robot.json.
+                              automatically -- written back into robot.json
+                              (surgical text patch, see patch_robot_json_origin()).
+                              Off by default (key absent/null).
 
 Unobservable joints (confidence "none") are written as value=null in the
 output JSON — never a fabricated 0 (see movements.<var>.observable).
@@ -52,6 +54,7 @@ import argparse
 import json
 import math
 import os
+import re
 import sys
 import time
 from collections import defaultdict
@@ -85,6 +88,12 @@ DEFAULT_CFG: Dict[str, Any] = {
     "min_cameras_per_marker": 2,
     "finger_block_joints": ["b", "c", "e"],
     "per_link_method": {},
+    # One switch: if set to a link name (e.g. "Arm1"), that link's
+    # jointToParent.origin Y/Z is fit together with the normal pose (same
+    # global_ba solve) and the result is written back into robot.json
+    # automatically. None/absent = off, no behaviour change. See
+    # doc/Homing_5_Pose.md "Kalibrier-Switch".
+    "fit_origin_link": None,
 }
 
 
@@ -277,20 +286,42 @@ def estimate_global_ba(fk: RobotFK, obs: Dict[int, Dict[str, Any]], var_names: L
     marker_ids = list(obs.keys())
     base = {k: 0.0 for k in STATE_KEYS}
     base.update(x0)
-    vec0 = np.array([base.get(v, 0.0) for v in var_names], dtype=float)
+
+    # One switch (pose_estimation.fit_origin_link): also free that link's
+    # jointToParent.origin Y/Z as 2 extra parameters of THIS SAME solve, no
+    # separate pass. fk.links[...] is mutated in place -- compute() re-reads
+    # it fresh every call (robot_fk.py), so this takes effect immediately and
+    # is left adopted on success (main() writes it back into robot.json).
+    origin_link = cfg.get("fit_origin_link")
+    origin = fk.links.get(origin_link, {}).get("jointToParent", {}).get("origin") if origin_link else None
+    origin = origin if isinstance(origin, list) and len(origin) == 3 else None
+    origin_before = list(origin) if origin else None
+    n_state = len(var_names)
+
+    vec0 = np.array([base.get(v, 0.0) for v in var_names]
+                     + (origin_before[1:3] if origin else []), dtype=float)
 
     def fun(vec):
-        st = _state_from_vec(var_names, vec, base)
+        st = _state_from_vec(var_names, vec[:n_state], base)
+        if origin:
+            origin[1], origin[2] = float(vec[n_state]), float(vec[n_state + 1])
         return residual_vector(st, fk, obs, marker_ids, cfg)
 
     loss = cfg.get("robust_loss", "huber")
     f_scale = float(cfg.get("huber_delta_mm", 8.0))
     try:
         sol = least_squares(fun, vec0, loss=loss, f_scale=f_scale,
-                            max_nfev=int(cfg.get("max_iterations", 200)) * max(1, len(var_names)))
+                            max_nfev=int(cfg.get("max_iterations", 200)) * max(1, len(vec0)))
-        return _state_from_vec(var_names, sol.x, base)
+        state = _state_from_vec(var_names, sol.x[:n_state], base)
+        if origin:
+            origin[1], origin[2] = float(sol.x[n_state]), float(sol.x[n_state + 1])
+            print(f"[INFO] fit_origin_link={origin_link}: Y,Z {origin_before[1:3]} "
+                  f"-> [{origin[1]:.3f}, {origin[2]:.3f}]")
+        return state
     except Exception as exc:
         print(f"[WARN] global_ba failed: {exc}")
+        if origin:
+            origin[1], origin[2] = origin_before[1], origin_before[2]  # restore on failure
         return dict(base)
 
 
@@ -510,127 +541,6 @@ def observability(chain: Dict[str, Any], obs: Dict[int, Dict[str, Any]]) -> Dict
     return info
 
 
-# ==================================================================
-# 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]:
     """
@@ -669,6 +579,36 @@ def estimate_pose(fk: RobotFK, obs: Dict[int, Dict[str, Any]], cfg: Dict[str, An
 # CLI
 # ==================================================================
 
+def patch_robot_json_origin(robot_path: str, link_name: str, yz: Tuple[float, float]) -> bool:
+    """
+    Surgically rewrite links.<link_name>.jointToParent.origin[1],[2] (Y,Z) in
+    the robot.json TEXT in place. robot.json has a hand-curated, compact
+    format (markers etc. one per line) -- a full json.load()+json.dump()
+    round-trip would reformat the whole file, so this only touches the one
+    "origin": [...] array belonging to <link_name> (X left untouched).
+    Returns True if a match was found and patched.
+    """
+    with open(robot_path, "r", encoding="utf-8") as f:
+        text = f.read()
+    link_m = re.search(r'"%s"\s*:\s*\{' % re.escape(link_name), text)
+    if not link_m:
+        return False
+    origin_m = re.search(r'"origin"\s*:\s*\[([^\]]*)\]', text[link_m.end():])
+    if not origin_m:
+        return False
+    parts = [p.strip() for p in origin_m.group(1).split(",")]
+    if len(parts) != 3:
+        return False
+    parts[1] = f"{float(yz[0]):.4f}".rstrip("0").rstrip(".")
+    parts[2] = f"{float(yz[1]):.4f}".rstrip("0").rstrip(".")
+    new_array = f"[{parts[0]}, {parts[1]}, {parts[2]}]"
+    start = link_m.end() + origin_m.start()
+    end = link_m.end() + origin_m.end()
+    with open(robot_path, "w", encoding="utf-8") as f:
+        f.write(text[:start] + '"origin": ' + new_array + text[end:])
+    return True
+
+
 def main() -> None:
     ap = argparse.ArgumentParser(description="Estimate robot joint angles from marker poses")
     ap.add_argument("markers", help="aruco_marker_poses.json (corner_pose) or aruco_positions_*.json (center)")
@@ -679,10 +619,6 @@ def main() -> None:
                     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"))
@@ -696,27 +632,9 @@ def main() -> None:
     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 ""))
+    if cfg.get("fit_origin_link"):
+        print(f"[INFO] fit_origin_link={cfg['fit_origin_link']} -> robot.json wird bei Erfolg aktualisiert")
 
-    # ── 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"]
 
@@ -754,6 +672,19 @@ def main() -> None:
     json.dump(out, open(out_path, "w", encoding="utf-8"), indent=2)
     print(f"[INFO] wrote {out_path}")
 
+    # "Uebernehmen": fit_origin_link's Y/Z is already adopted on `fk` (see
+    # estimate_global_ba) -- write it back into robot.json itself.
+    origin_link = cfg.get("fit_origin_link")
+    if origin_link:
+        origin = fk.links.get(origin_link, {}).get("jointToParent", {}).get("origin")
+        if isinstance(origin, list) and len(origin) == 3:
+            if patch_robot_json_origin(args.robot, origin_link, (origin[1], origin[2])):
+                print(f"[INFO] robot.json aktualisiert: {origin_link}.jointToParent.origin "
+                      f"= [{origin[0]}, {origin[1]:.3f}, {origin[2]:.3f}]")
+            else:
+                print(f"[WARN] {origin_link}.jointToParent.origin in {args.robot} nicht gefunden — "
+                      f"nicht aktualisiert (Wert nur in {out_path} sichtbar)")
+
 
 if __name__ == "__main__":
     main()

scripts/robot_1781069752019.json

@@ -98,7 +98,8 @@
     "max_iterations": 200,
     "min_cameras_per_marker": 2,
     "finger_block_joints": ["b", "c", "e"],
-    "per_link_method": {}
+    "per_link_method": {},
+    "fit_origin_link": "Arm1"
   },
   "robot_test_poses": {
     "4": {"x": 70, "y": 50, "z": -70, "a": 120, "b": 50, "c": 30, "e": 20},