Files

121 lines
7.5 KiB
Python

"""真机抓取编排(对应仿真 live_demo.py):采集(D405) → 点云感知 → HUG 抓取 → 重定向 L20 → 右臂执行。
感知/HUG/重定向三段【复用仿真同一批脚本】(子进程),只把采集换真机、坐标用标定 T_base_cam、执行换 real_arm。
⚠ 真机运动由人外部运行(安全分类器);合指需 L20Hand 映射(见 real_arm.set_hand 注释)。跑前臂已使能、在桌面上方、板/物不动、手放急停旁。
分阶段跑更安全,先只到【预抓取】:
.venv/bin/python scripts/real_grasp.py --stage pregrasp --object <名> --cap data/real_grasp
.venv/bin/python scripts/real_grasp.py --stage grasp --object <名> # 装手后:进给+合指+抬起
依赖:标定好的 camera.json(含 T_base_cam) 已在 --cap;对应物体模板 data/templates_<scene>/(感知用)。
"""
import argparse, json, os, subprocess, sys, time
from pathlib import Path
from project_paths import configured_path
import numpy as np
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 perceive_and_plan(cap, obj, scene, num, top, side, table_top):
"""① 采集(真机) ② 感知(写 perception.json,含物体 base_link 位置) ③ HUG ④ 重定向 → grasp_l20_<obj>_*.json(base_link)。
复用 realsense_capture / perceive_object / hug_infer_point / hug_to_l20。"""
env_cam = os.environ | {"A7L20_SCENE": scene}
print("① 采集 D405 + 读取双臂关节角...")
sh([sys.executable, str(ROOT / "scripts/realsense_capture.py"), "--out", str(cap), "--read-joints"])
# 把标定外参写进本帧 camera.json(相机没动,T_base_cam 通用)
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]
(cap / "camera.json").write_text(json.dumps(g, indent=2))
print("② 点云感知(含自体滤除)...")
out = sh([sys.executable, str(ROOT / "scripts/perceive_object.py"), "--capture", str(cap), "--self-filter"], env=env_cam)
print(" " + [l for l in out.splitlines() if l.startswith("[perceive]")][-1])
env_hug = {k: v for k, v in os.environ.items() if k not in ("all_proxy", "ALL_PROXY")} | {"HF_HUB_OFFLINE": "1", "HF_ENDPOINT": "https://hf-mirror.com"}
env_sdk = dict(os.environ) | {"PYTHONPATH": "", "LD_LIBRARY_PATH": ""}
print("③ HUG 抓取推理 ...")
sh([str(HUG_PY), str(ROOT / "scripts/hug_infer_point.py"), "--capture", str(cap), "--object", obj, "--num", str(num), "--perceive"], cwd=str(configured_path('HUG_ROOT', 'external/hug')), env=env_hug)
print("④ 重定向 L20 ...")
sh([str(SDK_PY), str(ROOT / "scripts/hug_to_l20.py"), "--npz", str(cap / f"hug_{obj}.npz"), "--top", str(top), "--side", side, "--table-top", str(table_top)], env=env_sdk)
return sorted(cap.glob(f"grasp_l20_{obj}_*.json"), key=lambda p: int(p.stem.split("_")[-1]))
def load_grasp(path):
"""grasp_l20 json → base_link 的腕位姿 R/pos、approach、hand_qpos。(HUG/重定向已用 T_base_cam 转到 base_link)
注意:返回的是 hand_base_link (wrist) 位姿,不是法兰位姿。
执行时需用 movej_wrist/movel_wrist 自动转换坐标。
"""
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]])
# 提取欧拉角(供 movej_wrist/movel_wrist 使用)
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("--object", required=True); ap.add_argument("--scene", default="ycb1")
ap.add_argument("--cap", default="data/real_grasp"); ap.add_argument("--num", type=int, default=16); ap.add_argument("--top", type=int, default=5)
ap.add_argument("--clearance", type=float, default=0.10); ap.add_argument("--speed", type=float, default=0.3)
ap.add_argument("--table-top", type=float, required=True, help="本次实测base_link桌面高度,不使用棋盘原点高度代替")
ap.add_argument("--d", default=None, help="工具参考点(指尖)相对法兰偏移 x,y,z;缺省用 camera.json 的 fingertip_offset_m")
ap.add_argument("--hand-rotation-offset", type=float, default=0.0, help="真机手安装角度修正(度,绕法兰z轴),正=逆时针")
ap.add_argument("--side", choices=["left", "right"], default="right", help="使用左手或右手")
a = ap.parse_args()
cap = ROOT / a.cap
cands = perceive_and_plan(cap, a.object, a.scene, a.num, a.top, a.side, 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("选候选0:", g["metrics"])
cam = json.loads((cap / "camera.json").read_text())
d = np.array([float(x) for x in a.d.split(",")]) if a.d else np.array(cam.get("fingertip_offset_m", [0, 0, 0]))
arm = RealArm(side=a.side, hand_rotation_offset_deg=a.hand_rotation_offset)
# 预抓取点 = 抓取腕位沿 approach 后退 clearance
pre_pos = g["wrist_pos"] - a.clearance * g["approach"] / (np.linalg.norm(g["approach"]) + 1e-9)
pre_euler = g["wrist_euler"]
print(f"抓取腕位(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(">> 预抓取:把手基座移到预抓取点、姿态=抓取姿态。自动转换到法兰坐标 → IK+MoveJ、FK 路径校验。")
ok = arm.movej_wrist(pre_pos, pre_euler, speed=a.speed, verify=True)
print("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):调用 set_hand_qpos 映射 16 关节 → SDK 20 u8")
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()