ChK/appRobotHoming
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Axis automatisch

Browse Source
This commit is contained in:
chk
2026-06-16 17:36:46 +02:00
parent 5f6d28673a
commit eae6b6098a
6 changed files with 158 additions and 286 deletions
Download Patch File Download Diff File Expand all files Collapse all files

148
doc/Homing_5_Pose.md
View File

@@ -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 densel­ben 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.

32
doc/Kalibrierung.md
View File

@@ -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.
---

18
public/index.html
View File

@@ -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>

243
scripts/5_pose_estimation.py
View File

@@ -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
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.
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.<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)))
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.<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()

BIN
scripts/__pycache__/5_pose_estimation.cpython-311.pyc
View File

Binary file not shown.

3
scripts/robot_1781069752019.json
View File

@@ -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},