121 lines
7.5 KiB
Python
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()
|