Files

89 lines
4.5 KiB
Python
Raw Permalink 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.
"""为当前场景的每个活跃物体,从其视觉 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()