Files

80 lines
3.7 KiB
Python

"""标定验证(只读):用已标定的 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()