359 lines
21 KiB
Python
359 lines
21 KiB
Python
"""抓取执行器: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()
|