178 lines
6.8 KiB
Python
178 lines
6.8 KiB
Python
"""真机抓取(手动点击版):采集 → 手动指定抓取点 → HUG → 重定向 → 执行。
|
|
|
|
跳过自动感知,直接在采集图像上点击要抓取的位置,HUG 根据点云和点击生成抓取。
|
|
适用于任意物体(不需要预设模板)。
|
|
|
|
用法:
|
|
python3 scripts/real_grasp_manual.py --side left --click-uv 400,300 --stage pregrasp
|
|
|
|
流程:
|
|
1. 先不带 --click-uv 运行一次,查看保存的 rgb.png,确定要抓取的像素坐标
|
|
2. 再带上 --click-uv 运行完整流程
|
|
"""
|
|
import argparse, json, os, subprocess, sys, time
|
|
from pathlib import Path
|
|
from project_paths import configured_path
|
|
import numpy as np
|
|
import cv2
|
|
sys.path.insert(0, str(Path(__file__).resolve().parent))
|
|
from real_arm import RealArm
|
|
|
|
ROOT = Path(__file__).resolve().parents[1]
|
|
HUG_PY = configured_path('HUG_PYTHON', configured_path('HUG_ROOT', 'external/hug') / '.venv/bin/python')
|
|
SDK_PY = configured_path('RETARGET_PYTHON', '.retarget/bin/python')
|
|
|
|
|
|
def sh(cmd, cwd=None, env=None):
|
|
r = subprocess.run(cmd, cwd=cwd, env=env, capture_output=True, text=True)
|
|
if r.returncode != 0:
|
|
print(r.stdout[-1500:]); print(r.stderr[-2000:])
|
|
raise SystemExit(f"子进程失败: {' '.join(map(str,cmd[:3]))}")
|
|
return r.stdout
|
|
|
|
|
|
def capture_and_hug(cap, click_uv, side, num, table_top=-0.265):
|
|
"""① 采集 D405 + 读关节角 ② HUG 推理(直接用点击坐标) ③ 重定向 → grasp_l20_*.json"""
|
|
|
|
print("① 采集 D405 + 读取双臂关节角...")
|
|
sh([sys.executable, str(ROOT / "scripts/realsense_capture.py"), "--out", str(cap), "--read-joints"])
|
|
|
|
# 把标定外参写进本帧 camera.json
|
|
calj = json.loads((ROOT / "data/real_calib/camera.json").read_text())
|
|
g = json.loads((cap / "camera.json").read_text())
|
|
for k in ("T_world_cam_opencv", "cam_pos_world", "T_base_board", "fingertip_offset_m", "extrinsic_status"):
|
|
if k in calj:
|
|
g[k] = calj[k]
|
|
|
|
# 如果没有指定点击坐标,使用图像中心
|
|
if click_uv is None:
|
|
w, h = g["width"], g["height"]
|
|
click_uv = (w // 2, h // 2)
|
|
print(f" 未指定点击坐标,使用图像中心: ({click_uv[0]}, {click_uv[1]})")
|
|
|
|
u, v = click_uv
|
|
# 从深度图读取该点深度
|
|
depth_img = cv2.imread(str(cap / "depth.png"), cv2.IMREAD_UNCHANGED)
|
|
depth_mm = depth_img[v, u]
|
|
print(f" 点击坐标: ({u}, {v}), 深度: {depth_mm} mm")
|
|
|
|
g["objects"] = {
|
|
"target": {
|
|
"uv": [int(u), int(v)],
|
|
"depth_mm": int(depth_mm)
|
|
}
|
|
}
|
|
|
|
(cap / "camera.json").write_text(json.dumps(g, indent=2))
|
|
|
|
env_hug = {k: v for k, v in os.environ.items() if k not in ("all_proxy", "ALL_PROXY")}
|
|
env_hug.update({"HF_HUB_OFFLINE": "1", "HF_ENDPOINT": "https://hf-mirror.com"})
|
|
env_sdk = dict(os.environ) | {"PYTHONPATH": "", "LD_LIBRARY_PATH": ""}
|
|
|
|
print("② HUG 抓取推理(基于点击坐标生成抓取姿态)...")
|
|
# hug_infer_point.py 支持从 camera.json 读取 objects.<name>.uv
|
|
sh([str(HUG_PY), str(ROOT / "scripts/hug_infer_point.py"),
|
|
"--capture", str(cap),
|
|
"--object", "target",
|
|
"--num", str(num)],
|
|
cwd=str(configured_path('HUG_ROOT', 'external/hug')), env=env_hug)
|
|
|
|
print(f"③ 重定向到 L20 {side} 手...")
|
|
sh([str(SDK_PY), str(ROOT / "scripts/hug_to_l20.py"),
|
|
"--npz", str(cap / "hug_target.npz"),
|
|
"--top", "5",
|
|
"--side", side,
|
|
"--table-top", str(table_top)],
|
|
env=env_sdk)
|
|
|
|
return sorted(cap.glob("grasp_l20_target_*.json"),
|
|
key=lambda p: int(p.stem.split("_")[-1]))
|
|
|
|
|
|
def load_grasp(path):
|
|
"""加载抓取位姿"""
|
|
from scipy.spatial.transform import Rotation as Rot
|
|
g = json.loads(Path(path).read_text())
|
|
q = np.asarray(g["wrist_quat_wxyz"], float)
|
|
R = Rot.from_quat([q[1], q[2], q[3], q[0]])
|
|
wrist_euler = R.as_euler('xyz')
|
|
return {
|
|
"wrist_pos": np.array(g["wrist_pos"], float),
|
|
"wrist_euler": wrist_euler,
|
|
"R": R,
|
|
"approach": np.array(g.get("approach", R.as_matrix()[:, 0]), float),
|
|
"hand_qpos": g.get("hand_qpos", {}),
|
|
"metrics": g.get("metrics", {})
|
|
}
|
|
|
|
|
|
def main():
|
|
ap = argparse.ArgumentParser()
|
|
ap.add_argument("--stage", choices=["pregrasp", "grasp"], default="pregrasp")
|
|
ap.add_argument("--side", choices=["left", "right"], required=True, help="使用左手或右手")
|
|
ap.add_argument("--click-uv", default=None, help="手动点击的像素坐标 u,v(逗号分隔)")
|
|
ap.add_argument("--cap", default="data/real_grasp_manual")
|
|
ap.add_argument("--num", type=int, default=16, help="HUG 采样数量")
|
|
ap.add_argument("--clearance", type=float, default=0.10)
|
|
ap.add_argument("--speed", type=float, default=0.3)
|
|
ap.add_argument("--hand-rotation-offset", type=float, default=0.0)
|
|
ap.add_argument("--table-top", type=float, required=True, help="本次实测的base_link桌面高度,禁止沿用旧默认值")
|
|
a = ap.parse_args()
|
|
|
|
cap = ROOT / a.cap
|
|
cap.mkdir(parents=True, exist_ok=True)
|
|
|
|
# 解析点击坐标
|
|
click_uv = None
|
|
if a.click_uv:
|
|
u, v = map(int, a.click_uv.split(","))
|
|
click_uv = (u, v)
|
|
|
|
cands = capture_and_hug(cap, click_uv, a.side, a.num, a.table_top)
|
|
|
|
from hand_table_check import filter_candidates
|
|
cands, rejected = filter_candidates(cands, a.side, a.table_top, a.clearance)
|
|
|
|
if not cands:
|
|
raise SystemExit("没有通过全手进给/闭合桌面检查的候选,未发送运动指令")
|
|
|
|
g = load_grasp(cands[0])
|
|
print(f"选候选 0: {g['metrics']}")
|
|
|
|
arm = RealArm(side=a.side, hand_rotation_offset_deg=a.hand_rotation_offset)
|
|
|
|
# 预抓取点
|
|
pre_pos = g["wrist_pos"] - a.clearance * g["approach"] / (np.linalg.norm(g["approach"]) + 1e-9)
|
|
pre_euler = g["wrist_euler"]
|
|
|
|
print(f"\n抓取腕位(base) {g['wrist_pos'].round(3).tolist()} approach {g['approach'].round(2).tolist()}")
|
|
print(f"预抓取点(后退{a.clearance*100:.0f}cm) {pre_pos.round(3).tolist()}")
|
|
print(f">> 预抓取:把手基座移到预抓取点。IK+MoveJ+路径校验。")
|
|
|
|
ok = arm.movej_wrist(pre_pos, pre_euler, speed=a.speed, verify=True)
|
|
print(f"MoveJ(预抓取) success={ok}")
|
|
|
|
if a.stage == "grasp" and ok:
|
|
print(">> 进给到抓取位(MoveL 直线)")
|
|
arm.movel_wrist(g["wrist_pos"], g["wrist_euler"], speed=a.speed)
|
|
time.sleep(1.0)
|
|
|
|
print(">> 合指(L20)")
|
|
if g["hand_qpos"]:
|
|
qpos_array = np.array([g["hand_qpos"].get(f"joint_{i}", 0.0) for i in range(16)])
|
|
arm.set_hand_qpos(qpos_array)
|
|
else:
|
|
print(" 警告:grasp json 无 hand_qpos,跳过合指")
|
|
|
|
time.sleep(2.0)
|
|
print(">> 抬起 ~15cm")
|
|
up_pos = g["wrist_pos"] + np.array([0, 0, 0.15])
|
|
arm.movel_wrist(up_pos, g["wrist_euler"], speed=a.speed)
|
|
|
|
arm.close()
|
|
print("完成。")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|