"""抓取执行器:ready → 预抓取 → 抓取位姿 → 接触触发合指 → 抬起 → 判定。右臂 + L20 右手。 用法(a7l20 .venv): MUJOCO_GL=glfw .venv/bin/python scripts/grasp_exec.py --manual jar [--render] [--view] MUJOCO_GL=glfw .venv/bin/python scripts/grasp_exec.py --grasp-json data/capture_ready/grasp_l20_jar.json --render grasp json 字段:object, wrist_pos(3), wrist_quat_wxyz(4)(= right_hand_hand_base_link 世界位姿), hand_qpos {关节名: 角度}(16 主动,可缺省=张开),approach(3, 可选,世界系单位向量,默认 = -掌心法线) 手系:掌心 = +x_hand,指向 = +z_hand,拇指在 +y_hand 侧、index→pinky 沿 -y_hand。 """ from __future__ import annotations import argparse, json, os, sys, time os.environ.setdefault("MUJOCO_GL", "glfw"); os.environ.setdefault("DISPLAY", ":1") import os from pathlib import Path import mujoco, mujoco.viewer, numpy as np, mink sys.path.insert(0, str(Path(__file__).resolve().parent)) from scipy.spatial.transform import Rotation as Rot, Slerp ROOT = Path(__file__).resolve().parents[1] SCENE = os.environ.get("A7L20_SCENE", "a7l20_scene") XML, META = ROOT / f"scene/{SCENE}.xml", ROOT / f"scene/{SCENE}.json" SIDE = "right" FINGERS = {"thumb": ["thumb_metacarpals", "thumb_proximal", "thumb_distal"], "index": ["index_proximal", "index_middle", "index_distal"], "middle": ["middle_proximal", "middle_middle", "middle_distal"], "ring": ["ring_proximal", "ring_middle", "ring_distal"], "pinky": ["pinky_proximal", "pinky_middle", "pinky_distal"]} FLEX = {"thumb": ["thumb_cmc_pitch", "thumb_mcp"], "index": ["index_mcp_pitch", "index_pip"], "middle": ["middle_mcp_pitch", "middle_pip"], "ring": ["ring_mcp_pitch", "ring_pip"], "pinky": ["pinky_mcp_pitch", "pinky_pip"]} # 手工闭合目标(rad):先转拇指对掌,再屈 CLOSED = {"thumb_cmc_yaw": 1.2, "thumb_cmc_roll": 0.6, "thumb_cmc_pitch": 0.6, "thumb_mcp": 1.0, "index_mcp_pitch": 1.1, "index_pip": 1.5, "middle_mcp_pitch": 1.1, "middle_pip": 1.5, "ring_mcp_pitch": 1.1, "ring_pip": 1.5, "pinky_mcp_pitch": 1.1, "pinky_pip": 1.5} PRE_FRAC = 0.35; THUMB_PRE_FRAC = float(os.environ.get("THUMB_PRE_FRAC", "0.0")); THUMB_MODE = os.environ.get("THUMB_MODE", "park") # oppose: 进给时拇指已对掌;park: 进给时拇指侧放(yaw/roll=0),到位后再摆到对掌 SQUEEZE = float(os.environ.get("GRASP_SQUEEZE", "0.25")) # 某指触物后额外屈曲量(rad):小/圆物(苹果)指尖接触易滑脱,加大挤入形成包络 # closure 模式预成形:四指屈曲 = PRE_FRAC × CLOSED(与 hug_to_l20 的桌面穿透检查一致) PRESHAPE = {"thumb_cmc_yaw": 1.0, "thumb_cmc_roll": 0.4, "index_mcp_pitch": 0.3, "middle_mcp_pitch": 0.3, "ring_mcp_pitch": 0.3, "pinky_mcp_pitch": 0.3} def add_pointcloud(scn, pts, cols, r=0.004): """把世界系点云 pts(Nx3) 按颜色 cols(Nx3, 0~1) 作为小球加进 mjvScene(viewer 的 user_scn)。""" for p, c in zip(pts, cols): if scn.ngeom >= scn.maxgeom: return g = scn.geoms[scn.ngeom] mujoco.mjv_initGeom(g, mujoco.mjtGeom.mjGEOM_SPHERE, np.array([r, 0, 0]), np.asarray(p, float), np.eye(3).ravel(), np.array([c[0], c[1], c[2], 1.0], np.float32)) scn.ngeom += 1 class Sim: def __init__(self, render: bool, view: bool): self.m = mujoco.MjModel.from_xml_path(str(XML)); self.d = mujoco.MjData(self.m) self.info = json.loads(META.read_text()) self.arm_j = self.info["arms"][SIDE]["joints"]; self.arm_a = self.info["arms"][SIDE]["actuators"] self.hand = self.info["hands"][SIDE]; self.base_body = self.hand["base_body"] self.hj_names = [j.replace(f"{SIDE}_hand_", "") for j in self.hand["active_joints"]] self.hand_aid = {n: self.m.actuator(f"{SIDE}_hand_{n}_act").id for n in self.hj_names} self.hand_qadr = {n: self.m.jnt_qposadr[self.m.joint(f"{SIDE}_hand_{n}").id] for n in self.hj_names} self.hand_ctrl = {n: 0.0 for n in self.hj_names} self.finger_bodies = {f: [self.m.body(f"{SIDE}_hand_{b}").id for b in bs] for f, bs in FINGERS.items()} self.palm_bid = self.m.body(self.base_body).id self.ctrl_dt = 0.01; self.nsub = int(round(self.ctrl_dt / self.m.opt.timestep)); self.t = 0.0 # 初始:ready self.set_pose(self.info["ready"]) # mink:只积分右臂 self.cfg = mink.Configuration(self.m); self.cfg.update(self.d.qpos.copy()) self.task = mink.FrameTask(self.base_body, "body", position_cost=1.0, orientation_cost=0.5, lm_damping=1e-2) self.posture = mink.PostureTask(self.m, cost=1e-3); self.posture.set_target(self.d.qpos.copy()) self.limits = [mink.ConfigurationLimit(self.m)] self.mask = np.zeros(self.m.nv) for jn in self.arm_j: self.mask[self.m.jnt_dofadr[self.m.joint(jn).id]] = 1.0 self.render = render; self.frames = []; self.view = view; self.viewer = None; self.focus_body = None; self.skeleton = None; self.pcd = None; self.verbose = True if render: self.renderer = mujoco.Renderer(self.m, 480, 848) if view: pass self.viewer = mujoco.viewer.launch_passive(self.m, self.d) self.viewer.cam.lookat[:] = (0.4, -0.1, 0.85); self.viewer.cam.distance = 1.6; self.viewer.cam.azimuth = 160; self.viewer.cam.elevation = -25 self.log = [] def set_pose(self, pose): m, d = self.m, self.d mujoco.mj_resetData(m, d) for side in ("left", "right"): for jn, an, q in zip(self.info["arms"][side]["joints"], self.info["arms"][side]["actuators"], pose["arm_qpos"][side]): d.qpos[m.jnt_qposadr[m.joint(jn).id]] = q; d.ctrl[m.actuator(an).id] = q mujoco.mj_forward(m, d) # ---- 基本量 def base_pose(self): return self.d.xpos[self.palm_bid].copy(), self.d.xmat[self.palm_bid].reshape(3, 3).copy() def obj_pos(self, body): return self.d.xpos[self.m.body(body).id].copy() def contacts(self, obj_body, any_body=False): """返回 {finger: n_contacts} 与掌心接触数(与物体子树;any_body=True 时与手以外的任何 body)""" m, d = self.m, self.d obj_root = m.body(obj_body).id def in_obj(bid): while bid != 0: if bid == obj_root: return True bid = m.body_parentid[bid] return False touch = {f: 0 for f in FINGERS}; palm = 0 for i in range(d.ncon): c = d.contact[i]; b1, b2 = m.geom_bodyid[c.geom1], m.geom_bodyid[c.geom2] hand_bids = set(sum(self.finger_bodies.values(), [])) | {self.palm_bid} for hb, ob in ((b1, b2), (b2, b1)): if in_obj(ob) or (any_body and hb in hand_bids and ob not in hand_bids and not self._is_hand_subtree(ob)): for f, bids in self.finger_bodies.items(): if hb in bids: touch[f] += 1 if hb == self.palm_bid: palm += 1 return touch, palm def touching(self, finger): m, d = self.m, self.d; out = set() for i in range(d.ncon): c = d.contact[i]; b1, b2 = m.geom_bodyid[c.geom1], m.geom_bodyid[c.geom2] for hb, ob in ((b1, b2), (b2, b1)): if hb in self.finger_bodies[finger] and not self._is_hand_subtree(ob) and c.dist < 0.0: out.add(m.body(ob).name) return sorted(out) def _is_hand_subtree(self, bid): m = self.m while bid != 0: if bid == self.palm_bid: return True bid = m.body_parentid[bid] return False # ---- 一步控制 def step(self, target=None): """target = (pos, R) 或 None(臂保持);手用 self.hand_ctrl""" if target is not None: pos, R = target q = np.empty(4); mujoco.mju_mat2Quat(q, R.reshape(9)) self.task.set_target(mink.SE3(np.concatenate([q, pos]))) for _ in range(3): v = mink.solve_ik(self.cfg, [self.task, self.posture], 0.01, "daqp", damping=1e-3, limits=self.limits) self.cfg.integrate_inplace(v * self.mask, 0.01) for jn, an in zip(self.arm_j, self.arm_a): self.d.ctrl[self.m.actuator(an).id] = self.cfg.q[self.m.jnt_qposadr[self.m.joint(jn).id]] for n, v in self.hand_ctrl.items(): self.d.ctrl[self.hand_aid[n]] = v for _ in range(self.nsub): mujoco.mj_step(self.m, self.d) self.t += self.ctrl_dt if self.viewer is not None: scn = self.viewer.user_scn; scn.ngeom = 0 if self.pcd is not None: # D405 相机点云快照(RGB 上色的小球) add_pointcloud(scn, self.pcd[0], self.pcd[1]) if self.skeleton is not None: # 绿色 = HUG 生成的人手骨架(世界系) from show_hug import add_skeleton; add_skeleton(scn, self.skeleton) self.viewer.sync(); time.sleep(self.ctrl_dt * 0.5) if self.render and int(round(self.t / self.ctrl_dt)) % 4 == 0: # 25 fps:左 front 全景 + 右 物体特写 self.renderer.update_scene(self.d, camera="front"); a = self.renderer.render().copy() if self.focus_body is not None: obj = self.d.xpos[self.m.body(self.focus_body).id] cam = mujoco.MjvCamera(); cam.type = mujoco.mjtCamera.mjCAMERA_FREE cam.lookat[:] = obj + np.array([0, 0, 0.03]); cam.distance = 0.45; cam.azimuth = 200; cam.elevation = -30 self.renderer.update_scene(self.d, camera=cam) if self.skeleton is not None: from show_hug import add_skeleton; add_skeleton(self.renderer.scene, self.skeleton) b = self.renderer.render().copy() a = np.concatenate([a, b], axis=1) self.frames.append(a) def move_to(self, pos1, R1, duration, hand=None, stop_fn=None): pos0, R0 = self.cfg_base_pose() sl = Slerp([0, 1], Rot.from_matrix(np.stack([R0, R1]))) n = int(duration / self.ctrl_dt) for k in range(1, n + 1): s = k / n; s = 3 * s**2 - 2 * s**3 if hand is not None: for kk, (a, b) in hand.items(): self.hand_ctrl[kk] = a + s * (b - a) self.step((pos0 + s * (pos1 - pos0), sl(s).as_matrix())) if stop_fn is not None and stop_fn(): return True return False def cfg_base_pose(self): """IK 参考位形下的手基座位姿(不是物理实际,避免接触反馈抖动)""" data = self.cfg.data bid = self.palm_bid return data.xpos[bid].copy(), data.xmat[bid].reshape(3, 3).copy() def hold(self, duration, target=None): for _ in range(int(duration / self.ctrl_dt)): self.step(target) def close_fingers(self, obj_body, closed: dict, duration=1.5, squeeze=SQUEEZE, target=None, thumb_lag=0.5): """按 ramp 合指;某指一碰到物体就停在当前角 + squeeze。四指先合、拇指滞后 thumb_lag(比例)再合, 避免拇指先把物体推出手心。返回各指是否接触。""" start = dict(self.hand_ctrl); frozen = {} pre_touch = {f: set(self.touching(f)) for f in FLEX} # 闭合前已有的接触(如拇指搭在桌上)不算触发 n = int(duration / duration if False else duration / self.ctrl_dt) for k in range(1, n + 1): s = k / n touch_obj, palm = self.contacts(obj_body) for f, js in FLEX.items(): if f in frozen: continue new_touch = set(self.touching(f)) - pre_touch[f] if touch_obj[f] > 0 or new_touch: frozen[f] = True if self.verbose: print(f" t={self.t:.2f}s {f} stopped: touching {self.touching(f)} at q={ {j: round(float(self.d.qpos[self.hand_qadr[j]]),2) for j in js} }") for j in js: cur = self.d.qpos[self.hand_qadr[j]] self.hand_ctrl[j] = min(cur + squeeze, self.m.actuator(self.hand_aid[j]).ctrlrange[1]) for j, v in closed.items(): f = j.split("_")[0] if f in frozen and j in FLEX.get(f, []): continue sj = s if not (f == "thumb" and j in FLEX["thumb"]) else float(np.clip((s - thumb_lag) / (1 - thumb_lag), 0, 1)) self.hand_ctrl[j] = start[j] + sj * (v - start[j]) self.step(target) touch, palm = self.contacts(obj_body) return touch, palm def go_ready(self, duration=2.5, open_hand=True): """关节空间插值回 ready,手张开;结束后把 mink 参考位形同步到实际状态。""" ready = self.info["ready"]["arm_qpos"][SIDE] q0 = [self.d.ctrl[self.m.actuator(an).id] for an in self.arm_a] h0 = dict(self.hand_ctrl) n = int(duration / self.ctrl_dt) for k in range(1, n + 1): s = k / n; s = 3 * s**2 - 2 * s**3 for an, a, b in zip(self.arm_a, q0, ready): self.d.ctrl[self.m.actuator(an).id] = a + s * (b - a) if open_hand: for j in self.hand_ctrl: self.hand_ctrl[j] = h0[j] * (1 - s) self.step(None) self.cfg.update(self.d.qpos.copy()) def release(self, duration=1.0, target=None): h0 = dict(self.hand_ctrl); n = int(duration / self.ctrl_dt) for k in range(1, n + 1): s = k / n for j in self.hand_ctrl: self.hand_ctrl[j] = h0[j] * (1 - s) self.step(target) def save_video(self, path): if not self.frames: return import cv2 h, w = self.frames[0].shape[:2] vw = cv2.VideoWriter(str(path), cv2.VideoWriter_fourcc(*"mp4v"), 25, (w, h)) for f in self.frames: vw.write(cv2.cvtColor(f, cv2.COLOR_RGB2BGR)) vw.release(); print(f"video -> {path} ({len(self.frames)} frames)") def manual_grasp(sim: Sim, obj: str): """俯抓:掌心朝下、指向 +x,中指根在物体正上方;拇指在 +y 侧。""" body = next(o["body"] for o in sim.info["objects"] if o["name"] == obj) c = sim.obj_pos(body) R = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]], float).T # 列 = x_hand(-z_w), y_hand(+y_w), z_hand(+x_w) R = np.stack([np.array([0, 0, -1.0]), np.array([0, 1.0, 0]), np.array([1.0, 0, 0])], axis=1) top = {"jar": 0.802, "can": 0.81, "bottle": 0.86, "toy": 0.83, "piano": 0.79}.get(obj, c[2] + 0.03) pos = c - R @ np.array([0.0, 0.012, 0.165]) + np.array([0, 0, (top - c[2]) + 0.045]) return {"object": obj, "body": body, "wrist_pos": pos, "R": R, "hand_qpos": CLOSED, "approach": np.array([0, 0, -1.0])} def load_grasp_json(sim: Sim, path: Path): g = json.loads(Path(path).read_text()) body = next(o["body"] for o in sim.info["objects"] if o["name"] == g["object"]) q = np.asarray(g["wrist_quat_wxyz"], float); R = Rot.from_quat([q[1], q[2], q[3], q[0]]).as_matrix() hug = {k: float(v) for k, v in g.get("hand_qpos", {}).items()} # 合指目标 = 全闭合,但拇指对掌(yaw/roll)和各指侧摆(mcp_roll)沿用 HUG 姿态;屈曲关节由接触触发停下 hq = dict(CLOSED) for k in ("thumb_cmc_yaw", "thumb_cmc_roll", "index_mcp_roll", "middle_mcp_roll", "ring_mcp_roll", "pinky_mcp_roll"): if k in hug: hq[k] = hug[k] # 预成形 = HUG 姿态但四指屈曲张开到 60%、拇指屈曲清零(只保留 yaw/roll 对掌),进给时不撞物体/桌面 pre = dict(hug) for f, js in FLEX.items(): for j in js: if j in pre: pre[j] = 0.0 if f == "thumb" else 0.6 * pre[j] hug_app = dict(hug) for j in FLEX["thumb"]: hug_app[j] = 0.0 if g.get("anchor") == "closure": # 闭合中心对齐:进给时全张开、拇指按 CLOSED 对掌(HUG 的拇指对掌是人手几何,L20 照搬会把物体往下压),到位后直接接触触发闭合 for f, js in FLEX.items(): for j in js: pre[j] = (THUMB_PRE_FRAC if f == "thumb" else PRE_FRAC) * CLOSED[j] for k in ("thumb_cmc_yaw", "thumb_cmc_roll"): pre[k] = (0.0 if THUMB_MODE == "park" else CLOSED[k]); hq[k] = CLOSED[k] hug_app = None approach = np.asarray(g.get("approach", R[:, 0]), float); approach /= np.linalg.norm(approach) skel = None if "source" in g and Path(g["source"]).exists(): skel = np.load(g["source"])["landmarks_world"][int(g.get("sample", 0))] return {"object": g["object"], "body": body, "wrist_pos": np.asarray(g["wrist_pos"], float), "R": R, "hand_qpos": hq, "approach": approach, "hand_pre": pre, "hand_hug": hug_app, "skeleton": skel} def run(sim: Sim, g: dict, lift=0.15, pre_dist=0.10, sink=0.03): body = g["body"]; z0 = sim.obj_pos(body)[2]; sim.focus_body = body; sim.skeleton = g.get("skeleton"); o0 = sim.obj_pos(body) def obj_moved(): return f"obj moved {np.linalg.norm(sim.obj_pos(body) - o0)*100:.1f} cm" pos, R = g["wrist_pos"], g["R"]; app = g["approach"] pre = pos - pre_dist * app print(f"[grasp] {g['object']}: wrist target {pos.round(3).tolist()}, approach {app.round(2).tolist()}, pre {pre.round(3).tolist()}") # 1) 预抓取(同时预成形) pre_hand = {k: (sim.hand_ctrl[k], v) for k, v in (g.get("hand_pre") or PRESHAPE).items()} sim.move_to(pre, R, 2.5, hand=pre_hand); sim.hold(0.3, (pre, R)) p, Rr = sim.base_pose(); e = np.linalg.norm(p - pre) print(f" pre-grasp reached: pos err {e*1000:.1f} mm, {obj_moved()}") # 2) 沿 approach 进给到抓取位姿 sim.move_to(pos, R, 2.0); sim.hold(0.3, (pos, R)) p, Rr = sim.base_pose(); e = np.linalg.norm(p - pos) ang = np.rad2deg(np.arccos(np.clip((np.trace(Rr.T @ R) - 1) / 2, -1, 1))) touch, palm = sim.contacts(body) print(f" grasp pose reached: pos err {e*1000:.1f} mm, ang err {ang:.1f} deg, contacts before close {touch} palm={palm}, {obj_moved()}") # 2b) 沿 approach 继续下沉至首次触物(最多 sink),补偿人手/机器手尺寸差带来的"悬空" if sink > 0: hit = sim.move_to(pos + sink * app, R, 1.5, stop_fn=lambda: sum(sim.contacts(body)[0].values()) + sim.contacts(body)[1] > 0) pos, _ = sim.cfg_base_pose(); pos = pos.copy() touch, palm = sim.contacts(body) print(f" sink: {'contact' if hit else 'no contact'} after {np.linalg.norm(pos - g['wrist_pos'])*100:.1f} cm, contacts {touch} palm={palm}, {obj_moved()}") # 3) 合指(接触触发):先到 HUG 姿态(0.8 s),再继续闭合直到各指触物 if g.get("anchor") == "closure" and THUMB_MODE == "park": t0 = {k: sim.hand_ctrl[k] for k in ("thumb_cmc_yaw", "thumb_cmc_roll")} for k_ in range(80): s_ = (k_ + 1) / 80 for k in t0: sim.hand_ctrl[k] = t0[k] + s_ * (CLOSED[k] - t0[k]) sim.step((pos, R)) touch, palm = sim.contacts(body); print(f" thumb swung to opposition: contacts {touch}, {obj_moved()}") if g.get("hand_hug"): sim.close_fingers(body, g["hand_hug"], duration=0.8, target=(pos, R)) touch, palm = sim.contacts(body); print(f" at HUG hand pose: contacts {touch} palm={palm}") touch, palm = sim.close_fingers(body, g["hand_qpos"], target=(pos, R)); sim.hold(0.5, (pos, R)) touch, palm = sim.contacts(body) print(f" closed: contacts {touch} palm={palm}; hand q = { {k: round(float(sim.d.qpos[sim.hand_qadr[k]]),2) for k in ('thumb_mcp','index_pip','middle_pip','ring_pip','pinky_pip')} }") # 4) 抬起 up = pos + np.array([0, 0, lift]) sim.move_to(up, R, 2.0); sim.hold(1.0, (up, R)) z1 = sim.obj_pos(body)[2]; p, _ = sim.base_pose(); dist = np.linalg.norm(sim.obj_pos(body) - p) touch, palm = sim.contacts(body) ok = (z1 - z0) > lift * 0.6 and dist < 0.25 print(f" lift: object dz = {z1 - z0:+.3f} m (target {lift}), obj-to-wrist {dist:.3f} m, contacts {touch} -> {'SUCCESS' if ok else 'FAIL'}") return ok def main(): ap = argparse.ArgumentParser() ap.add_argument("--manual", default=None, help="物体名,用手工俯抓姿态") ap.add_argument("--grasp-json", default=None) ap.add_argument("--render", action="store_true"); ap.add_argument("--view", action="store_true") ap.add_argument("--tag", default=None); ap.add_argument("--sink", type=float, default=0.0) a = ap.parse_args() sim = Sim(render=a.render, view=a.view) g = manual_grasp(sim, a.manual) if a.manual else load_grasp_json(sim, a.grasp_json) ok = run(sim, g, sink=a.sink) if a.render: (ROOT / "out").mkdir(exist_ok=True) tag = a.tag or (f"manual_{a.manual}" if a.manual else Path(a.grasp_json).stem) sim.save_video(ROOT / f"out/grasp_{tag}.mp4") import cv2 for i, k in enumerate(np.linspace(0, len(sim.frames) - 1, 6).astype(int)): cv2.imwrite(str(ROOT / f"out/grasp_{tag}_{i}.png"), cv2.cvtColor(sim.frames[k], cv2.COLOR_RGB2BGR)) if sim.viewer is not None: print("viewer: 抓取序列已跑完,窗口保持(物体仍在手里);关窗退出") while sim.viewer.is_running(): sim.step((sim.cfg_base_pose())) sys.exit(0 if ok else 1) if __name__ == "__main__": main()