373 lines
19 KiB
Python
373 lines
19 KiB
Python
"""拼装场景: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()
|