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 — 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 (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.
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
Vorheriger Schritt (Kamera/Triangulation): Homing_0_Camera.md
Bekannte Probleme / Ideen: Homing_2_improvement.md (geplant)

4.3 KiB Raw Blame History Unescape Escape