From 9bf49eff8de2976f3f44412310bfec90ee189a53 Mon Sep 17 00:00:00 2001 From: chk <79915315+ChKendel@users.noreply.github.com> Date: Thu, 25 Jun 2026 19:23:37 +0200 Subject: [PATCH] Multipoint Schritt 3 --- doc/Homing_5_Pose_MultiPoint_Weighted.md | 69 +++++-- scripts/5_pose_estimation.py | 96 +++++++++- .../5_pose_estimation.cpython-311.pyc | Bin 43578 -> 48940 bytes scripts/__pycache__/robot_fk.cpython-311.pyc | Bin 17891 -> 24548 bytes scripts/robot_fk.py | 101 ++++++++++- test/test_robot_fk_corners.py | 169 ++++++++++++++++++ 6 files changed, 411 insertions(+), 24 deletions(-) create mode 100644 test/test_robot_fk_corners.py diff --git a/doc/Homing_5_Pose_MultiPoint_Weighted.md b/doc/Homing_5_Pose_MultiPoint_Weighted.md index 1e38473..0a5973e 100644 --- a/doc/Homing_5_Pose_MultiPoint_Weighted.md +++ b/doc/Homing_5_Pose_MultiPoint_Weighted.md @@ -1,11 +1,16 @@ # Homing 5 – Verbesserungspfade: Mehrpunkt-Residuen & Gewichtung -> Reine Wegbeschreibung — **nichts davon ist umgesetzt**. Ergänzt -> [`Homing_5_Pose.md`](Homing_5_Pose.md) um drei mögliche Verbesserungen, die -> alle an derselben Stelle ansetzen: *was* ein Marker als Messung beiträgt und -> *wie stark* sie zählt. Stand: 2026-06-16, basiert auf Code-Durchsicht -> (`scripts/1_detect_aruco_observations.py`, `3b_corner_marker_poses.py`, -> `5_pose_estimation.py`), keine Code-Änderung. +> Ergänzt [`Homing_5_Pose.md`](Homing_5_Pose.md) um drei mögliche +> Verbesserungen, die alle an derselben Stelle ansetzen: *was* ein Marker als +> Messung beiträgt und *wie stark* sie zählt. +> +> **Status (2026-06-25):** **Qualitäts-Gewichtung** (Doc-Punkt 3 / Schritte 1+2) +> umgesetzt. **Mehrpunkt-Eckresiduen** (Doc-Punkt 1 / Schritte 3+4) umgesetzt +> als Opt-in-Modus `marker_observation="corner_points"` — 12 Eck-Residuen für +> Roboter-Links, 1 Center-Residuum für die (spin-unkalibrierten) Board/Rail- +> Marker auf dem Root-Link. Endgültiges Tuning/Validierung gegen Simulation +> steht aus (siehe Notiz unten). **Einzelkamera-Einbindung** (Doc-Punkt 2 / +> Schritte 5+6) weiterhin **offen** — siehe Status-Spalte unten. --- @@ -216,15 +221,21 @@ Reihenfolge folgt der Risiko-Einschätzung oben: erst risikoarmes Durchreichen, dann die beiden strukturellen Erweiterungen. Jeder Schritt ist einzeln umsetzbar und (wo möglich) einzeln testbar, bevor der nächste beginnt. -| # | Schritt | Testbar an | Bricht Bestehendes? | -|---|---|---|---| -| 1 | **(Punkt 3)** `quality`/`confidence` aus `{cam}_aruco_detection.json` bis nach `aruco_marker_poses.json` durchreichen (3b liest es, schreibt ein neues `weight`-Feld pro Marker) | Diff der Ausgabedatei: nur das neue Feld ist zusätzlich da, alles andere (Position, Normale, …) identisch zu vorher — reiner Additivitätstest | **Nein.** Rein additives Feld, kein Pflichtfeld, alte Leser ignorieren es | -| 2 | **(Punkt 3)** `residual_vector()` nutzt das neue Gewicht, hinter einem Schalter (`pose_estimation.use_marker_weight`, Default `false`) | A/B-Vergleich auf den appRobotRendering-Simulationsszenen (mit/ohne Schalter) gegen bekannte Grundwahrheit — genau der Test, der laut Nutzer beim ersten Versuch wenig brachte, jetzt wiederholbar | **Nein bei Default aus.** Mit `true`: Ergebnisse ändern sich gewollt — muss vor Produktiv-Default separat validiert werden | -| 3 | **(Punkt 1)** `robot_fk.py`: neue Methode liefert die 4 lokalen Eckpunkte eines Markers im Weltsystem (Baustein, noch ohne Anbindung) | Isoliert testbar, ganz ohne `5_pose_estimation.py`: gegen die wahren Eckpositionen aus `render_*.json` (Simulation liefert das schon) | **Nein.** Neue, bisher von niemandem aufgerufene Methode | -| 4 | **(Punkt 1)** Neuer `marker_observation`-Modus (z. B. `"corner_points"`) nutzt die 12 Eck-Residuen statt 6 Center/Normal-Residuen | Direkter Vorher/Nachher-Vergleich gegen dieselbe Validierungstabelle wie in `Homing_5_Pose.md` (10 Simulationsposen, bekannte GT) | **Nein als Opt-in** (Default bleibt `"corner_pose"`). Tuning-Punkt: `huber_delta_mm` ist auf die heutige Residuumsgröße kalibriert — mit 12 statt 6 Werten/Marker verschiebt sich die RMS-Größenordnung, müsste neu eingeordnet werden | -| 5 | **(Punkt 2)** 3b nimmt 1-Kamera-Beobachtungen mit auf (rohe 2D-Ecken + Kamera-Referenz), statt sie zu verwerfen | Output-Diff: nur neue Einträge für vorher fehlende Marker, bestehende ≥2-Kamera-Einträge unverändert | **Ja, konkret** — siehe Konsumenten-Recherche direkt unter der Tabelle. Mehrere Stellen brauchen einen Guard, **bevor** dieser Schritt scharf geschaltet wird | -| 6 | **(Punkt 2)** `residual_vector()` um Reprojektions-Residuen erweitert; `load_observations()`/`estimate_pose()` lesen zusätzlich Kamerakalibrierung | Zuerst an Simulationsszenen, bei denen gut beobachtete Marker künstlich auf „nur 1 Kamera" reduziert werden (GT bekannt) — saubere Kontrolle, ob das Residuum tatsächlich hilft, bevor reale Finger-Marker überhaupt existieren | **Nein strukturell** (additiver Residuumstyp), aber Regressionsrisiko durch falsche mm/px-Gewichtung — vor Produktiv-Default gegen die bestehende Validierungstabelle gegenprüfen (wie Schritt 4) | -| 7 | Zusammenführen: ein gemeinsames Gewichtsschema (Quality × Kamera-Anzahl × Residuumstyp) statt drei separater Schalter | End-to-End gegen Simulationsbenchmark **und** die drei realen Fixtures aus `Homing_5_Pose.md` | **Nein**, wenn alle Vorstufen additive Defaults hatten | +> **Status-Legende:** ✅ erledigt · ⬜ **offen** +> +> **Punkt-Zuordnung** (Doc-Abschnitt ↔ Chat-Nummerierung): Doc-Punkt 1 „Vier +> Eckpunkte" = Schritte 3+4 · Doc-Punkt 2 „Einzelkamera" = Schritte 5+6 · +> Doc-Punkt 3 „Qualitäts-Gewichtung" = Schritte 1+2 (erledigt). + +| # | Status | Schritt | Testbar an | Bricht Bestehendes? | +|---|---|---|---|---| +| 1 | ✅ erledigt (2026-06-17) | **(Doc-Punkt 3)** `quality`/`confidence` aus `{cam}_aruco_detection.json` bis nach `aruco_marker_poses.json` durchreichen (3b liest es, schreibt ein neues `weight`-Feld pro Marker) | Diff der Ausgabedatei: nur das neue Feld ist zusätzlich da, alles andere (Position, Normale, …) identisch zu vorher — reiner Additivitätstest | **Nein.** Rein additives Feld, kein Pflichtfeld, alte Leser ignorieren es | +| 2 | ✅ erledigt (2026-06-17) | **(Doc-Punkt 3)** `residual_vector()` nutzt das neue Gewicht, hinter einem Schalter (`pose_estimation.use_marker_weight`, Default `false`) | A/B-Vergleich auf den appRobotRendering-Simulationsszenen (mit/ohne Schalter) gegen bekannte Grundwahrheit — genau der Test, der laut Nutzer beim ersten Versuch wenig brachte, jetzt wiederholbar | **Nein bei Default aus.** Mit `true`: Ergebnisse ändern sich gewollt — muss vor Produktiv-Default separat validiert werden | +| 3 | ✅ erledigt (2026-06-25) | **(Doc-Punkt 1)** `robot_fk.py`: neue Methode liefert die 4 lokalen Eckpunkte eines Markers im Weltsystem (Baustein, noch ohne Anbindung) | Isoliert testbar, ganz ohne `5_pose_estimation.py`: gegen die wahren Eckpositionen aus `render_*.json` (Simulation liefert das schon) | **Nein.** Neue, bisher von niemandem aufgerufene Methode | +| 4 | 🟡 Code erledigt (2026-06-25), Tuning offen | **(Doc-Punkt 1)** Neuer `marker_observation`-Modus (z. B. `"corner_points"`) nutzt die 12 Eck-Residuen statt 6 Center/Normal-Residuen | Direkter Vorher/Nachher-Vergleich gegen dieselbe Validierungstabelle wie in `Homing_5_Pose.md` (10 Simulationsposen, bekannte GT) | **Nein als Opt-in** (Default bleibt `"corner_pose"`). Tuning-Punkt: `huber_delta_mm` ist auf die heutige Residuumsgröße kalibriert — mit 12 statt 6 Werten/Marker verschiebt sich die RMS-Größenordnung, müsste neu eingeordnet werden | +| 5 | ⬜ **offen** (Vorarbeit/Guards ✅) | **(Doc-Punkt 2)** 3b nimmt 1-Kamera-Beobachtungen mit auf (rohe 2D-Ecken + Kamera-Referenz), statt sie zu verwerfen | Output-Diff: nur neue Einträge für vorher fehlende Marker, bestehende ≥2-Kamera-Einträge unverändert | **Ja, konkret** — siehe Konsumenten-Recherche direkt unter der Tabelle. Mehrere Stellen brauchen einen Guard, **bevor** dieser Schritt scharf geschaltet wird | +| 6 | ⬜ **offen** | **(Doc-Punkt 2)** `residual_vector()` um Reprojektions-Residuen erweitert; `load_observations()`/`estimate_pose()` lesen zusätzlich Kamerakalibrierung | Zuerst an Simulationsszenen, bei denen gut beobachtete Marker künstlich auf „nur 1 Kamera" reduziert werden (GT bekannt) — saubere Kontrolle, ob das Residuum tatsächlich hilft, bevor reale Finger-Marker überhaupt existieren | **Nein strukturell** (additiver Residuumstyp), aber Regressionsrisiko durch falsche mm/px-Gewichtung — vor Produktiv-Default gegen die bestehende Validierungstabelle gegenprüfen (wie Schritt 4) | +| 7 | ⬜ **offen** | Zusammenführen: ein gemeinsames Gewichtsschema (Quality × Kamera-Anzahl × Residuumstyp) statt drei separater Schalter | End-to-End gegen Simulationsbenchmark **und** die drei realen Fixtures aus `Homing_5_Pose.md` | **Nein**, wenn alle Vorstufen additive Defaults hatten | ### Recherche zu Schritt 5: wer liest `aruco_marker_poses.json`? (2026-06-16) @@ -259,6 +270,34 @@ Homing-Seite selbst (`editRobot.js`, `homing.js`) ist bereits robust. Alle anderen Konsumenten (`homing.js`, `editRobot.js` → `assignByZRange`/`alignSetToMeasured`, `scripts/9_evaluateMarker.py`) waren schon robust oder sind Offline-Benchmark-Code ohne Produktionsrelevanz. +### Umsetzung Schritt 3+4 (Doc-Punkt 1) — Befunde (2026-06-25) + +- **Schritt 3** (`robot_fk.py`): `marker_corners_local/_world` + `all_markers_world` + liefern jetzt `corners_world` (4×3, in `corners_m`-Reihenfolge). Orientierung = + Spin um die Normale ∘ Minimal-Rotation [0,0,1]→Normale (exakt wie + boardViewer.html). Verifiziert in `test/test_robot_fk_corners.py`: + Selbst-Konsistenz (Center/Kanten/planar/Normalen-Rückgewinnung) **und** gegen + echte triangulierte Roboter-Ecken am Seed-Pose (~1 mm RMS, Identitäts-Paarung + schlägt jede Umordnung). Eckreihenfolge = `(+h,+h),(+h,-h),(-h,-h),(-h,+h)` + (= boardViewer-Zeiger (1,1,0)). +- **Wichtiger Konventions-Befund:** Die `spin`-Werte sind nur für die + **Roboter-Marker** kalibriert/visuell verifiziert, **nicht** für die Board/ + Rail-Marker (Set A0/rail, alle auf dem Root-Link `Board`). Deren Eckreihenfolge + liegt ~90° daneben. Lösung: `corner_points` nutzt 12 Eck-Residuen nur für + Roboter-Links (`corner_point_links`, Default = alle Nicht-Root-Links) und 1 + Center-Residuum für die Board/Rail-Marker. Da `Board` Root ist (Residuum + konstant bzgl. der Gelenke), kostet das nichts an Information. +- **Datenbefund (nicht Code):** 6 Marker des Board-Sets **A0 sind auf `Arm1` + fehlzugeordnet** (`[55,56,57,77,78,99]`), ~230 mm neben dem Modell. Sie + destabilisieren `corner_points` (12 statt 3 Residuen → falsches Minimum) und + ziehen auch `corner_pose` leicht. Auf bereinigten Markern konvergiert + `corner_points` ≈ `corner_pose`. → Marker-Zuordnung korrigieren (separate + Kalibrier-Aufgabe). +- **Offen (Schritt 4 Tuning):** `huber_delta_mm` ist auf 6 Residuen/Marker + kalibriert; mit 12 verschiebt sich die RMS-Größenordnung. Sauberes A/B + Tuning + gegen appRobotRendering-Simulations-GT (klare Daten) steht aus, bevor der Modus + produktiv als Default taugt. CLI: `--marker-observation corner_points`. + ## Offene Punkte - [ ] Keiner der drei Punkte/Schritte ist priorisiert/entschieden — reine Optionen. diff --git a/scripts/5_pose_estimation.py b/scripts/5_pose_estimation.py index 822faf6..e6b9216 100644 --- a/scripts/5_pose_estimation.py +++ b/scripts/5_pose_estimation.py @@ -20,9 +20,15 @@ Four switchable methods (robot.json -> pose_estimation.method): global_ba : all variables jointly, position + normal residuals, robust loss hybrid : sequential_fk init -> global_ba refine (default, most stable) -Observation input: - marker_observation = "corner_pose" -> aruco_marker_poses.json (pos + measured normal) - marker_observation = "center_point" -> aruco_positions_*.json (pos only) +Observation input (robot.json -> pose_estimation.marker_observation): + "corner_pose" (default) -> aruco_marker_poses.json: 3 pos + 3 normal residuals/marker + "corner_points" -> aruco_marker_poses.json: 12 corner residuals for + robot-link markers (4 triangulated corners vs FK + corners; no separate normal), 1 center residual for + root-link (Board: floor/rail) markers whose spin is + uncalibrated. Robust loss acts per corner. Opt-in; + needs `corners_m`. Links via corner_point_links. + "center_point" -> aruco_positions_*.json: position only Homing integration (appRobotHoming, see doc/Homing_5_Pose.md): --from-state seed/init state (flat {var: value}, or the @@ -89,6 +95,14 @@ DEFAULT_CFG: Dict[str, Any] = { "min_cameras_per_marker": 2, "finger_block_joints": ["b", "c", "e"], "per_link_method": {}, + # Nur im marker_observation="corner_points"-Modus relevant: welche Links die + # 4 Eck-Residuen nutzen. None/absent = alle Nicht-Root-Links (= der Roboter); + # der Root-Link (Board mit Boden-/Rail-Markern) nutzt ein Center-Residuum. + # Hintergrund: nur die Roboter-Marker-Spins sind kalibriert/verifiziert; die + # Board/Rail-Spins nicht — deren Eckreihenfolge wäre unzuverlässig. Board ist + # zudem Root (Residuum konstant bzgl. der Gelenke). Explizite Liste möglich, + # z.B. ["Arm1","Ellbow","Arm2","Hand","Palm","FingerA","FingerB"]. + "corner_point_links": None, # One switch: if set to a link name (e.g. "Arm1"), that link's # jointToParent.origin Y/Z is fit together with the normal pose (same # global_ba solve) and the result is written back into robot.json @@ -111,8 +125,12 @@ def load_pose_cfg(robot_data: Dict[str, Any]) -> Dict[str, Any]: def load_observations(path: str, use_normals: bool, min_cams: int = 2) -> Dict[int, Dict[str, Any]]: """ Load marker observations. Accepts aruco_marker_poses.json (with measured - normal + num_cameras) or aruco_positions_*.json (position only). - Returns: marker_id -> {pos_mm:(3,), normal:(3,)|None, link:str, n_cams:int} + normal + num_cameras + 4 triangulated corners) or aruco_positions_*.json + (position only). + Returns: marker_id -> {pos_mm:(3,), normal:(3,)|None, corners_mm:(4,3)|None, + link:str, n_cams:int, weight:float} + corners_mm (aus `corners_m`, m→mm) speist den marker_observation= + "corner_points"-Modus in residual_vector(). """ data = json.load(open(path, "r", encoding="utf-8")) out: Dict[int, Dict[str, Any]] = {} @@ -135,7 +153,14 @@ def load_observations(path: str, use_normals: bool, min_cams: int = 2) -> Dict[i nn = np.linalg.norm(nv) if nn > 1e-9: nrm = nv / nn - out[mid] = {"pos_mm": pos, "normal": nrm, "link": m.get("link", "?"), "n_cams": n_cams, + corners_mm = None + cm = m.get("corners_m") + if cm is not None: + arr = np.array(cm, dtype=float) + if arr.shape == (4, 3): + corners_mm = arr * 1000.0 + out[mid] = {"pos_mm": pos, "normal": nrm, "corners_mm": corners_mm, + "link": m.get("link", "?"), "n_cams": n_cams, "weight": float(m.get("weight", 1.0))} return out @@ -268,14 +293,64 @@ def model_markers(fk: RobotFK, state: Dict[str, float]) -> Dict[int, Dict[str, n return fk.all_markers_world(T) # mid -> {world_mm, normal_world, link, local_mm} +def _resolve_corner_links(fk: RobotFK, cfg: Dict[str, Any]) -> set: + """ + Welche Links im "corner_points"-Modus die 4 Eck-Residuen nutzen. + Explizite Liste in cfg["corner_point_links"], sonst alle Nicht-Root-Links + (der Root-Link Board trägt die unkalibrierten Boden-/Rail-Marker und nutzt + ein Center-Residuum). + """ + explicit = cfg.get("corner_point_links") + if isinstance(explicit, list) and explicit: + return set(explicit) + links = fk.links + roots = {ln for ln, ld in links.items() + if not ld.get("parent") or ld.get("parent") not in links} + return set(links.keys()) - roots + + def residual_vector(state: Dict[str, float], fk: RobotFK, obs: Dict[int, Dict[str, Any]], marker_ids: List[int], cfg: Dict[str, Any]) -> np.ndarray: - """Position (mm) + optional normal (scaled) residuals over the given markers.""" + """ + Residuen über die gegebenen Marker. Modus via pose_estimation.marker_observation: + + "corner_pose" (Default): 3 Position (mm) + optional 3 Normale + (×normal_weight) je Marker — wie bisher. + "corner_points": 12 Eck-Residuen (4 Ecken × xyz, mm) je Marker auf + einem Roboter-Link (corner_point_links), KEINE + separate Normale (Orientierung steckt in den + Ecken). Mehr unabhängige Messpunkte, robuster + Verlust greift auf Eck-Ebene. Marker auf dem + Root-Link (Board: Boden-/Rail-Marker mit + unkalibriertem Spin) oder ohne `corners_mm` + nutzen EIN Center-Residuum (3, "ein Punkt pro + Marker") — gleiche mm-Skala → ein huber_delta_mm. + """ model = model_markers(fk, state) res: List[float] = [] + use_mw = bool(cfg.get("use_marker_weight", False)) + obs_mode = str(cfg.get("marker_observation", "corner_pose")).lower() + + if obs_mode == "corner_points": + corner_links = _resolve_corner_links(fk, cfg) + for mid in marker_ids: + if mid not in model or mid not in obs: + continue + mw = float(obs[mid].get("weight", 1.0)) if use_mw else 1.0 + mm = model[mid] + oc = obs[mid].get("corners_mm") + mc = mm.get("corners_world") + if mm.get("link") in corner_links and oc is not None and mc is not None: + dc = (np.asarray(mc, float) - np.asarray(oc, float)) * mw # (4,3) + res.extend(dc.reshape(-1).tolist()) # 12 Werte + else: + dp = (np.asarray(mm["world_mm"], float) - obs[mid]["pos_mm"]) * mw + res.extend(dp.tolist()) # Center (1 Punkt) + return np.asarray(res, dtype=float) + + # Default: Center (mm) + optionale Normale (skaliert) w_n = float(cfg.get("normal_weight", 30.0)) use_n = bool(cfg.get("use_normals", True)) - use_mw = bool(cfg.get("use_marker_weight", False)) for mid in marker_ids: if mid not in model or mid not in obs: continue @@ -646,6 +721,9 @@ def main() -> None: ap.add_argument("-robot", "--robot", required=True) ap.add_argument("-out", "--out", default=None) ap.add_argument("--method", default=None, help="override robot.json method") + ap.add_argument("--marker-observation", default=None, dest="marker_observation", + help="override robot.json marker_observation " + "(corner_pose | corner_points | center_point)") ap.add_argument("--from-state", default=None, metavar="JSON", help="Seed/init state (flat {var:value} or {accumulated_state:{...}} as " "written by 4b_revolute_angle.py). Used as x0 for global_ba/hybrid " @@ -656,6 +734,8 @@ def main() -> None: cfg = load_pose_cfg(robot_data) if args.method: cfg["method"] = args.method + if args.marker_observation: + cfg["marker_observation"] = args.marker_observation fk = RobotFK(robot_data) obs = load_observations(args.markers, cfg.get("use_normals", True), diff --git a/scripts/__pycache__/5_pose_estimation.cpython-311.pyc b/scripts/__pycache__/5_pose_estimation.cpython-311.pyc index eaf25667b6d96cc241a65f28b0defc9e41f4dcb3..34c9dbe4066878ee8f6740e041ec6bdc60cce4bd 100644 GIT binary patch delta 12616 zcmcI~Yj{-Gb?7F{(#u#TjnsX#Znuog283Fo6 z9zpC_VA)pK7zBn!xA6lfxFJ>2`s?Sm)!TOu9Gx%+q7%# zGozUy@Fjol0j=3*@3q%nd#$zCUTdHIyK&8}pKI#huvl_AcpP7NXheLGYT;kub<6P@(IM4}McV*hJS3JrqdQ(F_KO3e z@QnWW8u74L12t>KBjOtPtrw4qPWW|-kBJTN>k^NNt?;`}d|ccBzYXGXaTEMDicg50 z@Y|Hbi6_J-#jZXe)+~NO?1st~(Jk(R-&V2r8SZ$S_>}lCytOk_kGKagJH$cpQK(xl zdc_0qyFuD09vs!+j7Xk_lQz(5O_@#%e>HDmtL8tn8o@~eIn}KJ4;ht+J2WUuWZa|p zLP1w&t{~KSLnH|GV-iE2Ez}2~rT`F<(&d$v9ev5Dt1he%~MtA~p;Cp~0}M2>y^P3m&f` z3u7>nI=N=yz?jnH3vR`J2Zf*{iL&sN+9taLPc;ksVWKb-pC{P^WDfFj^mrk^|wr_0OF*Y^;W+V|Tflxxq2~V=HClv4nhfmN$)<%5`(39yo>m_p@l(Zbj zfA*f1uC_(_R~P=5ty7~Dt&@55smijZg4cPD`#Suo&!i<{IoqI^Q|}x%Vn~%E7Wx%? zRegRWe}pY?tQQND3qT*q7xO3d95gj`-@l9 z9q4qfZ|rbI`a(ge5iFvTE*CnYjlwC(H+)j*1mp84lh`d~&h9`!aC*X7CetVcKK|0h zK)?kiDfz(eL@AiG)y#GL5Tlb`$ZQcdJsJ{$8d!2_!Mevyl2-{4r>mLj9WO+4R+x2Z zKGDIvKLG`C2_4#Fg6~ckR9^ocD&bk~b{P@|us>jw?1qX&?n?gvc?2sALATcvkjZ{{ zahVgk5jhkDav_hH(1pgNAZdgKvH?*w!7;kzsEj7`@=4E_s9uah4pq_*_ z6jl;^AfXNTL~;=8wZK6_8zccBF%Ex0asaC}UO@5?B3t1NrJC`OX2h@~ADL2{H=Vb8Vs)G2b(>?=TjJGQmh~FP z)@4pp=-4`~o$fhrI&WGwaCz3LM@Z|!4~vVm=FPx7*@B>x^5xH#ABEzZ+JvULHNk@p z$7R$gh=MfJ`SSJrx9MM$Z;ZA=oy$lLVYhw+0{{}aKG_$P6;IGBk#$&$%1Y>ciWHDt zT2(#Au}Vukr_yw>#>zx1gTr$J0r0&9YHz}yJOlvbTFm9yX12V1c&=_gY!$Hru3DF{ThW4jRVJ71PC%d>ZuRMRd={rQ91WvElJKEIG_Iz+Y!LTwR1#zHWDF{do&@~#NWtA;k4gT+0hh`- zsR3zuK^Q9!Y#3Z2Zck{sFY{NplOXjcJ5GQiC;3TDL__DFD|U!n_AW(A)Q0H24d=Dv z{5RSn{6)>odXPL%qCoado>{g_M@Hd|yo6vD0ot;R8U~?GF>m#G&NeioP++WU!s!MF z9`cWa(W;S}8ZT_~hafPKyP5$rs-kwljN~Ig=Vw#gGRIpU<}=}!chBye(=0iPuk60K zdv0^gQ5SdAEo%{N$+_+uhOZfJo{l*;$DNz+=og*6@PAp$<=d865#C7BZ!jkwLcT8` zcojgDO@ZLzP3T6YGqOvsu2QxlOcEN3*QX@(7=_8|d{N>sUtzoIo{R1~OL7aaoH(sZ zP{3*BbGE{lPF*;)Ske%)HpZ=uQ+w_jqxMbP+=Oj%zwf0F&&wHC~7K0i4# zxoE0f%FCbGGu6ASxmPZ#jOL`Tj9weP8n_-%-#=yY`!R!4y+1PNPlsQcxG-@(G82iJ zE92(Mxp2%}x2UT_vCx}MRdt`|2FIF%B5XpQGhUos>cOSS9KfQ1{=cT*(#-2aXVEX>N!(FKeiRn*-e(ZATJsx%@KVj0)$Jy(4o+jZSD}1 z@faf>f53f^l1k3KuafW8 z*{`hhW>x7AVEO-tkotKpQb2^x3DxqDUSX&WQ(dRg}g~sjl_G1exmWXf;XO zDO^!tVYULJOo4e1826Vb&{Z6M1J*O!WL7rDci$O=?F;v*VQE+zl!CBDsoPSsphn`D z_d)O~xg}ZY@&#B1-mGS9nPF*XE+c$zV(#3POv^DNr!Tga<#Y&rEFqUL-;4^*cdw`k zx?2s~UBZZzXL8@SR3@oCncipftdraO}`VpF?%3i&)W1MCx>^K(7RWJ(`P0i zD}jt8P2Ce7%B8<$tZort%asC{BZqL)#jRa%CO55&R2P!;eZ718dXev@bOxWCjo>^$ ze2@pjHXjZS3lLa%M-_}MV7td2Eh;ZA5UX^Oz@G0JJo)ac!C~JpOjnZSF^CfssS(c> zAXAr!I?7Rr_%U%GCX#OmqLAf=hr1g$SXOK`G-HAn+7+Xif%F0>}Y_l*|}KaCS5bH5k1d#Hk2lB!pd}NiJ(# zOr67i34$mIRM9j5agj%WL=ojLoD72@x*Z<}TzgfP~Q_vt5)t&)y*XfgCX^B@9Z)50R$JK`=O1BaFAO^qUA+ ztf6NCI^-yl45u7;ByuWYf+H_C)+h2ZBPjADQr7^2CIH!F$P2&lC*t2&LVL;`Oz4q9 z5Z;5b10!|1!{msn@ro5jV8>wYah6n*KZf?r7^&m~CgeOdoTYrOW=@Lb)y4Dbrh1o3 z%BFhmTI{p6SL!d;FP1mm495yK#0xgWEF0sNjZ?cZSn0U3>Efovik90|vBK_nVRy{B zJ#O9pp%s$J-7)K)xOLA|bk7fr*4gg&jWsc2&E1m5oBg*7VCy;6Cxa?T%(*&E{a2COZ-Qhc#^&Kk2j<8~)h zSnV&3Ul^YqI{);{)AN;!)+YFWyXJQDo&GytxLaI$X=J|OwIi|OhInzqTZ6Yo-Y)pP zBeCY5cymvzxaSUu744nXEZGXLn(X4|axit(ax&U(`kvo*(U&9U5; zcy0?_ZmTim=<7b^a%gFL?SZ9^EsK_&FC3Ze0AX#uvhCuwc@yOJh0XE8=9r}=ZfRMx zw7e}(YwqUTW=7@;<{r6T{Z>&dza^gEa=Z4n_gl`lH^BDadZ8muAa2gAGa^m`oQKOH(!T_texAoRg-h~5D5j)Xh{qZLR7PUFJ$BU*Y7Ztv$VtywaIfpX6Ymd z74+KHRrI^%Is6n|?5r)tOn}rP7zdCrgotk#(&@d_y!Ck9Uts+k0Dv<_F@P1{1(3|8 zA;qE3Z{5VdNZ;MsYG$;aMwA}<^R1r@vLd8n*c~}k{Q|BoS1y}VLL)1T99!|K7eEJ) zFb0#y56mZgAN!&pWQZj>NUV|%48Hdv7yo4{KNK!!gI6M^4M7zGJA!Je>3SiGf>pDe zH?f3a9ce(&hyc?z5<)P809_Zs>`Ud?f+cp=r44vat5dlOb%~tb(_U$eoxc}k_%Nok z_pwtGVwpjv)Y@I${MT6ZLj)gVy%thI=Kc~|>9kjs19N{LAjP~tsTa_G)Npu9epnHYjBeTyR((mnWv@sQ(MM}>ihyaOB7aIz}n@Jlxfo-Vw zw2fiSU&7#Pb8*vPx%7MV#-sHHMsXW`@6oP0TtMo?Gi~)}S#)S)-xmJQ=ulrP>*>`| z`>2E7?mKjhEkBK_T4tao)j$pI!Eq0PV@mg6AfX4r!%_NO90@g>R=JF+5zLv{raEy{ z_Czh6f80S2_T}k@!oejvd7y(G*%78C6fK!RfEw&eXnnyUbsN5)85bZTTTws2(oX>V zJ^aB2HNM?dm|@UkPzML7en0sKD5(d4%&M8CM^WKq8o^Nn%(m0^{*O?X(0N0!F44lt z|3&a;08)o_s2K8Z2$%#=zUmf^dQIpD;V5I2%wgFys7A!HYS}1{ghB2`OwODr)#GXe6MAGbZnh@ z#WD*ubn)@EZHzuTKc?(HrZ8dzR$V!&veG(XzUiy9{`k&rRGur-m!w6_0wuP3(hTPU zJtl1uz>!{}H;=c}6hNhn<{~`BOfmzgexMvAS8gP$0FTowls|EqSC?ivQmaHzwQ%Re zS-tsNNVE^YCwh9od$LtsB?%q)qLbjhe8J?JP4K6fxnwVWo~M81ZPq-bp$=~+I1He$ zCb`bmz##CMDkPJ7DBZ7HvWJ7m%k(cc}d)$G&I zzaDK?_51;LOBy#0w$Z<@mDQou!@hx*ncDI5Q{&ug^T0QMkp7jwQ}t*D#QWub%awm$ zuk=&oh^f(nrEGsha**~t?#RC4eD`vMErYYzlQ|`}QrKpjhdr$b@Wh_j5!~ZZGNegA z3Y4n~Eki^EJ2eZYV29E03=)6#S8xRl<7UEajQR7d1;v9Nh`DbT&ak{I9isu~z6nXS3N#3K|23k3 z^N~D3b=r9=Hjud+F^|bYFk}HfSLz4ladLz9M;nq~j#k1cJi{SR0TUM%E=PCcwC+xM z3|XJz_C9y6{zIVS;^F%yTxS@AkL;s3fmZ-zMMr;0|Mj_6{yVgG+Lh1rj;rZ&z7b;S z|WuJ>Gt5P^H zP!da9C066ddHUNg7FO6&?ZlyA>XCeax{aPC#ZEZiKnHpB0UraBZ5?f5dv+W2uZ8{{ zla{PWuaD$_P&Y(O>S(KT#Eseg#7(d0#LW>iU~NfJujssYWN6(Q$pg5J2ESb9&eY~Z zFsrWgtN^DQYPP3q#2v5bZs_iniI1DiXL5P=fpT$Zle4Y(h1pJ8BUVzJnw9GpW`$N4 z$q_T;8nLEkSJWcz%$7l_<>5}TC%Z-5bscW5L~?Ky&3sAhRT`Lz85k)}mBrmz^(FK> z6D72Y+74w{dBh-cVIF8@$tGKN;=|XClQvj7r76l|I~4S_$!DaTevMX_W>=*e=PSXS ziZvq^Ty{ZmPxeG6?MhAx6E5vkrOFXo*2gKkxECGF>yY++9sblutiJU~b`7kPOcU5g z4_+$cnFrzP=+vbWb9Jh3(#U`3Qi0lDxZlTRbEX=5?W5TSGFhl}r)YrZDO9$r->BA+ z=au@*hU{))U-G`ugwjJtUap89NZVngP^?bB4QaBcMB1~abTCo?mV5~M^rZVhO+VCR z&_vs2bs3ad-N20;FElyPH_9U)H)gy7;A@rjapfv*|w`P}DPQpVXoT&jy? zU3!!8J>#~U3qnU2I|sI25q4TKMW4KKw+XpP7z2{xff$6bMK;4!k~dzk!~zBs99cz& z=N>Vi#v*PH2l4vtuA#UDTkG6qNm9zpbd*zhovc2ATF!V^dO4(uH0?LWF>=YHzHTF8Gu zr><7;d+7D6jorrpYa%yw->_34k0F5{A%Pw&VSw$@C-jWBgf1NPDU7l@&>m{McG$qW z9HX9V#k`ZAzE))$$L`-puouD8^tEd{3~J~=|LIzQuc60Z*$_R1ZT$!a0N}TH_)J4u zu!2bueF4Fs4(>$(Ku@HGUhEJnmmM3XulJBou$@IB7z@Ew9A7|6m_T(ytYjDt;4gUs z(BN=gCeb!5=}`2LMfph31Nd-DH87TOkahuyVvOQ)K#D&GX%rcj+5Noy3<~lPGNn~K!(zf4!=h9S%T> z3a6N_0si+u4|0!z{`vLjx<$vj+q)LK2jJ#F`5qqbMCcu#avJ?!{>Kc${VXWMy)3ErOU(oG_yC7?ki8=)6 zZdeJjDU6I=CiT`83xb)Avj~1YqM=UI4jU0umx0O1NNTN&XiTR`;!!|0s1Y!$W>GgH ztHgdCyJfaFVN;sBSFt@$jn!GN^Z;)mCNiUbjrj(a?&*CSZi?WnSe*U85rL(qP@T+A z(~L0wlc73eG)+F;`+e-l`rI=R>#bu)nQ98bX8#$WM~M$0OyPcv{~OX{lTFh@J0t&& zAT8J2U%7W?A9h|4WpZK1B`9llWT{FG&kqO_1?wchD`+CkQd$74y&K!_&&UIrhp937 zv!sDp70Vo032#FY5dkCL0nCBRW93O4YE#1Kc8eje+nvaDyNAL`m`H9n!M&SY#BR*J z-~kGG1wjA+oSU4H$sCr<-Vj{;@+$P!YujsBrq0}f0j?av`3zGMu8JX_AE`!Z{_9QA ze*oGE^WFel@l$#U2@$deYpia!Cm0ODMMJm;Edw@lVInd@5L`#_RRp&X{4s(Uf=?0r z6agOosd*$jgkgz0rm8HhWBv!c0Qgd53H=ld@*(!az?LTBUe(;}@ z9|u{$bt;};)@|f<%iIbGF8uuU?YyR}nm%;1otCyk4YXxWCNOjF`;sMj z(rN#Z?tkyTd+&SiZg2VLzA#?8YHWBeGt+K>XZ)eIk&h1=3?E=6{y6jk`P6Z-VJV4Z z(QKMSbB7Id-m}DKpeC9(ozY)S^Sw2+U29{N}G zb|@^QYwkDnx6=D)7u4DW=}x*1NEcBL-2}LHx{KZc@5SCFv}cbIXTcmsgLm<1D4oTs zcnC~kyZF1Lu}AIE^~V1;nL6xFzV~c?_DW~B3KpNNRy$i(@r!4Q9ZsDuDEkk%qMzH&TS*2#n|&+)EPD?h z&$&@w1EbNbW2013y_#v*tGbM=7ICJ*p|_;|2S{4W#xoLP3KjGHPLN(c}8p47ETXa_{R-}{3pxjm4?%3{vnuz9<|RS^0mKU6rx&!4H_^NYF*{62Y4hoZ7(r|cP&0!jx=X@V~)ez@L@ zP1i}M%dA;JA)Y{htp!P}3xHFT-Cs zKn#XS!yg<5N6yh(|F&Rz{g&vd0@ z^VN>cZ{4%=tvnC>9yC3;=7{Zx?OI0GWH)n1|Ea9Nl(7_qvs(}>6+P!C=-+R zDPUNfuRhIeF-^tmr5+Ff?llap55r#>0sxaNoSoz;)AKy&yqmbAZ#Y-mtsT(LR`9H* zO{9bOH;os25g~fEN~5D*nX-1owDMmywG`v9T*YiRVjvTMebnG+knQI!&D8}1h#g0; z55X9M5U0(H$pQXEbA5?0NTwu9{sSSeXK=UVli6_~dM0|U`M8<5`AFMSWGA;Rs&$JF zPC%PR`ch|_83gi zR*n4d$}*D939B$!PgyysD}cy7UR}!{-H_Ex3}GAF6E-GGqG<{C!}rnjuzlRXb|j=H zb*f9ROTr{GhwKJJI4uP8n{b4!{ApGk@7GEUN8iyU)`qRBNN)Nx&e=~x?d&WDVmc$# z&EH&A64CRNvr!q8{9fvEGOyyJW0GHS2FJb3sqXeVhkfH-*%?#4o0Wo0^fP!?-6mqT zPqXa{GC##|Uk_*z^F5kFm+cX4Y&A5vY{GJ^2Z_+xSfmZ%W&m26qzKcoNvv7bpx>vc zu1tphpu64^Sb7oxnp?97pMbFNLQdG{ku}Sh;`PX^4;zgE$PsiD{<4w@h@=!T8$H0d zL`>E*?j2NvjJ*JanUEfoc0ET}4Vk4=)l=Rpj@qk^+R3hKMJ1DK-m_=(j;>`^o27OJ zoN#|vP1Spu*+=`Qj8h%cS?4WRGTm1*-52sMY=617M@jzXz0y2+P3$cOjLC{_{|Z z#p@(%<`uoSkWPMQZ)=8-rXbyFeysPy0a46@I?-`(OOw{y$Zu~N}_8B*pTC`XWspn^ZP_2CHWmqh?* z<~`m63UgrHg+O>F<~c?Y97cdH%I-iQoHr9o?EqkLlJ@I!c?7HJUHb>UqqyX_<|TQp zu-(hZBwRNm$oSLSD&7BxnC~F?E;5^xpkMgAX7cVI)b$|z{aZ+Kng4cM*?eL3d0s{J z1t8;lhW)_-$?qAE*pGpxDO$Jv+a{X~MR1r`_{?2xOI|_VZy<)Sx8bD_0I>yRo+mKo zS0TA- zO_cDTdD<#toe70{kR^H1it&td&#vYiVIr}1gs_vnAfm&C?4rLHVm`JV5oq^>k0ItY z=)9h9>P;sHxm#+m3Sk}mUTIY=hLAqzWHcU&%n6EAjfzH0-6O>4ZDihu>&+^!p4dXsv^yRGwMNx<_*>Gwm?4rVj!Cw;aP+2 zVi&-KQBadChyV@NqnUj2kjRtlW?nlF2^9q2M(`5=zl1-m^W(R<@{-&amvYS{`Tgt% zP}27l!iR-lN6@}(0znS~;mHZ_eiLw-c`yiJ_y{)tD}omR#Mghc1A7;Nm_F)MPtj;b z%`)H*4(?$OW7#$kU#`z15h_EoD!h{xEVzJW^8k3UmX6Sa9YXB4vL7MoMFcM)7~qF! zomKdJA?ndKw1<5L4c_Q?=_7x}HG}^4_DkUG?0uBvMj%{mKbFK^MRz)^3cU(@$moL= zvp&c^fabY=7xXVczcYS^a4AsVCPe3<7&L2-Fb(?+6kX}MSrXb8NoR_`61cq+&6JWO z5=s?I-kh<1gz{0Zddj_@-zqOO{?WikGebVv4JpVc$Kp;S`-BgqhVL&7Jh0Dg{D|iY4*O5UJ%S*Sb#iAD>_!6d8)Twe(XFJI~x!9{66&nTM8`s(X21sPr%9f(R(ap zjQ{rdqH>YlXEU42qG$Rd9L!SQaiS`69E+_8#8?lB7Ssr)A(5%YzC`S-#b%5B3y{D- zeIc(N;phWU-^*0UKwe4KXX%(Vd-6IXGUOTT1BalQgO&khoSoo5JJA%I*6h9ruV&a< z7{u~u4PS@T8hK2zrX14We)6vyFp6D7v%qra8->f2T`2q*fRt^{YdmtYl`wwcq-&!v zHAd2P(#*bzw+zj<~o!ra*85_Tkzyae3*{k)vbajWb4~{jPc=dFf z^)NQ~@Ez0T8C|HcbqLn;$?3MTd$8CCK*<0iv9ZtrY;u1%T>&>027?s+X!;$awbmZL z=!yRP=`_MipXsyY(}Ht_ysEr{AA4p+TG0xSp%(DlDof{2m{n9?>pd};O{99FpjWVHz39DL@ zXrg6d6D=y4MVvIya?pNCu_E3}ZSiTC=vnyd&q>2m*07aX==`u1%0>`e8RE{B z`KH*gBFR+7?d74+&%>9zVPQz{2Bs15P%U*flj^>FEM3N9VeXhVVxG=6+{A@BW4L2e^l2^gshX0%`;BW%}gQPf16 zQ*8m~{#aXk3w5Wq0e>a%FPz9snRIJ79dz0jwu#<~Di@`4(e{&Ox;UHxG)v;L%a+dV z7g}!#I{+@@y8rPpE@&T9RiVz^4HJ4*MdbYE#OrpSi~8{hNU1b zcDsxvu>|Ab*?j)}bKMyMELI_SgIAt!vvwfQ34YgkXV)Wu)yM&jgjgYB^N|W2mlYzw zr5_ePq|y=iB?NZcu#I*j`8sT9;+M}iRN%%kWM1Z8G<2OfL-6s!*%&+Ft%Nn{f6g~| zt^kgZJ$_f%;bhBE#P?7{1D0^9bXhcuxN6YMW3o?WP1qp1YUU$f>#GnfLQoD14g^*G z-p4(n1ZC#n?|!W!XAjD|j6kfv3b(zu+N!U%y!XWbsp7A`xa7`_NYIU769CO3gs~Px z2<_pjD!vMcjhnuIX-7)CxJyf1sIwm;9wgAFUhxRTK?7zXr?23Qbv zv;=@=QYF_g!#f;Zabhes_)y{*h2tB03wuM=C}?J0m6l&5jtO|WgM0S}fiD6^P;OZG z@{1AvtBY!M>?Maq?Cmw12QJusvd7~x>jSJtX>|y&1V;zl+Q20)&RgI02^SPQ)J2Fx z{pk#gka*xg7b^dA!rN`m{U1X}FukysbTk88*^?BM7#Fh7UL_L~pnPYz!Aj*W@^EiP^PCRdc!WA__~VSB85^dv||qALlzTB@aa zJTy4y@o09BXJ|}?kE_{at~ z4Z<+!=|et0N>cfIms%n}05UCOV*oyCsa=f0rvS|BTDHd{$#M`rt>Gs`pcdI<2g<~Q zn)r;Rf6;gWt5F0$MxY`141qWY-N4f42r%1jz7Mw~F3p>*B0 z0zPq!`Ms;y6!5{%pe%=}&zJ}yGv-!eo-xdVf#|8ox20j+0Mia@NF{Fb0a>U&PtjZ*Jks0_}k_!2guP)5b dA&oPJR8Y@9dG+7Ry9wz9rklO``6sV+{4ew9i;4gM diff --git a/scripts/__pycache__/robot_fk.cpython-311.pyc b/scripts/__pycache__/robot_fk.cpython-311.pyc index 0830b4a7cb01108b46942ebd1f4aa7b08a5441fb..419ce8200966cc4dbaecb9db0cd61b9f777111dd 100644 GIT binary patch delta 7102 zcmbtYeQXm+mhbj=5<4HxHzZI20ya1%Ukn=p3GpP583+ssA9DdV*y&Ev#BPV~c3^zY znvvMqz@ptSJ+ov^tTrHK299Aj9ILymZtfNx_rd9;TQ{-)$hHoPru9l`_I|J4;_wt2A)4XzUwsOyH|Ln=t^x~ z{Bz)3Wq4+g<(NSO#||1f4`;ey8Z_}{u7R^&Fns_$?%+WB3|crZmj~@uZY$@2(#Gd; zja=DR`sD+^iL3YuzX15nTy=^cPs?Qn?YudU;aa#>Zp{Vr3Z;%z+cs|PSK11>m$*7G zq=?(jt%I_d`v$iG$`UB+p>%R>+(sx%xgA_PxA}s3u#DR>z_@oVTKmKIJBmFOWYHpq zrI0^3%x27Cm;$I98HW8Yk5T>9+{UtMt))%o8E>#3fS@}WVNWtuWiS*EOF z^NN919Ox`yR_6Bc|-}>aS}nPRC3!Qxx>a%rP^Ts5EAYSu%BN%$lyRu-gV> zZ8$j?tCyBkY1PXW>fW?e#;u$wttndz)d2nntG3(nv5f7S4xYAOdbyJ65SbT{5k)) zLR^A49`q8ws0b5&$sY{zK|VwV!Xn2@Bk=0>5Y8VG6v7ES83Wyf2m2zU3NL0P^$d^m zB8f&MGAYOrJ{TmEyd;bWacV0x9h3wf6!20+9F-gPg~dsH4U0rr77_^iCGM!epXDX* zm=X$-3BN4ESl5{Y^i1v#4-+mdDX<{v^^~e%FQnB#+(zjP)+wpQS_hE0gjKhRvPYo` z&(hDcdbR>XG9bZ$Zkx0Q>uZseBOyrYkZeM-9>}6eJ_APNNwdgtBgY_$(m`4|b8gXa z<_-8o;dgd1Za5Q^^PnYdFp)giuV_dfgPFiC5Ba6xq5f!Pe>f0U_U!YnX!r-{E&m=| z=sfd98RICr{OZi)`$rP?Et-8x(!OPyedch!+x$-3)sDA2=IXT44aw3Cnxj7HsGl}G zt*DC6ZhQOm%<1W_8Nzxf3_g&sMyI!-` zPP0!7?9-7;ZPRVPs%^Y@VA?lhnDNaTW(}XMSwGh`=fBzg-uSig*%NoS&6mtK{j}_M z`$z2y#)U(P+K%Pk&m1L}|71pr&-$*Mopa6wu2*W6u3OFXhTGe<%I5k0g?0CiX_Y%Q z$F8Jf7noYTdN%N0<=nbC>G~$U+q`eVd2er`y8U9|v_UuGSy9R5b59+GGbJ;D+2$*w zbB4M8>sGB|lUCx=9Ijh+x1@RB?XwHcg}~1$wXM6g&GD`WXCCLjpkI5H6oJOCf4@4!FV@Hix7BG6PfXS^R zchr;%`4ATkk!uiwGKZ}j&VMU`qdpN4#}yuqE&+%jeST@2ml}HF0bb;ZYs(#iN;%Fp@(3gcfr}XafQ0NqiySk(!!(r0s^|pZC5IHgiI3uXxDk5+|HbjM5Eb|! z2>?V1p%9|*C=bDi@UR>_$t*kZhtgu=WPsH+Rtz?lBj0Lk{JVJw zUvb=4vb@-yv|&UTlX&!7Da-BXijs(kVV3Tul)aY+UU|9qFx_c>)F1Q^FesQ2M{S}D z+8+)p4f|oGh#)KR9jFNPfW5N7MIs?&lW_?53Pe{Dh>k*>1uHX>^rfEkgL4O9(ZL)J zR-6~akzfQioNmX!@YutfN)%mCL|-H>^kP`f2&4!ZNxhLk91h`8UV@P@@ZrtS#E3$8 znfQY;>59lO_lG~?CAXIh2;lRiJ3QQQAgl<{tn308yj$YOq_odBLSE7Yzh?Ng(8kUvM9?qt@gY5fak)=r%yNYz%30V!fP{u&DJ;t_zOl)}H#WQ7c<^y1 zfERG8VSg36-U4{FBFMvIm`!Bl`1o+ZuOGzh>>(==IK#jP1SA18YSSQ&0o!emZa^V* z0+A6CNh$!BXAlNWPdjC_HWP-^0F>SQYm{;r2n1s<<8aQA>+i*{#WnlZq_ucw=>uv8x-h~!zbB9*knJn&{HscYB&bsc_ zCyE-iqQ+!V<9yeG`D5$7eLwBgihK#1k0wFGR&@S?zXyZ9W4j7j%p7`Y2@^BqCO_&Q z+N;#5`a;7coBF%rs#P%~erv`oAAlR)fuH_>2OHG~+saC&Z6hpaxL}J}yBT`lpnl)8 z`jx2f#U6Q#4=N*nq$5ggph|tStRmhBm3P^z%qSCMPCDTtAah(9=At3SegN=vCzG{Z zFLh-_Xs&p_x8*Dok0jT!{N|GN3`(;I||W39y%MA zf}D3J-FLYID(N9LxXt)+^ZWBmqN+pxpKto%+FK>Jy0mInG z1D6iWw!ufw$gM-KUKef(n!Pb;Z%p~3>v6H`R@MAV3)>&`YQ=rY;y&FC&t+x`l?5{p zt*Rplw5T&#)S0k#s{F>6O8*B||2u^_2&{hGzFtifJ53X;z^I2dRT>SPQ7x&du}&ET zyxmb-Ep_UbrNxCa?8&(ku-ACI6Jw!k#j2P|!E2_}vkaARLlmyBA(sj-7IJ3J@@<1o zM-2Got3LqOW>mM@I?CjFQ8xpc_Jf-jsc&vtZ?tiFXim$NEoS5L^@}Fi#1+J>ldQyE zZpyI$;e$RgD_2mh5=%Mx$et^HDld10y2VxJaKOD+A>4aWcUJ2xWo2I(mankX9=JV2 zdrVUWDMk@jj10?^9Rg4u9PRS%yjXs$AZA~2a2*Q9`P6f5hpmKI=*(Rdf43>_hb@g>Znu87dmMuru-em)#_<+69kxs5M+;##HTxi zPC-_={MF8?OwM*k3#hRg@&RQSOe$>(NDi#!AqI{ zAMiUAURMc{RB8s2-Nz83EWVna=91d+czjC?#u=_?}-Hce^hcezHsJ2owl_* zxwTv80Zjp%`o!(pwCG500No*Xp`KvwMaw7v?!lU%IyP6uDFW_>_eEpKe{T8e$Xz7u z!(Ir8i*`<5(;-~ZMT^ACu)qL@A|y6YaDK(FhjBIpVgzo{2spWDgw$Hf1p7q?#m;or zs$*!86U3+Eyskprql>W(dE4nAA8c8>kfe8W69ax18j_9SW7R_#o#+Bsu=wwlb9d{};? zd~WiVtktz7>sqwct;yA`Gq$H>!xPf>n6xddUyvW{ee}9UjwZ>`8HajjOS|t`{id6* zXEq2bk3v8+?};SP`jg4}lS>7bwaz65$c$x1zEW_d;CU$+wM3ZWifM=Bygd!Rh9n+>_s!p7Nv*S`5%$| zllq^XC)vyQ`*vH{I6XwA$gV(A2?XvfZ(yB%(`2~P1UBXC=Z_9p9!gz=cBe^U?m%!} z8k0jP^DQK&kYIF{N;{DLv4Tulf~Z}8p$>O#gyp={)m)Bq%`*9y@aisCwXT5oI=0+I zLXXNlto;j;e?{^b$rB{ckbJ84`RdreR>Qt`)G5^b-+ z_B6?~BV#9HDOt*A8e5j~3e7LGOUz1&y-O|}Uoy}Jg53+TyCdJcn_b$*&?1Xw`qPFg vVy;+Xa!D_Hzxri6%l<~)*!=_cck17C|EBW|6AS%bOrKN!=U>xGA?bess)nI` delta 1236 zcmaKqU2GIp6vxkd zd(W9O^ZPaS+cb-QX&7M;E3$Xxk=($Q=*hODGWh`P?u_7RC_ z#cm8tDd&aaB8eL-tKs7q68QkGMnl+OF@$Sy*}rsQ?7@V{>$n!%g^geaJA^faU15q% z6xSiyHtfYdOiiiT7^Yu=Ouu(7wjsgegB$f@5;a!>S917{WgDY4mQTMSVy^5e4&!fb zNl1>&M_)|x*Eg?BxvHb0gz|A|k%y$^&91@K97X7p%ay}$Ufgg|Jd0}>@;|x(Tj%pb z-Li@rZ!^ue+@KT0z}pQ)Rl%SWc%NBpW`iAQ9s@X{HTO;cgAYsD(PkWAKGa&w4Yl6D zzaC1Dh7SYk=UA)qe{TufOCHv6ROcU6oRA|sniHN-0PM&3*_VI{zz+` z2^|29ImMBj;E=d<=0=UpbUTvx5YdS;-@EN#*bS#1EIzrttJ@F0Q>m8hQf1ttm-e;s zid8O*S(}7otq{m4obOnBG!wCQQK&@NP3RD^yRV2L0=aARCO=_nr#cqr=Q-` z`@PUsw=jT7sK=mx{R!=snVNg<7qjmtv+t%k@UuBkhiGW&0z`S|&OMrLEUkmiD?8ui z>xTzd*F)+m5fG?pwdzT|rY{&IX@^+igkcR(J8^G*cuwx9FSk&fgw5%$#sm4b!V7o_`&aDmrG@+{-s9?@8g25OZF3Bo7*N_LHC z@K*MTIGtYnAdS>_!p$t np.ndarray: + """ + Rotationsmatrix [0,0,1] → normal (kürzester Bogen). + + Repliziert THREE.Quaternion.setFromUnitVectors(vFrom=[0,0,1], vTo=n) + exakt (inkl. antiparallelem Sonderfall), damit die hier erzeugten + Ecken zur visuell verifizierten Orientierungs-Konvention in + boardViewer.html passen (qNormalLoc dort). + """ + n = np.asarray(normal, dtype=float) + nn = float(np.linalg.norm(n)) + n = n / nn if nn > 1e-12 else np.array([0.0, 0.0, 1.0]) + # three.js: quat = (cross([0,0,1], n), dot([0,0,1], n) + 1), dann normalisieren. + r = float(n[2]) + 1.0 + if r < 1e-12: + # antiparallel (n == [0,0,-1]): three.js else-Zweig für vFrom=[0,0,1] → (0,-1,0,0) + qx, qy, qz, qw = 0.0, -1.0, 0.0, 0.0 + else: + qx, qy, qz, qw = -float(n[1]), float(n[0]), 0.0, r # cross([0,0,1], n) = (-ny, nx, 0) + ql = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw) + qx, qy, qz, qw = qx / ql, qy / ql, qz / ql, qw / ql + return np.array([ + [1 - 2 * (qy * qy + qz * qz), 2 * (qx * qy - qz * qw), 2 * (qx * qz + qy * qw)], + [2 * (qx * qy + qz * qw), 1 - 2 * (qx * qx + qz * qz), 2 * (qy * qz - qx * qw)], + [2 * (qx * qz - qy * qw), 2 * (qy * qz + qx * qw), 1 - 2 * (qx * qx + qy * qy)], + ]) + + @staticmethod + def _marker_plane_corners(half: float) -> np.ndarray: + """ + Die 4 Eckpunkte in der Marker-Ebene (+Z = Normale), in DERSELBEN + Reihenfolge wie die von 3b_corner_marker_poses.py triangulierten + `corners_m` (ArUco 0..3, im Uhrzeigersinn von der Vorderseite gesehen). + + Ecke 0 zeigt in Richtung (+h, +h) — dieselbe Konvention wie der visuell + kalibrierte Orientierungszeiger (1,1,0) in boardViewer.html. Damit passt + sie zu den manuell/visuell gesetzten `spin`-Werten der ARM-Marker, die + im Homing die Gelenkwinkel bestimmen (gegen echte corners_m am + Seed-Pose verifiziert, test_robot_fk_corners.py, RMS ~1 mm). + + Hinweis: Die Board-Referenzmarker (Set A0) sind ~90° anders kalibriert, + ihre Eckreihenfolge passt unter dieser Konvention NICHT — egal, weil + Board der Root-Link ist: ihr Eck-Residuum ist konstant bzgl. der + Gelenkvariablen und beeinflusst die Schätzung nicht (der robuste + Huber-Verlust dämpft es als Ausreißer). Siehe Doc-Notiz. + + Die Drehrichtung 0→1→2→3 ist so, dass 3b's `corner_plane_normal()` + (outward = -cross(e01,e02)) wieder +Z liefert — identisch zur + Beobachtungs-Konvention. + """ + h = float(half) + return np.array([ + [ h, h, 0.0], # 0 + [ h, -h, 0.0], # 1 + [-h, -h, 0.0], # 2 + [-h, h, 0.0], # 3 + ]) + + @classmethod + def marker_corners_local(cls, + position: Sequence[float], + normal: Sequence[float], + size_mm: float, + spin_deg: float = 0.0) -> np.ndarray: + """ + Die 4 Marker-Ecken im LINK-lokalen Frame (mm), Reihenfolge wie `corners_m`. + + Orientierung = Spin um die Normale ∘ Minimal-Rotation [0,0,1]→Normale, + exakt wie boardViewer.html (qSpinLoc.multiply(qNormalLoc)). + """ + n = np.asarray(normal, dtype=float) + R = _rot_axis_angle(n, float(spin_deg)) @ cls._shortest_arc_R(n) + plane = cls._marker_plane_corners(float(size_mm) / 2.0) # (4,3) + return np.asarray(position, dtype=float) + (R @ plane.T).T # (4,3) + + @classmethod + def marker_corners_world(cls, + transforms: Dict[str, np.ndarray], + link_name: str, + position: Sequence[float], + normal: Sequence[float], + size_mm: float, + spin_deg: float = 0.0) -> np.ndarray: + """Die 4 Marker-Ecken im Weltframe (mm), Reihenfolge wie `corners_m`.""" + T = transforms.get(link_name, np.eye(4)) + local = cls.marker_corners_local(position, normal, size_mm, spin_deg) + return np.array([transform_point(T, c) for c in local]) + def all_markers_world(self, transforms: Dict[str, np.ndarray] ) -> Dict[int, Dict[str, Any]]: """ Returns ------- - dict marker_id -> {world_mm, local_mm, link, normal_world} + dict marker_id -> {world_mm, local_mm, link, normal_world, corners_world} + + corners_world: (4,3) Welt-mm in `corners_m`-Reihenfolge (für den + marker_observation="corner_points"-Modus in 5_pose_estimation.py). """ + default_size = float((self.robot.get("markerDefaults", {}) or {}).get("size", 25.0)) result: Dict[int, Dict[str, Any]] = {} for lname, ldata in self.links.items(): T = transforms.get(lname, np.eye(4)) @@ -210,11 +305,15 @@ class RobotFK: continue loc = np.array(m["position"], dtype=float) nor = np.array(m.get("normal", [0, 0, 1]), dtype=float) + size_mm = float(m.get("size", default_size)) + spin_deg = float(m.get("spin", 0.0)) + local_corners = self.marker_corners_local(loc, nor, size_mm, spin_deg) result[mid] = { "world_mm": transform_point(T, loc), "local_mm": loc, "link": lname, "normal_world": (R @ nor) / max(np.linalg.norm(R @ nor), 1e-12), + "corners_world": np.array([transform_point(T, c) for c in local_corners]), } return result diff --git a/test/test_robot_fk_corners.py b/test/test_robot_fk_corners.py new file mode 100644 index 0000000..bec443a --- /dev/null +++ b/test/test_robot_fk_corners.py @@ -0,0 +1,169 @@ +#!/usr/bin/env python3 +""" +test_robot_fk_corners.py +======================== +Isolierter Test für RobotFK.marker_corners_world/_local (doc/ +Homing_5_Pose_MultiPoint_Weighted.md, Schritt 3) — der Baustein für den +marker_observation="corner_points"-Modus. + +Kein pytest nötig (das Repo testet sonst per Jest): plain asserts + main(), +Aufruf: python test/test_robot_fk_corners.py + +Geprüft wird die KONVENTION (Eckreihenfolge/Spin/Winding), denn ein Versatz +dort gäbe systematischen Bias statt Verbesserung: + + A) Selbst-Konsistenz je Marker (versch. Normalen/Spins): + - Schwerpunkt der 4 Ecken == Marker-Center + - alle 4 Kanten == size, planar + - die aus den Ecken via 3b-Konvention (corner_plane_normal) zurück- + gewonnene Normale == Marker-Normale → Orientierung + Winding stimmen + B) Reale Daten: ROBOTER-Marker (Arm1/Ellbow/Arm2) am Seed-Pose eines echten + Captures — die FK-Ecken müssen zu den triangulierten `corners_m` passen, + UND die Identitäts-Paarung (Ecke i ↔ Ecke i) muss jede zyklische/ + gespiegelte Umordnung schlagen → beweist Ecke-0 + Drehrichtung gegen echte + Triangulation. Nur Roboter-Marker, weil deren Spin kalibriert/verifiziert + ist; die Board/Rail-Marker (Root-Link) sind spin-unkalibriert und werden im + Fit bewusst per Center-Residuum behandelt (nicht über Ecken). +""" +import json +import os +import sys + +import numpy as np + +HERE = os.path.dirname(os.path.abspath(__file__)) +sys.path.insert(0, os.path.join(HERE, "..", "scripts")) +from robot_fk import RobotFK # noqa: E402 + +ROBOT_JSON = os.path.join(HERE, "..", "scripts", "robot_1781069752019.json") +CAP_DIR = os.path.join(HERE, "homing", "20260616_133151") +CAPTURE = os.path.join(CAP_DIR, "aruco_marker_poses.json") +SEED = os.path.join(CAP_DIR, "state_Arm2.json") + + +def recovered_normal(corners3d: np.ndarray) -> np.ndarray: + """3b_corner_marker_poses.py corner_plane_normal() — identisch nachgebaut.""" + center = corners3d.mean(axis=0) + _, _, Vt = np.linalg.svd(corners3d - center) + n = Vt[-1] + cross = np.cross(corners3d[1] - corners3d[0], corners3d[2] - corners3d[0]) + if np.dot(n, cross) > 0: + n = -n + nn = np.linalg.norm(n) + return n / nn if nn > 1e-12 else n + + +def edge_lengths(c: np.ndarray): + return [float(np.linalg.norm(c[(i + 1) % 4] - c[i])) for i in range(4)] + + +# ── A) Selbst-Konsistenz ──────────────────────────────────────────────────── + +def test_self_consistency(): + fk = RobotFK.from_file(ROBOT_JSON) + cases = [ + ([0, 0, 1], 0.0), + ([0, 0, 1], 90.0), + ([0, 0, 1], 37.5), + ([0, -1, 0], 90.0), + ([-1, 0, 0], 0.0), + ([0, 0, -1], 12.0), # antiparalleler Sonderfall der Minimal-Rotation + ([1, 1, 0.3], 215.0), # schräge Normale + großer Spin + ] + size = 25.0 + pos = np.array([100.0, -50.0, 30.0]) + for normal, spin in cases: + local = RobotFK.marker_corners_local(pos, normal, size, spin) + assert local.shape == (4, 3), f"shape {local.shape}" + # Schwerpunkt == Center + d_center = np.linalg.norm(local.mean(axis=0) - pos) + assert d_center < 1e-9, f"centroid off by {d_center} (n={normal}, spin={spin})" + # Kanten == size, alle gleich (Quadrat) + for L in edge_lengths(local): + assert abs(L - size) < 1e-9, f"edge {L} != {size} (n={normal}, spin={spin})" + # planar: alle 4 in einer Ebene (Abstand zur SVD-Ebene ~0) + rel = local - local.mean(axis=0) + _, _, Vt = np.linalg.svd(rel) + planar = np.max(np.abs(rel @ Vt[-1])) + assert planar < 1e-9, f"not planar ({planar})" + # Normale aus den Ecken (3b-Konvention) == Marker-Normale + n_exp = np.asarray(normal, float) / np.linalg.norm(normal) + n_rec = recovered_normal(local) + dot = float(np.dot(n_exp, n_rec)) + assert dot > 0.99999, ( + f"recovered normal {n_rec} != {n_exp} (dot={dot:.6f}, spin={spin})") + print(f"[PASS] A) Selbst-Konsistenz: {len(cases)} Fälle " + "(Center, Kanten, planar, Normalen-Rückgewinnung).") + + +# ── B) Reale Capture-Daten (Board-Marker) ─────────────────────────────────── + +def best_pairing_error(model_c: np.ndarray, obs_c: np.ndarray): + """ + RMS der Eck-Paarung nach Center-Abzug, für Identität und alle 8 Umordnungen + (4 zyklische × {vorwärts, rückwärts}). Gibt (rms_identity, rms_best, name_best). + """ + m = model_c - model_c.mean(axis=0) + o = obs_c - obs_c.mean(axis=0) + best_rms, best_name, id_rms = None, None, None + for reverse in (False, True): + base = o[::-1] if reverse else o + for roll in range(4): + cand = np.roll(base, roll, axis=0) + rms = float(np.sqrt(np.mean(np.sum((m - cand) ** 2, axis=1)))) + name = f"{'rev' if reverse else 'fwd'}+roll{roll}" + if best_rms is None or rms < best_rms: + best_rms, best_name = rms, name + if not reverse and roll == 0: + id_rms = rms + return id_rms, best_rms, best_name + + +def test_against_real_capture(): + if not (os.path.exists(CAPTURE) and os.path.exists(SEED)): + print(f"[SKIP] B) Capture/Seed fehlt: {CAP_DIR}") + return + fk = RobotFK.from_file(ROBOT_JSON) + seed = json.load(open(SEED, "r", encoding="utf-8")).get("accumulated_state", {}) + transforms = fk.compute(seed) # Roboter-Marker brauchen den Pose-Zustand + model = fk.all_markers_world(transforms) + + # Root-Link (Board) bestimmen → davon wird NICHT über Ecken validiert. + roots = {ln for ln, ld in fk.links.items() + if not ld.get("parent") or ld.get("parent") not in fk.links} + + data = json.load(open(CAPTURE, "r", encoding="utf-8")) + checked = 0 + for m in data.get("markers", []): + mid = m["marker_id"] + if mid not in model or not m.get("corners_m"): + continue + if model[mid]["link"] in roots: # Board/Rail: bewusst Center-only + continue + obs_c = np.array(m["corners_m"], dtype=float) * 1000.0 # m → mm + model_c = np.asarray(model[mid]["corners_world"], float) + # Schlecht lokalisierte/fehlassoziierte Marker (Center weit daneben am + # Seed) überspringen — sie sagen nichts über die Eckreihenfolge aus. + if np.linalg.norm(model[mid]["world_mm"] - obs_c.mean(axis=0)) > 50.0: + continue + id_rms, best_rms, best_name = best_pairing_error(model_c, obs_c) + # 1) Form/Orientierung stimmt: Identitäts-RMS klein (Seed- + Triangulationsrauschen) + assert id_rms < 6.0, f"Roboter-Marker {mid}: Identitäts-RMS {id_rms:.2f}mm zu groß" + # 2) KEINE Umordnung ist besser als die Identität → Ecke-0 + Winding korrekt + assert best_name == "fwd+roll0", ( + f"Roboter-Marker {mid}: '{best_name}' (RMS {best_rms:.2f}) schlägt Identität " + f"(RMS {id_rms:.2f}) → Eckreihenfolge falsch") + checked += 1 + assert checked >= 4, f"zu wenige verwertbare Roboter-Marker geprüft ({checked})" + print(f"[PASS] B) Reale Capture-Daten: {checked} ROBOTER-Marker am Seed — " + "FK-Ecken passen zu corners_m, Identitäts-Paarung ist optimal.") + + +def main(): + test_self_consistency() + test_against_real_capture() + print("\n[OK] Alle Tests bestanden.") + + +if __name__ == "__main__": + main()