# Homing 1 – Schritt für Schritt: Gelenkwinkel-Schätzung (4b)

> Technische Detail-Doku zu [`Homing.md`](Homing.md) — dieser Teil: `aruco_marker_poses.json`
> → Gelenkwinkel je Link.
> Status: Gerüst, wird sukzessive ausgebaut.

---

## Scope

Die 4b-Kette aus `Homing.md` → Ablauf: `scripts/4b_revolute_angle.py`, sequenziell
Arm1 → Ellbow → Arm2 → Hand. Jeder Aufruf bekommt den Zustand des vorherigen Schritts
über `--from-state` (`accumulated_state`).

## Schätz-Methoden (Prioritätsreihenfolge je Gelenk)

Pro Gelenk wird in dieser Reihenfolge versucht, eine Schätzung zu finden. Jede
nächste Stufe ist ein **reiner Fallback** — sie greift nur, wenn die vorherige Stufe
**keine einzige** brauchbare Baseline liefert (z. B. Marker nicht sichtbar, oder
Marker-Paar zufällig parallel zur Drehachse):

1. **Primär** — zwei Marker auf dem Ziel-Link selbst. `v_model`/`v_obs`-Differenzvektor,
   ⟂ zur Gelenkachse projiziert, Winkel zwischen beiden gemessen. Braucht nur die
   Achs*richtung* (aus FK der schon gelösten Vorgänger), nicht die Pivot-*Position*.
2. **Fallback-1** *(Konzept 2026-06-16, noch nicht implementiert)* — zwei Marker auf
   dem **direkten Kind-Link**, deren Verbindungsvektor (im Kind-Lokalframe) parallel
   zur **eigenen** Achse des Kind-Links liegt → invariant gegen dessen noch
   unbekannten Winkel, daher als Stellvertreter für „zwei Marker am Ziel-Link" nutzbar.
   Beispiel: Ellbow (Achse X) ← Arm2-Marker 144↔148 bzw. 143↔146 (Arm2-Achse Y, ⟂ zu X,
   beide Paare exakt achsparallel in Arm2s Lokalframe). Wie Primär unabhängig von der
   Pivot-Position — eine separate „ist die nächste Achse senkrecht"-Prüfung ist nicht
   nötig, das ergibt sich automatisch aus der bestehenden Mindest-Baseline-Prüfung nach
   der Projektion.
3. **Fallback-2** *(implementiert)* — ein einzelner Marker auf dem Ziel-Link gegen den
   Gelenk-**Pivot** selbst (Pivot + Achse aus FK der Vorgänger). Einzige Stufe, die mit
   nur 1 sichtbaren Marker funktioniert — aber zusätzlich abhängig von der Pivot-
   *Position* (also den geschätzten Vorgänger-*Werten*, nicht nur deren Achsrichtung).

→ Code: `scripts/4b_revolute_angle.py` (`estimate_revolute_angle()`), Konstante
`PIVOT_FALLBACK_ID`, Feld `"method"` im Ergebnis-JSON.

### Befund 2026-06-16 (wichtig für Priorisierung)

Im Testlauf `test/homing/20260616_120456` waren am Ellbow nur Marker 129/132 sichtbar,
deren Verbindungsvektor exakt parallel zur Ellbow-Achse liegt → Primär-Methode liefert
nichts, Fallback-2 (Pivot) springt ein und meldet `z ≈ -4.33°` (intern konsistent,
Exit 0). Eine unabhängige Gegenrechnung (Least-Squares über alle Ellbow- *und*
Arm2-Marker, `z`+`a` frei) zeigt aber ein Minimum bei `z ≈ -38°` — die Fallback-2-Schätzung
liegt hier ca. 35–40° daneben. Die (noch nicht implementierte) Fallback-1-Rechnung mit
Arm2-Marker 144↔148/143↔146 hätte `z ≈ -44°` ergeben, sehr nah am Least-Squares-Minimum,
weil sie (wie Primär) nicht von der Pivot-Position abhängt. → Fallback-1 ist nicht nur
„nice to have", sondern in diesem Fall klar genauer als Fallback-2.
Detaillierte Aufarbeitung/Entscheidung: siehe `Homing_2_improvement.md` (geplant).

## robot.json-Struktur (Kurzreferenz)

- `links.<Name>.parent` — Name des Eltern-Links (Kette/Baum)
- `links.<Name>.jointToParent` — `{type, axis, origin, rotation, variable}`
- `links.<Name>.markers[]` — `{id, position, normal, size}`; `position` ist relativ
  zum **Pivot** des eigenen Links, im lokalen, noch nicht eigen-rotierten Frame.
- FK-Engine: `scripts/robot_fk.py` (`RobotFK.compute()`, `.joint_origin_world()`,
  `.joint_axis_world()`, `.marker_world()`).

## Offene Punkte / noch zu dokumentieren

- [ ] State-JSON-Schema im Detail (`accumulated_state`, `per_pair`, `method`, `circular_std_deg`)
- [ ] `--min-baseline` Tuning / Auswirkung
- [ ] Fallback-1 Implementierung (Aufwandsschätzung: klein-mittel, ~2-3h — Kern-Mathematik
      `_model_spoke_world()`/`_pair_estimate()` bereits vorhanden und wiederverwendbar;
      neu: Kind-Link-Suche, Achsparallelitäts-Filter, 3-stufige Tier-Logik) + Tests
- [ ] y-Restfehler (~2°) aus `Homing.md` → Offene Punkte

## Verweise

- Allgemeiner Ablauf: [`Homing.md`](Homing.md)
- Vorheriger Schritt (Kamera/Triangulation): [`Homing_0_Camera.md`](Homing_0_Camera.md)
- Bekannte Probleme / Ideen: `Homing_2_improvement.md` (geplant)