Claude studie mit mm

benchmark/eval_pose.py

@@ -17,10 +17,17 @@ from __future__ import annotations
 import argparse
 import json
 import sys
-from typing import Any, Dict
+from pathlib import Path
+from typing import Any, Dict, Optional
+
+import numpy as np
+
+sys.path.insert(0, str(Path(__file__).parent.parent / "pipeline"))
+from robot_fk import RobotFK  # noqa: E402
 
 LINEAR = {"x", "e"}
 JOINTS = ["x", "y", "z", "a", "b", "c", "e"]
+FINGER_LINK = "FingerA"
 
 
 def load_estimate(path: str) -> Dict[str, Dict[str, Any]]:
@@ -51,7 +58,23 @@ def joint_error(v: str, est: float, gt: float) -> float:
     return abs(((est - gt + 180.0) % 360.0) - 180.0)
 
 
-def evaluate(estimate_path: str, gt_path: str) -> Dict[str, Any]:
+def finger_error_mm(est: Dict[str, Any], gt: Dict[str, float],
+                    robot_path: Optional[str]) -> Optional[float]:
+    """Euklidischer Abstand der FingerA-Position (FK) zwischen Schätzung und GT."""
+    if not robot_path:
+        return None
+    fk = RobotFK.from_file(robot_path)
+    est_vals = {v: est[v]["value"] for v in JOINTS}
+    gt_vals  = {v: gt.get(v, 0.0) for v in JOINTS}
+    t_est = fk.compute(est_vals)
+    t_gt  = fk.compute(gt_vals)
+    p_est = t_est[FINGER_LINK][:3, 3]
+    p_gt  = t_gt[FINGER_LINK][:3, 3]
+    return float(np.linalg.norm(p_est - p_gt))
+
+
+def evaluate(estimate_path: str, gt_path: str,
+             robot_path: Optional[str] = None) -> Dict[str, Any]:
     est = load_estimate(estimate_path)
     gt = load_gt(gt_path)
 
@@ -74,6 +97,7 @@ def evaluate(estimate_path: str, gt_path: str) -> Dict[str, Any]:
         "max_abs_deg": max(ang_errs) if ang_errs else None,
         "mean_abs_mm": (sum(lin_errs) / len(lin_errs)) if lin_errs else None,
         "max_abs_mm": max(lin_errs) if lin_errs else None,
+        "finger_error_mm": finger_error_mm(est, gt, robot_path),
     }
     return {"rows": rows, "summary": summary}
 
@@ -83,11 +107,13 @@ def main() -> int:
     ap.add_argument("estimate", help="robot_state.json")
     ap.add_argument("gt", help="simulation/SceneX/pose.json")
     ap.add_argument("--out", default=None)
+    ap.add_argument("--robot", default=None,
+                    help="robot.json — aktiviert FK-basierten Fingerfehler in mm")
     ap.add_argument("--tolDeg", type=float, default=2.0)
     ap.add_argument("--tolMm", type=float, default=3.0)
     args = ap.parse_args()
 
-    res = evaluate(args.estimate, args.gt)
+    res = evaluate(args.estimate, args.gt, robot_path=args.robot)
     print(f"{'joint':>6} | {'est':>9} | {'gt':>9} | {'error':>9} | obs | nMk")
     print("-" * 58)
     worst = 0.0
@@ -102,6 +128,9 @@ def main() -> int:
     mm = f"{s['mean_abs_mm']:.2f}" if s["mean_abs_mm"] is not None else "-"
     xm = f"{s['max_abs_mm']:.2f}" if s["max_abs_mm"] is not None else "-"
     print(f"angles: mean {md}deg / max {xd}deg   |   linear: mean {mm}mm / max {xm}mm")
+    fe = s.get("finger_error_mm")
+    if fe is not None:
+        print(f"finger ({FINGER_LINK}) position error: {fe:.2f} mm")
 
     if args.out:
         json.dump(res, open(args.out, "w", encoding="utf-8"), indent=2)