89 lines
4.5 KiB
Python
89 lines
4.5 KiB
Python
"""为当前场景的每个活跃物体,从其视觉 mesh 在 home(沉降稳定)位姿下采样表面点,
|
||
生成平面 ICP 用的模板点云:data/templates_<scene>/<name>.npy + templates.json(元数据)。
|
||
|
||
模板约定:xy 归心到 (0,0)、yaw=0(= home 朝向),保留世界系的 z 剖面(物体躺在桌上的真实高度分布),
|
||
感知端用平面 SE(2)(dx,dy,dyaw)把模板配到观测点云上。这样换场景/换物体只重跑这一步,感知代码不动。
|
||
|
||
用法:MUJOCO_GL=glfw A7L20_SCENE=ycb1 .venv/bin/python scripts/build_templates.py [--num 2000]
|
||
"""
|
||
import argparse, json, os
|
||
os.environ.setdefault("MUJOCO_GL", "glfw"); os.environ.setdefault("DISPLAY", ":1")
|
||
from pathlib import Path
|
||
import mujoco, numpy as np, trimesh
|
||
|
||
ROOT = Path(__file__).resolve().parents[1]
|
||
SCENE = os.environ.get("A7L20_SCENE", "a7l20_scene")
|
||
|
||
|
||
def set_home(m, d, info):
|
||
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[m.joint(jn).id]] = q
|
||
mujoco.mj_forward(m, d)
|
||
|
||
|
||
def vis_geom_of_body(m, body_id, name):
|
||
"""返回该 body 下视觉 mesh geom 的 id(优先 obj_<name>_vis,否则该 body 第一个 mesh geom)。"""
|
||
want = f"obj_{name}_vis"
|
||
fallback = None
|
||
for gid in range(m.ngeom):
|
||
if m.geom_bodyid[gid] != body_id or m.geom_type[gid] != mujoco.mjtGeom.mjGEOM_MESH:
|
||
continue
|
||
mesh_name = mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_MESH, m.geom_dataid[gid])
|
||
if mesh_name == want:
|
||
return gid
|
||
if fallback is None:
|
||
fallback = gid
|
||
return fallback
|
||
|
||
|
||
def mesh_points_world(m, d, gid, num):
|
||
"""把 geom 引用的 mesh 顶点/面取出,用 trimesh 均匀采表面点(带外法线),再按该 geom 在 home 时的世界位姿变换到世界系。
|
||
返回 (pts Nx3, normals Nx3),均在世界系。法线供感知端的相机可见性测试用(面向相机的模板点才计入覆盖率)。"""
|
||
mid = m.geom_dataid[gid]
|
||
va, nv = m.mesh_vertadr[mid], m.mesh_vertnum[mid]
|
||
fa, nf = m.mesh_faceadr[mid], m.mesh_facenum[mid]
|
||
V = m.mesh_vert[va:va + nv].astype(np.float64)
|
||
F = m.mesh_face[fa:fa + nf].astype(np.int64)
|
||
tm = trimesh.Trimesh(vertices=V, faces=F, process=False)
|
||
pts, fidx = trimesh.sample.sample_surface_even(tm, num)
|
||
if len(pts) < num * 0.6: # even 采样可能不足,退化到面积加权采样补齐
|
||
pts, fidx = trimesh.sample.sample_surface(tm, num)
|
||
nrm = tm.face_normals[fidx]
|
||
R = d.geom_xmat[gid].reshape(3, 3); t = d.geom_xpos[gid]
|
||
return pts @ R.T + t, nrm @ R.T # 世界系(法线只转不平移)
|
||
|
||
|
||
def main():
|
||
ap = argparse.ArgumentParser()
|
||
ap.add_argument("--num", type=int, default=2000, help="每个物体采样点数")
|
||
a = ap.parse_args()
|
||
m = mujoco.MjModel.from_xml_path(str(ROOT / f"scene/{SCENE}.xml")); d = mujoco.MjData(m)
|
||
info = json.loads((ROOT / f"scene/{SCENE}.json").read_text())
|
||
set_home(m, d, info)
|
||
out = ROOT / f"data/templates_{SCENE}"; out.mkdir(parents=True, exist_ok=True)
|
||
meta = {}
|
||
for o in info["objects"]:
|
||
name, body = o["name"], o["body"]
|
||
bid = m.body(body).id
|
||
gid = vis_geom_of_body(m, bid, name)
|
||
assert gid is not None, f"{name}: 找不到 mesh 视觉 geom"
|
||
Pw, Nw = mesh_points_world(m, d, gid, a.num)
|
||
c = d.xpos[bid].copy() # home 时 body 原点世界坐标(= 感知要估计的量)
|
||
Pc = Pw.copy(); Pc[:, :2] -= c[:2] # xy 归心到 body 原点,z 保留世界高度
|
||
z_bot, z_top = float(Pc[:, 2].min()), float(Pc[:, 2].max())
|
||
foot_r = float(np.linalg.norm(Pc[:, :2], axis=1).max())
|
||
diag = float(np.linalg.norm(Pc.max(0) - Pc.min(0)))
|
||
np.savez(out / f"{name}.npz", pts=Pc.astype(np.float32), nrm=Nw.astype(np.float32))
|
||
meta[name] = {"body": body, "n": int(len(Pc)), "body_z": round(float(c[2]), 4),
|
||
"z_bottom": round(z_bot, 4), "z_top": round(z_top, 4), "z_extent": round(z_top - z_bot, 4),
|
||
"footprint_r": round(foot_r, 4), "diag": round(diag, 4)}
|
||
print(f" {name:22s} pts={len(Pc):4d} foot_r={foot_r*100:4.1f}cm z_ext={z_top-z_bot:.3f}m diag={diag:.3f}m")
|
||
(out / "templates.json").write_text(json.dumps({"scene": SCENE, "objects": meta}, indent=2))
|
||
print(f"wrote {out}/ ({len(meta)} templates)")
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|