ChK/appRobotHoming
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Multipoint Schritt 4

Browse Source
This commit is contained in:
chk
2026-06-25 19:58:23 +02:00
parent 9bf49eff8d
commit fab7032d56
3 changed files with 39 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
scripts/5_pose_estimation.py
View File

@@ -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)

3
scripts/robot_1781069752019.json
View File

@@ -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",