# 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** (`TIER_PRIMARY`) — 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** (`TIER_FALLBACK_1`, implementiert 2026-06-16) — zwei Marker auf
   dem **direkten Kind-Link**, deren Verbindungsvektor (im Kind-Lokalframe) parallel
   zur **eigenen** Achse des Kind-Links liegt (Toleranz `--child-axis-tol`, default
   1mm) → 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 war nicht nötig, das ergibt sich
   automatisch aus der bestehenden Mindest-Baseline-Prüfung nach der Projektion.
3. **Fallback-2** (`TIER_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). Letzter Rückfall, nur falls Fallback-1 auch nichts findet
   (kein Kind-Link, oder dessen Marker nicht sichtbar/nicht achsparallel).

→ Code: `scripts/4b_revolute_angle.py` (`estimate_revolute_angle()`); Konstanten
`TIER_PRIMARY`/`TIER_FALLBACK_1`/`TIER_FALLBACK_2`/`PIVOT_FALLBACK_ID`; Felder
`"method"` (top-level) und `"tier"`/`"link"` (je `per_pair`-Eintrag) im Ergebnis-JSON.

### Befund 2026-06-16 (Anlass für Fallback-1)

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. **Vor** Fallback-1 sprang Fallback-2 (Pivot) ein und meldete `z ≈ -4.33°`
(intern konsistent, Exit 0) — eine unabhängige Gegenrechnung (Least-Squares über alle
Ellbow- *und* Arm2-Marker, `z`+`a` frei) zeigte aber ihr Minimum bei `z ≈ -38°`, also
ca. 35–40° daneben. **Mit** Fallback-1 (Arm2-Marker 144↔148/143↔146) liefert derselbe
Lauf jetzt `z ≈ -44.15°` (circular_σ 0.80°) — deutlich näher am Least-Squares-Minimum.
Bestätigt auch downstream: die anschließende Arm2-Schätzung (Primär, eigene Marker)
hatte mit der alten Fallback-2-Kette einen Ausreißer und `circular_σ 45.8°`; mit der
Fallback-1-Kette sind alle vier Arm2-Paare konsistent, `circular_σ 5.1°`.
→ Fallback-1 war hier kein „nice to have", sondern ca. 35° Genauigkeitsgewinn.
Weitere/künftige Befunde: 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` / `--child-axis-tol` Tuning / Auswirkung
- [x] Fallback-1 Implementierung (2026-06-16, `scripts/4b_revolute_angle.py`)
- [ ] Mehrstufige Rekursion (Enkel-Links) für Fallback-1 — aktuell bewusst nur
      direkter Kind-Link, siehe Code-Kommentar bei `_child_links()`
- [ ] 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)
- Nächster Schritt (Verfeinerung nach 4b): [`Homing_5_Pose.md`](Homing_5_Pose.md)
- Bekannte Probleme / Ideen: `Homing_2_improvement.md` (geplant)