Files
a7l20-hug-sim/scene/build_scene.py
T

373 lines
19 KiB
Python
Raw 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.
"""拼装场景:P7(=A7_Lite) 双臂 + L20 左右手 + 桌子 + 物体 + 胸口相机 → scene/a7l20_scene.xml
用法:.venv/bin/python scene/build_scene.py [--kp 2.0] [--check]
"""
from __future__ import annotations
import os
import argparse
import json
import re
from pathlib import Path
import mujoco
import numpy as np
ROOT = Path(__file__).resolve().parents[1]
P7_XML = Path(os.environ.get("P7_SCENE_XML", ROOT / "external/p7_bimanual/model.xml")).expanduser()
if not P7_XML.is_absolute():
P7_XML = ROOT / P7_XML
P7_META = P7_XML.with_name("meta.json")
HAND_URDF = {"right": ROOT / "assets/l20_right_mj.urdf", "left": ROOT / "assets/l20_left_mj.urdf"}
FLANGE = {"right": "arm_right_R8_Link", "left": "arm_left_L8_Link"}
GRIPPER_ROOT = {"right": "grip_right_gripper", "left": "grip_left_gripper"}
OUT_XML = ROOT / "scene/a7l20_scene.xml"
OUT_META = ROOT / "scene/a7l20_scene.json"
ACTIVE_JOINTS = [
"thumb_cmc_roll", "thumb_cmc_yaw", "thumb_cmc_pitch", "thumb_mcp",
"index_mcp_roll", "index_mcp_pitch", "index_pip",
"middle_mcp_roll", "middle_mcp_pitch", "middle_pip",
"ring_mcp_roll", "ring_mcp_pitch", "ring_pip",
"pinky_mcp_roll", "pinky_mcp_pitch", "pinky_pip",
]
# URDF mimic:从动 = k × 主动(线性;真机是分段 LUT,仿真第一版用线性)
MIMIC = {"thumb_ip": ("thumb_mcp", 1.03), "index_dip": ("index_pip", 0.89), "middle_dip": ("middle_pip", 0.89),
"ring_dip": ("ring_pip", 0.89), "pinky_dip": ("pinky_pip", 0.89)}
TIP_LINKS = ["thumb_distal", "index_distal", "middle_distal", "ring_distal", "pinky_distal"]
# 真实桌面高度 75.5 cm(P7 自带 0.75,抬 +5mm 对齐现实)
TABLE_TOP_REAL = 0.755
# 胸口相机 = Intel RealSense D405:最佳深度 7~50cm、FOV 87°H×58°V(848×480+fovy58° 恰好给出 ~87° 水平)。
# 底盘紧贴桌边(躯干前缘 x≈0.12 ≈ 桌近边 x=0.14),单物体放桌中 (0.40, 0, ~0.80);
# 相机沿 45° 俯角、距物 ~0.33m → 世界 (0.17,0,1.03) = 底座局部 (0.17,0,-0.17),物体各面深度落在 0.29~0.36m 甜区。
CHEST_CAM_POS_LOCAL = (0.17, 0.0, -0.17)
CHEST_CAM_LOOKAT_WORLD = (0.40, 0.0, 0.80)
CHEST_CAM_FOVY = 58.0 # D405 竖直视场
def quat_wxyz_from_R(R: np.ndarray) -> np.ndarray:
q = np.empty(4)
mujoco.mju_mat2Quat(q, np.asarray(R, dtype=np.float64).reshape(9))
return q
def look_at_quat(pos, target, up=(0, 0, 1)) -> np.ndarray:
"""MuJoCo 相机沿 -z 看、+y 朝上。"""
f = np.asarray(target, float) - np.asarray(pos, float)
f /= np.linalg.norm(f)
z = -f
x = np.cross(f, up); x /= np.linalg.norm(x)
y = np.cross(z, x)
R = np.stack([x, y, z], axis=1)
assert np.linalg.det(R) > 0.99
return quat_wxyz_from_R(R)
def subtree_names(body):
bodies, joints = [body.name], [j.name for j in body.joints]
for b in body.bodies:
bb, jj = subtree_names(b)
bodies += bb; joints += jj
return bodies, joints
def remove_gripper(spec: mujoco.MjSpec, root_name: str) -> None:
body = spec.body(root_name)
bodies, joints = subtree_names(body)
bset, jset = set(bodies), set(joints)
for a in list(spec.actuators):
if a.target in jset:
spec.delete(a)
for e in list(spec.excludes):
if e.bodyname1 in bset or e.bodyname2 in bset:
spec.delete(e)
for s in list(spec.sensors):
if s.objname in bset or s.objname in jset:
spec.delete(s)
for eq in list(spec.equalities):
if eq.name1 in jset or eq.name2 in jset or eq.name1 in bset or eq.name2 in bset:
spec.delete(eq)
spec.delete(body)
print(f" removed {root_name}: {len(bodies)} bodies, {len(joints)} joints")
def palm_sign(hand_spec: mujoco.MjSpec) -> float:
"""弯食指,看 index_distal 沿 hand_base x 往哪边动:那一边就是掌心。"""
m = hand_spec.compile()
d = mujoco.MjData(m)
mujoco.mj_forward(m, d)
tip = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_BODY, "index_distal")
x0 = d.xpos[tip].copy()
jid = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_JOINT, "index_mcp_pitch")
d.qpos[m.jnt_qposadr[jid]] = 1.0
mujoco.mj_forward(m, d)
dx = d.xpos[tip][0] - x0[0]
return 1.0 if dx > 0 else -1.0
def prepare_hand(side: str, kp: float, kv: float) -> tuple[mujoco.MjSpec, float]:
hs = mujoco.MjSpec.from_file(str(HAND_URDF[side]))
hs.compiler.fusestatic = False # URDF 默认 true:编译时会把无关节的 hand_base_link 融进父体
for j in hs.joints:
j.damping = [0.05, 0.0, 0.0]
j.armature = 5e-4
# CAD 重叠不是真实碰撞:掌 vs 掌骨/近节、拇指链内部
palm = "hand_base_link"
for b in ("thumb_metacarpals_base1", "thumb_metacarpals_base2", "thumb_metacarpals", "thumb_proximal",
"index_metacarpals", "index_proximal", "middle_metacarpals", "middle_proximal",
"ring_metacarpals", "ring_proximal", "pinky_metacarpals", "pinky_proximal"):
hs.add_exclude(name=f"exh_{palm}_{b}", bodyname1=palm, bodyname2=b)
for b1, b2 in (("thumb_metacarpals_base1", "thumb_metacarpals"), ("thumb_metacarpals_base1", "thumb_proximal"),
("thumb_metacarpals_base2", "thumb_proximal"), ("index_metacarpals", "index_middle"),
("middle_metacarpals", "middle_middle"), ("ring_metacarpals", "ring_middle"), ("pinky_metacarpals", "pinky_middle")):
hs.add_exclude(name=f"exh_{b1}_{b2}", bodyname1=b1, bodyname2=b2)
for name in ACTIVE_JOINTS:
j = hs.joint(name)
lo, hi = float(j.range[0]), float(j.range[1])
a = hs.add_actuator(name=f"{name}_act", target=name, trntype=mujoco.mjtTrn.mjTRN_JOINT,
biastype=mujoco.mjtBias.mjBIAS_AFFINE)
a.ctrllimited = mujoco.mjtLimited.mjLIMITED_TRUE
a.ctrlrange = [lo, hi]
a.gainprm = [kp] + [0.0] * 9
a.biasprm = [0.0, -kp, -kv] + [0.0] * 7
a.forcelimited = mujoco.mjtLimited.mjLIMITED_TRUE
a.forcerange = [-1.5, 1.5]
for dep, (src, k) in MIMIC.items():
eq = hs.add_equality(name=f"{dep}_mimic", type=mujoco.mjtEq.mjEQ_JOINT, name1=dep, name2=src)
eq.data = [0.0, k, 0.0, 0.0, 0.0] + [0.0] * 6
sgn = palm_sign(hs.copy()) # compile 会改动 spec,用副本
return hs, sgn
def mount_quat(spec: mujoco.MjSpec, flange: str, side: str, palm_sgn: float) -> np.ndarray:
"""零位下:手 +z 沿法兰向外(链方向 -z),掌心(palm_sgn·x_hand)朝内侧(右臂 +y_world,左臂 -y_world)。"""
m = spec.compile()
d = mujoco.MjData(m)
mujoco.mj_forward(m, d)
bid = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_BODY, flange)
R_wf = d.xmat[bid].reshape(3, 3)
z_hand_w = -R_wf[:, 2] # 向外
inward = np.array([0.0, 1.0 if side == "right" else -1.0, 0.0])
palm_w = inward - np.dot(inward, z_hand_w) * z_hand_w
palm_w /= np.linalg.norm(palm_w)
x_hand_w = palm_sgn * palm_w
y_hand_w = np.cross(z_hand_w, x_hand_w)
R_wh = np.stack([x_hand_w, y_hand_w, z_hand_w], axis=1)
R_fh = R_wf.T @ R_wh
return quat_wxyz_from_R(R_fh)
def scale_objects(spec: mujoco.MjSpec, meta: dict, s: float, spacing: float) -> None:
"""物体 mesh 绕各自 body 原点等比放大 s 倍(顶点内嵌、几何无局部偏移,直接改 mesh.scale),
质量保持不变;沿 y 按 spacing 重新排布,使全部物体落进胸口相机的中心正方形裁剪内。"""
table_top = meta["scene"]["table"]["top"]
for mesh in spec.meshes:
if mesh.name.startswith("obj_"):
mesh.scale = [s, s, s]
objs = meta["scene"]["objects"]; n = len(objs)
for k, o in enumerate(objs):
b = spec.body(o["body"])
z = table_top + (float(b.pos[2]) - table_top) * s + 0.002
y = (k - (n - 1) / 2) * spacing * (1 if objs[-1]["home"][1] > objs[0]["home"][1] else -1)
b.pos = [float(b.pos[0]), y, z]
o["home"] = [float(b.pos[0]), y, z]; o["half_h"] = o["half_h"] * s
print(f" objects scaled x{s}, y spacing {spacing}: " + ", ".join(f"{o['name']}@y={o['home'][1]:+.2f}" for o in objs))
YCB_COLORS = [[0.85, 0.3, 0.25, 1], [0.3, 0.55, 0.85, 1], [0.35, 0.7, 0.35, 1], [0.9, 0.7, 0.2, 1], [0.6, 0.4, 0.75, 1],
[0.9, 0.5, 0.6, 1], [0.4, 0.75, 0.75, 1], [0.7, 0.55, 0.35, 1]]
def replace_with_ycb(spec: mujoco.MjSpec, meta: dict, names: list[str], spacing: float, x: float = 0.40) -> None:
"""删掉 P7 场景自带的 5 个物体,换成 YCB 物体(CoACD 凸分解碰撞),沿 y 等距排在桌上。"""
import sys; sys.path.insert(0, str(ROOT / "scene")); from ycb_objects import prepare, add_ycb_object
for o in meta["scene"]["objects"]:
spec.delete(spec.body(o["body"]))
table_top = meta["scene"]["table"]["top"]
infos = [prepare(n) for n in names]
# 沿 y 依次排布:相邻物体半宽之和 + 4 cm 间隙(至少 spacing),整体居中;第一个在 -y(右臂侧)
widths = [max(i["size"][0], i["size"][1]) for i in infos]
ys = [0.0]
for k in range(1, len(infos)):
ys.append(ys[-1] + max(spacing, widths[k - 1] / 2 + widths[k] / 2 + 0.04))
ys = [y - (ys[0] + ys[-1]) / 2 for y in ys]
if max(abs(y) for y in ys) + max(widths) / 2 > 0.26:
print(f" WARNING: 物体排布超出胸口相机裁剪区(|y|>0.26):ys={np.round(ys, 2).tolist()},考虑减少物体数")
objs = []
for k, info in enumerate(infos):
y = ys[k]
body, pos = add_ycb_object(spec, info, (x, y), table_top, rgba=YCB_COLORS[k % len(YCB_COLORS)])
short = re.sub(r"^\d+[-a-z]*_", "", info["name"])
objs.append({"name": short, "label": info["name"], "body": body, "geom": body, "kind": "ycb", "mass": info["mass_sim"],
"mass_real": info["mass_real"], "half_h": info["size"][2] / 2, "size": info["size"],
"home": list(pos)})
meta["scene"]["objects"] = objs
print(f" YCB objects (spacing {spacing:.2f}): " + ", ".join(f"{o['name']}@y={o['home'][1]:+.2f}" for o in objs))
def raise_table(spec: mujoco.MjSpec, meta: dict, top: float) -> None:
"""把桌子整体沿 z 抬到指定顶面高度(对齐现实桌高);物体随后按新 top 落盘。"""
cur = meta["scene"]["table"]["top"]; dz = top - cur
if abs(dz) < 1e-9:
return
tb = spec.body("table"); p = tb.pos
tb.pos = [float(p[0]), float(p[1]), float(p[2]) + dz]
meta["scene"]["table"]["top"] = top
print(f" table raised {dz*1000:+.0f} mm -> top {top} m")
def build(kp: float, kv: float, obj_scale: float = 1.0, obj_spacing: float = 0.13, ycb: list[str] | None = None) -> tuple[mujoco.MjSpec, dict]:
spec = mujoco.MjSpec.from_file(str(P7_XML))
meta = json.loads(P7_META.read_text())
raise_table(spec, meta, TABLE_TOP_REAL)
if ycb:
replace_with_ycb(spec, meta, ycb, obj_spacing)
elif obj_scale != 1.0 or obj_spacing != 0.13:
scale_objects(spec, meta, obj_scale, obj_spacing)
spec.modelname = "a7l20_bimanual"
spec.compiler.fusestatic = False
spec.visual.global_.offwidth = 1920
spec.visual.global_.offheight = 1080
print("P7 loaded:", len(spec.bodies), "bodies", len(spec.actuators), "actuators")
for side in ("left", "right"):
remove_gripper(spec, GRIPPER_ROOT[side])
for k in list(spec.keys):
spec.delete(k)
info = {"hands": {}, "arms": {}}
for side in ("left", "right"):
hs, sgn = prepare_hand(side, kp, kv)
q = mount_quat(spec, FLANGE[side], side, sgn)
frame = spec.body(FLANGE[side]).add_frame(pos=[0, 0, 0], quat=q)
frame.attach_body(hs.body("hand_base_link"), f"{side}_hand_", "")
info["hands"][side] = {"palm_sign_x": sgn, "mount_quat_wxyz": q.tolist(), "flange": FLANGE[side],
"prefix": f"{side}_hand_", "base_body": f"{side}_hand_hand_base_link",
"active_joints": [f"{side}_hand_{j}" for j in ACTIVE_JOINTS],
"actuators": [f"{side}_hand_{j}_act" for j in ACTIVE_JOINTS],
"tip_bodies": [f"{side}_hand_{t}" for t in TIP_LINKS]}
arm_prefix = "arm_left_L" if side == "left" else "arm_right_R"
hand_root_bodies = [f"{side}_hand_hand_base_link"] + [f"{side}_hand_{b}" for b in
("thumb_metacarpals_base1", "thumb_metacarpals_base2", "thumb_metacarpals",
"index_metacarpals", "middle_metacarpals", "ring_metacarpals", "pinky_metacarpals")]
for li in (6, 7, 8):
for hb in hand_root_bodies:
spec.add_exclude(name=f"ex_{side}_{li}_{hb}", bodyname1=f"{arm_prefix}{li}_Link", bodyname2=hb)
print(f" attached {side} hand at {FLANGE[side]} (palm = {'+' if sgn > 0 else '-'}x_hand), excludes vs L/R6-8")
# 相机
base = spec.body("base_base_link")
cam_pos_world = np.array([0, 0, 1.2]) + np.array(CHEST_CAM_POS_LOCAL)
q_cam = look_at_quat(cam_pos_world, CHEST_CAM_LOOKAT_WORLD)
base.add_camera(name="chest_cam", pos=list(CHEST_CAM_POS_LOCAL), quat=q_cam.tolist(), fovy=CHEST_CAM_FOVY)
spec.worldbody.add_camera(name="front", pos=[1.7, -1.3, 1.45], quat=look_at_quat([1.7, -1.3, 1.45], [0.35, 0, 0.9]).tolist(), fovy=45)
spec.worldbody.add_camera(name="side", pos=[0.4, -1.8, 1.2], quat=look_at_quat([0.4, -1.8, 1.2], [0.35, 0, 0.9]).tolist(), fovy=45)
info["cameras"] = {"chest_cam": {"parent": "base_base_link", "pos_local": list(CHEST_CAM_POS_LOCAL),
"lookat_world": list(CHEST_CAM_LOOKAT_WORLD), "fovy": CHEST_CAM_FOVY}}
# mesh 路径改绝对,导出的 XML 才能独立加载
for mesh in spec.meshes:
f = mesh.file
if f and not f.startswith("/"):
for side in ("left", "right"):
if mesh.name.startswith(f"{side}_hand_"):
mesh.file = str((ROOT / "assets/L20") / ("RIGHT" if side == "right" else "LEFT") / f)
# 关节/执行器信息
for side in ("left", "right"):
info["arms"][side] = {"joints": [f"arm_{side}_{'L' if side == 'left' else 'R'}{i}_Joint" for i in range(1, 8)],
"actuators": meta["arms"][side]["names"], "flange": FLANGE[side],
"ready": meta["arms"][side]["ready"]}
info["table"] = meta["scene"]["table"]
info["objects"] = [{k: o[k] for k in ("name", "body", "mass", "home", "half_h") if k in o} | ({"label": o["label"], "size": o["size"], "mass_real": o["mass_real"]} if o.get("kind") == "ycb" else {}) for o in meta["scene"]["objects"]]
info["obj_scale"] = obj_scale if not ycb else 1.0; info["ycb"] = ycb or []
return spec, info
def add_home_key(spec: mujoco.MjSpec, info: dict) -> None:
m = spec.compile()
qpos = m.qpos0.copy()
ctrl = np.zeros(m.nu)
for side in ("left", "right"):
for jn, q in zip(info["arms"][side]["joints"], info["arms"][side]["ready"]):
jid = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_JOINT, jn)
qpos[m.jnt_qposadr[jid]] = q
for an, q in zip(info["arms"][side]["actuators"], info["arms"][side]["ready"]):
ctrl[mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_ACTUATOR, an)] = q
key = spec.add_key(name="home")
key.qpos = qpos
key.ctrl = ctrl
def set_home(m: mujoco.MjModel, d: mujoco.MjData, info: dict) -> None:
mujoco.mj_resetData(m, d)
for side in ("left", "right"):
for jn, an, q in zip(info["arms"][side]["joints"], info["arms"][side]["actuators"], info["home"]["arm_qpos"][side]):
d.qpos[m.jnt_qposadr[mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_JOINT, jn)]] = q
d.ctrl[mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_ACTUATOR, an)] = q
for an in info["hands"][side]["actuators"]:
d.ctrl[mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_ACTUATOR, an)] = info["home"]["hand_ctrl"]
mujoco.mj_forward(m, d)
def sanity(model_path: Path, info: dict, steps: int = 3000) -> None:
m = mujoco.MjModel.from_xml_path(str(model_path))
d = mujoco.MjData(m)
print(f"standalone load ok: nq={m.nq} nv={m.nv} nu={m.nu} nbody={m.nbody} ngeom={m.ngeom} timestep={m.opt.timestep}")
set_home(m, d, info)
for _ in range(steps):
mujoco.mj_step(m, d)
assert np.all(np.isfinite(d.qpos)), "qpos NaN"
print(f" after {steps} steps: max|qvel|={np.abs(d.qvel).max():.4f}, ncon={d.ncon}")
pairs = {}
for i in range(d.ncon):
c = d.contact[i]
b1 = mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_BODY, m.geom_bodyid[c.geom1])
b2 = mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_BODY, m.geom_bodyid[c.geom2])
pairs[(b1, b2)] = pairs.get((b1, b2), 0) + 1
for (b1, b2), n in sorted(pairs.items(), key=lambda kv: -kv[1])[:12]:
print(f" contact {b1} <-> {b2} x{n}")
for o in info["objects"]:
bid = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_BODY, o["body"])
print(f" {o['name']:7s} z={d.xpos[bid][2]:.3f} (home {o['home'][2]:.3f})")
for side in ("left", "right"):
h = info["hands"][side]
base = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_BODY, h["base_body"])
tips = [d.xpos[mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_BODY, t)] for t in h["tip_bodies"]]
print(f" {side} hand base world={np.round(d.xpos[base],3)} index_tip={np.round(tips[1],3)} thumb_tip={np.round(tips[0],3)}")
# 手指位置伺服是否稳:给食指 pip 1.0 rad,看跟踪
aid = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_ACTUATOR, "right_hand_index_pip_act")
jid = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_JOINT, "right_hand_index_pip")
d.ctrl[aid] = 1.0
for _ in range(1000):
mujoco.mj_step(m, d)
print(f" right index_pip cmd=1.0 -> {d.qpos[m.jnt_qposadr[jid]]:.3f} rad, dip(mimic)={d.qpos[m.jnt_qposadr[mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_JOINT, 'right_hand_index_dip')]]:.3f}")
def main() -> None:
ap = argparse.ArgumentParser()
ap.add_argument("--kp", type=float, default=2.0)
ap.add_argument("--kv", type=float, default=0.05)
ap.add_argument("--steps", type=int, default=3000)
ap.add_argument("--obj-scale", type=float, default=1.5, help="物体等比放大倍数(L20 手比人手大约 1.3~1.5 倍)")
ap.add_argument("--obj-spacing", type=float, default=0.11, help="物体沿 y 的间距")
ap.add_argument("--ycb", default=None, help="逗号分隔的 YCB 物体名(assets/ycb/ 下目录名),给了就替换自带 5 物体")
ap.add_argument("--out", default="a7l20_scene", help="输出 scene/<out>.xml/.json")
args = ap.parse_args()
global OUT_XML, OUT_META
OUT_XML = ROOT / f"scene/{args.out}.xml"; OUT_META = ROOT / f"scene/{args.out}.json"
spec, info = build(args.kp, args.kv, args.obj_scale, args.obj_spacing, args.ycb.split(",") if args.ycb else None)
info["home"] = {"arm_qpos": {"left": [0.0] * 7, "right": [0.0] * 7}, "hand_ctrl": 0.0,
"note": "第一版 home = 双臂零位(手垂在桌前沿之后,不碰桌);ready 姿态待 IK 生成"}
for cand in (OUT_META, ROOT / "scene/a7l20_scene.json"):
if cand.exists() and "ready" in json.loads(cand.read_text()):
info["ready"] = json.loads(cand.read_text())["ready"]; break
m = spec.compile()
OUT_XML.write_text(spec.to_xml(), encoding="utf-8")
OUT_META.write_text(json.dumps(info, indent=2, ensure_ascii=False), encoding="utf-8")
print(f"wrote {OUT_XML} ({OUT_XML.stat().st_size/1e6:.1f} MB), {OUT_META}")
sanity(OUT_XML, info, args.steps)
if __name__ == "__main__":
main()