"""标定验证(只读):用已标定的 T_base_board + 法兰偏移 d,预测某锚点目标点的基座坐标, 和你实际把法兰对准该锚点后读到的位姿对比,看误差。臂掉使能移动、上使能后运行。 关键:验证时【手腕朝向换成和标定时不同的姿态】——若 d 解对了,换朝向仍能对上(<1cm);若 d 错了,会露馅。 用法:.venv/bin/python scripts/validate_touch.py --capture data/real_align 交互:法兰对准锚点 k 的黄线(align_lines.png)、上使能后 → 输入 k → 打印预测/实际/误差;done 结束。 """ import argparse, json, time from arm_contract import check_calibration_arm from pathlib import Path import numpy as np import roslibpy from scipy.spatial.transform import Rotation as Rot ROOT = Path(__file__).resolve().parents[1] def main(): ap = argparse.ArgumentParser() ap.add_argument("--capture", default="data/real_align") ap.add_argument("--arm", choices=["left", "right"], default="left") ap.add_argument("--host", default="192.168.10.21"); ap.add_argument("--port", type=int, default=9090) a = ap.parse_args() cam = json.loads((ROOT / a.capture / "camera.json").read_text()) check_calibration_arm(cam, a.arm) print(f"请使用对准图:{ROOT / a.capture / ('align_lines_' + a.arm + '.png')}") T_bb = np.array(cam["T_base_board"], float); d = np.array(cam["fingertip_offset_m"], float) objp = np.array(cam["board_objp"], float); C, R_ = cam["board_grid"] print(f"用标定: 相机基座位置 {np.round(cam['cam_pos_world'],3).tolist()}, 法兰偏移 d(mm) {np.round(d*1000,1).tolist()}") def target(k): assert k % C < C - 1 and k // C < R_ - 1, "该角点无右下方格,用 0~6/8~14/16~22/24~30" return (objp[k + 1] + objp[k + C]) / 2 ros = roslibpy.Ros(host=a.host, port=a.port); ros.run(timeout=8) st = {"m": None, "n": 0} topic = f"/{a.arm}/arm_state" print(f"验证 {a.arm} 臂:{topic}") tp = roslibpy.Topic(ros, topic, "lbot_arm_interfaces/msg/ArmState") tp.subscribe(lambda m: (st.__setitem__("m", m), st.__setitem__("n", st["n"] + 1))) def fresh(t=2.0): n0 = st["n"]; t0 = time.time() while st["n"] == n0 and time.time() - t0 < t: time.sleep(0.02) return st["m"] if st["n"] != n0 else None if fresh() is None: tp.unsubscribe(); ros.terminate() raise SystemExit(f"收不到 {topic}") print("== 法兰对准锚点 k 的黄线、上使能后输入 k(朝向请换成和标定不同的姿态)。done 结束 ==") errs = [] while True: s = input("锚点 k / done: ").strip().lower() if s == "done": break try: k = int(s) except ValueError: continue m = fresh() if m is None: print(" 位姿流卡住,重试"); continue p = m["pose"]["position"]; q = m["pose"]["orientation"] Rm = Rot.from_quat([q["x"], q["y"], q["z"], q["w"]]).as_matrix() actual = np.array([p["x"], p["y"], p["z"]]) + Rm @ d # 实际法兰参考点(基座系) pred = T_bb[:3, :3] @ target(k) + T_bb[:3, 3] # 标定预测该锚点目标点(基座系) e = np.linalg.norm(actual - pred) * 1000 errs.append(e) print(f" 角点{k}: 预测={pred.round(4).tolist()} 实际={actual.round(4).tolist()} 误差={e:.1f}mm") tp.unsubscribe(); ros.terminate() if errs: print(f"\n验证 {len(errs)} 点:误差 均值 {np.mean(errs):.1f}mm 最大 {np.max(errs):.1f}mm") print("<10mm 可用;>20mm 说明标定(尤其 d)不准,需重标(大朝向变化)。") if __name__ == "__main__": main()