Files
a7l20-hug-sim/scripts/move_right_board_diag1.py

43 lines
1.5 KiB
Python

"""移动右臂指尖到棋盘格编号 1 的斜线中心。"""
import sys
import json
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation as Rot
ROOT = Path(__file__).resolve().parents[1]
sys.path.insert(0, str(ROOT / "scripts"))
from real_arm import RealArm
TARGET_TIP_BASE = np.array([0.44926986912182487, -0.035088032594957054, -0.2747396703702292])
FINGERTIP_OFFSET = np.array([0.03768745091820422, -0.024041479611381478, -0.01619183968942585])
def main():
arm = RealArm(side="right", table_z=-0.265)
try:
state = arm.read()
samples = json.loads((ROOT / "data/handeye_fresh_20260928_y29kSV/touch_samples.json").read_text())["samples"]
joints = None
chosen = None
dz = None
for sample in samples:
R = Rot.from_quat(sample["quat_xyzw"])
flange_target = TARGET_TIP_BASE - R.apply(FINGERTIP_OFFSET)
trial, trial_dz = arm.ik_sweep(flange_target, R, seed=state["joints"])
if trial is not None:
joints, chosen, dz = trial, sample["corner"], trial_dz
break
if joints is None:
raise SystemExit("使用 8 组标定姿态尝试后仍然 IK 不可达")
print(f"法兰目标: {flange_target.tolist()},采用标定姿态角点: {chosen},绕工具 Z 修正: {dz}°")
if not arm.movej(joints, speed=0.15, accel=0.15, verify=True):
raise SystemExit("右臂移动失败")
finally:
arm.close()
if __name__ == "__main__":
main()