Files

82 lines
4.3 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.
"""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)