82 lines
4.3 KiB
Python
82 lines
4.3 KiB
Python
"""YCB 物体 → MuJoCo 资产:读 assets/ycb/<name>/google_16k/{textured.obj, nontextured.stl},
|
||
用 CoACD 做凸分解(缓存到 assets/ycb/<name>/coacd_*.obj),给出 mesh 底面高度、质量估计等。
|
||
|
||
用法(预处理,可选):.venv/bin/python scene/ycb_objects.py 005_tomato_soup_can-1 013_apple ...
|
||
"""
|
||
from __future__ import annotations
|
||
import json, sys
|
||
from pathlib import Path
|
||
import numpy as np
|
||
|
||
ROOT = Path(__file__).resolve().parents[1]
|
||
YCB = ROOT / "assets/ycb"
|
||
# 质量(kg)估计:按真实物体大致取,缺省 0.15
|
||
MASS = {"002_master_chef_can": 0.41, "003_cracker_box": 0.41, "004_sugar_box": 0.51, "005_tomato_soup_can-1": 0.35,
|
||
"006_mustard_bottle": 0.60, "007_tuna_fish_can": 0.17, "008_pudding_box": 0.19, "009_gelatin_box": 0.10,
|
||
"010_potted_meat_can": 0.37, "011_banana": 0.07, "013_apple": 0.07, "014_lemon": 0.03, "016_pear": 0.05,
|
||
"017_orange": 0.05, "021_bleach_cleanser": 1.1, "025_mug": 0.12, "036_wood_block": 0.73, "040_large_marker": 0.02,
|
||
"054_softball": 0.19, "055_baseball": 0.15, "056_tennis_ball": 0.06, "061_foam_brick": 0.03, "065-c_cups": 0.02,
|
||
"077_rubiks_cube": 0.09}
|
||
# 仿真里统一封顶,免得 L20 1.5 N 力矩限幅抓不动(真实质量另记在 meta 里)
|
||
MASS_CAP = 0.25
|
||
|
||
|
||
def prepare(name: str, max_hulls: int = 8, threshold: float = 0.05) -> dict:
|
||
import trimesh
|
||
d = YCB / name
|
||
obj = d / "textured.obj"; stl = d / "nontextured.stl"
|
||
src = obj if obj.exists() else stl
|
||
mesh = trimesh.load(str(src), force="mesh", process=False)
|
||
if not mesh.is_watertight:
|
||
mesh_c = trimesh.load(str(stl), force="mesh", process=True) if stl.exists() else mesh
|
||
else:
|
||
mesh_c = mesh
|
||
bmin, bmax = mesh.bounds
|
||
center_xy = (bmin[:2] + bmax[:2]) / 2
|
||
info = {"name": name, "visual": str(src), "bounds_min": bmin.tolist(), "bounds_max": bmax.tolist(),
|
||
"size": (bmax - bmin).tolist(), "center_xy": center_xy.tolist(), "bottom_z": float(bmin[2]),
|
||
"mass_real": MASS.get(name, 0.15), "mass_sim": min(MASS.get(name, 0.15), MASS_CAP), "hulls": []}
|
||
cache = d / "coacd_meta.json"
|
||
if cache.exists():
|
||
c = json.loads(cache.read_text())
|
||
if all(Path(h).exists() for h in c["hulls"]):
|
||
info["hulls"] = c["hulls"]; return info
|
||
import coacd
|
||
cm = coacd.Mesh(np.asarray(mesh_c.vertices, np.float64), np.asarray(mesh_c.faces, np.int64))
|
||
parts = coacd.run_coacd(cm, threshold=threshold, max_convex_hull=max_hulls, preprocess_mode="auto", resolution=2000, mcts_nodes=10, mcts_iterations=60, mcts_max_depth=2, seed=0)
|
||
hulls = []
|
||
for k, (v, f) in enumerate(parts):
|
||
h = trimesh.Trimesh(v, f, process=False).convex_hull
|
||
p = d / f"coacd_{k:02d}.obj"; h.export(str(p)); hulls.append(str(p))
|
||
info["hulls"] = hulls
|
||
cache.write_text(json.dumps({"hulls": hulls, "threshold": threshold, "max_hulls": max_hulls}))
|
||
print(f" {name}: {len(hulls)} hulls, size {np.round(info['size'], 3).tolist()} m, mass sim {info['mass_sim']} kg")
|
||
return info
|
||
|
||
|
||
def add_ycb_object(spec, info: dict, pos_xy, table_top: float, rgba=None):
|
||
"""把一个 YCB 物体作为自由体加入 spec;返回 body 名与初始 pos。"""
|
||
import mujoco
|
||
name = info["name"]; body_name = f"obj_{name}"
|
||
# body 原点 = 物体包围盒中心;底面落在桌面 +2mm
|
||
bmin, bmax = np.asarray(info["bounds_min"]), np.asarray(info["bounds_max"])
|
||
off = -(bmin + bmax) / 2
|
||
pos = [float(pos_xy[0]), float(pos_xy[1]), table_top + 0.002 + float(bmax[2] - bmin[2]) / 2]
|
||
b = spec.worldbody.add_body(name=body_name, pos=pos)
|
||
b.add_freejoint()
|
||
vis = spec.add_mesh(name=f"{body_name}_vis", file=info["visual"])
|
||
g = b.add_geom(name=body_name, type=mujoco.mjtGeom.mjGEOM_MESH, meshname=vis.name, pos=off.tolist(),
|
||
contype=0, conaffinity=0, group=2, mass=info["mass_sim"])
|
||
if rgba is not None: g.rgba = rgba
|
||
for k, hp in enumerate(info["hulls"]):
|
||
hm = spec.add_mesh(name=f"{body_name}_col{k}", file=hp)
|
||
gc = b.add_geom(name=f"{body_name}_c{k}", type=mujoco.mjtGeom.mjGEOM_MESH, meshname=hm.name, pos=off.tolist(),
|
||
condim=4, group=3, density=0, rgba=[1, 0, 0, 0.25])
|
||
gc.friction = [1.2, 0.01, 0.0005]; gc.solref = [0.008, 1.0]
|
||
return body_name, pos
|
||
|
||
|
||
if __name__ == "__main__":
|
||
for n in sys.argv[1:]:
|
||
prepare(n)
|