Files

359 lines
21 KiB
Python
Raw Permalink Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
"""抓取执行器: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()