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

Technische Detail-Doku zu 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):

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 Achsrichtung (aus FK der schon gelösten Vorgänger), nicht die Pivot-Position.
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.
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
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
Vorheriger Schritt (Kamera/Triangulation): Homing_0_Camera.md
Nächster Schritt (Verfeinerung nach 4b): Homing_5_Pose.md
Bekannte Probleme / Ideen: Homing_2_improvement.md (geplant)

4.9 KiB Raw Blame History Unescape Escape