diff --git a/doc/Homing_5_Pose.md b/doc/Homing_5_Pose.md
index 80c3665..fa41b21 100644
--- 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[]["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//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.
+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 `** 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.
diff --git a/doc/Kalibrierung.md b/doc/Kalibrierung.md
index 77ecb5d..7a8819a 100644
--- 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 /aruco_marker_poses.json \
- -robot scripts/robot_1781069752019.json \
- --from-state /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.
---
diff --git a/public/index.html b/public/index.html
index b0bc400..d135857 100755
--- a/public/index.html
+++ b/public/index.html
@@ -65,24 +65,6 @@
-
-
-
Result – Raw JSON
-
-
-
-
-
-
-
-
-
Result – Tree View
-
-
-
-
-
-
Board-Viewer
diff --git a/scripts/5_pose_estimation.py b/scripts/5_pose_estimation.py
index 0fff55d..4d181aa 100644
--- a/scripts/5_pose_estimation.py
+++ b/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 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.
+ robot.json -> pose_estimation.fit_origin_link = "Arm1" one switch: the
+ named link's jointToParent.origin Y/Z is fit
+ TOGETHER WITH the normal pose in the same
+ global_ba solve (complements the geometric
+ multi-pose method in doc/Kalibrierung.md
+ Schritt [4]) and the result is adopted
+ 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..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)))
- return _state_from_vec(var_names, sol.x, base)
+ max_nfev=int(cfg.get("max_iterations", 200)) * max(1, len(vec0)))
+ 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..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 (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()
diff --git a/scripts/__pycache__/5_pose_estimation.cpython-311.pyc b/scripts/__pycache__/5_pose_estimation.cpython-311.pyc
index 4831c79..63ef7df 100644
Binary files a/scripts/__pycache__/5_pose_estimation.cpython-311.pyc and b/scripts/__pycache__/5_pose_estimation.cpython-311.pyc differ
diff --git a/scripts/robot_1781069752019.json b/scripts/robot_1781069752019.json
index 022faff..fa5031d 100644
--- a/scripts/robot_1781069752019.json
+++ b/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},