43 lines
1.5 KiB
Python
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()
|