From fab7032d56c8c74f9820d3f5164ddc33fb1d11cd Mon Sep 17 00:00:00 2001 From: chk <79915315+ChKendel@users.noreply.github.com> Date: Thu, 25 Jun 2026 19:58:23 +0200 Subject: [PATCH] Multipoint Schritt 4 --- doc/Homing_5_Pose_MultiPoint_Weighted.md | 12 +++++-- scripts/5_pose_estimation.py | 40 ++++++++++++++++-------- scripts/robot_1781069752019.json | 3 +- 3 files changed, 39 insertions(+), 16 deletions(-) diff --git a/doc/Homing_5_Pose_MultiPoint_Weighted.md b/doc/Homing_5_Pose_MultiPoint_Weighted.md index 0a5973e..95b776a 100644 --- a/doc/Homing_5_Pose_MultiPoint_Weighted.md +++ b/doc/Homing_5_Pose_MultiPoint_Weighted.md @@ -293,10 +293,18 @@ Alle anderen Konsumenten (`homing.js`, `editRobot.js` → `assignByZRange`/`alig ziehen auch `corner_pose` leicht. Auf bereinigten Markern konvergiert `corner_points` ≈ `corner_pose`. → Marker-Zuordnung korrigieren (separate Kalibrier-Aufgabe). +- **Scharfgeschaltet (2026-06-25, gescopt):** robot.json + `marker_observation: "corner_points"` mit + `corner_point_links: ["Hand","Palm","FingerA","FingerB"]`. D.h. nur Hand/Finger + nutzen die 4 Ecken; Arme behalten Center+Normale, Board nur Center. Auf der + Arm-Capture (ohne Finger-Marker) **byte-identisch** zu `corner_pose` → keine + Regression im bestehenden Pfad. Die Arme können in die Liste, sobald die + A0→Arm1-Fehlzuordnung behoben ist. - **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`. + der Hand/Finger-Ecken gegen appRobotRendering-Simulations-GT steht aus (hier + fehlten Finger-Marker in den Captures). CLI: `--marker-observation corner_pose` + schaltet zum Vergleich zurück. ## Offene Punkte diff --git a/scripts/5_pose_estimation.py b/scripts/5_pose_estimation.py index e6b9216..95c7483 100644 --- a/scripts/5_pose_estimation.py +++ b/scripts/5_pose_estimation.py @@ -316,15 +316,16 @@ def residual_vector(state: Dict[str, float], fk: RobotFK, obs: Dict[int, Dict[st "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. + "corner_points": 12 Eck-Residuen (4 Ecken × xyz, mm) NUR für Marker + auf den `corner_point_links` (z.B. Hand/Finger), + KEINE separate Normale (Orientierung steckt in den + Ecken). Alle übrigen Marker verhalten sich wie im + Default-Modus (Center + optionale Normale) — außer + dem Root-Link (Board: Boden-/Rail-Marker, Spin + unkalibriert), der nur Center bekommt ("ein Punkt + pro Marker"). So lassen sich Ecken gezielt für + Hand/Finger scharfschalten, ohne Arme/Board zu + verändern. """ model = model_markers(fk, state) res: List[float] = [] @@ -333,19 +334,32 @@ def residual_vector(state: Dict[str, float], fk: RobotFK, obs: Dict[int, Dict[st if obs_mode == "corner_points": corner_links = _resolve_corner_links(fk, cfg) + roots = {ln for ln, ld in fk.links.items() + if not ld.get("parent") or ld.get("parent") not in fk.links} + w_n = float(cfg.get("normal_weight", 30.0)) + use_n = bool(cfg.get("use_normals", True)) 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] + link = mm.get("link") 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: + if 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) + continue + # Nicht-Eck-Marker verhalten sich wie im Default-Modus: Center + + # optionale Normale — AUSSER auf dem Root-Link (Board: Boden-/Rail- + # Marker mit unkalibriertem Spin), der nur Center bekommt ("ein + # Punkt pro Marker"). So bleiben Arme/Board unverändert, wenn nur + # Hand/Finger über corner_point_links auf Ecken laufen. + dp = (np.asarray(mm["world_mm"], float) - obs[mid]["pos_mm"]) * mw + res.extend(dp.tolist()) + if link not in roots and use_n and obs[mid]["normal"] is not None and "normal_world" in mm: + dn = (np.asarray(mm["normal_world"], float) - obs[mid]["normal"]) * w_n * mw + res.extend(dn.tolist()) return np.asarray(res, dtype=float) # Default: Center (mm) + optionale Normale (skaliert) diff --git a/scripts/robot_1781069752019.json b/scripts/robot_1781069752019.json index b88f418..da8816a 100644 --- a/scripts/robot_1781069752019.json +++ b/scripts/robot_1781069752019.json @@ -281,7 +281,8 @@ }, "pose_estimation": { "method": "hybrid", - "marker_observation": "corner_pose", + "marker_observation": "corner_points", + "corner_point_links": ["Hand", "Palm", "FingerA", "FingerB"], "use_normals": true, "normal_weight": 100, "robust_loss": "huber",