diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index b88159dd8e..b3878e7a78 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -53,6 +53,7 @@ "drone-agentic": "dimos.robot.drone.blueprints.agentic.drone_agentic:drone_agentic", "drone-basic": "dimos.robot.drone.blueprints.basic.drone_basic:drone_basic", "dual-xarm6-planner": "dimos.manipulation.blueprints:dual_xarm6_planner", + "g1-groot-wbc": "dimos.robot.unitree.g1.blueprints.basic.g1_groot_wbc:g1_groot_wbc", "keyboard-teleop-openarm": "dimos.robot.manipulators.openarm.blueprints:keyboard_teleop_openarm", "keyboard-teleop-openarm-mock": "dimos.robot.manipulators.openarm.blueprints:keyboard_teleop_openarm_mock", "keyboard-teleop-piper": "dimos.robot.manipulators.piper.blueprints:keyboard_teleop_piper", diff --git a/dimos/robot/unitree/g1/blueprints/basic/g1_groot_wbc.py b/dimos/robot/unitree/g1/blueprints/basic/g1_groot_wbc.py new file mode 100644 index 0000000000..c7e26b9915 --- /dev/null +++ b/dimos/robot/unitree/g1/blueprints/basic/g1_groot_wbc.py @@ -0,0 +1,179 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""G1 GROOT WBC target backed by MuJoCo during development.""" + +from __future__ import annotations + +import logging +import os +from pathlib import Path + +from dimos_lcm.std_msgs import Bool # type: ignore[import-untyped] + +from dimos.core.coordination.blueprints import Blueprint, autoconnect +from dimos.core.transport import LCMTransport +from dimos.mapping.costmapper import CostMapper +from dimos.mapping.voxels import VoxelGridMapper +from dimos.msgs.geometry_msgs.PointStamped import PointStamped +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.msgs.nav_msgs.Path import Path as PathMsg +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.navigation.replanning_a_star.module import ReplanningAStarPlanner +from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule + + +def _env_float(name: str, default: float) -> float: + raw = os.environ.get(name) + return default if raw is None or raw == "" else float(raw) + + +def _env_bool(name: str, default: bool) -> bool: + raw = os.environ.get(name) + if raw is None or raw == "": + return default + return raw.lower() in {"1", "true", "yes", "on"} + + +def _env_xyz(name: str, default: tuple[float, float, float]) -> tuple[float, float, float]: + raw = os.environ.get(name) + if raw is None or raw.strip() == "": + return default + parts = [float(p.strip()) for p in raw.split(",")] + if len(parts) != 3: + raise ValueError(f"{name} must be 'x,y,z'") + return (parts[0], parts[1], parts[2]) + + +def _command_center_blueprints() -> list[Blueprint]: + if not _env_bool("DIMOS_ENABLE_COMMAND_CENTER", True): + return [] + try: + from dimos.web.websocket_vis.websocket_vis_module import WebsocketVisModule + except ModuleNotFoundError as exc: + if exc.name not in {"socketio", "fastapi", "uvicorn", "starlette"}: + raise + + logging.getLogger(__name__).warning( + "Command Center unavailable; install the web extra to enable WASD controls" + ) + return [] + return [ + WebsocketVisModule.blueprint(port=int(os.environ.get("DIMOS_COMMAND_CENTER_PORT", "7779"))) + ] + + +_repo_root = Path(__file__).resolve().parents[6] +_default_mjcf = _repo_root / "data/mujoco_sim/g1_gear_wbc.xml" +_default_scene_mesh = _repo_root / "data/dimos_office_mesh/dimos_office_mesh.glb" +_mjcf_path = os.environ.get("DIMOS_MJCF_PATH") or str(_default_mjcf) +_mjcf_meshdir = os.environ.get("DIMOS_MJCF_MESHDIR") +if _mjcf_meshdir is None and Path(_mjcf_path).expanduser().resolve() == _default_mjcf: + _mjcf_meshdir = str(_repo_root / "data/g1_urdf/meshes") +_dof = int(os.environ.get("DIMOS_MUJOCO_DOF", "29")) +_scene_mesh_path_override = os.environ.get("DIMOS_SCENE_MESH_PATH") or None +_scene_mesh_path = _scene_mesh_path_override or ( + str(_default_scene_mesh) if _default_scene_mesh.exists() else None +) +_scene_mesh_scale = _env_float( + "DIMOS_SCENE_MESH_SCALE", + 0.05 if _scene_mesh_path_override else 2.0, +) +_scene_mesh_y_up = _env_bool("DIMOS_SCENE_MESH_Y_UP", bool(_scene_mesh_path_override)) +_scene_mesh_rotation = _env_xyz("DIMOS_SCENE_MESH_ROTATION_ZYX_DEG", (0.0, 0.0, 0.0)) +_scene_mesh_translation = _env_xyz("DIMOS_SCENE_MESH_TRANSLATION", (0.0, 0.0, 0.0)) +_scene_mesh_collision = _env_bool("DIMOS_SCENE_MESH_COLLISION", True) +_scene_mesh_visual = _env_bool("DIMOS_SCENE_MESH_VISUAL", False) +_scene_mesh_bake = _env_bool("DIMOS_SCENE_MESH_BAKE", False) +_enable_depth_cloud = _env_bool("DIMOS_ENABLE_DEPTH_CLOUD", False) +_disable_lidar = _env_bool("DIMOS_DISABLE_LIDAR", False) + +_sim_mjcf_path = _mjcf_path +if _scene_mesh_path and _scene_mesh_collision: + try: + from dimos.simulation.mujoco.mesh_scene import SceneMeshAlignment + from dimos.simulation.mujoco.scene_mesh_to_mjcf import load_or_bake + + _, _scene_wrapper = load_or_bake( + scene_mesh_path=_scene_mesh_path, + robot_mjcf_path=_mjcf_path, + alignment=SceneMeshAlignment( + scale=_scene_mesh_scale, + rotation_zyx_deg=_scene_mesh_rotation, + translation=_scene_mesh_translation, + y_up=_scene_mesh_y_up, + ), + meshdir=_mjcf_meshdir, + include_visual_mesh=_scene_mesh_visual, + rebake=_scene_mesh_bake, + ) + _scene_mjb_path = _scene_wrapper.with_name("compiled.mjb") + _sim_mjcf_path = str(_scene_mjb_path if _scene_mjb_path.exists() else _scene_wrapper) + except Exception as exc: + logging.getLogger(__name__).warning( + "Failed to bake scene mesh into MuJoCo; lidar will only see the base MJCF: %s", + exc, + ) + +g1_groot_wbc = autoconnect( + MujocoSimModule.blueprint( + address=_sim_mjcf_path, + meshdir=_mjcf_meshdir, + headless=_env_bool("DIMOS_MUJOCO_HEADLESS", True), + dof=_dof, + camera_name=os.environ.get("DIMOS_MUJOCO_CAMERA", "head_color"), + enable_color=False, + enable_depth=_enable_depth_cloud, + enable_pointcloud=(not _disable_lidar) or _enable_depth_cloud, + pointcloud_fps=_env_float("DIMOS_POINTCLOUD_FPS", 2.0), + lidar_camera_names=( + [] + if _disable_lidar + else ["lidar_front_camera", "lidar_left_camera", "lidar_right_camera"] + ), + lidar_camera_width=int(os.environ.get("DIMOS_LIDAR_CAMERA_WIDTH", "640")), + lidar_camera_height=int(os.environ.get("DIMOS_LIDAR_CAMERA_HEIGHT", "360")), + lidar_voxel_size=_env_float("DIMOS_LIDAR_VOXEL_SIZE", 0.05), + enable_kinematic_base_control=_env_bool("DIMOS_KINEMATIC_BASE_CONTROL", True), + enable_kinematic_joint_hold=_env_bool("DIMOS_MUJOCO_KINEMATIC_JOINT_HOLD", True), + ), + VoxelGridMapper.blueprint( + voxel_size=_env_float("DIMOS_GLOBAL_MAP_VOXEL_SIZE", 0.05), + ), + CostMapper.blueprint(), + ReplanningAStarPlanner.blueprint(), + *_command_center_blueprints(), +).transports( + { + ("joint_state", JointState): LCMTransport("/mujoco/joint_state", JointState), + ("odom", PoseStamped): LCMTransport("/odom", PoseStamped), + ("tele_cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), + ("nav_cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), + ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), + ("pointcloud", PointCloud2): LCMTransport("/lidar", PointCloud2), + ("lidar", PointCloud2): LCMTransport("/lidar", PointCloud2), + ("global_map", PointCloud2): LCMTransport("/global_map", PointCloud2), + ("global_costmap", OccupancyGrid): LCMTransport("/global_costmap", OccupancyGrid), + ("path", PathMsg): LCMTransport("/nav_path", PathMsg), + ("clicked_point", PointStamped): LCMTransport("/clicked_point", PointStamped), + ("goal_request", PoseStamped): LCMTransport("/goal_request", PoseStamped), + ("stop_movement", Bool): LCMTransport("/stop_movement", Bool), + ("point_goal", PointStamped): LCMTransport("/point_goal", PointStamped), + } +) + +__all__ = ["g1_groot_wbc"] diff --git a/dimos/simulation/engines/mujoco_engine.py b/dimos/simulation/engines/mujoco_engine.py index ada85dd477..5bc24a26fa 100644 --- a/dimos/simulation/engines/mujoco_engine.py +++ b/dimos/simulation/engines/mujoco_engine.py @@ -18,10 +18,12 @@ from collections.abc import Callable from dataclasses import dataclass +import math from pathlib import Path import threading import time -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, cast +import xml.etree.ElementTree as ET import mujoco import mujoco.viewer as viewer # type: ignore[import-untyped] @@ -40,6 +42,7 @@ # Step hook signature: called with the engine instance inside the sim thread. StepHook = Callable[["MujocoEngine"], None] +_MUJOCO_FROM_BINARY_PATH = "from_binary_path" @dataclass @@ -48,6 +51,7 @@ class CameraConfig: width: int = 640 height: int = 480 fps: float = 15.0 + scene_option: mujoco.MjvOption | None = None @dataclass @@ -84,6 +88,7 @@ def __init__( config_path: Path, headless: bool, cameras: list[CameraConfig] | None = None, + meshdir: str | Path | None = None, on_before_step: StepHook | None = None, on_after_step: StepHook | None = None, ) -> None: @@ -92,7 +97,7 @@ def __init__( self._on_after_step: StepHook | None = on_after_step xml_path = self._resolve_xml_path(config_path) - self._model = mujoco.MjModel.from_xml_path(str(xml_path)) + self._model = self._load_model(xml_path, meshdir=meshdir) self._xml_path = xml_path self._data = mujoco.MjData(self._model) @@ -101,6 +106,16 @@ def __init__( self._num_joints = len(self._joint_names) timestep = float(self._model.opt.timestep) self._control_frequency = 1.0 / timestep if timestep > 0.0 else 100.0 + self._root_free_qpos_adr: int | None = None + self._root_free_qvel_adr: int | None = None + self._root_kinematic_pose: tuple[float, float, float] | None = None + self._scene_body_ids = self._collect_body_ids("dimos_scene") + free_joint = int(mujoco.mjtJoint.mjJNT_FREE) # type: ignore[attr-defined] + for joint_id in range(self._model.njnt): + if self._model.jnt_type[joint_id] == free_joint: + self._root_free_qpos_adr = int(self._model.jnt_qposadr[joint_id]) + self._root_free_qvel_adr = int(self._model.jnt_dofadr[joint_id]) + break self._connected = False self._lock = threading.Lock() @@ -131,9 +146,36 @@ def _resolve_xml_path(self, config_path: Path) -> Path: resolved = config_path.expanduser() xml_path = resolved / "scene.xml" if resolved.is_dir() else resolved if not xml_path.exists(): - raise FileNotFoundError(f"MuJoCo XML not found: {xml_path}") + raise FileNotFoundError(f"MuJoCo model not found: {xml_path}") return xml_path + def _load_model(self, xml_path: Path, *, meshdir: str | Path | None) -> mujoco.MjModel: + if xml_path.suffix.lower() == ".mjb": + return self._load_binary_model(xml_path) + + if meshdir is None: + return mujoco.MjModel.from_xml_path(str(xml_path)) + + root = ET.parse(xml_path).getroot() + compiler = root.find("compiler") + if compiler is None: + compiler = ET.Element("compiler") + root.insert(0, compiler) + compiler.set("meshdir", str(Path(meshdir).expanduser().resolve())) + for include in root.iter("include"): + include_file = include.get("file") + if include_file and not Path(include_file).is_absolute(): + include.set("file", str((xml_path.parent / include_file).resolve())) + return mujoco.MjModel.from_xml_string(ET.tostring(root, encoding="unicode")) + + @staticmethod + def _load_binary_model(model_path: Path) -> mujoco.MjModel: + load_binary_model = cast( + "Callable[[str], mujoco.MjModel]", + getattr(mujoco.MjModel, _MUJOCO_FROM_BINARY_PATH), + ) + return load_binary_model(str(model_path)) + def _current_position(self, mapping: JointMapping) -> float: if mapping.joint_id is not None and mapping.qpos_adr is not None: return float(self._data.qpos[mapping.qpos_adr]) @@ -146,6 +188,46 @@ def _current_position(self, mapping: JointMapping) -> float: return float(self._data.actuator_length[mapping.actuator_id]) return 0.0 + def _collect_body_ids(self, root_name: str) -> set[int]: + root_id = mujoco.mj_name2id(self._model, mujoco.mjtObj.mjOBJ_BODY, root_name) + if root_id < 0: + return set() + body_ids = {root_id} + changed = True + while changed: + changed = False + for body_id in range(self._model.nbody): + parent_id = int(self._model.body_parentid[body_id]) + if parent_id in body_ids and body_id not in body_ids: + body_ids.add(body_id) + changed = True + return body_ids + + def _is_scene_geom(self, geom_id: int) -> bool: + if geom_id < 0 or not self._scene_body_ids: + return False + return int(self._model.geom_bodyid[geom_id]) in self._scene_body_ids + + def _has_blocking_scene_contact(self) -> bool: + """Return true for non-floor contacts between robot and baked scene.""" + if not self._scene_body_ids: + return False + for contact_idx in range(self._data.ncon): + contact = self._data.contact[contact_idx] + geom1 = int(contact.geom1) + geom2 = int(contact.geom2) + geom1_is_scene = self._is_scene_geom(geom1) + geom2_is_scene = self._is_scene_geom(geom2) + if geom1_is_scene == geom2_is_scene: + continue + if float(contact.dist) > 1e-4: + continue + normal = contact.frame[:3] + if abs(float(normal[2])) > 0.75: + continue + return True + return False + def _apply_control(self) -> None: with self._lock: if self._command_mode == "effort": @@ -250,10 +332,24 @@ def _render_cameras(self, now: float, cam_renderers: dict[str, _CameraRendererSt continue state.last_render_time = now - state.rgb_renderer.update_scene(self._data, camera=state.cam_id) + if state.cfg.scene_option is None: + state.rgb_renderer.update_scene(self._data, camera=state.cam_id) + else: + state.rgb_renderer.update_scene( + self._data, + camera=state.cam_id, + scene_option=state.cfg.scene_option, + ) rgb = state.rgb_renderer.render().copy() - state.depth_renderer.update_scene(self._data, camera=state.cam_id) + if state.cfg.scene_option is None: + state.depth_renderer.update_scene(self._data, camera=state.cam_id) + else: + state.depth_renderer.update_scene( + self._data, + camera=state.cam_id, + scene_option=state.cfg.scene_option, + ) depth = state.depth_renderer.render().copy() frame = CameraFrame( @@ -417,6 +513,107 @@ def hold_current_position(self) -> None: for i, mapping in enumerate(self._joint_mappings): self._joint_position_targets[i] = self._current_position(mapping) + def enforce_position_targets(self) -> None: + """Pin modeled joints to their current position targets. + + This is a development stub for stacks that do not yet run a real + whole-body controller. It leaves the floating base alone, but prevents + contact impulses from folding the articulated joints. + """ + with self._lock: + for i, mapping in enumerate(self._joint_mappings): + target = self._joint_position_targets[i] + if mapping.qpos_adr is not None: + self._data.qpos[mapping.qpos_adr] = target + self._joint_positions[i] = target + if mapping.dof_adr is not None: + self._data.qvel[mapping.dof_adr] = 0.0 + self._joint_velocities[i] = 0.0 + mujoco.mj_forward(self._model, self._data) + + @property + def has_root_freejoint(self) -> bool: + return self._root_free_qpos_adr is not None + + def apply_root_twist( + self, + linear_x: float, + linear_y: float, + angular_z: float, + *, + fixed_z: float | None = None, + ) -> bool: + """Integrate planar velocity onto the first freejoint root. + + The root is treated as kinematic once this method is used: we + maintain an internal desired x/y/yaw and write it back every tick. + That prevents contact impulses or gravity settling from slowly + walking the floating base when the commanded twist is zero. + """ + qpos_adr = self._root_free_qpos_adr + if qpos_adr is None: + return False + + dt = 1.0 / self._control_frequency + with self._lock: + qpos = self._data.qpos + if self._root_kinematic_pose is None: + qw, qx, qy, qz = qpos[qpos_adr + 3 : qpos_adr + 7] + yaw = math.atan2( + 2.0 * (qw * qz + qx * qy), + 1.0 - 2.0 * (qy * qy + qz * qz), + ) + self._root_kinematic_pose = ( + float(qpos[qpos_adr]), + float(qpos[qpos_adr + 1]), + yaw, + ) + + old_x, old_y, old_yaw = self._root_kinematic_pose + new_x = old_x + (math.cos(old_yaw) * linear_x - math.sin(old_yaw) * linear_y) * dt + new_y = old_y + (math.sin(old_yaw) * linear_x + math.cos(old_yaw) * linear_y) * dt + new_yaw = old_yaw + angular_z * dt + + qpos[qpos_adr] = new_x + qpos[qpos_adr + 1] = new_y + if fixed_z is not None: + qpos[qpos_adr + 2] = fixed_z + + qpos[qpos_adr + 3 : qpos_adr + 7] = [ + math.cos(new_yaw * 0.5), + 0.0, + 0.0, + math.sin(new_yaw * 0.5), + ] + mujoco.mj_forward(self._model, self._data) + + if self._has_blocking_scene_contact(): + qpos[qpos_adr] = old_x + qpos[qpos_adr + 1] = old_y + qpos[qpos_adr + 3 : qpos_adr + 7] = [ + math.cos(old_yaw * 0.5), + 0.0, + 0.0, + math.sin(old_yaw * 0.5), + ] + mujoco.mj_forward(self._model, self._data) + else: + self._root_kinematic_pose = (new_x, new_y, new_yaw) + + qvel_adr = self._root_free_qvel_adr + if qvel_adr is not None: + self._data.qvel[qvel_adr : qvel_adr + 6] = 0.0 + return True + + def get_root_pose(self) -> tuple[NDArray[np.float64], NDArray[np.float64]] | None: + qpos_adr = self._root_free_qpos_adr + if qpos_adr is None: + return None + with self._lock: + position = self._data.qpos[qpos_adr : qpos_adr + 3].copy() + qw, qx, qy, qz = self._data.qpos[qpos_adr + 3 : qpos_adr + 7].copy() + return position, np.array([qx, qy, qz, qw], dtype=np.float64) + def get_actuator_ctrl_range(self, joint_index: int) -> tuple[float, float] | None: mapping = self._joint_mappings[joint_index] if mapping.actuator_id is None: @@ -457,6 +654,15 @@ def get_camera_fovy(self, camera_name: str) -> float | None: return None return float(self._model.cam_fovy[cam_id]) + def get_camera_pose( + self, camera_name: str + ) -> tuple[NDArray[np.float64], NDArray[np.float64]] | None: + """Get a named camera's latest world pose from MuJoCo data.""" + cam_id = mujoco.mj_name2id(self._model, mujoco.mjtObj.mjOBJ_CAMERA, camera_name) + if cam_id < 0: + return None + return self._data.cam_xpos[cam_id].copy(), self._data.cam_xmat[cam_id].copy() + __all__ = [ "CameraConfig", diff --git a/dimos/simulation/engines/mujoco_shm.py b/dimos/simulation/engines/mujoco_shm.py index c0623c7915..f0021f8cd6 100644 --- a/dimos/simulation/engines/mujoco_shm.py +++ b/dimos/simulation/engines/mujoco_shm.py @@ -40,8 +40,8 @@ logger = setup_logger() -# Upper bound on joint count per sim. Arms + gripper are typically <= 10. -MAX_JOINTS = 16 +# Upper bound on joint count per sim. G1 is 29 DoF; leave room for hands. +MAX_JOINTS = 64 _FLOAT_BYTES = 8 # float64 _INT32_BYTES = 4 @@ -132,13 +132,13 @@ def create(cls, key: str) -> ManipShmSet: for buffer_name, size in _shm_sizes.items(): name = _buffer_name(key, buffer_name) try: - stale = _unregister(SharedMemory(name=name)) - stale.close() + stale = SharedMemory(name=name) try: stale.unlink() logger.info("ManipShmSet: unlinked stale SHM", name=name) except FileNotFoundError: pass + stale.close() except FileNotFoundError: pass buffers[buffer_name] = SharedMemory(create=True, size=size, name=name) diff --git a/dimos/simulation/engines/mujoco_sim_module.py b/dimos/simulation/engines/mujoco_sim_module.py index 3d2ff927fe..57450f3cc1 100644 --- a/dimos/simulation/engines/mujoco_sim_module.py +++ b/dimos/simulation/engines/mujoco_sim_module.py @@ -32,16 +32,22 @@ import time from typing import Any +import mujoco +import numpy as np +import open3d as o3d # type: ignore[import-untyped] from pydantic import Field import reactivex as rx +from reactivex.disposable import Disposable from scipy.spatial.transform import Rotation as R from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig -from dimos.core.stream import Out +from dimos.core.stream import In, Out from dimos.hardware.sensors.camera.spec import DepthCameraConfig, DepthCameraHardware +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.Image import Image, ImageFormat @@ -62,6 +68,13 @@ logger = setup_logger() _RX180 = R.from_euler("x", 180, degrees=True) +_LIDAR_GEOM_GROUPS = (0, 0, 1, 1, 0, 0) +_CMD_VEL_STALE_SEC = 0.5 +_ENGINE_CONNECT_TIMEOUT_SEC = 30.0 +_PUBLISH_THREAD_JOIN_TIMEOUT_SEC = 2.0 +_ENGINE_CONNECT_POLL_SEC = 0.1 +_STALE_FRAME_POLL_FRACTION = 0.5 +_RGBD_POINTCLOUD_VOXEL_SIZE = 0.005 def _default_identity_transform() -> Transform: @@ -75,6 +88,7 @@ class MujocoSimModuleConfig(ModuleConfig, DepthCameraConfig): """Configuration for the unified MuJoCo simulation module.""" address: str = "" + meshdir: str | None = None headless: bool = False dof: int = 7 @@ -86,10 +100,17 @@ class MujocoSimModuleConfig(ModuleConfig, DepthCameraConfig): base_frame_id: str = "link7" base_transform: Transform | None = Field(default_factory=_default_identity_transform) align_depth_to_color: bool = True + enable_color: bool = True enable_depth: bool = True enable_pointcloud: bool = False pointcloud_fps: float = 5.0 camera_info_fps: float = 1.0 + lidar_camera_names: list[str] = Field(default_factory=list) + lidar_camera_width: int = 640 + lidar_camera_height: int = 360 + lidar_voxel_size: float = 0.05 + enable_kinematic_base_control: bool = False + enable_kinematic_joint_hold: bool = False class MujocoSimModule( @@ -111,6 +132,9 @@ class MujocoSimModule( pointcloud: Out[PointCloud2] camera_info: Out[CameraInfo] depth_camera_info: Out[CameraInfo] + joint_state: Out[JointState] + odom: Out[PoseStamped] + cmd_vel: In[Twist] def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) @@ -122,6 +146,22 @@ def __init__(self, **kwargs: Any) -> None: self._stop_event = threading.Event() self._publish_thread: threading.Thread | None = None self._camera_info_base: CameraInfo | None = None + self._cmd_vel_lock = threading.Lock() + self._cmd_vel = Twist.zero() + self._last_cmd_vel_time = 0.0 + self._kinematic_base_z: float | None = None + + @property + def _camera_enabled(self) -> bool: + return self.config.enable_color or self.config.enable_depth or self.config.enable_pointcloud + + @property + def _primary_camera_needed(self) -> bool: + return ( + self.config.enable_color + or self.config.enable_depth + or (self.config.enable_pointcloud and not self.config.lidar_camera_names) + ) @property def _camera_link(self) -> str: @@ -161,63 +201,115 @@ def get_depth_scale(self) -> float: @rpc def start(self) -> None: + super().start() if not self.config.address: raise RuntimeError("MujocoSimModule: config.address (MJCF path) is required") - # SHM key — adapter derives the same key from the same MJCF path. shm_key = shm_key_from_path(self.config.address) self._shm = ManipShmWriter(shm_key) + camera_configs = self._make_camera_configs() - # Build engine with SHM hooks installed. self._engine = MujocoEngine( config_path=Path(self.config.address), headless=self.config.headless, - cameras=[ - CameraConfig( - name=self.config.camera_name, - width=self.config.width, - height=self.config.height, - fps=float(self.config.fps), - ) - ], + cameras=camera_configs, + meshdir=self.config.meshdir, on_before_step=self._apply_shm_commands, - on_after_step=self._publish_shm_state, + on_after_step=self._after_step, ) - # Detect gripper (extra joint beyond dof). dof = self.config.dof joint_names = list(self._engine.joint_names) - if len(joint_names) > dof: - ctrl_range = self._engine.get_actuator_ctrl_range(dof) - joint_range = self._engine.get_joint_range(dof) - if ctrl_range is None or joint_range is None: - raise ValueError(f"Gripper joint at index {dof} missing ctrl/joint range in MJCF") - self._gripper_idx = dof - self._gripper_ctrl_range = ctrl_range - self._gripper_joint_range = joint_range - logger.info( - "MujocoSimModule: gripper detected", - idx=dof, - ctrl_range=ctrl_range, - joint_range=joint_range, - ) + self._detect_gripper(joint_names) - # Start physics (sim thread spawned inside engine.connect()). if not self._engine.connect(): raise RuntimeError("MujocoSimModule: engine.connect() failed") self._shm.signal_ready(num_joints=len(joint_names)) + self._stop_event.clear() + + self._start_kinematic_base_control() + self._start_camera_publishers() + self._start_pointcloud_publisher() + + logger.info( + "MujocoSimModule started", + address=self.config.address, + dof=dof, + camera=self.config.camera_name, + camera_enabled=self._camera_enabled, + shm_key=shm_key, + ) - # Camera intrinsics. + def _make_camera_configs(self) -> list[CameraConfig]: + camera_configs: list[CameraConfig] = [] + if self._primary_camera_needed: + camera_configs.append( + CameraConfig( + name=self.config.camera_name, + width=self.config.width, + height=self.config.height, + fps=float(self.config.fps), + ) + ) + + lidar_scene_option = mujoco.MjvOption() + geomgroup = lidar_scene_option.geomgroup # type: ignore[attr-defined] + for group_id, enabled in enumerate(_LIDAR_GEOM_GROUPS): + geomgroup[group_id] = enabled + for lidar_name in self.config.lidar_camera_names: + if lidar_name == self.config.camera_name and self._primary_camera_needed: + continue + camera_configs.append( + CameraConfig( + name=lidar_name, + width=self.config.lidar_camera_width, + height=self.config.lidar_camera_height, + fps=float(self.config.pointcloud_fps), + scene_option=lidar_scene_option, + ) + ) + return camera_configs + + def _detect_gripper(self, joint_names: list[str]) -> None: + dof = self.config.dof + if len(joint_names) <= dof: + return + assert self._engine is not None + ctrl_range = self._engine.get_actuator_ctrl_range(dof) + joint_range = self._engine.get_joint_range(dof) + if ctrl_range is None or joint_range is None: + raise ValueError(f"Gripper joint at index {dof} missing ctrl/joint range in MJCF") + self._gripper_idx = dof + self._gripper_ctrl_range = ctrl_range + self._gripper_joint_range = joint_range + logger.info( + "MujocoSimModule: gripper detected", + idx=dof, + ctrl_range=ctrl_range, + joint_range=joint_range, + ) + + def _start_kinematic_base_control(self) -> None: + if not self.config.enable_kinematic_base_control: + return + assert self._engine is not None + if not self._engine.has_root_freejoint: + logger.warning("Kinematic base control requested, but MJCF has no freejoint root") + root_pose = self._engine.get_root_pose() + self._kinematic_base_z = None if root_pose is None else float(root_pose[0][2]) + self.register_disposable(Disposable(self.cmd_vel.subscribe(self._on_cmd_vel))) + + def _start_camera_publishers(self) -> None: + if not self._primary_camera_needed: + return self._build_camera_info() - self._stop_event.clear() self._publish_thread = threading.Thread( target=self._publish_loop, daemon=True, name="MujocoSimPublish" ) self._publish_thread.start() - # Periodic camera_info publishing. interval_sec = 1.0 / self.config.camera_info_fps self.register_disposable( rx.interval(interval_sec).subscribe( @@ -226,29 +318,24 @@ def start(self) -> None: ) ) - # Optional pointcloud generation. - if self.config.enable_pointcloud and self.config.enable_depth: - pc_interval = 1.0 / self.config.pointcloud_fps - self.register_disposable( - rx.interval(pc_interval).subscribe( - on_next=lambda _: self._generate_pointcloud(), - on_error=lambda e: logger.error("Pointcloud error", error=str(e)), - ) + def _start_pointcloud_publisher(self) -> None: + if not self.config.enable_pointcloud: + return + if not (self._primary_camera_needed or self.config.lidar_camera_names): + return + pc_interval = 1.0 / self.config.pointcloud_fps + self.register_disposable( + rx.interval(pc_interval).subscribe( + on_next=lambda _: self._generate_pointcloud(), + on_error=lambda e: logger.error("Pointcloud error", error=str(e)), ) - - logger.info( - "MujocoSimModule started", - address=self.config.address, - dof=dof, - camera=self.config.camera_name, - shm_key=shm_key, ) @rpc def stop(self) -> None: self._stop_event.set() if self._publish_thread and self._publish_thread.is_alive(): - self._publish_thread.join(timeout=2.0) + self._publish_thread.join(timeout=_PUBLISH_THREAD_JOIN_TIMEOUT_SEC) self._publish_thread = None errors: list[tuple[str, BaseException]] = [] @@ -296,8 +383,33 @@ def _apply_shm_commands(self, engine: MujocoEngine) -> None: ctrl_value = self._gripper_joint_to_ctrl(gripper_cmd) engine.set_position_target(self._gripper_idx, ctrl_value) - def _publish_shm_state(self, engine: MujocoEngine) -> None: - """Post-step hook: publish joint state to SHM.""" + def _on_cmd_vel(self, msg: Twist) -> None: + with self._cmd_vel_lock: + self._cmd_vel = Twist(msg) + self._last_cmd_vel_time = time.monotonic() + + def _apply_cmd_vel(self, engine: MujocoEngine) -> None: + if not self.config.enable_kinematic_base_control: + return + with self._cmd_vel_lock: + cmd = Twist(self._cmd_vel) + age = time.monotonic() - self._last_cmd_vel_time + if age > _CMD_VEL_STALE_SEC: + cmd = Twist.zero() + engine.apply_root_twist( + cmd.linear.x, + cmd.linear.y, + cmd.angular.z, + fixed_z=self._kinematic_base_z, + ) + + def _after_step(self, engine: MujocoEngine) -> None: + self._apply_cmd_vel(engine) + if self.config.enable_kinematic_joint_hold: + engine.enforce_position_targets() + self._publish_state(engine) + + def _publish_state(self, engine: MujocoEngine) -> None: shm = self._shm if shm is None: return @@ -306,6 +418,26 @@ def _publish_shm_state(self, engine: MujocoEngine) -> None: velocities=engine.joint_velocities, efforts=engine.joint_efforts, ) + self.joint_state.publish( + JointState( + frame_id="mujoco", + name=engine.joint_names, + position=engine.joint_positions, + velocity=engine.joint_velocities, + effort=engine.joint_efforts, + ) + ) + root_pose = engine.get_root_pose() + if root_pose is not None: + position, quat_xyzw = root_pose + self.odom.publish( + PoseStamped( + ts=time.time(), + frame_id="world", + position=Vector3(position), + orientation=Quaternion(quat_xyzw), + ) + ) if self._gripper_idx is not None: positions = engine.joint_positions if self._gripper_idx < len(positions): @@ -354,12 +486,12 @@ def _publish_loop(self) -> None: published_count = 0 # Wait for engine to actually be connected (sim thread may take a tick). - deadline = time.monotonic() + 30.0 + deadline = time.monotonic() + _ENGINE_CONNECT_TIMEOUT_SEC while not self._stop_event.is_set() and not engine.connected: if time.monotonic() > deadline: logger.error("MujocoSimModule: timed out waiting for engine to connect") return - self._stop_event.wait(timeout=0.1) + self._stop_event.wait(timeout=_ENGINE_CONNECT_POLL_SEC) if self._stop_event.is_set(): return @@ -377,18 +509,19 @@ def _publish_loop(self) -> None: return if frame is None or frame.timestamp <= last_timestamp: - self._stop_event.wait(timeout=interval * 0.5) + self._stop_event.wait(timeout=interval * _STALE_FRAME_POLL_FRACTION) continue last_timestamp = frame.timestamp ts = time.time() - color_img = Image( - data=frame.rgb, - format=ImageFormat.RGB, - frame_id=self._color_optical_frame, - ts=ts, - ) - self.color_image.publish(color_img) + if self.config.enable_color: + color_img = Image( + data=frame.rgb, + format=ImageFormat.RGB, + frame_id=self._color_optical_frame, + ts=ts, + ) + self.color_image.publish(color_img) if self.config.enable_depth: depth_img = Image( @@ -469,7 +602,12 @@ def _publish_tf(self, ts: float, frame: CameraFrame | None) -> None: ) def _generate_pointcloud(self) -> None: - if self._engine is None or self._camera_info_base is None: + if self._engine is None: + return + if self.config.lidar_camera_names: + self._generate_lidar_pointcloud() + return + if self._camera_info_base is None: return frame = self._engine.read_camera(self.config.camera_name) if frame is None: @@ -493,10 +631,42 @@ def _generate_pointcloud(self) -> None: camera_info=self._camera_info_base, depth_scale=1.0, ) - pcd = pcd.voxel_downsample(0.005) + pcd = pcd.voxel_downsample(_RGBD_POINTCLOUD_VOXEL_SIZE) self.pointcloud.publish(pcd) except Exception as exc: logger.error("Pointcloud generation error", error=str(exc)) + def _generate_lidar_pointcloud(self) -> None: + if self._engine is None: + return + try: + from dimos.simulation.mujoco.depth_camera import depth_image_to_point_cloud + + all_points: list[np.ndarray] = [] + latest_ts = 0.0 + for camera_name in self.config.lidar_camera_names: + frame = self._engine.read_camera(camera_name) + if frame is None: + continue + points = depth_image_to_point_cloud( + frame.depth, + frame.cam_pos, + frame.cam_mat.reshape(3, 3), + fov_degrees=frame.fovy, + ) + if points.size: + all_points.append(points) + latest_ts = max(latest_ts, frame.timestamp) + if not all_points: + return + cloud = o3d.geometry.PointCloud() + cloud.points = o3d.utility.Vector3dVector(np.vstack(all_points)) + cloud = cloud.voxel_down_sample(self.config.lidar_voxel_size) + self.pointcloud.publish( + PointCloud2(pointcloud=cloud, ts=latest_ts or time.time(), frame_id="world") + ) + except Exception as exc: + logger.error("Multi-camera lidar fusion error", error=str(exc)) + __all__ = ["MujocoSimModule", "MujocoSimModuleConfig"] diff --git a/dimos/simulation/mujoco/collision_spec.py b/dimos/simulation/mujoco/collision_spec.py new file mode 100644 index 0000000000..fb870328e3 --- /dev/null +++ b/dimos/simulation/mujoco/collision_spec.py @@ -0,0 +1,843 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Per-prim collision-shape decision-making for ``bake_scene_mjcf``. + +The bake's job is to turn each USD/glTF/OBJ prim into one or more MuJoCo +````s. This module separates the *decision* (what shape to emit) +from the *emission* (the OBJ/MJCF writing). Three layers cooperate: + +1. **Generic geometric heuristics** — applied to every prim regardless of + source. Tiny-prim skip, aspect-ratio box override, near-convex check. + Safe defaults; no scene-specific knowledge. + +2. **Primitive auto-fit** — try OBB box / Ritter sphere / PCA cylinder / + PCA capsule. Accept the best fit if + ``hull_volume / primitive_volume >= fill_threshold``. Geometric only. + +3. **Sidecar overrides** — a JSON file (``.collision.json`` next + to the source mesh, or explicit path) with ``fnmatch`` patterns over + USD prim paths. Lets users skip lamps, force cylinders on pillars, + tune CoACD per pattern. Whoever produces this file (a human, a + future UE-side extractor, an LLM…) doesn't matter to the bake — the + sidecar is the contract. + +The dispatcher ``emit_for_prim()`` walks: sidecar override → generic +heuristics → primitive auto-fit → CoACD fallback, and returns a list +of ``GeomEmission`` describing every ```` the wrapper should +include for the prim. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +import fnmatch +import json +from pathlib import Path +from typing import Any, Literal + +import numpy as np +from scipy.spatial import ConvexHull, QhullError # type: ignore[import-untyped] + +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +PrimitiveFit = dict[str, Any] +OverrideConfig = dict[str, Any] + + +# --------------------------------------------------------------------------- # +# Sidecar spec dataclasses # +# --------------------------------------------------------------------------- # + + +@dataclass +class CollisionSpec: + """User-facing collision configuration loaded from ``.collision.json``. + + Patterns in ``prim_overrides`` are matched with ``fnmatch`` (Unix-shell + globs) against the USD prim path of each prim — e.g. ``/Root/SM_Pillar_*``. + First-match wins; iteration order of the dict is preserved (Python 3.7+). + + Each override value is a dict with at minimum ``"type"``: + + - ``"box"`` / ``"sphere"`` / ``"cylinder"`` / ``"capsule"`` / ``"plane"``: + force the corresponding primitive. Auto-fit picks the parameters + unless explicit ``"size"`` / ``"pos"`` / ``"quat"`` is provided. + - ``"hull"``: force single convex hull, no CoACD. + - ``"decompose"``: force CoACD even if auto-fit would have accepted a + primitive. Optional ``"max_hulls"`` overrides ``coacd_max_hulls``. + - ``"skip"``: emit no collision geom. Visual mesh still drawn. + - ``"auto"``: same as the global default (useful to scope a pattern + back to default behaviour inside a wider override). + + Optional override keys: + + - ``"friction"``: list ``[slide, spin, roll]``. + - ``"max_hulls"``: per-pattern CoACD cap. + """ + + #: Fallback policy when no pattern matches. ``"auto"`` runs the full + #: heuristics→primitive→CoACD pipeline. ``"hull"`` always emits one + #: convex hull. ``"skip"`` emits nothing (visual only). + default: Literal["auto", "hull", "skip"] = "auto" + + #: A primitive is accepted in auto-fit if + #: ``hull_volume / primitive_volume >= fill_threshold``. Higher = + #: stricter (more prims fall through to CoACD). + fill_threshold: float = 0.85 + + #: Prims whose largest extent is below this (metres) emit no geom. + #: Catches trim/fasteners that the robot can't meaningfully contact. + tiny_prim_extent_m: float = 0.03 + + #: If one axis is ``>= aspect_ratio_box`` times the smaller two, the + #: prim is forced to a box even if auto-fit fill ratio is borderline. + #: Catches wall panels, floor slabs, doors. + aspect_ratio_box: float = 20.0 + + #: If mesh's hull is this close to its actual mesh volume, use one + #: hull and skip CoACD entirely (mesh is already near-convex). + near_convex_threshold: float = 0.9 + + #: CoACD concavity threshold (URLab default). Lower = finer + #: decomposition (more sub-hulls). + coacd_threshold: float = 0.05 + + #: Hard cap on hulls per CoACD invocation. -1 = unlimited. + coacd_max_hulls: int = 64 + + #: Only run decomposition when the prim's single-hull volume exceeds + #: this (m³). Smaller furniture-scale prims use one hull regardless. + shell_volume_m3: float = 2.0 + + #: Preserve large non-rectangular sheet footprints with thin triangle + #: prisms. This helps moderate indoor scenes with angular floors, but + #: is disabled by ``bake_scene_mjcf`` for very large scenes unless + #: explicitly overridden. + enable_sheet_prisms: bool = True + + #: Scene-level guard used by ``bake_scene_mjcf``. Above this many + #: source prims, sheet prisms can explode the geom count; use sidecar + #: overrides for specific floors instead. + sheet_prism_max_scene_prims: int = 2500 + + #: ``USD-path-glob -> override-dict``. See class docstring. + prim_overrides: dict[str, OverrideConfig] = field(default_factory=dict) + + @classmethod + def from_json(cls, path: Path | str) -> CollisionSpec: + """Load a sidecar. Unknown keys are ignored to keep the format forwards-compatible.""" + path = Path(path) + raw = json.loads(path.read_text()) + known = { + "default", + "fill_threshold", + "tiny_prim_extent_m", + "aspect_ratio_box", + "near_convex_threshold", + "coacd_threshold", + "coacd_max_hulls", + "shell_volume_m3", + "enable_sheet_prisms", + "sheet_prism_max_scene_prims", + "prim_overrides", + } + kwargs = {k: v for k, v in raw.items() if k in known} + # Ignore "$schema" and any future top-level keys silently. + return cls(**kwargs) + + @classmethod + def auto_discover(cls, scene_path: Path | str) -> CollisionSpec: + """Return the sidecar next to ``scene_path`` if it exists, else defaults.""" + scene_path = Path(scene_path) + sidecar = scene_path.with_suffix(".collision.json") + if sidecar.exists(): + logger.info(f"loading collision sidecar: {sidecar}") + return cls.from_json(sidecar) + return cls() + + def resolve(self, prim_path: str) -> OverrideConfig: + """Find the matching override for ``prim_path`` (USD path). + + Returns a dict with at least ``"type"``. Falls back to + ``{"type": self.default}`` when no pattern matches. + """ + for pattern, override in self.prim_overrides.items(): + if fnmatch.fnmatchcase(prim_path, pattern): + # Pattern's "auto" defers to global default. + if override.get("type") == "auto": + return {**override, "type": self.default} + return override + return {"type": self.default} + + +# --------------------------------------------------------------------------- # +# Emission record # +# --------------------------------------------------------------------------- # + + +@dataclass +class GeomEmission: + """One MuJoCo ```` to emit, in dimos world frame. + + Either ``mesh_path`` is set (mesh-type geom — also emits a + ```` asset) or the primitive parameters (``size``, ``pos``, + ``quat``) are set. + """ + + name: str + purpose: Literal["collision", "visual"] + kind: Literal["mesh", "box", "sphere", "cylinder", "capsule", "plane"] + + #: For ``kind="mesh"``: absolute path to the OBJ. + mesh_path: Path | None = None + + #: Primitive size (semantics depend on ``kind``): + #: * box → (hx, hy, hz) (half-extents) + #: * sphere → (r,) + #: * cylinder→ (r, half_height) + #: * capsule → (r, half_height) (caps extend beyond half_height) + #: * plane → (hx, hy, _grid_spacing) — last is cosmetic only + size: tuple[float, ...] | None = None + + #: World-frame position of the primitive centre. ``None`` for meshes + #: (their geometry is already in world frame). + pos: tuple[float, float, float] | None = None + + #: World-frame orientation (wxyz, MuJoCo convention). ``None`` → + #: identity. Not used for meshes. + quat: tuple[float, float, float, float] | None = None + + #: Optional friction override (slide, spin, roll). + friction: tuple[float, float, float] | None = None + + +# --------------------------------------------------------------------------- # +# Math helpers # +# --------------------------------------------------------------------------- # + + +def _matrix_to_quat_wxyz(R: np.ndarray) -> tuple[float, float, float, float]: + """3x3 right-handed rotation → quaternion ``(w, x, y, z)``. + + Standard Shepperd's method; avoids the singularity at ``trace == -1``. + """ + R = np.asarray(R, dtype=np.float64) + tr = R[0, 0] + R[1, 1] + R[2, 2] + if tr > 0: + s = 0.5 / np.sqrt(tr + 1.0) + w = 0.25 / s + x = (R[2, 1] - R[1, 2]) * s + y = (R[0, 2] - R[2, 0]) * s + z = (R[1, 0] - R[0, 1]) * s + elif (R[0, 0] > R[1, 1]) and (R[0, 0] > R[2, 2]): + s = 2.0 * np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) + w = (R[2, 1] - R[1, 2]) / s + x = 0.25 * s + y = (R[0, 1] + R[1, 0]) / s + z = (R[0, 2] + R[2, 0]) / s + elif R[1, 1] > R[2, 2]: + s = 2.0 * np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) + w = (R[0, 2] - R[2, 0]) / s + x = (R[0, 1] + R[1, 0]) / s + y = 0.25 * s + z = (R[1, 2] + R[2, 1]) / s + else: + s = 2.0 * np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) + w = (R[1, 0] - R[0, 1]) / s + x = (R[0, 2] + R[2, 0]) / s + y = (R[1, 2] + R[2, 1]) / s + z = 0.25 * s + return (float(w), float(x), float(y), float(z)) + + +def _quat_z_to(axis: np.ndarray) -> tuple[float, float, float, float]: + """Quaternion that rotates ``+Z`` onto ``axis`` (unit vector). + + Used for cylinder/capsule placement — MuJoCo's primitive long-axis + is local +Z; we orient by aligning Z to the prim's PCA principal + direction. + """ + z = np.array([0.0, 0.0, 1.0]) + a = axis / (np.linalg.norm(axis) + 1e-12) + d = float(np.dot(z, a)) + if d > 0.99999: + return (1.0, 0.0, 0.0, 0.0) + if d < -0.99999: + # 180° about any axis perpendicular to Z; use X. + return (0.0, 1.0, 0.0, 0.0) + cross = np.cross(z, a) + s = float(np.sqrt(2.0 * (1.0 + d))) + w = s * 0.5 + xyz = cross / s + return (w, float(xyz[0]), float(xyz[1]), float(xyz[2])) + + +# --------------------------------------------------------------------------- # +# Primitive fits # +# --------------------------------------------------------------------------- # + + +#: Lower bound on any primitive's half-extent / radius. MuJoCo rejects +#: a ```` with any size component <= 0, but truly flat prims (road +#: tiles, ceiling panels) can come out of the OBB / cylinder fit with one +#: axis at exactly 0. Clamping to 1 mm yields a valid geom that's still +#: physically reasonable as a thin slab. +_MIN_SIZE_M = 1e-3 +_SHEET_PRISM_THICKNESS_M = 0.03 +_SHEET_BOX_FILL_MIN = 0.85 +_SHEET_BOX_FILL_MAX = 1.15 +_HORIZONTAL_BOX_MAX_THICKNESS_M = 0.05 +_SHEET_PRISM_MIN_FOOTPRINT_AREA_M2 = 2.0 +_SHEET_PRISM_MAX_TRIANGLES = 1024 + + +def _fit_aabb_box(vertices: np.ndarray) -> PrimitiveFit: + """Axis-aligned bounding box. Identity quat.""" + mn, mx = vertices.min(0), vertices.max(0) + half_ext = np.maximum((mx - mn) / 2.0, _MIN_SIZE_M) + center = (mx + mn) / 2.0 + return { + "type": "box", + "size": tuple(map(float, half_ext)), + "pos": tuple(map(float, center)), + "quat": (1.0, 0.0, 0.0, 0.0), + "volume": float(np.prod(2.0 * half_ext)), + } + + +def _fit_obb_box(vertices: np.ndarray) -> PrimitiveFit: + """Oriented bounding box via PCA. Tighter than AABB when the prim + is rotated relative to world axes (most UE props are world-aligned, + so OBB ≈ AABB, but rotated assets benefit).""" + centroid = vertices.mean(0) + centered = vertices - centroid + cov = np.cov(centered.T) + _, evecs = np.linalg.eigh(cov) + # Ensure right-handed. + if np.linalg.det(evecs) < 0: + evecs[:, 0] = -evecs[:, 0] + local = centered @ evecs + mn, mx = local.min(0), local.max(0) + half_ext = np.maximum((mx - mn) / 2.0, _MIN_SIZE_M) + local_center = (mx + mn) / 2.0 + world_center = centroid + evecs @ local_center + return { + "type": "box", + "size": tuple(map(float, half_ext)), + "pos": tuple(map(float, world_center)), + "quat": _matrix_to_quat_wxyz(evecs), + "volume": float(np.prod(2.0 * half_ext)), + } + + +def _fit_sphere(vertices: np.ndarray) -> PrimitiveFit: + """Centroid + farthest-vertex. Looser than Welzl/Ritter but fine for + fill-ratio comparison.""" + centroid = vertices.mean(0) + r = max(float(np.linalg.norm(vertices - centroid, axis=1).max()), _MIN_SIZE_M) + return { + "type": "sphere", + "size": (r,), + "pos": tuple(map(float, centroid)), + "quat": (1.0, 0.0, 0.0, 0.0), + "volume": float((4.0 / 3.0) * np.pi * r**3), + } + + +def _fit_cylinder(vertices: np.ndarray) -> PrimitiveFit: + """Cylinder along PCA principal axis.""" + centroid = vertices.mean(0) + centered = vertices - centroid + cov = np.cov(centered.T) + evals, evecs = np.linalg.eigh(cov) + axis = evecs[:, -1] # largest variance + proj = centered @ axis + half_h = max(float((proj.max() - proj.min()) / 2.0), _MIN_SIZE_M) + centre_along = float((proj.max() + proj.min()) / 2.0) + pos = centroid + axis * centre_along + # radius = max perp distance from axis line + perp = centered - np.outer(centered @ axis, axis) + r = max(float(np.linalg.norm(perp, axis=1).max()), _MIN_SIZE_M) + return { + "type": "cylinder", + "size": (r, half_h), + "pos": tuple(map(float, pos)), + "quat": _quat_z_to(axis), + "volume": float(np.pi * r * r * 2.0 * half_h), + } + + +def _fit_capsule(vertices: np.ndarray) -> PrimitiveFit: + """Capsule along PCA principal axis. MuJoCo capsule half-height is + the *cylindrical* portion only; total length = 2*(half_h + r).""" + cyl = _fit_cylinder(vertices) + r, h = cyl["size"] + new_h = max(float(h - r), _MIN_SIZE_M) + vol = float(np.pi * r * r * 2.0 * new_h) + float((4.0 / 3.0) * np.pi * r**3) + return { + "type": "capsule", + "size": (r, new_h), + "pos": cyl["pos"], + "quat": cyl["quat"], + "volume": vol, + } + + +def _hull_volume(vertices: np.ndarray) -> float | None: + """Convex-hull volume in m³, or ``None`` if qhull rejects the points.""" + try: + return float(ConvexHull(vertices).volume) + except (QhullError, ValueError): + return None + + +def _mesh_volume(vertices: np.ndarray, triangles: np.ndarray) -> float: + """Signed mesh volume (Gauss / divergence theorem on triangle fans). + + Closed meshes return a positive number; for non-closed inputs the + absolute value is a coarse estimate.""" + v0 = vertices[triangles[:, 0]] + v1 = vertices[triangles[:, 1]] + v2 = vertices[triangles[:, 2]] + return float(abs(np.sum(np.einsum("ij,ij->i", v0, np.cross(v1, v2))) / 6.0)) + + +def _best_primitive_fit( + vertices: np.ndarray, + hull_vol: float, + candidates: tuple[str, ...] = ("box", "cylinder", "sphere", "capsule"), +) -> PrimitiveFit | None: + """Try every primitive in ``candidates``; return the one with the + highest fill ratio. Returns ``None`` if no fit succeeds (e.g. < 4 + points).""" + fitters = { + "box": _fit_obb_box, + "sphere": _fit_sphere, + "cylinder": _fit_cylinder, + "capsule": _fit_capsule, + } + fits: list[PrimitiveFit] = [] + for kind in candidates: + try: + f = fitters[kind](vertices) + if f["volume"] <= 0: + continue + f["fill_ratio"] = hull_vol / f["volume"] + fits.append(f) + except Exception as e: + logger.debug(f" primitive fit {kind} failed: {e}") + if not fits: + return None + return max(fits, key=lambda f: f["fill_ratio"]) + + +# --------------------------------------------------------------------------- # +# Generic geometric heuristics # +# --------------------------------------------------------------------------- # + + +def _is_tiny(extent: np.ndarray, threshold_m: float) -> bool: + return bool(extent.max() < threshold_m) + + +def _is_slab(extent: np.ndarray, aspect_ratio: float) -> bool: + """Wall / floor / door / panel — one axis is much smaller than the + other two (or one much larger than the other two — covers beams).""" + sorted_ext = np.sort(extent) + if sorted_ext[0] < 1e-6: + return True + return bool((sorted_ext[2] / sorted_ext[0]) >= aspect_ratio) + + +def _sheet_footprint_stats( + vertices: np.ndarray, + triangles: np.ndarray, + thin_axis: int, +) -> tuple[float, float] | None: + """Return ``(projected_aabb_area, projected_triangle_fill)`` for a sheet.""" + axes = [i for i in range(3) if i != thin_axis] + projected = vertices[:, axes] + span = projected.max(axis=0) - projected.min(axis=0) + box_area = float(span[0] * span[1]) + if box_area < 1e-9: + return None + + tri = projected[triangles] + edge_a = tri[:, 1] - tri[:, 0] + edge_b = tri[:, 2] - tri[:, 0] + area = 0.5 * np.abs(edge_a[:, 0] * edge_b[:, 1] - edge_a[:, 1] * edge_b[:, 0]).sum() + fill = float(area / box_area) + return box_area, fill + + +def _is_boxlike_sheet( + vertices: np.ndarray, + triangles: np.ndarray, + thin_axis: int, +) -> bool: + """Whether a thin mesh roughly fills its projected bounding rectangle. + + A single primitive box is only acceptable when the source sheet's + projected triangle area is close to the projected AABB area. Low + ratios mean an L-shape / beam strip / holes; high ratios usually mean + overlapping, folded, or angled sheets inside one prim. + """ + stats = _sheet_footprint_stats(vertices, triangles, thin_axis) + if stats is None: + return False + _, fill = stats + return _SHEET_BOX_FILL_MIN <= fill <= _SHEET_BOX_FILL_MAX + + +def _should_emit_triangle_prisms( + vertices: np.ndarray, + triangles: np.ndarray, + thin_axis: int, +) -> bool: + """Use exact-ish triangle prisms only for large horizontal sheets. + + This avoids placing huge slabs over angular floors and roof strips, + without exploding tiny decorative meshes into thousands of geoms. + """ + if thin_axis != 2: + return False + if len(triangles) > _SHEET_PRISM_MAX_TRIANGLES: + return False + stats = _sheet_footprint_stats(vertices, triangles, thin_axis) + if stats is None: + return False + footprint_area, _ = stats + return footprint_area >= _SHEET_PRISM_MIN_FOOTPRINT_AREA_M2 + + +def _thin_sheet_hulls( + vertices: np.ndarray, + triangles: np.ndarray, + thickness: float = _SHEET_PRISM_THICKNESS_M, +) -> list[tuple[np.ndarray, np.ndarray]]: + """Represent a thin non-rectangular sheet as convex triangle prisms.""" + hulls: list[tuple[np.ndarray, np.ndarray]] = [] + faces = np.asarray( + [ + [0, 1, 2], + [5, 4, 3], + [0, 3, 4], + [0, 4, 1], + [1, 4, 5], + [1, 5, 2], + [2, 5, 3], + [2, 3, 0], + ], + dtype=np.int32, + ) + + for tri_idx in triangles: + tri = vertices[tri_idx].astype(np.float64) + if not np.isfinite(tri).all(): + continue + normal = np.cross(tri[1] - tri[0], tri[2] - tri[0]) + norm = float(np.linalg.norm(normal)) + if norm < 1e-9: + continue + offset = normal / norm * (thickness * 0.5) + prism = np.vstack((tri + offset, tri - offset)).astype(np.float32) + hulls.append((prism, faces)) + + return hulls + + +def _is_flat_horizontal_box(extent: np.ndarray, thin_axis: int) -> bool: + """Thin in world Z, broad in world X/Y, and flat enough to box safely. + + PCA boxes are unstable for nearly flat floors/ceilings: any small + triangulation asymmetry can rotate the OBB basis and turn a walkable + surface into a shallow ramp. For world-horizontal slabs, the AABB is + the physically safer collision approximation. + """ + if thin_axis != 2: + return False + xy_min = float(min(extent[0], extent[1])) + z_extent = float(extent[2]) + if xy_min < 1e-6: + return False + return z_extent <= _HORIZONTAL_BOX_MAX_THICKNESS_M + + +# --------------------------------------------------------------------------- # +# Dispatcher: per-prim decision # +# --------------------------------------------------------------------------- # + + +@dataclass +class PrimDecision: + """What the dispatcher decided for one prim. Consumed by the bake + which materialises ``GeomEmission`` records and writes OBJs.""" + + #: ``"skip"`` (no collision), ``"primitive"`` (one ```` with + #: kind ∈ {box, sphere, cylinder, capsule, plane}), or ``"hulls"`` + #: (one or more mesh ````s from convex-hull decomposition). + mode: Literal["skip", "primitive", "hulls"] + + #: For ``"primitive"``: the fit dict (``type``, ``size``, ``pos``, + #: ``quat``, ``volume``, ``fill_ratio``). + primitive: PrimitiveFit | None = None + + #: For ``"hulls"``: list of ``(vertices, triangles)`` ready to write. + hulls: list[tuple[np.ndarray, np.ndarray]] = field(default_factory=list) + + #: For diagnostics: which rule fired. + reason: str = "" + + #: Optional friction override from sidecar. + friction: tuple[float, float, float] | None = None + + +def decide_for_prim( + vertices: np.ndarray, + triangles: np.ndarray, + prim_path: str, + spec: CollisionSpec, +) -> PrimDecision: + """Resolve sidecar + heuristics + auto-fit for a single prim. + + Pure function — does no I/O. The caller (bake) materialises the + decision: writes hull OBJs to disk, emits MJCF lines. + + Args: + vertices: ``(N, 3)`` float, world-frame after ``SceneMeshAlignment``. + triangles: ``(M, 3)`` int vertex indices. + prim_path: USD-style prim path used as sidecar key. + spec: parsed sidecar. + """ + extent = vertices.max(0) - vertices.min(0) + override = spec.resolve(prim_path) + kind = override.get("type", spec.default) + friction = override.get("friction") + if friction is not None: + friction = tuple(float(x) for x in friction) + + # 0. Explicit "skip" — short-circuit. + if kind == "skip": + return PrimDecision(mode="skip", reason="sidecar:skip", friction=friction) + + # 1. Tiny-prim guard (applies to "auto" path; explicit overrides win). + if kind in ("auto",) and _is_tiny(extent, spec.tiny_prim_extent_m): + return PrimDecision(mode="skip", reason="tiny-prim", friction=friction) + + # 2. Explicit primitive in sidecar — fit if size/pos not provided. + if kind in ("box", "sphere", "cylinder", "capsule", "plane"): + fit = _resolve_explicit_primitive(vertices, kind, override) + fit["fill_ratio"] = float("nan") # unknown — user asserted this shape + return PrimDecision( + mode="primitive", primitive=fit, reason=f"sidecar:{kind}", friction=friction + ) + + # 3. Explicit hull / decompose paths. + if kind == "hull": + return PrimDecision( + mode="hulls", + hulls=[(vertices, triangles)], # signal: single-hull, no decomp + reason="sidecar:hull", + friction=friction, + ) + if kind == "decompose": + max_h = int(override.get("max_hulls", spec.coacd_max_hulls)) + hulls = _coacd_decompose(vertices, triangles, spec.coacd_threshold, max_h) + return PrimDecision( + mode="hulls", + hulls=hulls, + reason="sidecar:decompose", + friction=friction, + ) + + # 4. From here on: kind == "auto". Generic heuristics first. + + # 4a. Aspect-ratio: slab/beam → force box (fill ratio may be + # marginal because of moulding/profile, but a box collision is + # the right physical answer for walls and slabs). Non-rectangular + # sheets are emitted as triangle prisms so we don't fill holes or + # angular roof/floor outlines with one huge invisible slab. + if _is_slab(extent, spec.aspect_ratio_box): + thin_axis = int(np.argmin(extent)) + if ( + spec.enable_sheet_prisms + and not _is_boxlike_sheet(vertices, triangles, thin_axis) + and _should_emit_triangle_prisms(vertices, triangles, thin_axis) + ): + hulls = _thin_sheet_hulls(vertices, triangles) + if hulls: + return PrimDecision( + mode="hulls", + hulls=hulls, + reason=f"thin-sheet:triangle-prisms({len(hulls)})", + friction=friction, + ) + + if _is_flat_horizontal_box(extent, thin_axis): + fit = _fit_aabb_box(vertices) + reason = "aspect-ratio:horizontal-slab" + else: + fit = _fit_obb_box(vertices) + reason = "aspect-ratio:slab" + fit["fill_ratio"] = float("nan") + return PrimDecision(mode="primitive", primitive=fit, reason=reason, friction=friction) + + # 4b. Need hull volume for the rest. + hull_vol = _hull_volume(vertices) + if hull_vol is None: + return PrimDecision(mode="skip", reason="degenerate (qhull rejected)", friction=friction) + + # 4c. Try primitive auto-fit. + auto_fit = _best_primitive_fit(vertices, hull_vol) + if auto_fit is not None and 0.0 < auto_fit["fill_ratio"] <= 1.5: + # fill_ratio > 1 happens for non-closed hulls; cap to keep this + # finite when reporting. Accept if within tolerance. + if auto_fit["fill_ratio"] >= spec.fill_threshold: + return PrimDecision( + mode="primitive", + primitive=auto_fit, + reason=f"auto:{auto_fit['type']}({auto_fit['fill_ratio']:.2f})", + friction=friction, + ) + + # 4d. Near-convex shortcut: skip CoACD, single hull. + mesh_vol = _mesh_volume(vertices, triangles) + if hull_vol > 0 and mesh_vol / hull_vol > spec.near_convex_threshold: + return PrimDecision( + mode="hulls", + hulls=[(vertices, triangles)], + reason=f"near-convex({mesh_vol / hull_vol:.2f})", + friction=friction, + ) + + # 4e. Small concave prim → single hull (matches today's behaviour + # for furniture-scale things; faster than CoACD). + if hull_vol < spec.shell_volume_m3: + return PrimDecision( + mode="hulls", + hulls=[(vertices, triangles)], + reason="small-shell:single-hull", + friction=friction, + ) + + # 4f. Large concave shell → CoACD. + hulls = _coacd_decompose(vertices, triangles, spec.coacd_threshold, spec.coacd_max_hulls) + return PrimDecision( + mode="hulls", + hulls=hulls, + reason=f"coacd:{len(hulls)}", + friction=friction, + ) + + +# --------------------------------------------------------------------------- # +# Helpers used by the dispatcher # +# --------------------------------------------------------------------------- # + + +def _resolve_explicit_primitive( + vertices: np.ndarray, + kind: str, + override: OverrideConfig, +) -> PrimitiveFit: + """Build a primitive fit dict from a sidecar override. + + If the override supplies ``size`` (and optionally ``pos`` / ``quat``), + those win. Otherwise we auto-fit the requested primitive and use + those params. ``plane`` is special-cased — we always derive from + the prim's xy footprint at its min z. + """ + if kind == "plane": + mn = vertices.min(0) + mx = vertices.max(0) + return { + "type": "plane", + "size": (float((mx[0] - mn[0]) / 2.0), float((mx[1] - mn[1]) / 2.0), 0.5), + "pos": ( + float((mx[0] + mn[0]) / 2.0), + float((mx[1] + mn[1]) / 2.0), + float(mn[2]), + ), + "quat": (1.0, 0.0, 0.0, 0.0), + "volume": 0.0, + } + + fitters = { + "box": _fit_obb_box, + "sphere": _fit_sphere, + "cylinder": _fit_cylinder, + "capsule": _fit_capsule, + } + fit = fitters[kind](vertices) + # Apply explicit overrides if provided. + if "size" in override: + fit["size"] = tuple(float(x) for x in override["size"]) + if "pos" in override: + fit["pos"] = tuple(float(x) for x in override["pos"]) + if "quat" in override: + fit["quat"] = tuple(float(x) for x in override["quat"]) + return fit + + +def _coacd_decompose( + vertices: np.ndarray, + triangles: np.ndarray, + threshold: float, + max_hulls: int, +) -> list[tuple[np.ndarray, np.ndarray]]: + """Run CoACD on a single prim, return list of ``(verts, tris)`` hulls. + + CoACD is imported lazily — it ships its own C library and we don't + want every import of ``collision_spec`` to pay that cost. + """ + import coacd # type: ignore[import-not-found, import-untyped] + + # CoACD's C lib prints a lot per invocation; quiet it once per process. + if not getattr(_coacd_decompose, "_silenced", False): + coacd.set_log_level("error") + _coacd_decompose._silenced = True # type: ignore[attr-defined] + + mesh = coacd.Mesh(vertices.astype(np.float64), triangles.astype(np.int32)) + # CoACD's MCTS defaults (mcts_iterations=150, resolution=2000) are tuned + # for offline asset prep — minutes per shell on a multi-thousand-prim + # scene. We dial both down ~5x; the resulting hulls are slightly + # noisier but the bake finishes in minutes, not hours. For a one-off + # final bake users can override via the sidecar (future work) or call + # ``bake_scene_mjcf`` directly with a custom ``CollisionSpec``. + parts = coacd.run_coacd( + mesh, + threshold=threshold, + max_convex_hull=max_hulls, + resolution=500, + mcts_iterations=30, + mcts_nodes=10, + ) + out: list[tuple[np.ndarray, np.ndarray]] = [] + for v, t in parts: + v = np.asarray(v, dtype=np.float32) + t = np.asarray(t, dtype=np.int32) + if len(v) >= 4 and len(t) >= 1: + out.append((v, t)) + return out + + +__all__ = [ + "CollisionSpec", + "GeomEmission", + "PrimDecision", + "decide_for_prim", +] diff --git a/dimos/simulation/mujoco/mesh_scene.py b/dimos/simulation/mujoco/mesh_scene.py new file mode 100644 index 0000000000..cf186ddf22 --- /dev/null +++ b/dimos/simulation/mujoco/mesh_scene.py @@ -0,0 +1,767 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Load a 3D scene mesh from disk for ray-casting and MuJoCo collision. + +Supports: + * ``.glb`` / ``.gltf`` / ``.obj`` / ``.ply`` / ``.stl`` — via Open3D's + ``read_triangle_mesh``. + * ``.usdz`` / ``.usd`` / ``.usdc`` — via ``pxr.Usd`` (install ``usd-core``). + +Returned form is a single concatenated ``open3d.geometry.TriangleMesh`` +in world frame, with optional scale + Y-up→Z-up + translation applied. + +The same mesh can feed ray-casting and MJCF collision wrapping so the +geometric query path and physical scene share one transform. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from typing import Any + +import numpy as np +import open3d as o3d # type: ignore[import-untyped] + + +@dataclass +class SceneMeshAlignment: + """How to transform a raw scene mesh into dimos world frame. + + Apply order: scale → rotation (y_up swap then zyx euler) → translation. + """ + + scale: float = 1.0 + """Multiplicative scale. Use 0.01 if the source is centimeters.""" + + rotation_zyx_deg: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Yaw / pitch / roll in degrees, applied after the y_up swap.""" + + translation: tuple[float, float, float] = (0.0, 0.0, 0.0) + """World-frame offset applied last.""" + + y_up: bool = True + """Most exporters (Blender, glTF, Apple USDZ) are Y-up. When ``True`` + rotate the mesh -90 deg about world X to match dimos's Z-up convention.""" + + +def _world_rotation(alignment: SceneMeshAlignment) -> np.ndarray: + """Compose the y-up swap + ZYX Euler into one 3x3.""" + rad = np.radians(alignment.rotation_zyx_deg) + cz, sz = np.cos(rad[0]), np.sin(rad[0]) + cy, sy = np.cos(rad[1]), np.sin(rad[1]) + cx, sx = np.cos(rad[2]), np.sin(rad[2]) + rz = np.array([[cz, -sz, 0], [sz, cz, 0], [0, 0, 1]], dtype=np.float64) + ry = np.array([[cy, 0, sy], [0, 1, 0], [-sy, 0, cy]], dtype=np.float64) + rx = np.array([[1, 0, 0], [0, cx, -sx], [0, sx, cx]], dtype=np.float64) + rzyx = rz @ ry @ rx + if alignment.y_up: + y_to_z = np.array( + [[1, 0, 0], [0, 0, -1], [0, 1, 0]], + dtype=np.float64, + ) + return rzyx @ y_to_z + return rzyx + + +def _average_per_face_vertex( + per_fv: np.ndarray, face_verts: np.ndarray, n_verts: int +) -> np.ndarray: + """Scatter-average ``(n_face_verts, 3)`` values onto ``(n_verts, 3)`` indices.""" + out = np.zeros((n_verts, 3), dtype=np.float32) + counts = np.zeros(n_verts, dtype=np.int32) + np.add.at(out, face_verts, per_fv) + np.add.at(counts, face_verts, 1) + counts = np.maximum(counts, 1)[:, None] + return out / counts + + +def _color_from_displaycolor( + mesh: Any, + n_verts: int, + face_counts: np.ndarray, + face_verts: np.ndarray, +) -> np.ndarray | None: + """Per-vertex RGB from ``primvars:displayColor`` if present and valued. + + Handles the four standard interpolations: ``constant`` / ``vertex`` / + ``uniform`` / ``faceVarying``. Returns ``None`` when the primvar + isn't authored with a value (Sketchfab USDZ exports typically declare + the primvar but leave it empty — colors live on the bound material). + """ + from pxr import UsdGeom # type: ignore[import-not-found, import-untyped] + + pv = UsdGeom.PrimvarsAPI(mesh.GetPrim()).GetPrimvar("displayColor") + if not pv or not pv.HasValue(): + return None + raw = pv.Get() + if raw is None: + return None + colors = np.asarray(raw, dtype=np.float32) + if colors.ndim != 2 or colors.shape[1] != 3 or colors.size == 0: + return None + interp = pv.GetInterpolation() + + if interp == UsdGeom.Tokens.constant: + return np.tile(colors[0:1], (n_verts, 1)) + + if interp == UsdGeom.Tokens.vertex and len(colors) == n_verts: + return colors + + if interp == UsdGeom.Tokens.uniform and len(colors) == len(face_counts): + per_fv = np.repeat(colors, face_counts, axis=0) + return _average_per_face_vertex(per_fv, face_verts, n_verts) + + if interp == UsdGeom.Tokens.faceVarying and len(colors) == len(face_verts): + return _average_per_face_vertex(colors, face_verts, n_verts) + + return None + + +def _color_from_material( + prim: Any, material_color_cache: dict[str, np.ndarray | None] +) -> np.ndarray | None: + """Per-prim RGB from the bound material's ``inputs:diffuseColor``. + + Walks ``UsdShadeMaterialBindingAPI`` → surface shader → ``inputs:diffuseColor``, + handling ``UsdPreviewSurface`` (the format Sketchfab USDZ uses). Texture + inputs aren't sampled — if ``diffuseColor`` is connected to a ``UsdUVTexture`` + rather than authored as a literal, this returns ``None`` and the caller + falls back to the next strategy. + + Results are cached per material path so we don't re-walk the shader graph + for every prim that shares a material. + """ + from pxr import UsdShade # type: ignore[import-not-found, import-untyped] + + mat_api = UsdShade.MaterialBindingAPI(prim) + bound = mat_api.ComputeBoundMaterial()[0] + if not bound: + return None + mat_path = str(bound.GetPath()) + if mat_path in material_color_cache: + return material_color_cache[mat_path] + + color = _resolve_diffuse_color(bound) + material_color_cache[mat_path] = color + return color + + +def _resolve_diffuse_color(material: Any) -> np.ndarray | None: + """Pull a literal ``diffuseColor`` out of a UsdShade material's surface shader.""" + from pxr import UsdShade # type: ignore[import-not-found, import-untyped] + + surface = material.ComputeSurfaceSource("")[0] + if not surface: + return None + diffuse_input = surface.GetInput("diffuseColor") + if not diffuse_input: + return None + # If the input is connected (texture-driven), bail — we don't sample images. + if diffuse_input.HasConnectedSource(): + connected = diffuse_input.GetConnectedSource()[0] + if connected: + shader = UsdShade.Shader(connected.GetPrim()) + if shader and shader.GetIdAttr().Get() == "UsdUVTexture": + return None + val = diffuse_input.Get() + if val is None: + return None + arr = np.asarray(val, dtype=np.float32).reshape(-1) + if arr.size != 3: + return None + return arr # (3,) RGB in [0, 1] + + +def _load_usd_mesh(path: Path) -> o3d.geometry.TriangleMesh: + """Walk every Mesh prim in a USD stage and concatenate to one o3d mesh. + + Also extracts per-vertex colors from ``primvars:displayColor`` when + present so downstream consumers can render textured-looking Sketchfab + exports without having to chase materials/textures. + """ + try: + from pxr import Usd, UsdGeom # type: ignore[import-not-found, import-untyped] + except ImportError as e: + raise ImportError("loading .usdz/.usd requires usd-core: `uv pip install usd-core`") from e + + stage = Usd.Stage.Open(str(path)) + if stage is None: + raise RuntimeError(f"could not open USD stage: {path}") + + all_pts: list[np.ndarray] = [] + all_tris: list[np.ndarray] = [] + all_colors: list[np.ndarray] = [] + any_color = False + vtx_offset = 0 + material_color_cache: dict[str, np.ndarray | None] = {} + + for prim in stage.Traverse(): + if not prim.IsA(UsdGeom.Mesh): + continue + mesh = UsdGeom.Mesh(prim) + pts_attr = mesh.GetPointsAttr().Get() + if pts_attr is None or len(pts_attr) == 0: + continue + pts = np.asarray(pts_attr, dtype=np.float32) + face_verts = np.asarray(mesh.GetFaceVertexIndicesAttr().Get(), dtype=np.int32) + face_counts = np.asarray(mesh.GetFaceVertexCountsAttr().Get(), dtype=np.int32) + + # Bake the prim's local-to-world transform into the points so the + # composite scene comes out in stage-root coordinates. + xform = UsdGeom.Xformable(prim).ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + m = np.asarray(xform, dtype=np.float64).T # USD matrices are row-major + pts_h = np.hstack([pts, np.ones((len(pts), 1), dtype=np.float32)]) + pts_world = (m @ pts_h.T).T[:, :3].astype(np.float32) + + # Per-prim color resolution. Try in order: + # 1. ``primvars:displayColor`` (vertex / faceVarying / uniform / constant) + # 2. Bound material's ``inputs:diffuseColor`` (UsdPreviewSurface — what + # Sketchfab USDZ uses, with one constant color per material). + # 3. Neutral grey fallback. + prim_colors = _color_from_displaycolor(mesh, len(pts), face_counts, face_verts) + if prim_colors is None: + mat_color = _color_from_material(prim, material_color_cache) + if mat_color is not None: + prim_colors = np.tile(mat_color[None, :], (len(pts), 1)) + if prim_colors is not None: + any_color = True + else: + prim_colors = np.full((len(pts), 3), 0.7, dtype=np.float32) + + # USD allows quads / n-gons; fan-triangulate so o3d gets pure tris. + tris: list[tuple[int, int, int]] = [] + cursor = 0 + for n in face_counts: + for k in range(1, n - 1): + tris.append( + ( + int(face_verts[cursor]) + vtx_offset, + int(face_verts[cursor + k]) + vtx_offset, + int(face_verts[cursor + k + 1]) + vtx_offset, + ) + ) + cursor += n + + if not tris: + continue + all_pts.append(pts_world) + all_tris.append(np.asarray(tris, dtype=np.int32)) + all_colors.append(prim_colors) + vtx_offset += len(pts_world) + + if not all_pts: + raise RuntimeError(f"no Mesh prims with triangles found in {path}") + + pts = np.concatenate(all_pts, axis=0).astype(np.float64) + tris = np.concatenate(all_tris, axis=0) + + mesh = o3d.geometry.TriangleMesh() + mesh.vertices = o3d.utility.Vector3dVector(pts) + mesh.triangles = o3d.utility.Vector3iVector(tris) + if any_color: + colors = np.concatenate(all_colors, axis=0).astype(np.float64) + mesh.vertex_colors = o3d.utility.Vector3dVector(np.clip(colors, 0.0, 1.0)) + return mesh + + +def load_scene_mesh( + path: str | Path, + alignment: SceneMeshAlignment | None = None, +) -> o3d.geometry.TriangleMesh: + """Load a scene mesh from disk and apply alignment to put it in dimos world frame. + + Args: + path: file path. Supported extensions: ``.usdz``, ``.usd``, ``.usdc``, + ``.glb``, ``.gltf``, ``.obj``, ``.ply``, ``.stl``. + alignment: scale / rotation / translation to apply. + + Returns: + an ``open3d.geometry.TriangleMesh`` in dimos world frame with vertex + normals computed. + """ + path = Path(path) + if not path.exists(): + raise FileNotFoundError(f"scene mesh not found: {path}") + suffix = path.suffix.lower() + if suffix in {".usdz", ".usd", ".usdc", ".usda"}: + mesh = _load_usd_mesh(path) + elif suffix in {".glb", ".gltf"}: + # GEOMETRY-ONLY GLB load. Used by floor-z probing and ray-casting; + # it does not need PBR materials. ``trimesh.load(path, force="mesh")`` + # would flatten the scene by decompressing every embedded texture and + # sampling per-vertex colors. For a scene with hundreds of 4K PBR + # textures, that allocates ~10 GB transiently and OOMs 32 GB boxes. + # We open in Scene mode (no flattening, no texture decode), walk the + # instance graph applying each instance's world transform, and emit a + # single concatenated mesh — peak stays under ~1 GB. + import trimesh + + scene_or_mesh: Any = trimesh.load(str(path)) + if isinstance(scene_or_mesh, trimesh.Trimesh): + verts_world = np.asarray(scene_or_mesh.vertices, dtype=np.float64) + faces_world = np.asarray(scene_or_mesh.faces, dtype=np.int64) + else: + scene = scene_or_mesh + verts_chunks: list[np.ndarray] = [] + faces_chunks: list[np.ndarray] = [] + v_off = 0 + for node_name in scene.graph.nodes_geometry: + xform, geom_name = scene.graph[node_name] + geom = scene.geometry.get(geom_name) + if geom is None or not isinstance(geom, trimesh.Trimesh) or len(geom.faces) == 0: + continue + v_local = np.asarray(geom.vertices, dtype=np.float64) + f_local = np.asarray(geom.faces, dtype=np.int64) + m = np.asarray(xform, dtype=np.float64) + v_h = np.hstack([v_local, np.ones((len(v_local), 1), dtype=np.float64)]) + v_world = (m @ v_h.T).T[:, :3] + verts_chunks.append(v_world) + faces_chunks.append(f_local + v_off) + v_off += len(v_local) + if not verts_chunks: + raise RuntimeError(f"glTF loaded but no Trimesh instances found: {path}") + verts_world = np.concatenate(verts_chunks, axis=0) + faces_world = np.concatenate(faces_chunks, axis=0) + + mesh = o3d.geometry.TriangleMesh() + mesh.vertices = o3d.utility.Vector3dVector(verts_world) + mesh.triangles = o3d.utility.Vector3iVector(faces_world.astype(np.int32)) + else: + mesh = o3d.io.read_triangle_mesh(str(path)) + if len(mesh.triangles) == 0: + raise RuntimeError(f"o3d.io.read_triangle_mesh returned an empty mesh for {path}") + + align = alignment or SceneMeshAlignment() + if align.scale != 1.0: + mesh.scale(align.scale, center=np.zeros(3)) + rot = _world_rotation(align) + if not np.allclose(rot, np.eye(3)): + mesh.rotate(rot, center=np.zeros(3)) + if any(align.translation): + mesh.translate(np.asarray(align.translation, dtype=np.float64)) + + mesh.compute_vertex_normals() + return mesh + + +def make_raycasting_scene( + mesh: o3d.geometry.TriangleMesh, +) -> o3d.t.geometry.RaycastingScene: + """Wrap a TriangleMesh into Open3D's BVH-backed ray-casting scene.""" + scene = o3d.t.geometry.RaycastingScene() + scene.add_triangles(o3d.t.geometry.TriangleMesh.from_legacy(mesh)) + return scene + + +@dataclass +class ScenePrimMesh: + """One USD ``Mesh`` prim's geometry, ready to write to OBJ. + + Used by ``load_scene_prims`` to keep prims separate so MuJoCo can + treat each as its own (approximately convex) collision shape. When + the loader handles a non-USD format the input is returned as a + single-element list with the whole mesh in it. + """ + + name: str + """Sanitized identifier (safe for MJCF asset names) — typically the + USD prim path with non-alphanumerics replaced.""" + + vertices: np.ndarray + """``(N, 3)`` float32, in world frame after alignment.""" + + triangles: np.ndarray + """``(M, 3)`` int32 vertex indices.""" + + +def _usd_matrix_to_numpy(matrix: Any) -> np.ndarray: + """Convert a USD row-major transform into column-vector numpy form.""" + return np.asarray(matrix, dtype=np.float64).T + + +def _transform_points(points: np.ndarray, matrix: np.ndarray) -> np.ndarray: + points_h = np.hstack([points, np.ones((len(points), 1), dtype=np.float64)]) + return np.asarray((matrix @ points_h.T).T[:, :3], dtype=np.float64) + + +def _triangulated_usd_mesh(usd_mesh: Any) -> tuple[np.ndarray, np.ndarray] | None: + pts_attr = usd_mesh.GetPointsAttr().Get() + if pts_attr is None or len(pts_attr) == 0: + return None + + face_verts_attr = usd_mesh.GetFaceVertexIndicesAttr().Get() + face_counts_attr = usd_mesh.GetFaceVertexCountsAttr().Get() + if face_verts_attr is None or face_counts_attr is None: + return None + + pts = np.asarray(pts_attr, dtype=np.float64) + face_verts = np.asarray(face_verts_attr, dtype=np.int32) + face_counts = np.asarray(face_counts_attr, dtype=np.int32) + + tris: list[tuple[int, int, int]] = [] + cursor = 0 + for n in face_counts: + if n < 3: + cursor += int(n) + continue + for k in range(1, int(n) - 1): + tris.append( + ( + int(face_verts[cursor]), + int(face_verts[cursor + k]), + int(face_verts[cursor + k + 1]), + ) + ) + cursor += int(n) + + if not tris: + return None + return pts, np.asarray(tris, dtype=np.int32) + + +def _aligned_scene_points( + pts_stage: np.ndarray, + *, + rotation: np.ndarray, + scale: float, + translation: np.ndarray, +) -> np.ndarray: + return np.asarray((rotation @ (scale * pts_stage).T).T + translation, dtype=np.float64) + + +def _clean_scene_name(raw: str) -> str: + return "".join(c if c.isalnum() else "_" for c in raw) + + +def _path_has_prefix(path: Any, prefix: Any) -> bool: + try: + return bool(path.HasPrefix(prefix)) + except AttributeError: + path_str = str(path) + prefix_str = str(prefix).rstrip("/") + return path_str == prefix_str or path_str.startswith(prefix_str + "/") + + +def _point_instancer_prototype_paths(stage: Any, UsdGeom: Any) -> tuple[Any, ...]: + paths: list[Any] = [] + for prim in stage.Traverse(): + if not prim.IsA(UsdGeom.PointInstancer): + continue + paths.extend(UsdGeom.PointInstancer(prim).GetPrototypesRel().GetTargets()) + return tuple(paths) + + +def _is_point_instancer_prototype_mesh(prim: Any, prototype_paths: tuple[Any, ...]) -> bool: + path = prim.GetPath() + return any(_path_has_prefix(path, proto_path) for proto_path in prototype_paths) + + +def _collect_prototype_meshes( + proto_prim: Any, + *, + Usd: Any, + UsdGeom: Any, +) -> list[tuple[str, np.ndarray, np.ndarray]]: + """Return prototype mesh triangles in prototype-root coordinates.""" + time = Usd.TimeCode.Default() + xform_cache = UsdGeom.XformCache(time) + proto_world = _usd_matrix_to_numpy(xform_cache.GetLocalToWorldTransform(proto_prim)) + proto_world_inv = np.linalg.inv(proto_world) + + meshes: list[tuple[str, np.ndarray, np.ndarray]] = [] + for mesh_prim in Usd.PrimRange(proto_prim): + if not mesh_prim.IsA(UsdGeom.Mesh): + continue + triangulated = _triangulated_usd_mesh(UsdGeom.Mesh(mesh_prim)) + if triangulated is None: + continue + pts, tris = triangulated + mesh_world = _usd_matrix_to_numpy(xform_cache.GetLocalToWorldTransform(mesh_prim)) + pts_proto = _transform_points(pts, proto_world_inv @ mesh_world) + mesh_path = str(mesh_prim.GetPath()).lstrip("/") + meshes.append((_clean_scene_name(mesh_path), pts_proto, tris)) + return meshes + + +def _compute_point_instance_transforms(instancer: Any, *, Usd: Any, UsdGeom: Any) -> list[Any]: + time = Usd.TimeCode.Default() + try: + return list( + instancer.ComputeInstanceTransformsAtTime( + time, + time, + UsdGeom.PointInstancer.ExcludeProtoXform, + ) + ) + except TypeError: + return list(instancer.ComputeInstanceTransformsAtTime(time, time)) + + +def _expand_point_instancer( + prim: Any, + *, + Usd: Any, + UsdGeom: Any, + rotation: np.ndarray, + scale: float, + translation: np.ndarray, + start_index: int, +) -> list[ScenePrimMesh]: + instancer = UsdGeom.PointInstancer(prim) + proto_targets = list(instancer.GetPrototypesRel().GetTargets()) + proto_indices_attr = instancer.GetProtoIndicesAttr().Get() + if not proto_targets or proto_indices_attr is None: + return [] + + proto_indices = list(proto_indices_attr) + instance_transforms = _compute_point_instance_transforms( + instancer, + Usd=Usd, + UsdGeom=UsdGeom, + ) + if not instance_transforms: + return [] + + time = Usd.TimeCode.Default() + xform_cache = UsdGeom.XformCache(time) + instancer_world = _usd_matrix_to_numpy(xform_cache.GetLocalToWorldTransform(prim)) + + prototype_cache: dict[str, list[tuple[str, np.ndarray, np.ndarray]]] = {} + prims: list[ScenePrimMesh] = [] + instancer_name = _clean_scene_name(str(prim.GetPath()).lstrip("/")) + + for instance_index, instance_transform in enumerate(instance_transforms): + if instance_index >= len(proto_indices): + continue + proto_index = int(proto_indices[instance_index]) + if proto_index < 0 or proto_index >= len(proto_targets): + continue + + proto_path = proto_targets[proto_index] + proto_prim = prim.GetStage().GetPrimAtPath(proto_path) + if not proto_prim or not proto_prim.IsValid(): + continue + + proto_key = str(proto_path) + if proto_key not in prototype_cache: + prototype_cache[proto_key] = _collect_prototype_meshes( + proto_prim, + Usd=Usd, + UsdGeom=UsdGeom, + ) + prototype_meshes = prototype_cache[proto_key] + if not prototype_meshes: + continue + + instance_matrix = _usd_matrix_to_numpy(instance_transform) + mesh_to_stage = instancer_world @ instance_matrix + for mesh_name, pts_proto, tris in prototype_meshes: + pts_stage = _transform_points(pts_proto, mesh_to_stage) + pts_world = _aligned_scene_points( + pts_stage, + rotation=rotation, + scale=scale, + translation=translation, + ) + prims.append( + ScenePrimMesh( + name=( + f"{instancer_name}_i{instance_index:05d}_" + f"{mesh_name}__{start_index + len(prims)}" + ), + vertices=pts_world.astype(np.float32), + triangles=tris, + ) + ) + return prims + + +def _load_glb_prims(path: Path, alignment: SceneMeshAlignment) -> list[ScenePrimMesh]: + """Enumerate per-instance prims from a glTF/GLB. + + ``trimesh.load(file.glb)`` returns a ``Scene`` whose ``graph`` records + the world transform for every geometry instance. Iterating + ``graph.nodes_geometry`` is the trimesh equivalent of USD's + ``stage.Traverse()`` — it yields one entry per instance, even when + multiple instances share the same underlying mesh (typical for chairs, + cabinets, etc.). Without this enumeration, ``trimesh.load(... force="mesh")`` + collapses the whole scene to one mesh and CoACD produces a single coarse + decomposition, which is essentially useless for collision against + multi-object scenes. + """ + import trimesh + + loaded: Any = trimesh.load(str(path)) + R = _world_rotation(alignment) + T = np.asarray(alignment.translation, dtype=np.float64) + s = float(alignment.scale) + + if isinstance(loaded, trimesh.Trimesh): + # Single-mesh GLB (no scene graph). Treat as one prim. + pts = np.asarray(loaded.vertices, dtype=np.float64) + faces = np.asarray(loaded.faces, dtype=np.int32) + if len(faces) == 0: + return [] + pts_world = (R @ (s * pts).T).T + T + return [ + ScenePrimMesh( + name="scene", + vertices=pts_world.astype(np.float32), + triangles=faces, + ) + ] + + scene = loaded + prims: list[ScenePrimMesh] = [] + for node_name in scene.graph.nodes_geometry: + xform, geom_name = scene.graph[node_name] + geom = scene.geometry.get(geom_name) + if geom is None or not isinstance(geom, trimesh.Trimesh): + continue + if len(geom.faces) == 0: + continue + + pts_local = np.asarray(geom.vertices, dtype=np.float64) + faces = np.asarray(geom.faces, dtype=np.int32) + + # Local → scene-root via the instance transform. + m = np.asarray(xform, dtype=np.float64) + pts_h = np.hstack([pts_local, np.ones((len(pts_local), 1), dtype=np.float64)]) + pts_stage = (m @ pts_h.T).T[:, :3] + + # Scene-root → dimos world via SceneMeshAlignment. + pts_world = (R @ (s * pts_stage).T).T + T + + clean = "".join(c if c.isalnum() else "_" for c in str(node_name)) + prims.append( + ScenePrimMesh( + name=f"{clean}__{len(prims)}", + vertices=pts_world.astype(np.float32), + triangles=faces, + ) + ) + return prims + + +def load_scene_prims( + path: str | Path, + alignment: SceneMeshAlignment | None = None, +) -> list[ScenePrimMesh]: + """Load a USD/USDZ scene as one ``ScenePrimMesh`` per placed Mesh prim. + + Per-prim splitting is what MuJoCo wants for non-trivial scenes: + each prim's convex hull approximates the prim well, while the + convex hull of the *whole* scene is its bounding box. Falls back + to a single ScenePrimMesh for non-USD inputs (a single ``.obj`` or + ``.glb`` doesn't carry per-part semantics in our loader). + + USD ``PointInstancer`` prims are expanded into their concrete + placements. Prototype meshes under the instancer's ``Prototypes`` + scope are skipped during ordinary traversal so they are not also + baked at their authoring origin. + + Same alignment rules as ``load_scene_mesh``. + """ + path = Path(path) + align = alignment or SceneMeshAlignment() + suffix = path.suffix.lower() + + if suffix in {".glb", ".gltf"}: + return _load_glb_prims(path, align) + + if suffix not in {".usdz", ".usd", ".usdc", ".usda"}: + # Non-USD, non-glTF (e.g. .obj/.ply/.stl): one part, whole mesh. + whole = load_scene_mesh(path, alignment=align) + return [ + ScenePrimMesh( + name="scene", + vertices=np.asarray(whole.vertices, dtype=np.float32), + triangles=np.asarray(whole.triangles, dtype=np.int32), + ) + ] + + try: + from pxr import Usd, UsdGeom # type: ignore[import-not-found, import-untyped] + except ImportError as e: + raise ImportError("loading .usdz/.usd requires usd-core: `uv pip install usd-core`") from e + + stage = Usd.Stage.Open(str(path)) + if stage is None: + raise RuntimeError(f"could not open USD stage: {path}") + + R = _world_rotation(align) + T = np.asarray(align.translation, dtype=np.float64) + s = float(align.scale) + + prototype_paths = _point_instancer_prototype_paths(stage, UsdGeom) + prims: list[ScenePrimMesh] = [] + for prim in stage.Traverse(): + if prim.IsA(UsdGeom.PointInstancer): + prims.extend( + _expand_point_instancer( + prim, + Usd=Usd, + UsdGeom=UsdGeom, + rotation=R, + scale=s, + translation=T, + start_index=len(prims), + ) + ) + continue + + if not prim.IsA(UsdGeom.Mesh): + continue + if _is_point_instancer_prototype_mesh(prim, prototype_paths): + continue + + triangulated = _triangulated_usd_mesh(UsdGeom.Mesh(prim)) + if triangulated is None: + continue + pts, tris = triangulated + + # Local → stage-root via the USD prim's accumulated transform. + xform = UsdGeom.Xformable(prim).ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + pts_stage = _transform_points(pts, _usd_matrix_to_numpy(xform)) + + # Stage-root → dimos world via SceneMeshAlignment (scale → rot → trans). + pts_world = _aligned_scene_points(pts_stage, rotation=R, scale=s, translation=T) + + # MJCF asset names: strip the leading slash, swap remaining + # path separators / dots for underscores. USD prim paths can + # collide on the same leaf; suffix the index so each is unique. + raw = str(prim.GetPath()).lstrip("/") + clean = _clean_scene_name(raw) + prims.append( + ScenePrimMesh( + name=f"{clean}__{len(prims)}", + vertices=pts_world.astype(np.float32), + triangles=tris, + ) + ) + + if not prims: + raise RuntimeError(f"no Mesh prims with triangles found in {path}") + return prims + + +__all__ = [ + "SceneMeshAlignment", + "ScenePrimMesh", + "load_scene_mesh", + "load_scene_prims", + "make_raycasting_scene", +] diff --git a/dimos/simulation/mujoco/scene_mesh_to_mjcf.py b/dimos/simulation/mujoco/scene_mesh_to_mjcf.py new file mode 100644 index 0000000000..540cbc43b4 --- /dev/null +++ b/dimos/simulation/mujoco/scene_mesh_to_mjcf.py @@ -0,0 +1,1042 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Bake a scene mesh into an MJCF wrapper around a robot MJCF. + +The bake walks every prim returned by :func:`load_scene_prims` and asks +:func:`dimos.simulation.mujoco.collision_spec.decide_for_prim` what to +emit for it. The dispatcher returns one of three modes: + +- ``"primitive"`` -- a single MuJoCo primitive ```` (box / sphere / + cylinder / capsule / plane). Used when the prim is approximately + prismatic (auto-fit) or when a sidecar override forces it. +- ``"hulls"`` -- one or more mesh ````s. Either a single convex + hull (small / near-convex prims) or a CoACD decomposition (genuine + concave shells: stairs, planters). +- ``"skip"`` -- no collision geom at all. Used for sidecar-tagged + decoration (lamps, signs) and prims smaller than a threshold. + +Hulls produced by either path are validated with :func:`_valid_hull` +(matrix-rank coplanarity check + scipy ``Qt`` qhull pre-flight); when a +hull is invalid we fall back to a thin OBB box via +:func:`_fallback_box_geom` rather than dropping the geometry, so the +robot doesn't sink through holes. + +When ``include_visual_mesh=True`` the bake additionally writes the +prim's original triangles as a non-colliding visual geom (group 2, +``contype=0 conaffinity=0``). UE's USD exporter culls hidden faces on +static meshes (a floor slab ships with only top + bottom face pairs, +no sides) -- we route visual writes through :func:`_write_visual_obj`, +which substitutes the prim's convex hull when it isn't watertight, so +the viewer renders solid geometry instead of see-through slabs. + +Per-prim work is fanned across worker processes since each prim's +decision is independent and CoACD calls dominate wall time. Standalone +CLI bakes use forked processes; in an already-threaded DimOS runtime we +use ``forkserver`` so workers do not inherit the parent process's active +threads. + +Output is cached at ``~/.cache/dimos/scene_meshes//`` keyed on +the SHA256 of (source mesh, robot MJCF, alignment, meshdir, sidecar +spec, visual flag, schema version). :func:`load_or_bake` is the +recommended entry point -- it handles a three-tier cache: + + 1. ``compiled.mjb`` exists -> load directly (~1 s) + 2. ``wrapper.xml`` + OBJs exist -> compile XML, save ``.mjb`` + 3. Nothing exists -> full bake, then compile + save ``.mjb`` +""" + +from __future__ import annotations + +import argparse +from collections.abc import Callable +from concurrent.futures import ProcessPoolExecutor, as_completed +from dataclasses import asdict, dataclass, replace +import hashlib +import multiprocessing +import os +from pathlib import Path +import time +from typing import Any, cast + +import numpy as np +import open3d as o3d # type: ignore[import-untyped] + +from dimos.simulation.mujoco.collision_spec import ( + CollisionSpec, + decide_for_prim, +) +from dimos.simulation.mujoco.mesh_scene import ( + SceneMeshAlignment, + ScenePrimMesh, + load_scene_prims, +) +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +CACHE_DIR = Path.home() / ".cache" / "dimos" / "scene_meshes" +_MUJOCO_FROM_BINARY_PATH = "from_binary_path" +_MUJOCO_SAVE_MODEL = "mj_saveModel" + + +# ```` comes BEFORE ```` so the wrapper's absolute +# meshdir is the *last* compiler block in the merged spec -- MuJoCo's +# "last compiler wins" rule then routes the robot MJCF's +# ```` entries to ``/X.STL``. If +# the order is reversed the robot's own ```` +# overrides the absolute path with a relative one that resolves against +# the wrapper's cache directory and the include's meshes can't be found. +# +# The dummy ```` on dimos_scene bypasses MuJoCo's auto- +# computation of body inertia from geom volumes -- the body is static +# (no joint) so the values don't affect physics, but without this any +# zero-volume visual mesh (road tiles, ceiling panels, flat slabs) +# triggers ``Error: mesh volume is too small`` at compile time. +_WRAPPER_TEMPLATE = """\ + + + + + + + + +{asset_meshes} + + + + +{scene_geoms} + + + +""" + +# ``inertia="shell"`` makes MuJoCo compute mesh inertia from surface +# area instead of enclosed volume -- robust to non-watertight visual +# meshes from art tools. Safe for closed CoACD hulls too, so we apply +# it universally for one fewer code path. +_ASSET_LINE = ' ' + +# Collision (group 3) -- actually collides. rgba alpha < 1 lets the +# user toggle visibility independently of the visual mesh. +_COL_MESH_LINE = ( + ' ' +) +_COL_BOX_LINE = ( + ' ' +) +_COL_SPHERE_LINE = ( + ' ' +) +_COL_CYL_LINE = ( + ' ' +) +_COL_CAP_LINE = ( + ' ' +) +_COL_PLANE_LINE = ( + ' ' +) + +# Visual (group 2) -- drawn, doesn't collide. +_VISUAL_GEOM_LINE = ( + ' ' +) + + +# Constants kept from the prior implementation -- conservative +# fallback thresholds for hull validity / box-fallback geometry. +_DEGENERATE_EPS = 1e-3 +_MIN_HULL_EXTENT_M = 5e-3 +_FALLBACK_BOX_THICKNESS_M = 0.03 +_MIN_FALLBACK_BOX_EXTENT_M = 0.25 +_MIN_FALLBACK_BOX_AREA_M2 = 0.05 + +_CACHE_KEY_LEN = 12 +# Bump when the bake pipeline's output format changes so old caches +# invalidate on the next call. Increment for any change that could +# affect MJCF emission (new geom kinds, rewritten visual policy, etc.). +# This is only a local cache salt; it is not a persisted file format +# contract and old cache directories can safely stay on disk. +_CACHE_SCHEMA_VERSION = "dispatcher-v8-point-instancer" + + +@dataclass +class _BakeArtifacts: + """Aggregated stats + emission lines from one bake.""" + + asset_lines: list[str] + geom_lines: list[str] + n_primitive: int + n_hulls_total: int + n_box_fallbacks: int + n_skipped: int + n_visuals: int + n_degenerate_dropped: int + decision_reasons: dict[str, int] + + +def _resolve_existing_file(path: str | Path, label: str) -> Path: + resolved = Path(path).expanduser().resolve() + if not resolved.exists(): + raise FileNotFoundError(f"{label} not found: {resolved}") + if not resolved.is_file(): + raise ValueError(f"{label} must be a file, got: {resolved}") + return resolved + + +def bake_scene_mjcf( + scene_mesh_path: str | Path, + robot_mjcf_path: str | Path, + alignment: SceneMeshAlignment | None = None, + meshdir: str | Path | None = None, + cache_root: Path | None = None, + collision_spec: CollisionSpec | None = None, + include_visual_mesh: bool = False, + rebake: bool = False, +) -> Path: + """Convert ``scene_mesh_path`` to OBJs + MJCF wrapper around the robot. + + Args: + scene_mesh_path: ``.usdz`` / ``.usda`` / ``.glb`` / ``.obj`` / + etc. Anything ``mesh_scene.load_scene_prims`` accepts. + robot_mjcf_path: the base robot MJCF the wrapper will + ````. + alignment: scale / translation / rotation / y-up swap to bake + into world frame before any geom is emitted. + meshdir: directory MuJoCo resolves the *robot's* unqualified + mesh filenames against. Defaults to ``robot_mjcf_path.parent``. + Override when the robot ships its assets in a sibling + ``assets/`` folder (typical for menagerie robots). + cache_root: override the cache root (defaults to + ``~/.cache/dimos/scene_meshes``). + collision_spec: per-prim policy. ``None`` auto-discovers a + sidecar ``.collision.json`` next to the source, or + falls back to ``CollisionSpec()`` defaults (auto-fit + primitives, CoACD on large concave shells). + include_visual_mesh: also emit a non-colliding visual geom for + every prim showing its original triangles. The viewer + renders these instead of the collision hulls -- much nicer + for visual debugging, but doubles disk usage. When ``True`` + non-watertight prim meshes are substituted with their convex + hull so they don't appear see-through. + rebake: ignore an existing ``wrapper.xml`` in the computed cache + directory and regenerate the scene collision geometry. + + Returns: + Path to the wrapper MJCF. Pass to ``MujocoSimModule`` instead of + the raw robot MJCF, or use :func:`load_or_bake` to also get an + ``.mjb`` cache. + """ + scene_mesh_path = _resolve_existing_file(scene_mesh_path, "scene mesh") + robot_mjcf_path = _resolve_existing_file(robot_mjcf_path, "robot MJCF") + align = alignment or SceneMeshAlignment() + spec = collision_spec or CollisionSpec.auto_discover(scene_mesh_path) + + meshdir = Path(meshdir).expanduser().resolve() if meshdir else robot_mjcf_path.parent + + cache_key = _cache_key( + scene_mesh_path, + robot_mjcf_path, + align, + meshdir, + spec=spec, + include_visual_mesh=include_visual_mesh, + ) + root = (cache_root or CACHE_DIR).expanduser() + cache_dir = root / cache_key + wrapper_path = cache_dir / "wrapper.xml" + + if not rebake and _cache_hit(wrapper_path): + logger.info(f"bake_scene_mjcf: cache hit at {cache_dir}") + return wrapper_path + + cache_dir.mkdir(parents=True, exist_ok=True) + + logger.info(f"bake_scene_mjcf: loading + aligning {scene_mesh_path}") + prims = load_scene_prims(scene_mesh_path, alignment=align) + logger.info(f"bake_scene_mjcf: {len(prims)} prims to process") + if spec.enable_sheet_prisms and len(prims) > spec.sheet_prism_max_scene_prims: + logger.info( + "bake_scene_mjcf: disabling thin-sheet triangle prisms for " + f"{len(prims)}-prim scene; use a collision sidecar to opt in" + ) + spec = replace(spec, enable_sheet_prisms=False) + scene_center, scene_extent = _scene_bounds(prims) + + artifacts = _bake_prims( + prims, + cache_dir, + spec=spec, + include_visual_mesh=include_visual_mesh, + ) + if not artifacts.geom_lines: + raise RuntimeError( + "bake_scene_mjcf: every prim got skipped or produced only " + "degenerate hulls; nothing left to collide against. Check " + "the source mesh and alignment." + ) + + logger.info( + f"bake_scene_mjcf: {artifacts.n_primitive} primitive geoms, " + f"{artifacts.n_hulls_total} hull geoms, " + f"{artifacts.n_box_fallbacks} box fallbacks, " + f"{artifacts.n_visuals} visual passthrough meshes, " + f"{artifacts.n_skipped} skipped, " + f"{artifacts.n_degenerate_dropped} degenerate hulls dropped" + ) + # Top-10 decision reasons -- useful when tuning a sidecar. + for reason, n in sorted(artifacts.decision_reasons.items(), key=lambda kv: -kv[1])[:10]: + logger.info(f" reason {reason:32s} {n}") + + _write_wrapper( + wrapper_path=wrapper_path, + cache_key=cache_key, + meshdir=meshdir, + robot_mjcf_path=robot_mjcf_path, + asset_lines=artifacts.asset_lines, + geom_lines=artifacts.geom_lines, + statistic_center=scene_center, + statistic_extent=scene_extent, + ) + return wrapper_path + + +def load_or_bake( + scene_mesh_path: str | Path, + robot_mjcf_path: str | Path, + alignment: SceneMeshAlignment | None = None, + meshdir: str | Path | None = None, + cache_root: Path | None = None, + collision_spec: CollisionSpec | None = None, + include_visual_mesh: bool = False, + rebake: bool = False, +) -> tuple[Any, Path]: + """Three-tier cache: ``.mjb`` -> ``wrapper.xml`` -> full bake. + + Returns ``(MjModel, wrapper_path)``. + + 1. If ``compiled.mjb`` exists in the bake's cache dir -> load it + directly (~1 s for a 5k-prim mall). + 2. Else if ``wrapper.xml`` exists -> compile XML, save the ``.mjb`` + beside it, return the model. + 3. Else -> run the full bake, compile the resulting wrapper, save + ``.mjb``, return the model. + + Set ``rebake=True`` to force step 3 even when caches exist. + + The cache key incorporates source-mesh signature, robot MJCF, + alignment, sidecar spec and the schema version -- change any of + those and a fresh cache directory is created automatically (the + old one stays on disk until you clean it). + """ + import mujoco # type: ignore[import-untyped] + + scene_mesh_path = _resolve_existing_file(scene_mesh_path, "scene mesh") + robot_mjcf_path = _resolve_existing_file(robot_mjcf_path, "robot MJCF") + load_binary_model = cast( + "Callable[[str], Any]", + getattr(mujoco.MjModel, _MUJOCO_FROM_BINARY_PATH), + ) + save_model = cast( + "Callable[[Any, str], None]", + getattr(mujoco, _MUJOCO_SAVE_MODEL), + ) + + if not rebake: + # Compute the cache key without baking, so we can probe for an + # existing .mjb / wrapper.xml without doing any work. + sp = scene_mesh_path + rp = robot_mjcf_path + al = alignment or SceneMeshAlignment() + sd = collision_spec or CollisionSpec.auto_discover(sp) + md = Path(meshdir).expanduser().resolve() if meshdir else rp.parent + key = _cache_key(sp, rp, al, md, spec=sd, include_visual_mesh=include_visual_mesh) + root = (cache_root or CACHE_DIR).expanduser() + cache_dir = root / key + mjb = cache_dir / "compiled.mjb" + wrapper = cache_dir / "wrapper.xml" + + wrapper_is_current = _cache_hit(wrapper) + + if ( + mjb.exists() + and wrapper_is_current + and mjb.stat().st_mtime_ns >= wrapper.stat().st_mtime_ns + ): + logger.info(f"load_or_bake: loading compiled binary {mjb}") + t0 = time.time() + model = load_binary_model(str(mjb)) + logger.info( + f" loaded in {time.time() - t0:.1f}s " + f"(nbody={model.nbody} ngeom={model.ngeom} nmesh={model.nmesh})" + ) + return model, wrapper + + if wrapper_is_current and any(cache_dir.glob("*.obj")): + logger.info(f"load_or_bake: wrapper cached, compiling XML -> .mjb at {wrapper}") + t0 = time.time() + model = mujoco.MjModel.from_xml_path(str(wrapper)) + logger.info( + f" compiled in {time.time() - t0:.1f}s " + f"(nbody={model.nbody} ngeom={model.ngeom} nmesh={model.nmesh})" + ) + save_model(model, str(mjb)) + logger.info(f" saved compiled binary: {mjb} ({mjb.stat().st_size / 1e6:.1f} MB)") + return model, wrapper + + # Full bake path. + wrapper = bake_scene_mjcf( + scene_mesh_path=scene_mesh_path, + robot_mjcf_path=robot_mjcf_path, + alignment=alignment, + meshdir=meshdir, + cache_root=cache_root, + collision_spec=collision_spec, + include_visual_mesh=include_visual_mesh, + rebake=rebake, + ) + logger.info(f"load_or_bake: compiling baked wrapper {wrapper}") + t0 = time.time() + model = mujoco.MjModel.from_xml_path(str(wrapper)) + logger.info( + f" compiled in {time.time() - t0:.1f}s " + f"(nbody={model.nbody} ngeom={model.ngeom} nmesh={model.nmesh})" + ) + mjb = wrapper.with_name("compiled.mjb") + save_model(model, str(mjb)) + logger.info(f" saved compiled binary: {mjb} ({mjb.stat().st_size / 1e6:.1f} MB)") + return model, wrapper + + +# --------------------------------------------------------------------------- # +# Cache key # +# --------------------------------------------------------------------------- # + + +def _cache_key( + scene_mesh_path: Path, + robot_mjcf_path: Path, + alignment: SceneMeshAlignment, + meshdir: Path, + *, + spec: CollisionSpec, + include_visual_mesh: bool, +) -> str: + """SHA256-12 over every input that affects bake output. + + Source-mesh signature is ``(path, size, mtime_ns)`` -- much faster + than reading the whole file (a mall scene's USDA + Assets is a few + hundred MB) and reliably invalidates when the artist re-exports. + Sidecar spec is hashed by its JSON encoding so any field change + (new override pattern, tuned threshold) invalidates correctly. + """ + import json + + def _file_signature(path: Path) -> str: + st = path.stat() + return f"{path}:{st.st_size}:{st.st_mtime_ns}" + + h = hashlib.sha256() + h.update(_CACHE_SCHEMA_VERSION.encode()) + h.update(_file_signature(scene_mesh_path).encode()) + h.update(_file_signature(robot_mjcf_path).encode()) + h.update(repr(sorted(asdict(alignment).items())).encode()) + h.update(str(meshdir).encode()) + h.update(json.dumps(asdict(spec), sort_keys=True).encode()) + h.update(b"visual=" + (b"1" if include_visual_mesh else b"0")) + return h.hexdigest()[:_CACHE_KEY_LEN] + + +def _cache_hit(wrapper_path: Path) -> bool: + if not wrapper_path.exists(): + return False + try: + text = wrapper_path.read_text() + except OSError: + return False + return " _BakeArtifacts: + """Fan per-prim work across cores; aggregate the resulting MJCF lines. + + Standalone bakes use ``fork`` so workers inherit the parent's + already-imported modules. Runtime bakes inside DimOS may happen + after other modules have started threads; in that case use + ``forkserver`` so workers do not inherit locks from the parent + process's C extension state. + """ + asset_lines: list[str] = [] + geom_lines: list[str] = [] + n_primitive = 0 + n_hulls_total = 0 + n_box_fallbacks = 0 + n_skipped = 0 + n_visuals = 0 + n_degenerate = 0 + reasons: dict[str, int] = {} + + work_items = [(prim, cache_dir, spec, include_visual_mesh) for prim in prims] + n_workers = max(1, (os.cpu_count() or 4) - 1) + if _native_thread_count() > 1: + n_workers = min(n_workers, 8) + start_method = ( + "forkserver" if "forkserver" in multiprocessing.get_all_start_methods() else "spawn" + ) + else: + start_method = "fork" + logger.info( + f"_bake_prims: fanning {len(prims)} prims across {n_workers} workers ({start_method})" + ) + + t0 = time.time() + mp_ctx = multiprocessing.get_context(start_method) + executor = ProcessPoolExecutor(max_workers=n_workers, mp_context=mp_ctx) + + progress_every = 25 if len(prims) <= 500 else 250 + with executor as ex: + futures = [ex.submit(_process_one_prim, item) for item in work_items] + done = 0 + for fut in as_completed(futures): + a_lines, g_lines, mode, reason, counters = fut.result() + asset_lines.extend(a_lines) + geom_lines.extend(g_lines) + reasons[reason] = reasons.get(reason, 0) + 1 + if mode == "primitive": + n_primitive += 1 + elif mode == "skip": + n_skipped += 1 + n_hulls_total += counters["hulls"] + n_box_fallbacks += counters["box_fallbacks"] + n_visuals += counters["visuals"] + n_degenerate += counters["degenerate"] + done += 1 + if done % progress_every == 0 or done == len(prims): + elapsed = time.time() - t0 + eta = elapsed * (len(prims) - done) / max(done, 1) + logger.info( + f" prim {done}/{len(prims)} " + f"({100 * done / len(prims):.0f}%) " + f"elapsed={elapsed:.0f}s eta={eta:.0f}s " + f"hulls_so_far={n_hulls_total}" + ) + + return _BakeArtifacts( + asset_lines=asset_lines, + geom_lines=geom_lines, + n_primitive=n_primitive, + n_hulls_total=n_hulls_total, + n_box_fallbacks=n_box_fallbacks, + n_skipped=n_skipped, + n_visuals=n_visuals, + n_degenerate_dropped=n_degenerate, + decision_reasons=reasons, + ) + + +def _native_thread_count() -> int: + try: + return len(os.listdir("/proc/self/task")) + except OSError: + return 1 + + +# --------------------------------------------------------------------------- # +# Geom emission helpers # +# --------------------------------------------------------------------------- # + + +def _emit_primitive_geom( + prim_name: str, + fit: dict[str, Any] | None, + friction_attr: str, +) -> str | None: + """Render one ``PrimDecision.primitive`` dict to MJCF text. + + Returns ``None`` if ``fit`` is missing required fields (defensive -- + ``decide_for_prim`` should always populate them, but a malformed + sidecar override could slip through). + """ + if fit is None: + return None + kind = fit.get("type") + pos = _fmt_vec(np.asarray(fit["pos"])) + size = _fmt_vec(np.asarray(fit["size"])) + quat = ( + _fmt_vec(np.asarray(fit["quat"])) + if "quat" in fit and fit["quat"] is not None + else "1 0 0 0" + ) + name = f"{prim_name}_col" + if kind == "box": + return _COL_BOX_LINE.format( + name=name, pos=pos, quat=quat, size=size, friction=friction_attr + ) + if kind == "sphere": + return _COL_SPHERE_LINE.format(name=name, pos=pos, size=size, friction=friction_attr) + if kind == "cylinder": + return _COL_CYL_LINE.format( + name=name, pos=pos, quat=quat, size=size, friction=friction_attr + ) + if kind == "capsule": + return _COL_CAP_LINE.format( + name=name, pos=pos, quat=quat, size=size, friction=friction_attr + ) + if kind == "plane": + return _COL_PLANE_LINE.format( + name=name, pos=pos, quat=quat, size=size, friction=friction_attr + ) + return None + + +# --------------------------------------------------------------------------- # +# Hull validity & box fallback (preserved from prior implementation) # +# --------------------------------------------------------------------------- # + + +def _valid_hull(v: np.ndarray, f: np.ndarray) -> bool: + """Reject hulls that MuJoCo's qhull would choke on at compile time. + + Four layers: + 1. trivial -- < 4 vertices or < 4 faces. + 2. extent -- all-axis ``> 5 mm`` (matches MuJoCo's mj_loadXML + coplanarity tolerance for ~100mm-wide hulls). + 3. rank -- centred vertex matrix must have rank 3 (catches + coplanar hulls the extent check misses, e.g. a T-shaped + hull whose XY extent is large but Z is zero). + 4. scipy ConvexHull pre-flight with ``Qt`` -- same options + MuJoCo uses; if scipy can't build it, mj_loadXML can't either. + """ + if len(v) < 4 or len(f) < 4: + return False + extent = v.max(axis=0) - v.min(axis=0) + if (extent < _DEGENERATE_EPS).any(): + return False + if float(extent.min()) < _MIN_HULL_EXTENT_M: + return False + centered = v.astype(np.float64) - v.astype(np.float64).mean(axis=0) + if np.linalg.matrix_rank(centered, tol=_DEGENERATE_EPS) < 3: + return False + try: + from scipy.spatial import ConvexHull, QhullError # type: ignore[import-untyped] + + ConvexHull(v, qhull_options="Qt") + except (QhullError, ValueError): + return False + return True + + +def _fallback_box_geom(name: str, vertices: np.ndarray, friction_attr: str = "") -> str | None: + """Emit a thin OBB box geom for vertices that can't form a valid hull. + + The thickness floor (``_FALLBACK_BOX_THICKNESS_M = 3 cm``) keeps the + box thick enough that the robot can stand on it without falling + through. Returns ``None`` for prims too small to bother (< 25 cm + largest extent or < 0.05 m^2 face area) -- those fall through to + the degenerate counter. + """ + finite = vertices[np.isfinite(vertices).all(axis=1)].astype(np.float64) + if len(finite) < 3: + return None + aabb_extent = finite.max(axis=0) - finite.min(axis=0) + sorted_extents = np.sort(aabb_extent) + if sorted_extents[-1] < _MIN_FALLBACK_BOX_EXTENT_M: + return None + if sorted_extents[-1] * sorted_extents[-2] < _MIN_FALLBACK_BOX_AREA_M2: + return None + + center, rotation, extent = _oriented_box(finite) + extent = np.maximum(extent, _FALLBACK_BOX_THICKNESS_M) + half_size = 0.5 * extent + quat = _rotation_matrix_to_wxyz(rotation) + return _COL_BOX_LINE.format( + name=name, + pos=_fmt_vec(center), + quat=_fmt_vec(quat), + size=_fmt_vec(half_size), + friction=friction_attr, + ) + + +def _oriented_box( + vertices: np.ndarray, +) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + """OBB via trimesh's ``bounding_box_oriented``. + + Falls back to AABB if trimesh's OBB fitter produces non-finite + output or the prim has < 3 vertices. + """ + try: + import trimesh # type: ignore[import-untyped] + + tm = trimesh.Trimesh(vertices=vertices, faces=np.empty((0, 3), dtype=np.int32)) + obb = tm.bounding_box_oriented + transform = np.asarray(obb.primitive.transform, dtype=np.float64) + extent = np.asarray(obb.primitive.extents, dtype=np.float64) + rotation = transform[:3, :3] + center = transform[:3, 3] + if np.linalg.det(rotation) < 0: + rotation[:, 0] *= -1.0 + if np.isfinite(center).all() and np.isfinite(rotation).all() and np.isfinite(extent).all(): + return center, rotation, np.abs(extent) + except Exception: + pass + + lo = vertices.min(axis=0) + hi = vertices.max(axis=0) + return (lo + hi) * 0.5, np.eye(3), hi - lo + + +def _rotation_matrix_to_wxyz(rotation: np.ndarray) -> np.ndarray: + """3x3 rotation -> ``(w, x, y, z)`` quaternion.""" + from scipy.spatial.transform import Rotation # type: ignore[import-untyped] + + xyzw = Rotation.from_matrix(rotation).as_quat() + return np.array([xyzw[3], xyzw[0], xyzw[1], xyzw[2]], dtype=np.float64) + + +def _fmt_vec(values: np.ndarray) -> str: + return " ".join(f"{float(v):.9g}" for v in values) + + +def _scene_bounds(prims: list[ScenePrimMesh]) -> tuple[np.ndarray, float]: + """Return a viewer-friendly center and extent for the aligned scene. + + MuJoCo's viewer uses ``statistic.center`` / ``statistic.extent`` for + camera framing and clipping. The included robot MJCF's defaults are + much too small for baked building-scale scenes, so wrappers need to + advertise the scene bounds explicitly. + """ + mins: list[np.ndarray] = [] + maxs: list[np.ndarray] = [] + for prim in prims: + vertices = np.asarray(prim.vertices, dtype=np.float64) + if vertices.ndim != 2 or vertices.shape[1] != 3 or len(vertices) == 0: + continue + finite = vertices[np.isfinite(vertices).all(axis=1)] + if len(finite) == 0: + continue + mins.append(finite.min(axis=0)) + maxs.append(finite.max(axis=0)) + + if not mins: + return np.zeros(3, dtype=np.float64), 1.0 + + scene_min = np.min(np.vstack(mins), axis=0) + scene_max = np.max(np.vstack(maxs), axis=0) + center = (scene_min + scene_max) * 0.5 + diagonal = scene_max - scene_min + extent = max(float(np.linalg.norm(diagonal) * 0.5 * 1.1), 1.0) + return center, extent + + +# --------------------------------------------------------------------------- # +# OBJ I/O # +# --------------------------------------------------------------------------- # + + +def _write_hull_obj(obj_file: Path, vertices: np.ndarray, faces: np.ndarray) -> None: + """Write a CoACD/single-hull mesh. No watertight check -- hulls are + closed by construction.""" + _write_mesh_obj(obj_file, vertices, faces) + + +def _write_visual_obj(obj_file: Path, vertices: np.ndarray, faces: np.ndarray) -> None: + """Write a *renderable* OBJ -- closed under all viewing angles. + + UE's static-mesh exporter culls hidden faces (a floor slab ships + with only top + bottom face pairs, no sides), so writing the + artist's geometry verbatim produces meshes that appear see-through + in MuJoCo's viewer from any oblique angle. We check + ``trimesh.is_watertight`` and, if not, substitute the prim's + convex hull (which is always closed). + + For non-prismatic prims (chairs, plants) the hull is a coarse + visual approximation; for the most common offenders (floor / roof + / wall / ceiling slabs that are box-shaped to begin with) the hull + matches the original exactly. Watertight prims (full furniture + meshes from UE) keep their original geometry. + """ + import trimesh # type: ignore[import-untyped] + + tm = trimesh.Trimesh( + vertices=np.asarray(vertices, dtype=np.float64), + faces=np.asarray(faces, dtype=np.int32), + process=False, + ) + if not tm.is_watertight: + try: + hull = tm.convex_hull + if len(hull.vertices) >= 4 and len(hull.faces) >= 4: + vertices = np.asarray(hull.vertices, dtype=np.float64) + faces = np.asarray(hull.faces, dtype=np.int32) + except Exception: + pass # fall back to original; visual may look hollow + _write_mesh_obj(obj_file, vertices, faces) + + +def _write_mesh_obj(obj_file: Path, vertices: np.ndarray, faces: np.ndarray) -> None: + o3d_mesh = o3d.geometry.TriangleMesh() + o3d_mesh.vertices = o3d.utility.Vector3dVector(vertices.astype(np.float64)) + o3d_mesh.triangles = o3d.utility.Vector3iVector(faces) + o3d_mesh.compute_vertex_normals() + if not o3d.io.write_triangle_mesh( + str(obj_file), + o3d_mesh, + write_vertex_normals=True, + write_vertex_colors=False, + ): + raise RuntimeError(f"open3d failed to write OBJ: {obj_file}") + + +# --------------------------------------------------------------------------- # +# Wrapper writer + CLI # +# --------------------------------------------------------------------------- # + + +def _write_wrapper( + *, + wrapper_path: Path, + cache_key: str, + meshdir: Path, + robot_mjcf_path: Path, + asset_lines: list[str], + geom_lines: list[str], + statistic_center: np.ndarray, + statistic_extent: float, +) -> None: + visual_zfar = max(float(statistic_extent) * 20.0, 10000.0) + wrapper_xml = _WRAPPER_TEMPLATE.format( + model_name=f"robot_with_scene_{cache_key}", + meshdir=str(meshdir), + robot_mjcf_abs=str(robot_mjcf_path), + statistic_center=_fmt_vec(statistic_center), + statistic_extent=f"{float(statistic_extent):.9g}", + visual_zfar=f"{visual_zfar:.9g}", + asset_meshes="\n".join(asset_lines), + scene_geoms="\n".join(geom_lines), + ) + wrapper_path.write_text(wrapper_xml) + logger.info(f"_write_wrapper: wrote {wrapper_path}") + + +def cli_main() -> None: + """``python -m dimos.simulation.mujoco.scene_mesh_to_mjcf [opts]``. + + Bake (or load from cache), optionally launch the MuJoCo viewer. + """ + p = argparse.ArgumentParser( + prog="python -m dimos.simulation.mujoco.scene_mesh_to_mjcf", + description="Bake a USD/GLB/OBJ scene into an MJCF wrapping a robot MJCF.", + ) + p.add_argument("scene", type=Path, help="scene mesh path (.usda, .usdz, .glb, ...)") + p.add_argument("robot", type=Path, help="robot MJCF path") + p.add_argument( + "--scale", + type=float, + default=1.0, + help="multiplicative scale (use 0.01 for UE / centimeter sources). Default 1.0.", + ) + p.add_argument( + "--no-y-up", + action="store_true", + help="source is already Z-up (UE exports with metersPerUnit=0.01 and " + "upAxis=Z). Default assumes Y-up source (Blender, glTF, Apple USDZ).", + ) + p.add_argument( + "--collision-spec", + type=Path, + default=None, + help="path to a collision-spec sidecar JSON. Default auto-discovers " + "``.collision.json`` next to the source.", + ) + p.add_argument( + "--meshdir", + type=Path, + default=None, + help="override meshdir for the robot's relative " + "lookups. Default: robot MJCF's parent directory.", + ) + p.add_argument( + "--visual", + action="store_true", + help="emit visual passthrough meshes (group 2). Off by default -- " + "saves disk and render cost, but the MuJoCo viewer only shows " + "collision shapes without it.", + ) + p.add_argument( + "--rebake", + action="store_true", + help="ignore cached .mjb and wrapper.xml; do a full re-bake.", + ) + p.add_argument( + "--view", + action="store_true", + help="launch the MuJoCo native viewer after baking (blocks).", + ) + args = p.parse_args() + + try: + scene_path = _resolve_existing_file(args.scene, "scene mesh") + robot_path = _resolve_existing_file(args.robot, "robot MJCF") + except (FileNotFoundError, ValueError) as exc: + p.error(str(exc)) + + align = SceneMeshAlignment(scale=args.scale, y_up=not args.no_y_up) + spec = ( + CollisionSpec.from_json(args.collision_spec) + if args.collision_spec is not None + else CollisionSpec.auto_discover(scene_path) + ) + + model, wrapper = load_or_bake( + scene_mesh_path=scene_path, + robot_mjcf_path=robot_path, + alignment=align, + meshdir=args.meshdir, + collision_spec=spec, + include_visual_mesh=args.visual, + rebake=args.rebake, + ) + print(f"wrapper: {wrapper}") + print(f"loaded: {model.nbody} bodies, {model.ngeom} geoms, {model.nmesh} meshes") + print(f"joints: {model.njnt}, dof: {model.nv}") + + if args.view: + import mujoco.viewer # type: ignore[import-untyped] + + viewer: Any = mujoco.viewer + # ``launch`` runs MuJoCo's interactive viewer with its own + # internal physics loop. Blocks until the user closes it. + # Press F1 in the viewer for the keyboard cheatsheet; ``Tab`` + # toggles the rendering panel where you can switch geom groups + # (group 3 = scene collision, group 2 = visual passthrough, + # group 1 = robot visual, group 0 = robot collision). + print("\nlaunching MuJoCo viewer (press Esc / close window to exit)") + viewer.launch(model) + + +if __name__ == "__main__": + cli_main() + + +__all__ = ["bake_scene_mjcf", "load_or_bake"] diff --git a/dimos/simulation/mujoco/test_collision_spec.py b/dimos/simulation/mujoco/test_collision_spec.py new file mode 100644 index 0000000000..0bf46478c0 --- /dev/null +++ b/dimos/simulation/mujoco/test_collision_spec.py @@ -0,0 +1,130 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import numpy as np + +from dimos.simulation.mujoco.collision_spec import CollisionSpec, decide_for_prim + + +def test_rectangular_flat_sheet_stays_single_horizontal_box() -> None: + vertices = np.asarray( + [ + [0.0, 0.0, 0.0], + [2.0, 0.0, 0.0], + [2.0, 2.0, 0.0], + [0.0, 2.0, 0.0], + ], + dtype=np.float64, + ) + triangles = np.asarray([[0, 1, 2], [0, 2, 3]], dtype=np.int32) + + decision = decide_for_prim(vertices, triangles, "flat_rect", CollisionSpec()) + + assert decision.mode == "primitive" + assert decision.reason == "aspect-ratio:horizontal-slab" + assert decision.primitive is not None + assert decision.primitive["quat"] == (1.0, 0.0, 0.0, 0.0) + + +def test_large_non_rectangular_flat_sheet_uses_triangle_prisms() -> None: + vertices = np.asarray( + [ + [0.0, 0.0, 0.0], + [2.0, 0.0, 0.0], + [2.0, 1.0, 0.0], + [0.0, 1.0, 0.0], + [1.0, 1.0, 0.0], + [1.0, 2.0, 0.0], + [0.0, 2.0, 0.0], + ], + dtype=np.float64, + ) + triangles = np.asarray( + [ + [0, 1, 2], + [0, 2, 3], + [3, 4, 5], + [3, 5, 6], + ], + dtype=np.int32, + ) + + decision = decide_for_prim(vertices, triangles, "flat_l_shape", CollisionSpec()) + + assert decision.mode == "hulls" + assert decision.reason == "thin-sheet:triangle-prisms(4)" + assert len(decision.hulls) == 4 + + +def test_sheet_prisms_can_be_disabled() -> None: + vertices = np.asarray( + [ + [0.0, 0.0, 0.0], + [2.0, 0.0, 0.0], + [2.0, 1.0, 0.0], + [0.0, 1.0, 0.0], + [1.0, 1.0, 0.0], + [1.0, 2.0, 0.0], + [0.0, 2.0, 0.0], + ], + dtype=np.float64, + ) + triangles = np.asarray( + [ + [0, 1, 2], + [0, 2, 3], + [3, 4, 5], + [3, 5, 6], + ], + dtype=np.int32, + ) + + decision = decide_for_prim( + vertices, + triangles, + "flat_l_shape", + CollisionSpec(enable_sheet_prisms=False), + ) + + assert decision.mode == "primitive" + assert decision.reason == "aspect-ratio:horizontal-slab" + + +def test_small_non_rectangular_flat_sheet_stays_single_horizontal_box() -> None: + vertices = np.asarray( + [ + [0.0, 0.0, 0.0], + [1.0, 0.0, 0.0], + [1.0, 0.5, 0.0], + [0.0, 0.5, 0.0], + [0.5, 0.5, 0.0], + [0.5, 1.0, 0.0], + [0.0, 1.0, 0.0], + ], + dtype=np.float64, + ) + triangles = np.asarray( + [ + [0, 1, 2], + [0, 2, 3], + [3, 4, 5], + [3, 5, 6], + ], + dtype=np.int32, + ) + + decision = decide_for_prim(vertices, triangles, "small_flat_l_shape", CollisionSpec()) + + assert decision.mode == "primitive" + assert decision.reason == "aspect-ratio:horizontal-slab" diff --git a/dimos/simulation/mujoco/test_mesh_scene.py b/dimos/simulation/mujoco/test_mesh_scene.py new file mode 100644 index 0000000000..99fb9afcad --- /dev/null +++ b/dimos/simulation/mujoco/test_mesh_scene.py @@ -0,0 +1,63 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import numpy as np +import pytest + +from dimos.simulation.mujoco.mesh_scene import SceneMeshAlignment, load_scene_prims + +pytest.importorskip("pxr.Usd") +from pxr import Gf, Usd, UsdGeom # type: ignore[import-not-found, import-untyped] + + +def test_load_scene_prims_expands_usd_point_instancers(tmp_path): + scene_path = tmp_path / "instanced.usda" + stage = Usd.Stage.CreateNew(str(scene_path)) + + root = UsdGeom.Xform.Define(stage, "/Root") + root.AddTranslateOp().Set(Gf.Vec3d(10, 0, 0)) + + instancer = UsdGeom.PointInstancer.Define(stage, "/Root/Instancer") + instancer.CreatePositionsAttr([Gf.Vec3f(1, 2, 3), Gf.Vec3f(4, 5, 6)]) + instancer.CreateOrientationsAttr( + [ + Gf.Quath(1, Gf.Vec3h(0, 0, 0)), + Gf.Quath(1, Gf.Vec3h(0, 0, 0)), + ] + ) + instancer.CreateScalesAttr([Gf.Vec3f(1, 1, 1), Gf.Vec3f(2, 1, 1)]) + instancer.CreateProtoIndicesAttr([0, 0]) + + proto = UsdGeom.Xform.Define(stage, "/Root/Instancer/Prototypes/Proto") + mesh = UsdGeom.Mesh.Define(stage, "/Root/Instancer/Prototypes/Proto/Tri") + mesh.CreatePointsAttr( + [ + Gf.Vec3f(0, 0, 0), + Gf.Vec3f(1, 0, 0), + Gf.Vec3f(0, 1, 0), + ] + ) + mesh.CreateFaceVertexCountsAttr([3]) + mesh.CreateFaceVertexIndicesAttr([0, 1, 2]) + instancer.CreatePrototypesRel().SetTargets([proto.GetPath()]) + + stage.Save() + + prims = load_scene_prims(scene_path, SceneMeshAlignment(y_up=False)) + + assert len(prims) == 2 + assert np.allclose(prims[0].vertices.min(axis=0), [11, 2, 3]) + assert np.allclose(prims[0].vertices.max(axis=0), [12, 3, 3]) + assert np.allclose(prims[1].vertices.min(axis=0), [14, 5, 6]) + assert np.allclose(prims[1].vertices.max(axis=0), [16, 6, 6]) diff --git a/dimos/simulation/utils/xml_parser.py b/dimos/simulation/utils/xml_parser.py index 052657ea95..3a4362cf4e 100644 --- a/dimos/simulation/utils/xml_parser.py +++ b/dimos/simulation/utils/xml_parser.py @@ -45,7 +45,7 @@ class _ActuatorSpec: def build_joint_mappings(xml_path: Path, model: mujoco.MjModel) -> list[JointMapping]: - specs = _parse_actuator_specs(xml_path) + specs = _parse_actuator_specs(xml_path) if xml_path.suffix.lower() != ".mjb" else [] if specs: return _build_joint_mappings_from_specs(specs, model) if int(model.nu) > 0: diff --git a/pyproject.toml b/pyproject.toml index 47b8a25f62..e4b345128f 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -176,6 +176,7 @@ agents = [ web = [ "fastapi>=0.115.6", + "python-socketio>=5.11.0,<6", "sse-starlette>=2.2.1", "uvicorn>=0.34.0", "jinja2>=3.1.6", @@ -253,6 +254,10 @@ sim = [ "mujoco>=3.3.4", "playground>=0.0.5", "pygame>=2.6.1", + "trimesh", + "usd-core>=24.0", + "vhacdx", + "coacd>=1.0.11", # CoACD decomposer for scene_mesh_to_mjcf bake ] navigation = [ diff --git a/uv.lock b/uv.lock index 993fc21f74..d367f57ebc 100644 --- a/uv.lock +++ b/uv.lock @@ -1138,6 +1138,21 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/56/f3/4da9d5c5308ef2019ab65a8a9f519ac95004446902d01e859f9ac6b8cdd6/cmeel_zlib-1.3.1-0-py3-none-musllinux_1_2_x86_64.whl", hash = "sha256:1e36ac8dccca22ff1f6e4df428ae5597f6288d9e6f85b08c9b767dc63e90fb55", size = 285662, upload-time = "2025-02-11T12:20:37.298Z" }, ] +[[package]] +name = "coacd" +version = "1.0.11" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, +] +wheels = [ + { url = "https://files.pythonhosted.org/packages/1d/3b/434beab9e1754ca0ae3a619b383243dd85aa65d03a4dc7333c8296c97a92/coacd-1.0.11-cp39-abi3-macosx_11_0_arm64.whl", hash = "sha256:adbc58259a721cc5ede24cc8c2671a95e75a8a52dc1ee4d953d80d236b192da9", size = 3299264, upload-time = "2026-05-04T19:23:36.183Z" }, + { url = "https://files.pythonhosted.org/packages/cb/a7/06f63baa29198f681ba60848e3271cc625eddd61cd4e59071f56ffce9362/coacd-1.0.11-cp39-abi3-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:e376fadb22790444c7253f0cee9104a1af01ec965488c1318e84e3b2dbf1e2a3", size = 2573832, upload-time = "2026-05-04T19:23:38.008Z" }, + { url = "https://files.pythonhosted.org/packages/0e/b4/057c78f7b16b87871cfcecc6febe70522e77a2a33f8788960377152c224d/coacd-1.0.11-cp39-abi3-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:a60f700a52e5b40462e508c14bb756cd63ce7e6a95ff72ae0b1592be1dbb0106", size = 2640170, upload-time = "2026-05-04T19:23:39.322Z" }, + { url = "https://files.pythonhosted.org/packages/00/83/c50472ce98175fddd86d4aba861fea89f30350a5487880b3a81d34915a85/coacd-1.0.11-cp39-abi3-win_amd64.whl", hash = "sha256:4de22f70d1a3fa8c44698c8006a223fe5fb0ee84b76adecf3726cf2003e9145f", size = 1465079, upload-time = "2026-05-04T19:23:41.604Z" }, +] + [[package]] name = "coal" version = "3.0.2" @@ -1890,6 +1905,7 @@ all = [ { name = "anthropic" }, { name = "catkin-pkg" }, { name = "cerebras-cloud-sdk" }, + { name = "coacd" }, { name = "ctransformers" }, { name = "ctransformers", extra = ["cuda"], marker = "sys_platform != 'darwin'" }, { name = "cupy-cuda12x", marker = "platform_machine == 'x86_64'" }, @@ -1951,6 +1967,7 @@ all = [ { name = "pymavlink" }, { name = "pyrealsense2", marker = "sys_platform != 'darwin'" }, { name = "python-multipart" }, + { name = "python-socketio" }, { name = "pyturbojpeg" }, { name = "pyyaml" }, { name = "reactivex" }, @@ -1978,7 +1995,9 @@ all = [ { name = "typer" }, { name = "ultralytics" }, { name = "unitree-webrtc-connect-leshy" }, + { name = "usd-core" }, { name = "uvicorn" }, + { name = "vhacdx" }, { name = "xacro" }, { name = "xarm-python-sdk" }, { name = "xformers", marker = "platform_machine == 'x86_64'" }, @@ -2004,6 +2023,7 @@ base = [ { name = "langchain-text-splitters" }, { name = "ollama" }, { name = "openai" }, + { name = "python-socketio" }, { name = "rerun-sdk" }, { name = "sounddevice" }, { name = "soundfile" }, @@ -2105,9 +2125,13 @@ psql = [ { name = "psycopg2-binary" }, ] sim = [ + { name = "coacd" }, { name = "mujoco" }, { name = "playground" }, { name = "pygame" }, + { name = "trimesh" }, + { name = "usd-core" }, + { name = "vhacdx" }, ] unitree = [ { name = "anthropic" }, @@ -2125,6 +2149,7 @@ unitree = [ { name = "langchain-text-splitters" }, { name = "ollama" }, { name = "openai" }, + { name = "python-socketio" }, { name = "rerun-sdk" }, { name = "sounddevice" }, { name = "soundfile" }, @@ -2149,6 +2174,7 @@ unitree-dds = [ { name = "langchain-text-splitters" }, { name = "ollama" }, { name = "openai" }, + { name = "python-socketio" }, { name = "rerun-sdk" }, { name = "sounddevice" }, { name = "soundfile" }, @@ -2165,6 +2191,7 @@ web = [ { name = "fastapi" }, { name = "ffmpeg-python" }, { name = "jinja2" }, + { name = "python-socketio" }, { name = "soundfile" }, { name = "sse-starlette" }, { name = "uvicorn" }, @@ -2322,6 +2349,7 @@ requires-dist = [ { name = "bleak", specifier = ">=3.0.2" }, { name = "catkin-pkg", marker = "extra == 'misc'" }, { name = "cerebras-cloud-sdk", marker = "extra == 'misc'" }, + { name = "coacd", marker = "extra == 'sim'", specifier = ">=1.0.11" }, { name = "colorlog", specifier = "==6.9.0" }, { name = "cryptography", specifier = ">=46.0.5" }, { name = "ctransformers", marker = "extra == 'cpu'", specifier = "==0.2.27" }, @@ -2409,6 +2437,7 @@ requires-dist = [ { name = "pyrealsense2", marker = "sys_platform != 'darwin' and extra == 'manipulation'" }, { name = "python-dotenv" }, { name = "python-multipart", marker = "extra == 'misc'", specifier = ">=0.0.27" }, + { name = "python-socketio", marker = "extra == 'web'", specifier = ">=5.11.0,<6" }, { name = "pyturbojpeg", specifier = "==1.8.2" }, { name = "pyturbojpeg", marker = "extra == 'docker'" }, { name = "pyyaml", marker = "extra == 'manipulation'", specifier = ">=6.0" }, @@ -2442,6 +2471,7 @@ requires-dist = [ { name = "torchreid", marker = "extra == 'misc'", specifier = "==0.2.5" }, { name = "transformers", extras = ["torch"], marker = "extra == 'perception'", specifier = ">=4.53.0,<4.54" }, { name = "trimesh", marker = "extra == 'manipulation'" }, + { name = "trimesh", marker = "extra == 'sim'" }, { name = "typeguard", marker = "extra == 'misc'" }, { name = "typer", specifier = ">=0.19.2,<1" }, { name = "typer", marker = "extra == 'docker'", specifier = ">=0.19.2,<1" }, @@ -2449,7 +2479,9 @@ requires-dist = [ { name = "ultralytics", marker = "extra == 'perception'", specifier = ">=8.3.70" }, { name = "unitree-sdk2py-dimos", marker = "extra == 'unitree-dds'", specifier = ">=1.0.2" }, { name = "unitree-webrtc-connect-leshy", marker = "extra == 'unitree'", specifier = ">=2.0.7" }, + { name = "usd-core", marker = "extra == 'sim'", specifier = ">=24.0" }, { name = "uvicorn", marker = "extra == 'web'", specifier = ">=0.34.0" }, + { name = "vhacdx", marker = "extra == 'sim'" }, { name = "xacro", marker = "extra == 'manipulation'" }, { name = "xarm-python-sdk", marker = "extra == 'manipulation'", specifier = ">=1.17.0" }, { name = "xarm-python-sdk", marker = "extra == 'misc'", specifier = ">=1.17.0" }, @@ -11001,6 +11033,28 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/39/08/aaaad47bc4e9dc8c725e68f9d04865dbcb2052843ff09c97b08904852d84/urllib3-2.6.3-py3-none-any.whl", hash = "sha256:bf272323e553dfb2e87d9bfd225ca7b0f467b919d7bbd355436d3fd37cb0acd4", size = 131584, upload-time = "2026-01-07T16:24:42.685Z" }, ] +[[package]] +name = "usd-core" +version = "26.5" +source = { registry = "https://pypi.org/simple" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/9b/a0/639e148c16a0ec201cc4848aa3da4aba8805e17a2d9e2398eec399fd3051/usd_core-26.5-cp310-none-macosx_10_15_universal2.whl", hash = "sha256:d6a3a567e313841b7390ea7a930bf5aef08bdb912974c725becd725d83edb0f9", size = 39723088, upload-time = "2026-04-24T20:17:23.663Z" }, + { url = "https://files.pythonhosted.org/packages/d7/26/6cb620a64f3fafa38b84008d916eee47c70e5313c5d88c9087edf4d57522/usd_core-26.5-cp310-none-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:85a1484024cdcefd77aac32a3b98e698f655e01951d62cc4d3fb3826e232400c", size = 28820064, upload-time = "2026-04-24T20:17:27.161Z" }, + { url = "https://files.pythonhosted.org/packages/00/d7/7814c95ca0b13a26313e5256472f90cfa2ab7f7cf3103b0d3611d41156e6/usd_core-26.5-cp310-none-win_amd64.whl", hash = "sha256:dff985cbfe24870a5dfe1c578acd918a358cd1680a17777d83b55d50f5560c18", size = 13450099, upload-time = "2026-04-24T20:17:29.994Z" }, + { url = "https://files.pythonhosted.org/packages/39/3a/adf7a4043e70974b84d3a572f928ffdd1176a070595cd17f028062622ade/usd_core-26.5-cp311-none-macosx_10_15_universal2.whl", hash = "sha256:b5416a108080311632b975da71b4ea480757ac6e7ea19b30bcd0eed6a3b6081f", size = 39723550, upload-time = "2026-04-24T20:17:32.975Z" }, + { url = "https://files.pythonhosted.org/packages/e2/7f/575b0ddc2a3effa1dc1f50ed67ae0def8f9ed961c69bfbb89a0a1c9ceaf8/usd_core-26.5-cp311-none-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:60076c97f0de2611dc39d2d25826e3b22a2b0e391c73806b4a072d69929f329e", size = 28825210, upload-time = "2026-04-24T20:17:37.136Z" }, + { url = "https://files.pythonhosted.org/packages/9f/51/9fb7c817f1ee7aff02adde8ec4805ff4add06482e036fe0914ab8e9cdbc5/usd_core-26.5-cp311-none-win_amd64.whl", hash = "sha256:1ff2031095ecdc2f9ff4e245114e6ab7001f7dec8fe75436b5beb72e1a280f57", size = 13450734, upload-time = "2026-04-24T20:17:39.641Z" }, + { url = "https://files.pythonhosted.org/packages/8d/cc/04870cc3ae8e1b3a4e168efea47e389cfab6ab4f619005da2443a10390d4/usd_core-26.5-cp312-none-macosx_10_15_universal2.whl", hash = "sha256:a9df2864e84b83ffc9cc0f2777a49170180f84f2b679bcd014d72036a51d057c", size = 39775789, upload-time = "2026-04-24T20:17:43.025Z" }, + { url = "https://files.pythonhosted.org/packages/77/62/963d3aba966539917d01e4a2169a1c07f7b3df087fc16ee39fc764214969/usd_core-26.5-cp312-none-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:caa2447252aeada8c158faacd4d448f29cf1617aeccef5bb954734b93c8f3f62", size = 28743527, upload-time = "2026-04-24T20:17:46.631Z" }, + { url = "https://files.pythonhosted.org/packages/3b/b0/645ae6e27a9768e570c1044efd6d2369c10c5c2412669314b3d6cd914803/usd_core-26.5-cp312-none-win_amd64.whl", hash = "sha256:6d887b010c756508d2e1f770626201f1f4ba5227c052c1135ba9c19932c4da8e", size = 13494028, upload-time = "2026-04-24T20:17:49.599Z" }, + { url = "https://files.pythonhosted.org/packages/2e/cd/128de2e16d597eb0868dde7cc837a908b28ec2a0d90d4697714b6770449b/usd_core-26.5-cp313-none-macosx_10_15_universal2.whl", hash = "sha256:ce5e90a6795b93d7e744694e5209ea2f1754f9d596e67a89f0cc3590e9fff578", size = 39776038, upload-time = "2026-04-24T20:17:52.535Z" }, + { url = "https://files.pythonhosted.org/packages/f1/10/88838fd371592cfc3d972547ab4361e2deef5891d89c22a509de0e6696ce/usd_core-26.5-cp313-none-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:dd4d3de388e6dfec91fa5ee9fa29800d43ebe86cbf7a10380ec02b15386fca67", size = 28743992, upload-time = "2026-04-24T20:17:55.995Z" }, + { url = "https://files.pythonhosted.org/packages/62/05/da8f44024e0f947c13da3bdae0d4ac6c04cb86de92a6f1b9bf03e6bb8ae8/usd_core-26.5-cp313-none-win_amd64.whl", hash = "sha256:b077ea37dfeb15ca6b24ca33b65c2fe9b1656138e1fda74e4eae9793a149a7d5", size = 13494201, upload-time = "2026-04-24T20:17:59.015Z" }, + { url = "https://files.pythonhosted.org/packages/3d/57/01cc4e412feaad5aaee09d09ead2afbd5b4022e3d3b5461adcbf726ca3f8/usd_core-26.5-cp314-none-macosx_10_15_universal2.whl", hash = "sha256:5b0acd9a1d804cb73d58815365ccb141727f635f4e6764609fade3bf4ef5cbba", size = 39927684, upload-time = "2026-04-24T20:18:01.828Z" }, + { url = "https://files.pythonhosted.org/packages/fd/0d/5b87f5d7c3501bd5296b0bba7ba8a3eaf639ded53b9a17e910ee3363dfc0/usd_core-26.5-cp314-none-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:755c469ec762f3b69d87f5e8af8f8098e4c107bf4c15ce570a042ac2fc2dbb76", size = 28776483, upload-time = "2026-04-24T20:18:05.082Z" }, + { url = "https://files.pythonhosted.org/packages/5a/48/d29a4649df00455174a5979fc8291021199bb2115d623378364b58055bb5/usd_core-26.5-cp314-none-win_amd64.whl", hash = "sha256:7654b5dfef6e7177849aa7e69962feb82a5312ad08469983214aae5821601296", size = 14043860, upload-time = "2026-04-24T20:18:07.896Z" }, +] + [[package]] name = "uuid-utils" version = "0.14.0" @@ -11099,6 +11153,60 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/e4/16/c1fd27e9549f3c4baf1dc9c20c456cd2f822dbf8de9f463824b0c0357e06/uvloop-0.22.1-cp314-cp314t-musllinux_1_2_x86_64.whl", hash = "sha256:6cde23eeda1a25c75b2e07d39970f3374105d5eafbaab2a4482be82f272d5a5e", size = 4296730, upload-time = "2025-10-16T22:17:00.744Z" }, ] +[[package]] +name = "vhacdx" +version = "0.0.10" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/15/e3/d2abc3dc4c1cb216c2efdc70b36f80efeb1bdbd7d420a676ddc9d9d980e1/vhacdx-0.0.10.tar.gz", hash = "sha256:fcc23201e319d79fe25e064847efc254bd39ac30af28cc761409e1f9142dd033", size = 58125, upload-time = "2025-12-02T20:58:45.358Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/c4/f4/da308d86daaa9c636851357cbd928715d47963beecd525b3749d2d5c9537/vhacdx-0.0.10-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:4bc7be82fab608cb7231e95a0a10700be1e9422a36b21e7d49c782a598c8d37c", size = 222760, upload-time = "2025-12-02T20:57:30.778Z" }, + { url = "https://files.pythonhosted.org/packages/e0/8a/e3462a43ec6712b74d921e4af9d5a2998752378c5554bde9a594dbb0cf0c/vhacdx-0.0.10-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:4b63d1c5ad0e64c300a3a9d9404f4778df367b8c545639dfb932db4b76704ff3", size = 208812, upload-time = "2025-12-02T20:57:33.486Z" }, + { url = "https://files.pythonhosted.org/packages/fb/d1/b717275adb108431f1404193542fab7ecf4c5bae221f1552bbd570fe0e5d/vhacdx-0.0.10-cp310-cp310-manylinux_2_26_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:f9bcf3fe1c555598e41348108b55a0fc67534e7fef2367452c301014518c1476", size = 236999, upload-time = "2025-12-02T20:57:34.971Z" }, + { url = "https://files.pythonhosted.org/packages/bf/84/97e2305f6bd4a4de3d40bb234c38282cbcf2fa30653ff5ae4f7df9d8f3ec/vhacdx-0.0.10-cp310-cp310-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:9506ca89289da63e5a3d1ac97aa7413aece47d65cbaa4b0c409469555add0e06", size = 250035, upload-time = "2025-12-02T20:57:36.037Z" }, + { url = "https://files.pythonhosted.org/packages/9d/66/eb1d8d64742b9e73557e075cea6ee7e4976dd89b84c7d3197ca3621d5a85/vhacdx-0.0.10-cp310-cp310-musllinux_1_2_aarch64.whl", hash = "sha256:06faf9caa0abceddd5fa505e4299e2ebf14bc26c58a1e521013717cbf37bea61", size = 1224134, upload-time = "2025-12-02T20:57:37.217Z" }, + { url = "https://files.pythonhosted.org/packages/47/db/e829b21b071db94f45079c4ace2f967c684f08b10ea285919a95e9d5fe21/vhacdx-0.0.10-cp310-cp310-musllinux_1_2_x86_64.whl", hash = "sha256:3a6b43b42290697e2bd04087977d1e3841d287c528e414c765581ecec62e66f6", size = 1284300, upload-time = "2025-12-02T20:57:38.78Z" }, + { url = "https://files.pythonhosted.org/packages/ff/aa/b401565542b927ce3e0a6d5e72acef79343a449ee1a7ad94a5c7266bab26/vhacdx-0.0.10-cp310-cp310-win_amd64.whl", hash = "sha256:27eb3b293ccef1332d477346d564bb4c474bb451e9b753e3ce9cac01cbb90a0c", size = 193069, upload-time = "2025-12-02T20:57:40.318Z" }, + { url = "https://files.pythonhosted.org/packages/b7/2c/d49df6fec3294cef3c8c88c54784162bd8350c427fecd9b16335772b760f/vhacdx-0.0.10-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:8584f33ed020b6cce678b8febcf84af22bced617ef31c85bf31fd7e2b4bba9fe", size = 224113, upload-time = "2025-12-02T20:57:41.59Z" }, + { url = "https://files.pythonhosted.org/packages/68/1d/bd2456baa6b16977c106adc2386b6e7a34c3e57ade6aeeab68bb61ceb16f/vhacdx-0.0.10-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:a9b63cdb5f34dfee386b64a01f7e1571ef0c2555244ea3d83a09d78273123bce", size = 210118, upload-time = "2025-12-02T20:57:42.749Z" }, + { url = "https://files.pythonhosted.org/packages/49/ab/15adb78489b51c2a898642755be727ecd7c3de37cac6e434ce420b8ce27c/vhacdx-0.0.10-cp311-cp311-manylinux_2_26_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:915eab6c19fdf63ab256855331db546575284786a480aa2d67437db0e86b0d17", size = 238276, upload-time = "2025-12-02T20:57:43.95Z" }, + { url = "https://files.pythonhosted.org/packages/a6/f1/464c761dbe24f58d6fc354bf51729342981fb7a621e170e0d3512fadbec8/vhacdx-0.0.10-cp311-cp311-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:6e335bb9af540e6867ff051166a399075823fdd8fc1fc27e9530995cc1bda1eb", size = 251383, upload-time = "2025-12-02T20:57:45.246Z" }, + { url = "https://files.pythonhosted.org/packages/b2/22/c7b4117c5431189a6a019e8fc2cf590df3ab196c38b4b7c3622292205d9b/vhacdx-0.0.10-cp311-cp311-musllinux_1_2_aarch64.whl", hash = "sha256:c3ddbaa38eb65c3aec9b0e39a822223474c931c0e18d3e93a3a499870ffa45ad", size = 1225200, upload-time = "2025-12-02T20:57:46.639Z" }, + { url = "https://files.pythonhosted.org/packages/6c/62/c679ad28ce7854771913255e1abc588b3643c2147fb5c51a8553224aa1dd/vhacdx-0.0.10-cp311-cp311-musllinux_1_2_x86_64.whl", hash = "sha256:d398fcc13e330ed1fd2540a7d572aeca0be9621411def78e10c7ea4132f959ee", size = 1285935, upload-time = "2025-12-02T20:57:48.51Z" }, + { url = "https://files.pythonhosted.org/packages/de/c8/a8260b780e4578d7ef19b70343f9717f74ff48f9950138c96c78f209ec01/vhacdx-0.0.10-cp311-cp311-win_amd64.whl", hash = "sha256:c9665a3ef887babcac8b5822f01288e8f06b4a949fadbbe1861670b358f111ee", size = 194137, upload-time = "2025-12-02T20:57:50.207Z" }, + { url = "https://files.pythonhosted.org/packages/cf/9c/66375e65634c80f6efb46e81915126bf3e55dc9d6615217590cbc8316d2e/vhacdx-0.0.10-cp312-cp312-macosx_10_13_x86_64.whl", hash = "sha256:7dd17d697d6d4d7cf66f1e947e0530041913981e05f7025236bec28a350b1a33", size = 224998, upload-time = "2025-12-02T20:57:51.639Z" }, + { url = "https://files.pythonhosted.org/packages/4e/e3/fc2644d3e7d0b2b52e2f681eb2878c0e1b9cafc53946f66736d0f01e237c/vhacdx-0.0.10-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:189ded39b709436cb732cdf694d4cf22e877aefb97e2ab2b55bf7ada9c030f93", size = 211130, upload-time = "2025-12-02T20:57:53.018Z" }, + { url = "https://files.pythonhosted.org/packages/e3/93/0b0f1977f5b3c2e1bbea5ef85e37a808ff73f1b7daf42950c57090e90dc7/vhacdx-0.0.10-cp312-cp312-manylinux_2_26_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:f3b03d35ab56a93beee338175dbe0b87552353e5dfb3ff37467e88f56cedf7cc", size = 239661, upload-time = "2025-12-02T20:57:54.144Z" }, + { url = "https://files.pythonhosted.org/packages/94/98/d2a6aeb1c6570a1fc1be29ee03db795f643ab03c6df7635522f23796b39d/vhacdx-0.0.10-cp312-cp312-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:ea8c54ed193fa0db0248928fbf5d438b3872d615a506889d5b89fc6467d6411a", size = 252938, upload-time = "2025-12-02T20:57:55.275Z" }, + { url = "https://files.pythonhosted.org/packages/94/2e/1e678efc161a0d7fe1806f5e037ce11cc5964db7e08ccfc220ef63951863/vhacdx-0.0.10-cp312-cp312-musllinux_1_2_aarch64.whl", hash = "sha256:a5c898104140c72e4dc789e6125812671eee5e412916e83eff24a6148248ff5e", size = 1226696, upload-time = "2025-12-02T20:57:56.438Z" }, + { url = "https://files.pythonhosted.org/packages/90/5b/b302a0420a241c4910f4870eb9f39e6ada59858db441cc35bda511c17982/vhacdx-0.0.10-cp312-cp312-musllinux_1_2_x86_64.whl", hash = "sha256:abdd0ba17786e578206594731df15c90e2751b6884220c8673124f47fd7ac620", size = 1287794, upload-time = "2025-12-02T20:57:57.694Z" }, + { url = "https://files.pythonhosted.org/packages/73/e9/f9729603ac75047a257f1b4ddac60cbde72b0abfd49ffed305751ba630a2/vhacdx-0.0.10-cp312-cp312-win_amd64.whl", hash = "sha256:79e7db59b4042295b21b79d55ba486a9a480550f696d466f158a30ed920dd0ec", size = 195033, upload-time = "2025-12-02T20:57:58.95Z" }, + { url = "https://files.pythonhosted.org/packages/0e/54/c2fc08d9324bbd92735caf9207cbabada3a8dd9d270d6e46ca69eb7f883d/vhacdx-0.0.10-cp313-cp313-macosx_10_13_x86_64.whl", hash = "sha256:0599bc2a96de8fc9aeff460b3e88b8572e84ae95b8fc6c9888ef4b92023c22d5", size = 225014, upload-time = "2025-12-02T20:58:00.938Z" }, + { url = "https://files.pythonhosted.org/packages/3b/9e/42adb642a12915acc9cb2bfab21710a6aabf045c26967ba0ff0e08a872d0/vhacdx-0.0.10-cp313-cp313-macosx_11_0_arm64.whl", hash = "sha256:dc648829a1e973f34ee11393a4f334ab55e3e0e9c4b9f6d6349af966fdf1895a", size = 211127, upload-time = "2025-12-02T20:58:02.107Z" }, + { url = "https://files.pythonhosted.org/packages/51/3d/63e090cd966817b89643d7e52e13df45043b22a42c7fbf702866bdd75bc0/vhacdx-0.0.10-cp313-cp313-manylinux_2_26_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:74c03f7315a434ec83cd0bff1e6bce6af4c01df61d677f48f3ffb36800606ee7", size = 239471, upload-time = "2025-12-02T20:58:03.173Z" }, + { url = "https://files.pythonhosted.org/packages/b8/b4/07ab1c828bae0eb5c72cd9a4cbe8b0376d374509be3c7055e1a399bf85c3/vhacdx-0.0.10-cp313-cp313-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:1fcd02acc3733ec3a0a0d28ca7f526d4c56f14a3ceb4b12fce45acf72c09054a", size = 253019, upload-time = "2025-12-02T20:58:04.318Z" }, + { url = "https://files.pythonhosted.org/packages/05/cb/bc8a8858b300d2c092da11096ae0586ece446b4c41cb26620bf00d1d8232/vhacdx-0.0.10-cp313-cp313-musllinux_1_2_aarch64.whl", hash = "sha256:4b9f8a80ca4c54d7fa76419a2ebd9e9386cd177dc4d2b97f2208ac57c9a7e8aa", size = 1226933, upload-time = "2025-12-02T20:58:05.907Z" }, + { url = "https://files.pythonhosted.org/packages/15/52/213230883874615f1661903bce1ace5013d03b34696efce8d53c662a3358/vhacdx-0.0.10-cp313-cp313-musllinux_1_2_x86_64.whl", hash = "sha256:847bd43e82afb439dd3fa972618d786d0b98d8ef04a8e8a6381f6945204d2505", size = 1288871, upload-time = "2025-12-02T20:58:07.432Z" }, + { url = "https://files.pythonhosted.org/packages/32/25/f0e6806824f88d47ab8bc1c9bf6f11634fd7b382d635d0696825f3b5672f/vhacdx-0.0.10-cp313-cp313-win_amd64.whl", hash = "sha256:ab300c5f3fe4e54f99af92f9ea27c977b09df5f59190b0a3e025161110f71ce7", size = 195091, upload-time = "2025-12-02T20:58:08.783Z" }, + { url = "https://files.pythonhosted.org/packages/b7/b8/5137c048728fddd3315a79c94ba8663f3707f9268af9af15b15e1ef3cd85/vhacdx-0.0.10-cp314-cp314-macosx_10_15_x86_64.whl", hash = "sha256:147030c7683be4f21a3cdfb202b121c01716694b61ddad794345fcd9fa43d0ec", size = 225247, upload-time = "2025-12-02T20:58:09.918Z" }, + { url = "https://files.pythonhosted.org/packages/1b/08/5c731863db402e9878380f68be8722fabbcaf8cfe8d06237aaf15f116d95/vhacdx-0.0.10-cp314-cp314-macosx_11_0_arm64.whl", hash = "sha256:069eb4381917b790921a33d4cc6ed07f7ed5362474232110baf8dd3dadcd768d", size = 211339, upload-time = "2025-12-02T20:58:10.951Z" }, + { url = "https://files.pythonhosted.org/packages/04/3a/e93ce9b653a9f435c530df8d5ad68a80b8bdc2b8518abc225fef9e7f349a/vhacdx-0.0.10-cp314-cp314-manylinux_2_26_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:7702892008b1150619c1f66a62ef88d1cb8f92b09d9c39a0bfb87d147f1c5ae2", size = 239974, upload-time = "2025-12-02T20:58:12.101Z" }, + { url = "https://files.pythonhosted.org/packages/77/dc/ef34f97a65385bc1f8ed4718fa5f7d96313e299e76761f1b69efaf597797/vhacdx-0.0.10-cp314-cp314-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:b4d550dfc377471b36f11065fc12cfbbd1750d63b10a336644bfdcbf27aa8382", size = 253245, upload-time = "2025-12-02T20:58:13.303Z" }, + { url = "https://files.pythonhosted.org/packages/f4/6c/57051066bd0589b7fe68c32061821180f520b6a7ef4efa072b755dde63d3/vhacdx-0.0.10-cp314-cp314-musllinux_1_2_aarch64.whl", hash = "sha256:edce8f0ff516a111b6f1d644782a1d496b3e9e34ff4ce09849c9b8071627bca5", size = 1227432, upload-time = "2025-12-02T20:58:14.73Z" }, + { url = "https://files.pythonhosted.org/packages/1d/49/3488f2bd991027bd86f072cf776935c80b4e630bd3bc43c3289bc6eeeba0/vhacdx-0.0.10-cp314-cp314-musllinux_1_2_x86_64.whl", hash = "sha256:4c463abbdce73d5d0b94eab2c9f43f2b55a4d0e788d87af659cc78029b960bf9", size = 1289126, upload-time = "2025-12-02T20:58:16.009Z" }, + { url = "https://files.pythonhosted.org/packages/77/56/2f4506382a1133bf441cba2010017064e8f7af940d100141799d7e867e58/vhacdx-0.0.10-cp314-cp314-win_amd64.whl", hash = "sha256:b93c834f2bf1fa6630da5d3f77e94ea8e542fca31e385244a7ec905a32155549", size = 198706, upload-time = "2025-12-02T20:58:17.378Z" }, + { url = "https://files.pythonhosted.org/packages/db/f6/4fabfe65f3123abda09adc416a396caf8c2ad1b29c34a5178ec71754a163/vhacdx-0.0.10-cp314-cp314t-macosx_10_15_x86_64.whl", hash = "sha256:4c0f1bafc53e156472b0367533c2d3ec7a96b676b6d57aa92dd3e37519331b07", size = 228276, upload-time = "2025-12-02T20:58:18.545Z" }, + { url = "https://files.pythonhosted.org/packages/dc/70/bdc742628adcf9966cea81be7a651300bc399b492d10a763781af6d27041/vhacdx-0.0.10-cp314-cp314t-macosx_11_0_arm64.whl", hash = "sha256:b0f8643dcb1f0774320fc1389ead0d0da4536e4c0fecfd4c8133baec0b6fa556", size = 214287, upload-time = "2025-12-02T20:58:19.696Z" }, + { url = "https://files.pythonhosted.org/packages/84/6a/f2e37ad333d3f671e1d59ba76bb61edc5520146539d52ee29e555becb4ac/vhacdx-0.0.10-cp314-cp314t-manylinux_2_26_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:4547f3e55eb935d163d89c10ffdcadf8797c3b435a9dc82be4e0e27b1e3abff0", size = 240923, upload-time = "2025-12-02T20:58:21.105Z" }, + { url = "https://files.pythonhosted.org/packages/5b/7a/f0325cd7ece95dbbc10d0c3f6d241d47beb3b99ae4dafe2e450082cd7bd9/vhacdx-0.0.10-cp314-cp314t-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:ee09c4f2b6385546001b5e8609f428417fac147cfd3ea020fbc7dec0f11c489b", size = 254257, upload-time = "2025-12-02T20:58:22.176Z" }, + { url = "https://files.pythonhosted.org/packages/ac/56/53347b910351eb4cf32a797e177f18b8d82b1ef4e4325607254cfe88ad2a/vhacdx-0.0.10-cp314-cp314t-musllinux_1_2_aarch64.whl", hash = "sha256:8b94d198e4716f9220985523f374617432ef5530910f3730051f3e7fcba71798", size = 1228434, upload-time = "2025-12-02T20:58:23.302Z" }, + { url = "https://files.pythonhosted.org/packages/be/f5/f86c63da38b0446ef6652e8e72b84451e440418eaac0f554737e159ae36e/vhacdx-0.0.10-cp314-cp314t-musllinux_1_2_x86_64.whl", hash = "sha256:39c6d31ed27e3f33e9411927e1567ba37a18ba7ce9295efd1b24414b7313b503", size = 1288854, upload-time = "2025-12-02T20:58:24.46Z" }, + { url = "https://files.pythonhosted.org/packages/0c/d1/b30dec954a24b41358297a3fbe7c30d8e2e818831f552cb34c904baa04e4/vhacdx-0.0.10-cp314-cp314t-win_amd64.whl", hash = "sha256:fc6a613082ec522a020e4f6a09f39ed42546de9aebe99548aa84938b1440871c", size = 204896, upload-time = "2025-12-02T20:58:25.825Z" }, +] + [[package]] name = "virtualenv" version = "20.36.1"