Files

76 lines
4.6 KiB
Python

"""可视化:把 HUG 人手骨架(世界系)画进场景,并把右臂/右手摆到重定向后的抓取位姿,近距离渲染 / 实时查看。
用法:MUJOCO_GL=glfw .venv/bin/python scripts/show_hug.py --grasp-json data/capture_ready/grasp_l20_jar_0.json [--view] [--hand hug|pre|closed]
输出:out/show_<stem>_{close,top}.png
"""
import argparse, json, os, sys
os.environ.setdefault("MUJOCO_GL", "glfw"); os.environ.setdefault("DISPLAY", ":1")
import os
from pathlib import Path
import mujoco, mujoco.viewer, numpy as np, cv2, time
sys.path.insert(0, str(Path(__file__).resolve().parent))
from grasp_exec import Sim, load_grasp_json, FLEX
ROOT = Path(__file__).resolve().parents[1]
SCENE = os.environ.get("A7L20_SCENE", "a7l20_scene")
PAIRS = [(0,1),(1,2),(2,3),(3,4),(0,5),(5,6),(6,7),(7,8),(9,10),(10,11),(11,12),(13,14),(14,15),(15,16),(0,17),(17,18),(18,19),(19,20),(5,9),(9,13),(13,17)]
def add_skeleton(scn, P, rgba=(0.1, 0.9, 0.2, 0.9), r=0.006):
for p in P:
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(rgba, np.float32))
scn.ngeom += 1
for a, b in PAIRS:
if scn.ngeom >= scn.maxgeom: return
g = scn.geoms[scn.ngeom]
mujoco.mjv_initGeom(g, mujoco.mjtGeom.mjGEOM_CAPSULE, np.zeros(3), np.zeros(3), np.eye(3).ravel(), np.array(rgba, np.float32))
mujoco.mjv_connector(g, mujoco.mjtGeom.mjGEOM_CAPSULE, r * 0.5, np.asarray(P[a], float), np.asarray(P[b], float))
scn.ngeom += 1
def main():
ap = argparse.ArgumentParser(); ap.add_argument("--grasp-json", required=True); ap.add_argument("--view", action="store_true")
ap.add_argument("--hand", default="hug", choices=["hug", "pre", "closed"]); a = ap.parse_args()
sim = Sim(render=False, view=False)
g = load_grasp_json(sim, a.grasp_json); gj = json.loads(Path(a.grasp_json).read_text())
P = np.load(gj["source"])["landmarks_world"][gj["sample"]]
hand = {"hug": g["hand_hug"], "pre": g["hand_pre"], "closed": g["hand_qpos"]}[a.hand]
# 与 grasp_exec.run 相同的路径:预抓取(预成形)→ 沿 approach 进给 → 手到指定姿态
pre = g["wrist_pos"] - 0.10 * g["approach"]
sim.move_to(pre, g["R"], 2.5, hand={k: (sim.hand_ctrl[k], v) for k, v in g["hand_pre"].items()})
sim.move_to(g["wrist_pos"], g["R"], 2.0); sim.hold(0.3, (g["wrist_pos"], g["R"]))
for k, v in hand.items(): sim.hand_ctrl[k] = v
for _ in range(100): sim.step((g["wrist_pos"], g["R"]))
p, R = sim.base_pose(); print("hand base at", p.round(3), "target", g["wrist_pos"].round(3))
gj_scale = gj.get("scale", 1.0); c0 = P[[4, 8, 12, 16, 20]].mean(0); Ps = c0 + gj_scale * (P - c0)
print(f"DIP 关节位置对比(世界系,人手按 scale={gj_scale} 放大)")
for n, i in (("thumb", 3), ("index", 7), ("middle", 11), ("ring", 15), ("pinky", 19)):
rb = sim.d.xpos[sim.m.body(f"right_hand_{n}_distal").id]
print(f" {n:7s} robot {rb.round(3).tolist()} human {Ps[i].round(3).tolist()} diff(cm) {((rb - Ps[i])*100).round(1).tolist()}")
for n, i in (("index_mcp", 5), ("pinky_mcp", 17)):
rb = sim.d.xpos[sim.m.body(f"right_hand_{n.split('_')[0]}_proximal").id]
print(f" {n:9s} robot {rb.round(3).tolist()} human {Ps[i].round(3).tolist()} diff(cm) {((rb - Ps[i])*100).round(1).tolist()}")
obj = sim.obj_pos(g["body"]); print("object", obj.round(3))
stem = Path(a.grasp_json).stem
r = mujoco.Renderer(sim.m, 600, 800)
cams = {"close": (obj + np.array([0.35, -0.45, 0.30]), obj), "top": (obj + np.array([0.02, -0.02, 0.6]), obj), "side": (obj + np.array([0.0, -0.6, 0.15]), obj)}
for name, (cp, tgt) in cams.items():
cam = mujoco.MjvCamera(); cam.type = mujoco.mjtCamera.mjCAMERA_FREE
cam.lookat[:] = tgt; d = cp - tgt; cam.distance = np.linalg.norm(d)
cam.azimuth = np.degrees(np.arctan2(d[1], d[0])); cam.elevation = -np.degrees(np.arcsin(d[2] / cam.distance))
r.update_scene(sim.d, camera=cam); add_skeleton(r.scene, P)
cv2.imwrite(str(ROOT / f"out/show_{stem}_{name}.png"), cv2.cvtColor(r.render(), cv2.COLOR_RGB2BGR))
print("saved out/show_%s_{close,top,side}.png" % stem)
if a.view:
pass
with mujoco.viewer.launch_passive(sim.m, sim.d) as v:
v.cam.lookat[:] = obj; v.cam.distance = 0.7; v.cam.azimuth = 150; v.cam.elevation = -25
while v.is_running():
v.user_scn.ngeom = 0; add_skeleton(v.user_scn, P); sim.step((g["wrist_pos"], g["R"])); v.sync()
if __name__ == "__main__":
main()