85 lines
4.9 KiB
Markdown
85 lines
4.9 KiB
Markdown
# 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)
|