From 8d7b80b62f8f2ae3283fe5eb39d3982e5c446649 Mon Sep 17 00:00:00 2001 From: matafela Date: Fri, 7 Aug 2026 16:06:48 +0800 Subject: [PATCH 1/6] fix --- .../overview/sim/planners/curobo_planner.md | 41 +- .../lab/sim/planners/curobo/curobo_planner.py | 398 +++++++++++++++- .../sim/planners/curobo/curobo_sphere_data.py | 275 +++++++++++ .../lab/sim/planners/curobo/curobo_yaml.py | 445 ++++++++++++------ examples/sim/planners/curobo_planner.py | 30 +- tests/sim/planners/test_curobo_planner.py | 427 ++++++++++++++++- 6 files changed, 1427 insertions(+), 189 deletions(-) create mode 100644 embodichain/lab/sim/planners/curobo/curobo_sphere_data.py diff --git a/docs/source/overview/sim/planners/curobo_planner.md b/docs/source/overview/sim/planners/curobo_planner.md index cc72d4fb7..75df7c6c6 100644 --- a/docs/source/overview/sim/planners/curobo_planner.md +++ b/docs/source/overview/sim/planners/curobo_planner.md @@ -89,9 +89,15 @@ different planning GPU. A CPU value is rejected because cuRobo itself has no CPU backend. The robot configuration must be a cuRobo V2 robot profile with collision -spheres and self-collision data; the adapter generates this from the robot's -URDF automatically. A plain URDF alone is not sufficient for collision planning -without that sphere-fitting step. +spheres; the adapter generates this from the robot's URDF automatically. A plain +URDF alone is not sufficient for robot-to-world collision planning without that +sphere-fitting step. + +:::{warning} +cuRobo self-collision checking is temporarily disabled in this backend. +Robot-to-world collision checking remains enabled, but planned trajectories +are not currently rejected when two robot links collide with each other. +::: The adapter automatically rebases simulator-world Cartesian goals and dynamic obstacle poses through the live simulator control-part base, so parallel arena @@ -138,9 +144,16 @@ The collision world is always auto-generated from live `RigidObject` meshes via `CuroboWorldCfg.rigid_objects`: the adapter reads each object's mesh (`get_vertices` / `get_triangles`) and world pose (`get_local_pose`) and writes a cached cuRobo scene YAML on the first plan, using -`CuroboWorldCfg.obstacle_representation` (`"sphere"` by default for fast -collision queries; use `"cuboid"` for a local-frame AABB placed as an OBB via -the object pose, or `"mesh"` for the exact triangle mesh). +`CuroboWorldCfg.obstacle_representation` (`"sphere"` by default; use +`"cuboid"` for a local-frame AABB placed as an OBB via the object pose, or +`"mesh"` for the exact triangle mesh). Sphere worlds remain sphere-based in +the generated YAML, cache, collision-model visualization, and runtime checker. +cuRobo V2 can parse those spheres but omits sphere storage from its generic +world checker. EmbodiChain registers an analytic sphere obstacle type with that +checker before constructing the backend; its signed distance is evaluated as +the center distance minus the obstacle radius. The runtime sphere cache is +sized automatically from the scene, and backend initialization verifies that +no fitted sphere was silently dropped. Generated poses are authored in the cuRobo base/world frame, so this is exact when the robot base sits at the simulator world origin. For obstacles that move or live in an offset base frame, also declare their names in @@ -225,11 +238,17 @@ robot's URDF and solver, so nothing robot-specific needs to be hardcoded: The generated YAML is cached on disk (default `$XDG_CACHE_HOME/embodichain_curobo` or `~/.cache/embodichain_curobo`) keyed by the URDF path, URDF content, control part, tool frame, and fit parameters, so editing the URDF or changing the fit -settings regenerates automatically and subsequent inits reuse the cache. Tune the -fit with `CuroboPlannerCfg.auto_gen` (`fit_type="voxel"` by default for fast -first-generation; `"morphit"` for best quality; `force=True` to bypass the cache). -The default `sphere_density=0.1` keeps the per-link sphere count low (~80 for a -Panda) so planning stays fast; raise it for tighter collision coverage. +settings regenerates automatically and subsequent inits reuse the cache. Sphere +fitting always uses DexSim's `SphereFitType.MORPHIT`, with at most 2 convex hulls +per robot link and 16 per obstacle. The default `sphere_density=0.1` keeps the +per-link sphere count low (~80 for a Panda) so planning stays fast; raise it for +tighter collision coverage, or set `force=True` to bypass the cache. + +For an Open3D overlay of the live robot/obstacle meshes and the exact spheres +read back from those YAML caches, call +`planner.visualize_collision_models(control_part)`. Robot sphere centers are +transformed by the simulator's live link poses. The interactive cuRobo example +calls this once after planner initialization; close the Open3D window to continue. ## Generate a motion diff --git a/embodichain/lab/sim/planners/curobo/curobo_planner.py b/embodichain/lab/sim/planners/curobo/curobo_planner.py index 91e2255c7..50b2c81fc 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_planner.py +++ b/embodichain/lab/sim/planners/curobo/curobo_planner.py @@ -76,10 +76,12 @@ ) # Bumped whenever the auto-generated robot-YAML schema/logic changes so that -# cached YAMLs from an older generator are regenerated instead of reused. v2: -# exclude URDF mimic joints from cspace/lock_joints (cuRobo folds them into -# their active joint and raises KeyError when locking one). -_CUROBO_ROBOT_YAML_GENERATOR_VERSION = "v2" +# cached YAMLs from an older generator are regenerated instead of reused. v2 +# excluded URDF mimic joints from cspace/lock_joints; v3 switched both robot +# and obstacle fitting to DexSim MorphIt with fixed convex-hull limits; v4 +# removes self-collision metadata because the backend temporarily disables +# cuRobo self-collision checking. +_CUROBO_ROBOT_YAML_GENERATOR_VERSION = "v4-no-self-collision" # cuRobo 0.8 does not expose PyTorch's CUDA stream-capture error mode. The # temporary adapter below therefore replaces ``torch.cuda.graph`` only while @@ -159,9 +161,11 @@ class CuroboWorldCfg: obstacle_representation: str = "sphere" """Collision representation used when generating the YAML from :attr:`rigid_objects`. - ``"sphere"`` (default) fits spheres with cuRobo's - ``fit_spheres_to_mesh`` (fast collision queries, approximate, and requires - CUDA + cuRobo + trimesh). ``"cuboid"`` emits a local-frame AABB per object, + ``"sphere"`` (default) fits spheres with DexSim's MorphIt implementation + (approximate, and requires CUDA + Open3D). cuRobo V2 can parse sphere + obstacles but omits their collision storage; EmbodiChain registers an + analytic sphere SDF with the generic Warp checker at backend creation. + ``"cuboid"`` emits a local-frame AABB per object, placed as an OBB via the object pose. ``"mesh"`` emits the full triangle mesh (exact, no CUDA). """ @@ -175,6 +179,8 @@ class CuroboWorldCfg: cuRobo V2 accepts integer ``cuboid`` and ``mesh`` capacities. A ``voxel`` cache, when needed for dynamic voxel worlds, must instead be a dictionary with V2's ``layers``, ``dims``, and ``voxel_size`` fields. + + Analytic sphere obstacles use a separate runtime cache sized from the scene. """ dynamic_obstacle_names: list[str] = [] @@ -241,10 +247,6 @@ class CuroboAutoGenCfg: regenerates automatically. """ - fit_type: str = "voxel" - """cuRobo sphere-fit strategy for auto-generation: ``"voxel"`` (default, - fast), ``"morphit"`` (best, slower), or ``"surface"`` (crude).""" - num_spheres: int | None = None """Per-link sphere count. ``None`` auto-estimates from bounding-box volume scaled by :attr:`sphere_density`.""" @@ -253,17 +255,17 @@ class CuroboAutoGenCfg: """Multiplier on the auto-estimated per-link sphere count (ignored when :attr:`num_spheres` is set). - The cuRobo volume-based estimate over-fits at ``1.0`` (~668 spheres for a + The volume-based estimate over-fits at ``1.0`` (~668 spheres for a Franka Panda, making planning pathologically slow). ``0.1`` (default) yields ~50-100 spheres - enough coverage for collision-aware planning while keeping each plan fast. Increase for tighter coverage on complex robots. """ surface_radius: float = 0.005 - """Fixed radius used only by the ``surface`` strategy.""" + """Fixed radius used if MorphIt falls back to surface sampling.""" iterations: int = 200 - """Adam iterations for the ``morphit`` strategy.""" + """Adam iterations for MorphIt.""" collision_sphere_buffer: float = 0.0 """Padding added to every fitted sphere's radius (m).""" @@ -628,6 +630,182 @@ def _configure_curobo_logging(log_level: str) -> None: logging.getLogger("curobo").setLevel(levels[normalized]) +def _enable_curobo_sphere_collision_support() -> None: + """Register analytic sphere obstacles with cuRobo V2's generic checker. + + cuRobo 0.8 publishes ``Sphere`` as a scene type but omits sphere storage + from ``SceneData``. Its Warp checker is intentionally extensible through + ``OBSTACLE_SDF_MODULES``; register EmbodiChain's analytic sphere storage and + extend the aggregate scene container before importing the motion planner. + """ + import sys + + kernel_module_names = ( + "curobo._src.geom.collision.wp_collision_kernel", + "curobo._src.geom.collision.wp_sweep_collision_kernel", + ) + preloaded_kernel_modules = { + name for name in kernel_module_names if name in sys.modules + } + data_package = importlib.import_module("curobo._src.geom.data") + sphere_module_path = "embodichain.lab.sim.planners.curobo.curobo_sphere_data" + if sphere_module_path not in data_package.OBSTACLE_SDF_MODULES: + data_package.OBSTACLE_SDF_MODULES.append(sphere_module_path) + sphere_module = importlib.import_module(sphere_module_path) + scene_data_module = importlib.import_module("curobo._src.geom.data.data_scene") + scene_data_cls = scene_data_module.SceneData + if not getattr(scene_data_cls, "_embodichain_sphere_support", False): + original_from_scene = scene_data_cls.from_scene_cfg + original_from_batch = scene_data_cls.from_batch_scene_cfg + original_create_cache = scene_data_cls.create_cache + original_get_valid_data = scene_data_cls.get_valid_data + original_get_names = scene_data_cls.get_obstacle_names + original_load_scene = scene_data_cls.load_from_scene_cfg + original_update_pose = scene_data_cls.update_obstacle_pose + original_enable_obstacle = scene_data_cls.enable_obstacle + original_clear = scene_data_cls.clear + + @classmethod + def from_scene_cfg_with_spheres(cls, scene_cfg, device_cfg, *args, **kwargs): + data = original_from_scene(scene_cfg, device_cfg, *args, **kwargs) + num_envs = kwargs.get("num_envs", args[0] if args else 1) + env_idx = kwargs.get("env_idx", args[1] if len(args) > 1 else 0) + data.spheres = ( + sphere_module.SphereData.from_scene_cfg( + scene_cfg, + device_cfg, + env_idx=env_idx, + num_envs=num_envs, + ) + if scene_cfg.sphere + else None + ) + return data + + @classmethod + def from_batch_scene_cfg_with_spheres( + cls, scene_cfg_list, device_cfg, *args, **kwargs + ): + data = original_from_batch(scene_cfg_list, device_cfg, *args, **kwargs) + data.spheres = ( + sphere_module.SphereData.from_batch_scene_cfg( + scene_cfg_list, device_cfg + ) + if any(scene.sphere for scene in scene_cfg_list) + else None + ) + return data + + @classmethod + def create_cache_with_spheres(cls, *args, **kwargs): + data = original_create_cache(*args, **kwargs) + data.spheres = None + return data + + def get_valid_data_with_spheres(self): + valid_data = original_get_valid_data(self) + spheres = getattr(self, "spheres", None) + if spheres is not None: + valid_data.append(spheres) + return valid_data + + def get_obstacle_names_with_spheres(self, env_idx=0): + names = original_get_names(self, env_idx) + spheres = getattr(self, "spheres", None) + if spheres is not None: + names.extend(spheres.get_names(env_idx)) + return names + + def load_from_scene_cfg_with_spheres( + self, scene_cfg, env_idx=0, store_reference=True + ): + original_load_scene(self, scene_cfg, env_idx, store_reference) + if scene_cfg.sphere: + spheres = getattr(self, "spheres", None) + if spheres is None or len(scene_cfg.sphere) > spheres.max_n: + self.spheres = sphere_module.SphereData.create_cache( + len(scene_cfg.sphere), self.num_envs, self.device_cfg + ) + self.spheres.load_batch(scene_cfg.sphere, env_idx) + + def update_obstacle_pose_with_spheres(self, name, pose, env_idx=0): + spheres = getattr(self, "spheres", None) + if spheres is not None and name in spheres.get_names(env_idx): + spheres.update_pose(name, pose, env_idx) + return + original_update_pose(self, name, pose, env_idx) + + def enable_obstacle_with_spheres(self, name, enabled=True, env_idx=0): + spheres = getattr(self, "spheres", None) + if spheres is not None and name in spheres.get_names(env_idx): + spheres.set_enabled(name, enabled, env_idx) + return + original_enable_obstacle(self, name, enabled, env_idx) + + def clear_with_spheres(self, env_idx=None): + original_clear(self, env_idx) + spheres = getattr(self, "spheres", None) + if spheres is not None: + spheres.clear(env_idx) + + def has_spheres(self): + return getattr(self, "spheres", None) is not None + + scene_data_cls.from_scene_cfg = from_scene_cfg_with_spheres + scene_data_cls.from_batch_scene_cfg = from_batch_scene_cfg_with_spheres + scene_data_cls.create_cache = create_cache_with_spheres + scene_data_cls.get_valid_data = get_valid_data_with_spheres + scene_data_cls.get_obstacle_names = get_obstacle_names_with_spheres + scene_data_cls.load_from_scene_cfg = load_from_scene_cfg_with_spheres + scene_data_cls.update_obstacle_pose = update_obstacle_pose_with_spheres + scene_data_cls.enable_obstacle = enable_obstacle_with_spheres + scene_data_cls.clear = clear_with_spheres + scene_data_cls.has_spheres = has_spheres + scene_data_cls._embodichain_sphere_support = True + + collision_scene_module = importlib.import_module( + "curobo._src.geom.collision.collision_scene" + ) + collision_scene_cls = collision_scene_module.SceneCollision + if not getattr(collision_scene_cls, "_embodichain_sphere_support", False): + original_collision_types = collision_scene_cls.collision_types.fget + + def collision_types_with_spheres(self): + collision_types = original_collision_types(self) + collision_types["sphere"] = self.data.has_spheres() + return collision_types + + collision_scene_cls.collision_types = property(collision_types_with_spheres) + collision_scene_cls._embodichain_sphere_support = True + + # If another package imported the generic kernels first, add the sphere + # overloads to their existing Warp function sets as well. + import warp as wp + + for module_name, include_sdf_only in ( + ("curobo._src.geom.collision.wp_collision_kernel", False), + ("curobo._src.geom.collision.wp_sweep_collision_kernel", True), + ): + if module_name not in preloaded_kernel_modules: + continue + kernel_module = sys.modules.get(module_name) + if kernel_module is None: + continue + kernel_module.is_obs_enabled = wp.func( + sphere_module.is_obs_enabled, module=module_name + ) + kernel_module.load_obstacle_transform = wp.func( + sphere_module.load_obstacle_transform, module=module_name + ) + if include_sdf_only: + kernel_module.compute_local_sdf = wp.func( + sphere_module.compute_local_sdf, module=module_name + ) + kernel_module.compute_local_sdf_with_grad = wp.func( + sphere_module.compute_local_sdf_with_grad, module=module_name + ) + + def _require_curobo(log_level: str = "error") -> "Any": """Lazily import and bundle the cuRobo V2 public facade types. @@ -646,8 +824,10 @@ def _require_curobo(log_level: str = "error") -> "Any": # cuRobo 0.8 references ``wp.torch.*``, which Warp >= 1.13 relocated. _ensure_warp_torch_compat() try: + _enable_curobo_sphere_collision_support() planner_mod = importlib.import_module("curobo.motion_planner") batch_mod = importlib.import_module("curobo.batch_motion_planner") + scene_mod = importlib.import_module("curobo.scene") types_mod = importlib.import_module("curobo.types") except ModuleNotFoundError as exc: raise ImportError( @@ -666,6 +846,7 @@ def _require_curobo(log_level: str = "error") -> "Any": Pose=types_mod.Pose, GoalToolPose=types_mod.GoalToolPose, DeviceCfg=types_mod.DeviceCfg, + Scene=scene_mod.Scene, ) @@ -731,7 +912,8 @@ class CuroboPlanner(BasePlanner): Cartesian (``EEF_MOVE``) targets are forwarded to cuRobo unchanged because the backend accepts them directly and performs its own collision-aware IK - and trajectory optimization. + and trajectory optimization. Robot-to-world collision checking remains + enabled, but robot self-collision checking is temporarily disabled. By default the returned collision-checked samples are arc-length resampled to the action's ``sample_interval`` waypoint count (``preserve_plan_samples=False``); set @@ -1130,11 +1312,18 @@ def _build_backend( planning_mode: MoveType, ) -> "_CuroboBackend": """Construct and validate one cuRobo planner on the selected CUDA device.""" + robot_config = self._load_runtime_robot_config(profile.robot_config_path) + ( + runtime_scene_model, + runtime_collision_cache, + expected_sphere_names, + ) = self._prepare_runtime_scene_model(scene_model, collision_cache) with torch.cuda.device(self._curobo_device): planner_cfg = self._bindings.MotionPlannerCfg.create( - robot=profile.robot_config_path, - scene_model=scene_model, - collision_cache=collision_cache, + robot=robot_config, + scene_model=runtime_scene_model, + collision_cache=runtime_collision_cache, + self_collision_check=False, device_cfg=self._bindings.DeviceCfg(device=self._curobo_device), max_batch_size=batch_size, multi_env=bool(self.cfg.world.multi_env), @@ -1143,6 +1332,7 @@ def _build_backend( ), use_cuda_graph=use_cuda_graph, ) + self._disable_curobo_self_collision_rollouts(planner_cfg) # cuRobo 0.8 reads interpolation_dt from the trajectory optimizer # config rather than accepting it in MotionPlannerCfg.create(). planner_cfg.trajopt_solver_config.interpolation_dt = float( @@ -1160,6 +1350,7 @@ def _build_backend( ) self._validate_base_link_name(profile, planner) tool_frame = self._resolve_tool_frame(profile, planner) + self._validate_runtime_sphere_obstacles(planner, expected_sphere_names) except Exception: self._close_planner(planner) raise @@ -1174,6 +1365,129 @@ def _build_backend( planning_mode=planning_mode, ) + def _prepare_runtime_scene_model( + self, + scene_model: str | list[dict] | None, + collision_cache: dict[str, int | dict[str, int | float | list[float]]] | None, + ) -> tuple["Any", dict | None, list[list[str]]]: + """Inspect sphere obstacles before cuRobo constructs its runtime scene. + + cuRobo V2 exposes ``Sphere`` in its public scene model, but its world + collision data omits sphere storage. EmbodiChain registers analytic + sphere storage with the generic Warp checker before this method runs; + this inspection records the names that must reach that storage. + + Returns: + Runtime scene model, collision cache, and sphere names expected in + each collision environment. + """ + if scene_model is None: + return None, collision_cache, [] + + raw_scene_model: "Any" = scene_model + if isinstance(scene_model, str): + scene_path = Path(scene_model) + if not scene_path.is_absolute(): + content_mod = importlib.import_module("curobo.content") + scene_path = Path(content_mod.get_scene_configs_path()) / scene_path + try: + with scene_path.open(encoding="utf-8") as scene_file: + raw_scene_model = yaml.safe_load(scene_file) + except (OSError, yaml.YAMLError) as exc: + raise ValueError( + f"Unable to load cuRobo V2 scene configuration " + f"'{scene_model}': {exc}" + ) from exc + + raw_scenes = ( + raw_scene_model if isinstance(raw_scene_model, list) else [raw_scene_model] + ) + parsed_scenes = [ + self._bindings.Scene.create(scene) if isinstance(scene, dict) else scene + for scene in raw_scenes + ] + expected_names = [ + [sphere.name for sphere in (scene.sphere or [])] for scene in parsed_scenes + ] + if not any(expected_names): + return scene_model, collision_cache, [] + + sphere_count = sum(len(names) for names in expected_names) + logger.log_info( + f"Loaded {sphere_count} cuRobo scene sphere(s) into EmbodiChain's " + "analytic sphere collision storage." + ) + return scene_model, collision_cache, expected_names + + @staticmethod + def _validate_runtime_sphere_obstacles( + planner: "Any", expected_names: list[list[str]] + ) -> None: + """Ensure every sphere reached cuRobo's collision checker.""" + if not expected_names: + return + checker = planner.scene_collision_checker + for env_idx, names in enumerate(expected_names): + loaded_names = set(checker.get_obstacle_names(env_idx)) + missing = sorted(set(names) - loaded_names) + if missing: + raise RuntimeError( + "cuRobo parsed sphere obstacles but did not register them " + f"in environment {env_idx}: {missing}." + ) + + @staticmethod + def _load_runtime_robot_config(robot_config_path: str) -> dict: + """Load robot YAML and add cuRobo 0.8's required empty placeholders. + + Self-collision metadata is intentionally absent from EmbodiChain's + generated cache. cuRobo 0.8 nevertheless builds internal sphere-pair + bookkeeping while loading robot spheres, even when + ``self_collision_check=False``, and assumes these two values are + mappings. Supplying empty runtime-only mappings avoids its ``None`` + dereference without restoring self-collision checking or persisting + self-collision configuration. + """ + with open(robot_config_path, encoding="utf-8") as robot_config_file: + robot_config = yaml.safe_load(robot_config_file) + kinematics = robot_config["robot_cfg"]["kinematics"] + kinematics["self_collision_buffer"] = {} + kinematics["self_collision_ignore"] = {} + return robot_config + + @staticmethod + def _disable_curobo_self_collision_rollouts(planner_cfg: "Any") -> None: + """Disable self-collision in every cuRobo 0.8 rollout configuration. + + cuRobo 0.8's ``MotionPlannerCfg.create(self_collision_check=False)`` + disables the constraint in optimizer rollouts, but does not propagate + the flag to the IK/TrajOpt metrics rollouts or the PRM graph rollout. + Those metrics can therefore reject a converged solution as infeasible. + Apply the same public cost-manager switch to all rollout variants + before the planner materializes them. + """ + rollouts: list[Any] = [] + for solver_attr in ("ik_solver_config", "trajopt_solver_config"): + solver_cfg = getattr(planner_cfg, solver_attr, None) + core_cfg = getattr(solver_cfg, "core_cfg", None) + metrics_rollout = getattr(core_cfg, "metrics_rollout_config", None) + if metrics_rollout is not None: + rollouts.append(metrics_rollout) + rollouts.extend(getattr(core_cfg, "optimizer_rollout_configs", None) or []) + + graph_cfg = getattr(planner_cfg, "graph_planner_config", None) + graph_rollout = getattr(graph_cfg, "rollout_config", None) + if graph_rollout is not None: + rollouts.append(graph_rollout) + + visited: set[int] = set() + for rollout in rollouts: + if id(rollout) in visited: + continue + visited.add(id(rollout)) + for cost_cfg in rollout.get_cost_manager_configs(): + cost_cfg.disable_self_collision() + def _warmup_backend(self, backend: "_CuroboBackend") -> None: """Warm one goal type without forcing cuRobo to reset captured graphs. @@ -1468,7 +1782,6 @@ def _auto_generate_robot_yaml( cache_path, tool_frame=tool_frame, urdf_path=urdf_path, - fit_type=auto.fit_type, num_spheres=auto.num_spheres, sphere_density=auto.sphere_density, surface_radius=auto.surface_radius, @@ -1495,7 +1808,6 @@ def _robot_yaml_cache_key( pass hasher.update(control_part.encode("utf-8")) hasher.update((tool_frame or "").encode("utf-8")) - hasher.update(auto.fit_type.encode("utf-8")) hasher.update(str(auto.num_spheres).encode("utf-8")) hasher.update(str(auto.sphere_density).encode("utf-8")) hasher.update(str(auto.surface_radius).encode("utf-8")) @@ -1538,7 +1850,6 @@ def _auto_generate_world_yaml(self, world_cfg: CuroboWorldCfg) -> str: rigid_objects, cache_path, representation=world_cfg.obstacle_representation, - fit_type=auto.fit_type, num_spheres=auto.num_spheres, sphere_density=auto.sphere_density, surface_radius=auto.surface_radius, @@ -1557,7 +1868,7 @@ def _world_yaml_cache_key(self, world_cfg: CuroboWorldCfg) -> str: hasher = hashlib.md5() hasher.update(world_cfg.obstacle_representation.encode("utf-8")) auto = self.cfg.auto_gen - hasher.update(auto.fit_type.encode("utf-8")) + hasher.update(_CUROBO_ROBOT_YAML_GENERATOR_VERSION.encode("utf-8")) hasher.update(str(auto.num_spheres).encode("utf-8")) hasher.update(str(auto.sphere_density).encode("utf-8")) hasher.update(str(auto.surface_radius).encode("utf-8")) @@ -1576,6 +1887,49 @@ def _world_yaml_cache_key(self, world_cfg: CuroboWorldCfg) -> str: hasher.update(pose.detach().to("cpu").to(torch.float32).numpy().tobytes()) return hasher.hexdigest() + def visualize_collision_models( + self, + control_part: str, + env_id: int = 0, + ) -> None: + """Visualize cached robot and obstacle spheres at their simulator poses. + + This materializes the same content-addressed YAML caches used by the + planner, then reads sphere centers and radii back from those files. The + robot spheres are transformed by each link's live + :meth:`~embodichain.lab.sim.objects.Articulation.get_link_pose`; static + obstacle spheres retain the exact world positions serialized in the + world cache. + + Args: + control_part: Robot control part whose cuRobo profile/cache is used. + env_id: Simulator environment instance to visualize. + + Raises: + ValueError: If obstacles are configured with a non-sphere + representation. + """ + from .curobo_yaml import visualize_curobo_collision_models + + profile = self._materialize_profile(control_part) + world_cfg = self.cfg.world + rigid_objects = world_cfg.rigid_objects + world_yaml_path = None + if rigid_objects: + if world_cfg.obstacle_representation != "sphere": + raise ValueError( + "Obstacle collision-model visualization requires " + "CuroboWorldCfg.obstacle_representation='sphere'." + ) + world_yaml_path = self._auto_generate_world_yaml(world_cfg) + visualize_curobo_collision_models( + self.robot, + profile.robot_config_path, + rigid_objects, + world_yaml_path, + env_id, + ) + def _resolve_sim_joint_names(self, control_part: str) -> list[str]: """Return simulator control-part joints in the robot's canonical order.""" control_parts = getattr(self.robot, "control_parts", None) diff --git a/embodichain/lab/sim/planners/curobo/curobo_sphere_data.py b/embodichain/lab/sim/planners/curobo/curobo_sphere_data.py new file mode 100644 index 000000000..ad4f34b09 --- /dev/null +++ b/embodichain/lab/sim/planners/curobo/curobo_sphere_data.py @@ -0,0 +1,275 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# 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. +# ---------------------------------------------------------------------------- + +"""Analytic sphere-obstacle storage for cuRobo V2's generic Warp checker.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import TYPE_CHECKING + +import torch +import warp as wp + +from curobo._src.geom.data.helper_pose import ( + get_obs_idx, + load_transform_from_inv_pose, +) +from curobo._src.util.logging import log_and_raise + +if TYPE_CHECKING: + from curobo._src.geom.types import SceneCfg, Sphere + from curobo._src.types.device_cfg import DeviceCfg + from curobo._src.types.pose import Pose + +__all__ = [ + "SphereData", + "SphereDataWarp", + "compute_local_sdf", + "compute_local_sdf_with_grad", + "is_obs_enabled", + "load_obstacle_transform", +] + +_SDF_EPS = 1.0e-8 + + +@wp.struct +class SphereDataWarp: + """Warp view of batched analytic sphere obstacles.""" + + radius: wp.array(dtype=wp.float32) + inv_pose: wp.array2d(dtype=wp.float32) + enable: wp.array(dtype=wp.uint8) + n_per_env: wp.array(dtype=wp.int32) + max_n: wp.int32 + num_envs: wp.int32 + + +@dataclass +class SphereData: + """GPU tensor storage for analytic sphere obstacles.""" + + radius: torch.Tensor + inv_pose: torch.Tensor + enable: torch.Tensor + count: torch.Tensor + names: list[list[str | None]] + max_n: int + num_envs: int + device_cfg: "DeviceCfg" + + @classmethod + def create_cache( + cls, max_n: int, num_envs: int, device_cfg: "DeviceCfg" + ) -> "SphereData": + """Create an empty fixed-capacity sphere cache.""" + radius = torch.zeros( + (num_envs, max_n), + dtype=device_cfg.dtype, + device=device_cfg.device, + ) + inv_pose = torch.zeros( + (num_envs, max_n, 8), + dtype=device_cfg.dtype, + device=device_cfg.device, + ) + inv_pose[..., 3] = 1.0 + enable = torch.zeros( + (num_envs, max_n), dtype=torch.uint8, device=device_cfg.device + ) + count = torch.zeros((num_envs,), dtype=torch.int32, device=device_cfg.device) + return cls( + radius=radius, + inv_pose=inv_pose, + enable=enable, + count=count, + names=[[None for _ in range(max_n)] for _ in range(num_envs)], + max_n=max_n, + num_envs=num_envs, + device_cfg=device_cfg, + ) + + @classmethod + def from_scene_cfg( + cls, + scene_cfg: "SceneCfg", + device_cfg: "DeviceCfg", + env_idx: int = 0, + num_envs: int = 1, + max_n: int | None = None, + ) -> "SphereData": + """Create storage from one cuRobo scene.""" + spheres = scene_cfg.sphere or [] + capacity = max_n if max_n is not None else max(len(spheres), 1) + instance = cls.create_cache(capacity, num_envs, device_cfg) + if spheres: + instance.load_batch(spheres, env_idx) + return instance + + @classmethod + def from_batch_scene_cfg( + cls, + scene_cfg_list: list["SceneCfg"], + device_cfg: "DeviceCfg", + max_n: int | None = None, + ) -> "SphereData": + """Create storage from independent batched scenes.""" + num_envs = len(scene_cfg_list) + counts = [len(scene.sphere or []) for scene in scene_cfg_list] + capacity = max_n if max_n is not None else max(max(counts), 1) + instance = cls.create_cache(capacity, num_envs, device_cfg) + for env_idx, scene in enumerate(scene_cfg_list): + if scene.sphere: + instance.load_batch(scene.sphere, env_idx) + return instance + + def load_batch(self, spheres: list["Sphere"], env_idx: int) -> None: + """Replace one environment's sphere obstacles.""" + if len(spheres) > self.max_n: + log_and_raise( + f"Cannot load {len(spheres)} spheres, max cache size is {self.max_n}" + ) + if not spheres: + self.clear(env_idx) + return + num_spheres = len(spheres) + centers = torch.as_tensor( + [sphere.pose[:3] for sphere in spheres], + dtype=self.device_cfg.dtype, + device=self.device_cfg.device, + ) + inverse_poses = torch.zeros( + (num_spheres, 7), + dtype=self.device_cfg.dtype, + device=self.device_cfg.device, + ) + # Sphere orientation is immaterial. Identity rotation plus translated + # origin is the exact world-to-local transform and avoids cuRobo's + # CUDA-only generic pose-inverse kernel. + inverse_poses[:, :3] = -centers + inverse_poses[:, 3] = 1.0 + self.radius[env_idx, :num_spheres] = torch.as_tensor( + [sphere.radius for sphere in spheres], + dtype=self.device_cfg.dtype, + device=self.device_cfg.device, + ) + self.inv_pose[env_idx, :num_spheres, :7] = inverse_poses + self.enable[env_idx, :num_spheres] = 1 + self.enable[env_idx, num_spheres:] = 0 + self.names[env_idx][:num_spheres] = [sphere.name for sphere in spheres] + self.names[env_idx][num_spheres:] = [None] * (self.max_n - num_spheres) + self.count[env_idx] = num_spheres + + def update_pose(self, name: str, pose: Pose, env_idx: int = 0) -> None: + """Update a named sphere pose.""" + idx = self.get_idx(name, env_idx) + position = pose.position.reshape(-1, 3)[0].to(self.inv_pose) + self.inv_pose[env_idx, idx, :3] = -position + self.inv_pose[env_idx, idx, 3:7] = torch.as_tensor( + [1.0, 0.0, 0.0, 0.0], + dtype=self.device_cfg.dtype, + device=self.device_cfg.device, + ) + + def set_enabled(self, name: str, enabled: bool, env_idx: int = 0) -> None: + """Enable or disable a named sphere.""" + self.enable[env_idx, self.get_idx(name, env_idx)] = int(enabled) + + def get_idx(self, name: str, env_idx: int = 0) -> int: + """Return a named sphere's local index.""" + try: + return self.names[env_idx].index(name) + except ValueError: + log_and_raise( + f"Sphere with name '{name}' not found in environment {env_idx}" + ) + raise AssertionError("unreachable") + + def get_names(self, env_idx: int = 0) -> list[str]: + """Return active sphere names.""" + return self.names[env_idx][: int(self.count[env_idx].item())] + + def clear(self, env_idx: int | None = None) -> None: + """Disable all spheres in one or every environment.""" + if env_idx is None: + self.enable.zero_() + self.count.zero_() + self.names = [ + [None for _ in range(self.max_n)] for _ in range(self.num_envs) + ] + else: + self.enable[env_idx].zero_() + self.count[env_idx] = 0 + self.names[env_idx] = [None for _ in range(self.max_n)] + + def to_warp(self) -> SphereDataWarp: + """Return the Warp view consumed by cuRobo's generic kernels.""" + data = SphereDataWarp() + data.radius = wp.from_torch(self.radius.view(-1), dtype=wp.float32) + data.inv_pose = wp.from_torch(self.inv_pose.view(-1, 8), dtype=wp.float32) + data.enable = wp.from_torch(self.enable.view(-1), dtype=wp.uint8) + data.n_per_env = wp.from_torch(self.count.view(-1), dtype=wp.int32) + data.max_n = self.max_n + data.num_envs = self.num_envs + return data + + +def is_obs_enabled( + obs_set: SphereDataWarp, env_idx: wp.int32, local_idx: wp.int32 +) -> wp.bool: + """Return whether a sphere slot is active.""" + flat_idx = get_obs_idx(env_idx, local_idx, obs_set.max_n) + return obs_set.enable[flat_idx] == wp.uint8(1) + + +def load_obstacle_transform( + obs_set: SphereDataWarp, env_idx: wp.int32, local_idx: wp.int32 +) -> wp.transform: + """Load a sphere's world-to-local transform.""" + flat_idx = get_obs_idx(env_idx, local_idx, obs_set.max_n) + return load_transform_from_inv_pose(obs_set.inv_pose, flat_idx) + + +def compute_local_sdf( + obs_set: SphereDataWarp, + env_idx: wp.int32, + local_idx: wp.int32, + local_pt: wp.vec3, +) -> wp.float32: + """Return analytic signed distance to a sphere surface.""" + flat_idx = get_obs_idx(env_idx, local_idx, obs_set.max_n) + return wp.length(local_pt) - obs_set.radius[flat_idx] + + +def compute_local_sdf_with_grad( + obs_set: SphereDataWarp, + env_idx: wp.int32, + local_idx: wp.int32, + local_pt: wp.vec3, +) -> wp.vec4: + """Return analytic sphere SDF and negative spatial gradient.""" + flat_idx = get_obs_idx(env_idx, local_idx, obs_set.max_n) + distance = wp.length(local_pt) + gx = wp.float32(0.0) + gy = wp.float32(0.0) + gz = wp.float32(0.0) + if distance > _SDF_EPS: + inverse_distance = -1.0 / distance + gx = local_pt[0] * inverse_distance + gy = local_pt[1] * inverse_distance + gz = local_pt[2] * inverse_distance + return wp.vec4(distance - obs_set.radius[flat_idx], gx, gy, gz) diff --git a/embodichain/lab/sim/planners/curobo/curobo_yaml.py b/embodichain/lab/sim/planners/curobo/curobo_yaml.py index 1b24eec68..9e2b0f175 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_yaml.py +++ b/embodichain/lab/sim/planners/curobo/curobo_yaml.py @@ -17,7 +17,7 @@ The :func:`generate_curobo_robot_yaml` helper pulls the robot's URDF path and each link's collision mesh (vertices/faces) from the simulator, fits collision -spheres to every link mesh with cuRobo's sphere-fitting library, and writes a +spheres to every link mesh with DexSim's sphere-fitting library, and writes a complete cuRobo V2 robot configuration YAML. The cuRobo planner adapter calls this automatically (with on-disk caching) on the first plan; see :class:`~embodichain.lab.sim.planners.curobo.curobo_planner.CuroboAutoGenCfg`. @@ -28,7 +28,7 @@ from __future__ import annotations -from typing import TYPE_CHECKING, Sequence +from typing import TYPE_CHECKING, Any, Sequence import torch @@ -38,7 +38,17 @@ if TYPE_CHECKING: from embodichain.lab.sim.objects import RigidObject, Robot -__all__ = ["generate_curobo_robot_yaml", "generate_curobo_world_yaml"] +__all__ = [ + "generate_curobo_robot_yaml", + "generate_curobo_world_yaml", + "visualize_curobo_collision_models", + "visualize_curobo_robot_collision_model", + "visualize_curobo_world_collision_model", +] + + +_ROBOT_MAX_CONVEX_HULL_NUM = 2 +_OBSTACLE_MAX_CONVEX_HULL_NUM = 16 def _parse_mimic_joint_names(urdf_path: str) -> set[str]: @@ -79,6 +89,34 @@ def _parse_mimic_joint_names(urdf_path: str) -> set[str]: return mimic_joints +def _to_open3d_legacy_mesh( + vertices: torch.Tensor, + faces: torch.Tensor, + o3d: Any, +) -> Any: + """Create a legacy Open3D triangle mesh from tensor-like geometry.""" + mesh = o3d.geometry.TriangleMesh() + mesh.vertices = o3d.utility.Vector3dVector( + torch.as_tensor(vertices).detach().to(torch.float64).cpu().numpy() + ) + mesh.triangles = o3d.utility.Vector3iVector( + torch.as_tensor(faces).detach().to(torch.int32).cpu().reshape(-1, 3).numpy() + ) + mesh.compute_vertex_normals() + return mesh + + +def _to_open3d_tensor_mesh( + vertices: torch.Tensor, + faces: torch.Tensor, + o3d: Any, +) -> Any: + """Create the Open3D tensor mesh expected by DexSim's ``sphere_fit``.""" + return o3d.t.geometry.TriangleMesh.from_legacy( + _to_open3d_legacy_mesh(vertices, faces, o3d) + ) + + def generate_curobo_robot_yaml( robot: Robot, control_part: str, @@ -86,7 +124,6 @@ def generate_curobo_robot_yaml( *, tool_frame: str | None = None, urdf_path: str | None = None, - fit_type: str = "morphit", num_spheres: int | None = None, sphere_density: float = 1.0, surface_radius: float = 0.005, @@ -99,12 +136,12 @@ def generate_curobo_robot_yaml( """Fit collision spheres to each robot link's mesh and write a cuRobo robot YAML. Extracts the URDF path and per-link vertices/faces from ``robot``, fits - collision spheres to every link mesh with cuRobo's :func:`fit_spheres_to_mesh`, + collision spheres to every link mesh with DexSim's :func:`sphere_fit`, and writes a complete cuRobo V2 robot configuration YAML that the cuRobo planner loads as its robot model. .. attention:: - Requires a CUDA GPU and cuRobo installed (sphere fitting runs on GPU). + Requires a CUDA GPU, DexSim, Open3D, and cuRobo (sphere fitting runs on GPU). Link meshes from ``robot.get_link_vert_face`` are assumed to be in the link-local rest frame -- the convention cuRobo collision spheres use, since cuRobo applies each link's transform via FK at runtime. @@ -123,14 +160,12 @@ def generate_curobo_robot_yaml( cache key and the generation use the same file). Must be the full assembled URDF, not a solver's sub-chain URDF, or gripper links are silently dropped from the collision model. - fit_type: cuRobo sphere-fit strategy - ``"morphit"`` (default, best), - ``"voxel"`` (faster), or ``"surface"`` (crude, fixed radius). - num_spheres: Per-link sphere count. If ``None``, cuRobo auto-estimates + num_spheres: Per-link sphere count. If ``None``, DexSim auto-estimates it from the link's bounding-box volume. sphere_density: Multiplier on the auto sphere count (ignored when ``num_spheres`` is set). - surface_radius: Fixed radius used only by the ``surface`` strategy. - iterations: Adam iterations for the ``morphit`` strategy. + surface_radius: Fixed radius used by MorphIt's surface fallback. + iterations: Adam iterations for MorphIt. collision_sphere_buffer: Padding added to every sphere's radius (m). max_acceleration: cspace maximum acceleration. max_jerk: cspace maximum jerk. @@ -140,33 +175,19 @@ def generate_curobo_robot_yaml( The ``output_path`` that was written. Raises: - ImportError: If cuRobo or trimesh is not installed. + ImportError: If DexSim or Open3D is not installed. RuntimeError: If CUDA is unavailable or no spheres could be fitted. """ import os - import trimesh + import open3d as o3d import yaml - from curobo._src.geom.sphere_fit.fit_spheres import fit_spheres_to_mesh - from curobo._src.geom.sphere_fit.types import SphereFitType + from dexsim.kit.meshproc import SphereFitType, sphere_fit from curobo._src.robot.parser.parser_urdf import UrdfRobotParser - from curobo.types import DeviceCfg if not torch.cuda.is_available(): raise RuntimeError("generate_curobo_robot_yaml requires a CUDA GPU.") - fit_type_map = { - "morphit": SphereFitType.MORPHIT, - "voxel": SphereFitType.VOXEL, - "surface": SphereFitType.SURFACE, - } - if fit_type not in fit_type_map: - raise ValueError( - f"fit_type must be one of {list(fit_type_map)}, got {fit_type!r}." - ) - fit_type_enum = fit_type_map[fit_type] - device_cfg = DeviceCfg(device=device) - urdf_path = urdf_path or robot.cfg.fpath link_vert_dict: dict = {} link_face_dict: dict = {} @@ -175,32 +196,20 @@ def generate_curobo_robot_yaml( link_vert_dict[link_name] = verts link_face_dict[link_name] = faces - # 1. Parse the URDF kinematic tree (no meshes) for base_link + parent map. + # 1. Parse the URDF kinematic tree (no meshes) for the base link. # ``robot.root_link_name`` is avoided because it touches an uninitialized # ``entities`` attribute on some Robot instances; cuRobo's parser resolves # the root link directly from the URDF. # Mimic joints are detected from the URDF XML (not cuRobo's parser, which # exposes no mimic accessor) so they can be excluded from cspace/lock_joints - # in step 4/5 - cuRobo folds them into their active joint and raises + # below - cuRobo folds them into their active joint and raises # KeyError if they are locked. mimic_joints: set[str] = _parse_mimic_joint_names(urdf_path) base_link: str | None = None - urdf_parent_map: dict[str, str | None] = {} try: parser = UrdfRobotParser(urdf_path, load_meshes=False, build_scene_graph=True) parser.build_link_parent() base_link = parser.root_link - # Build the full parent map for every URDF link so self_collision_ignore - # can walk multiple hops (the parent of a non-collision link still - # connects two collision links, e.g. fr3_link8 between fr3_link7 and - # fr3_hand). - for link_name in parser.get_link_names_from_urdf(): - try: - urdf_parent_map[link_name] = parser.get_link_parameters( - link_name - ).parent_link_name - except Exception: # noqa: BLE001 (e.g. root link has no parent entry) - urdf_parent_map[link_name] = None except Exception as exc: # noqa: BLE001 logger.log_warning(f"Could not parse URDF kinematic tree ({exc}).") if base_link is None: @@ -215,35 +224,28 @@ def generate_curobo_robot_yaml( faces = link_face_dict[link_name] if verts is None or faces is None or verts.numel() == 0 or faces.numel() == 0: continue - verts_np = torch.as_tensor(verts).detach().to(torch.float32).cpu().numpy() - faces_np = torch.as_tensor(faces).detach().to(torch.int64).cpu().numpy() - mesh = trimesh.Trimesh(vertices=verts_np, faces=faces_np, process=False) - if len(mesh.vertices) == 0: - continue - mesh.fill_holes() - trimesh.repair.fix_normals(mesh) - trimesh.repair.fix_inversion(mesh) - trimesh.repair.fix_winding(mesh) + mesh = _to_open3d_tensor_mesh(verts, faces, o3d) try: - fit_result = fit_spheres_to_mesh( + is_success, centers, radii = sphere_fit( mesh, num_spheres=num_spheres, sphere_density=sphere_density, surface_radius=surface_radius, - fit_type=fit_type_enum, + fit_type=SphereFitType.MORPHIT, iterations=iterations, - device_cfg=device_cfg, + max_convex_hull_num=_ROBOT_MAX_CONVEX_HULL_NUM, + device=device, ) except Exception as exc: # noqa: BLE001 logger.log_warning(f"Sphere fitting failed for link {link_name!r}: {exc}") continue - if fit_result.num_spheres == 0: + if not is_success: continue collision_spheres[link_name] = [ {"center": list(c), "radius": float(r)} for c, r in zip( - fit_result.centers.detach().cpu().tolist(), - fit_result.radii.detach().cpu().tolist(), + centers.detach().cpu().tolist(), + radii.detach().cpu().tolist(), ) ] @@ -253,42 +255,7 @@ def generate_curobo_robot_yaml( ) collision_link_names = list(collision_spheres.keys()) - # 3. self_collision_ignore: ignore link pairs within two kinematic hops - # (parent/grandparent, children/grandchildren, siblings). cuRobo's curated - # profiles (e.g. franka.yml) ignore adjacent-plus-near links because their - # spheres physically overlap near joints; a neighbor-only matrix leaves - # those pairs colliding and makes reachable start poses fail validation. - self_collision_ignore: dict[str, list[str]] = {} - if urdf_parent_map: - children_map: dict[str, list[str]] = {} - for link_name, parent in urdf_parent_map.items(): - if parent is not None: - children_map.setdefault(parent, []).append(link_name) - collision_set = set(collision_link_names) - - def _two_hop_neighbors(link: str) -> set[str]: - neighbors: set[str] = set() - parent = urdf_parent_map.get(link) - if parent is not None: - neighbors.add(parent) - grandparent = urdf_parent_map.get(parent) - if grandparent is not None: - neighbors.add(grandparent) - for sibling in children_map.get(parent, []): - if sibling != link: - neighbors.add(sibling) - for child in children_map.get(link, []): - neighbors.add(child) - for grandchild in children_map.get(child, []): - neighbors.add(grandchild) - return neighbors - - for link_name in collision_link_names: - self_collision_ignore[link_name] = [ - n for n in _two_hop_neighbors(link_name) if n in collision_set - ] - - # 4. cspace from the robot's joints + init qpos. Mimic joints are excluded - + # 3. cspace from the robot's joints + init qpos. Mimic joints are excluded - # cuRobo drives them from their active joint and rejects them in cspace. joint_names = list(robot.joint_names) init_qpos = list(robot.cfg.init_qpos) if robot.cfg.init_qpos is not None else [] @@ -314,14 +281,14 @@ def _two_hop_neighbors(link: str) -> set[str]: "null_space_weight": [1.0] * len(cspace_pairs), } - # 5. lock_joints: actuated joints outside the control part, pinned to init values. - # Mimic joints are already excluded from cspace_pairs (see step 4). + # 4. lock_joints: actuated joints outside the control part, pinned to init values. + # Mimic joints are already excluded from cspace_pairs (see step 3). control_joints = set((robot.control_parts or {}).get(control_part, [])) lock_joints: dict[str, float] = { jname: val for jname, val in cspace_pairs if jname not in control_joints } - # 6. tool_frames default to the last link of the control part. + # 5. tool_frames default to the last link of the control part. if tool_frame is None: part_links = robot.get_control_part_link_names(control_part) if not part_links: @@ -330,7 +297,7 @@ def _two_hop_neighbors(link: str) -> set[str]: ) tool_frame = part_links[-1] - # 7. Assemble and write the YAML, mirroring franka.yml's schema. + # 6. Assemble and write the YAML, mirroring franka.yml's schema. data = { "robot_cfg": { "kinematics": { @@ -343,8 +310,6 @@ def _two_hop_neighbors(link: str) -> set[str]: "collision_spheres": collision_spheres, "collision_sphere_buffer": float(collision_sphere_buffer), "mesh_link_names": collision_link_names, - "self_collision_buffer": {ln: 0.0 for ln in collision_link_names}, - "self_collision_ignore": self_collision_ignore, "lock_joints": lock_joints, "cspace": cspace, "use_global_cumul": True, @@ -373,7 +338,6 @@ def _mesh_to_obstacle_entry( pose: torch.Tensor, *, representation: str = "cuboid", - fit_type: str = "voxel", num_spheres: int | None = None, sphere_density: float = 1.0, surface_radius: float = 0.005, @@ -383,9 +347,8 @@ def _mesh_to_obstacle_entry( ) -> list[tuple[str, str, dict]]: """Convert one mesh + pose into cuRobo world-YAML obstacle entry/entries. - Pure tensor helper (no simulator / cuRobo import for ``cuboid``/``mesh``) so - it is unit-testable without CUDA. ``sphere`` lazily imports cuRobo + trimesh - and runs on CUDA. + Pure tensor helper (no simulator import for ``cuboid``/``mesh``) so it is + unit-testable without CUDA. ``sphere`` lazily imports DexSim + Open3D. Args: name: Obstacle name (cuRobo key under ``cuboid``/``mesh``/``sphere``). @@ -396,13 +359,11 @@ def _mesh_to_obstacle_entry( frame (the same frame static collision YAMLs are authored in). representation: ``"cuboid"`` (local-frame AABB -> OBB via ``pose``, default), ``"mesh"`` (exact triangle mesh), or ``"sphere"`` (fit - spheres with cuRobo's :func:`fit_spheres_to_mesh`). - fit_type: cuRobo sphere-fit strategy (``"voxel"``/``"morphit"``/ - ``"surface"``); only used by ``"sphere"``. + spheres with DexSim's :func:`sphere_fit`). num_spheres: Per-mesh sphere count; ``None`` auto-estimates (sphere only). sphere_density: Multiplier on the auto sphere count (sphere only). - surface_radius: Fixed radius for the ``"surface"`` strategy (sphere only). - iterations: Adam iterations for ``"morphit"`` (sphere only). + surface_radius: Fixed radius for MorphIt's surface fallback (sphere only). + iterations: Adam iterations for MorphIt (sphere only). collision_sphere_buffer: Padding added to each fitted radius (sphere only). device: CUDA device for sphere fitting (sphere only). @@ -414,7 +375,7 @@ def _mesh_to_obstacle_entry( ValueError: If ``representation`` is unsupported, ``pose`` is malformed, or the mesh has no geometry for the requested representation. RuntimeError: If ``"sphere"`` is requested without CUDA. - ImportError: If ``"sphere"`` is requested without cuRobo/trimesh. + ImportError: If ``"sphere"`` is requested without DexSim/Open3D. """ if representation not in _REPRESENTATIONS: raise ValueError( @@ -476,49 +437,29 @@ def _mesh_to_obstacle_entry( ) if not torch.cuda.is_available(): raise RuntimeError( - "The 'sphere' representation requires CUDA for cuRobo sphere fitting." + "The 'sphere' representation requires CUDA for DexSim MorphIt fitting." ) - import trimesh + import open3d as o3d - from curobo._src.geom.sphere_fit.fit_spheres import fit_spheres_to_mesh - from curobo._src.geom.sphere_fit.types import SphereFitType - from curobo.types import DeviceCfg + from dexsim.kit.meshproc import SphereFitType, sphere_fit - fit_type_map = { - "morphit": SphereFitType.MORPHIT, - "voxel": SphereFitType.VOXEL, - "surface": SphereFitType.SURFACE, - } - if fit_type not in fit_type_map: - raise ValueError( - f"fit_type must be one of {list(fit_type_map)}, got {fit_type!r}." - ) - mesh = trimesh.Trimesh( - vertices=vertices.numpy(), - faces=faces.reshape(-1, 3).to(torch.int64).numpy(), - process=False, - ) - mesh.fill_holes() - trimesh.repair.fix_normals(mesh) - trimesh.repair.fix_inversion(mesh) - trimesh.repair.fix_winding(mesh) - fit_result = fit_spheres_to_mesh( + mesh = _to_open3d_tensor_mesh(vertices, faces, o3d) + is_success, centers, fitted_radii = sphere_fit( mesh, num_spheres=num_spheres, sphere_density=sphere_density, surface_radius=surface_radius, - fit_type=fit_type_map[fit_type], + fit_type=SphereFitType.MORPHIT, iterations=iterations, - device_cfg=DeviceCfg(device=device), + max_convex_hull_num=_OBSTACLE_MAX_CONVEX_HULL_NUM, + device=device, ) - if fit_result.num_spheres == 0: + if not is_success: raise RuntimeError(f"No spheres could be fitted for object {name!r}.") - centers_local = ( - fit_result.centers.detach().to("cpu").reshape(-1, 3).to(torch.float32) - ) - radii = fit_result.radii.detach().to("cpu").reshape(-1).to(torch.float32) + float( + centers_local = centers.detach().to("cpu").reshape(-1, 3).to(torch.float32) + radii = fitted_radii.detach().to("cpu").reshape(-1).to(torch.float32) + float( collision_sphere_buffer ) rotation = matrix_from_quat(pose[3:7]) @@ -544,7 +485,6 @@ def generate_curobo_world_yaml( *, representation: str = "cuboid", env_id: int = 0, - fit_type: str = "voxel", num_spheres: int | None = None, sphere_density: float = 1.0, surface_radius: float = 0.005, @@ -571,15 +511,14 @@ def generate_curobo_world_yaml( rigid_objects: ``RigidObject`` instances to bake into the collision world. output_path: Destination YAML file path. representation: ``"cuboid"`` (default, AABB->OBB, no CUDA), ``"mesh"`` - (exact triangle mesh, no CUDA), or ``"sphere"`` (cuRobo sphere fit, - requires CUDA + cuRobo + trimesh). + (exact triangle mesh, no CUDA), or ``"sphere"`` (DexSim MorphIt + sphere fit, requiring CUDA + DexSim + Open3D). env_id: Environment instance index to read geometry/pose from (the static world is shared, so env 0 is representative). - fit_type: cuRobo sphere-fit strategy (sphere representation only). num_spheres: Per-object sphere count; ``None`` auto-estimates (sphere only). sphere_density: Multiplier on the auto sphere count (sphere only). - surface_radius: Fixed radius for the ``"surface"`` strategy (sphere only). - iterations: Adam iterations for ``"morphit"`` (sphere only). + surface_radius: Fixed radius for MorphIt's surface fallback (sphere only). + iterations: Adam iterations for MorphIt (sphere only). collision_sphere_buffer: Padding added to each fitted radius (sphere only). device: CUDA device for sphere fitting (sphere only). @@ -624,7 +563,6 @@ def generate_curobo_world_yaml( faces, pose, representation=representation, - fit_type=fit_type, num_spheres=num_spheres, sphere_density=sphere_density, surface_radius=surface_radius, @@ -644,3 +582,216 @@ def generate_curobo_world_yaml( with open(output_path, "w") as yaml_file: yaml.dump(data, yaml_file, default_flow_style=False, sort_keys=False) return output_path + + +# ============================================================================= +# Cached collision-model visualization +# ============================================================================= + + +def _collision_visualization_geometries( + meshes: list[tuple[str, Any]], + centers: torch.Tensor, + radii: torch.Tensor, + *, + sphere_name: str, + sphere_color: list[float], + mesh_color: list[float], +) -> list[dict[str, Any]]: + """Build Open3D draw entries in the style of DexSim's ``sphere_fit_visual``.""" + import open3d as o3d + + mesh_material = o3d.visualization.rendering.MaterialRecord() + mesh_material.shader = "defaultLit" + mesh_material.base_color = mesh_color + + geometries = [ + {"name": name, "geometry": mesh, "material": mesh_material} + for name, mesh in meshes + ] + + spheres_mesh = o3d.geometry.TriangleMesh() + centers_np = centers.detach().cpu().numpy().reshape(-1, 3) + radii_np = radii.detach().cpu().numpy().reshape(-1) + for center, radius in zip(centers_np, radii_np): + sphere = o3d.geometry.TriangleMesh.create_sphere(float(radius)) + sphere.translate(center) + spheres_mesh += sphere + spheres_mesh.compute_vertex_normals() + + sphere_material = o3d.visualization.rendering.MaterialRecord() + sphere_material.shader = "defaultLitSSR" + sphere_material.base_color = sphere_color + sphere_material.base_roughness = 0.05 + sphere_material.base_reflectance = 0.0 + sphere_material.base_clearcoat = 1.0 + sphere_material.thickness = 1.0 + sphere_material.transmission = 0.2 + sphere_material.absorption_distance = 10.0 + sphere_material.absorption_color = sphere_color[:3] + geometries.append( + { + "name": sphere_name, + "geometry": spheres_mesh, + "material": sphere_material, + } + ) + return geometries + + +def visualize_curobo_robot_collision_model( + robot: Robot, + robot_yaml_path: str, + env_id: int = 0, + *, + draw: bool = True, +) -> list[dict[str, Any]]: + """Visualize a robot's live link meshes and cached collision spheres. + + Sphere centers and radii are always loaded from ``robot_yaml_path``. Each + link-local cached center is transformed by the link's live simulator pose + from :meth:`Articulation.get_link_pose`, making frame errors directly + visible against the corresponding world-space mesh. + + Args: + robot: Live simulator robot. + robot_yaml_path: Cached auto-generated cuRobo robot YAML. + env_id: Simulator environment instance to visualize. + draw: Open an Open3D window immediately. ``False`` returns draw entries + for composition with another collision model. + + Returns: + Open3D geometry dictionaries suitable for :func:`open3d.visualization.draw`. + """ + import open3d as o3d + import yaml + + with open(robot_yaml_path, encoding="utf-8") as yaml_file: + data = yaml.safe_load(yaml_file) + kinematics = data["robot_cfg"]["kinematics"] + cached_spheres = kinematics.get("collision_spheres", {}) + sphere_buffer = float(kinematics.get("collision_sphere_buffer", 0.0)) + + meshes: list[tuple[str, Any]] = [] + world_centers: list[torch.Tensor] = [] + radii: list[float] = [] + for link_name, link_spheres in cached_spheres.items(): + vertices, faces = robot.get_link_vert_face(link_name) + if vertices is None or faces is None or vertices.numel() == 0: + continue + link_pose = torch.as_tensor( + robot.get_link_pose(link_name, env_ids=[env_id], to_matrix=True)[0], + dtype=torch.float32, + ).cpu() + mesh = _to_open3d_legacy_mesh(vertices, faces, o3d) + mesh.transform(link_pose.numpy()) + meshes.append((f"robot_mesh/{link_name}", mesh)) + + centers_local = torch.as_tensor( + [sphere["center"] for sphere in link_spheres], dtype=torch.float32 + ).reshape(-1, 3) + centers_world = centers_local @ link_pose[:3, :3].T + link_pose[:3, 3] + world_centers.extend(centers_world.unbind()) + radii.extend(float(sphere["radius"]) + sphere_buffer for sphere in link_spheres) + + if not world_centers: + raise ValueError( + f"Robot cache {robot_yaml_path!r} contains no collision spheres." + ) + geometries = _collision_visualization_geometries( + meshes, + torch.stack(world_centers), + torch.tensor(radii, dtype=torch.float32), + sphere_name="robot_spheres", + sphere_color=[0.0, 0.2, 0.8, 0.5], + mesh_color=[0.5, 0.5, 0.5, 1.0], + ) + if draw: + o3d.visualization.draw(geometries, title="cuRobo robot collision model") + return geometries + + +def visualize_curobo_world_collision_model( + rigid_objects: Sequence[RigidObject], + world_yaml_path: str, + env_id: int = 0, + *, + draw: bool = True, +) -> list[dict[str, Any]]: + """Visualize live obstacle meshes and spheres loaded from a cached world YAML. + + Args: + rigid_objects: Live simulator obstacles represented by the cache. + world_yaml_path: Cached auto-generated cuRobo world YAML. It must use + the ``sphere`` representation. + env_id: Simulator environment instance whose live meshes are shown. + draw: Open an Open3D window immediately. ``False`` returns draw entries + for composition with the robot collision model. + + Returns: + Open3D geometry dictionaries suitable for :func:`open3d.visualization.draw`. + """ + import open3d as o3d + import yaml + + with open(world_yaml_path, encoding="utf-8") as yaml_file: + data = yaml.safe_load(yaml_file) + sphere_entries = data.get("sphere", {}) if isinstance(data, dict) else {} + if not sphere_entries: + raise ValueError( + f"World cache {world_yaml_path!r} contains no sphere representation." + ) + + meshes: list[tuple[str, Any]] = [] + for idx, obj in enumerate(rigid_objects): + name = getattr(obj, "uid", None) or f"obstacle_{idx}" + vertices = obj.get_vertices(env_ids=[env_id], scale=True)[0] + faces = obj.get_triangles(env_ids=[env_id])[0] + if vertices is None or faces is None or vertices.numel() == 0: + continue + pose = torch.as_tensor( + obj.get_local_pose(to_matrix=True)[env_id], dtype=torch.float32 + ).cpu() + mesh = _to_open3d_legacy_mesh(vertices, faces, o3d) + mesh.transform(pose.numpy()) + meshes.append((f"obstacle_mesh/{name}", mesh)) + + centers = torch.as_tensor( + [entry["position"] for entry in sphere_entries.values()], dtype=torch.float32 + ).reshape(-1, 3) + radii = torch.as_tensor( + [entry["radius"] for entry in sphere_entries.values()], dtype=torch.float32 + ).reshape(-1) + geometries = _collision_visualization_geometries( + meshes, + centers, + radii, + sphere_name="obstacle_spheres", + sphere_color=[0.8, 0.15, 0.0, 0.5], + mesh_color=[0.45, 0.55, 0.45, 1.0], + ) + if draw: + o3d.visualization.draw(geometries, title="cuRobo obstacle collision model") + return geometries + + +def visualize_curobo_collision_models( + robot: Robot, + robot_yaml_path: str, + rigid_objects: Sequence[RigidObject] | None = None, + world_yaml_path: str | None = None, + env_id: int = 0, +) -> None: + """Draw cached robot and obstacle collision spheres in one Open3D window.""" + import open3d as o3d + + geometries = visualize_curobo_robot_collision_model( + robot, robot_yaml_path, env_id, draw=False + ) + if rigid_objects and world_yaml_path is not None: + geometries.extend( + visualize_curobo_world_collision_model( + rigid_objects, world_yaml_path, env_id, draw=False + ) + ) + o3d.visualization.draw(geometries, title="cuRobo collision models") diff --git a/examples/sim/planners/curobo_planner.py b/examples/sim/planners/curobo_planner.py index 452e376d6..86c216ccd 100644 --- a/examples/sim/planners/curobo_planner.py +++ b/examples/sim/planners/curobo_planner.py @@ -457,11 +457,23 @@ def _build_scene( if robot is None: raise RuntimeError(f"Failed to add robot '{robot_type}' to the cuRobo demo.") target_xpos = _resolve_batched_target(target_xpos, robot.num_instances) - if robot_type == "w1": - # Keep the W1-specific IK diagnostic batched so it remains useful when - # checking solver and cuRobo reachability across multiple environments. - is_success, ik_qpos = robot.compute_ik(pose=target_xpos, name=control_part) - print(f"robot compute ik success: {is_success}, ik_qpos: {ik_qpos}") + # if robot_type == "w1": + # Keep the W1-specific IK diagnostic batched so it remains useful when + # checking solver and cuRobo reachability across multiple environments. + # import ipdb; ipdb.set_trace() + init_qpos = torch.tensor( + robot.cfg.init_qpos, dtype=torch.float32, device=robot.device + ) + arm_init_qpos = ( + init_qpos[robot.get_joint_ids(control_part)] + .unsqueeze(0) + .expand(num_envs, -1) + .clone() + ) + is_success, ik_qpos = robot.compute_ik( + pose=target_xpos, name=control_part, joint_seed=arm_init_qpos + ) + print(f"robot target xpos ik success: {is_success}, ik_qpos: {ik_qpos}") # This object is also exported into the cuRobo collision world below via # CuroboWorldCfg.rigid_objects, so the simulator and planner share geometry @@ -710,6 +722,7 @@ def main() -> None: seed=args.seed, ) use_independent_worlds = args.num_envs > 1 + visualize_collision_models = not args.headless and not use_independent_worlds if use_independent_worlds: for name, poses in obstacle_poses.items(): yaw_deg = torch.rad2deg(torch.atan2(poses[:, 1, 0], poses[:, 0, 0])) @@ -733,7 +746,7 @@ def main() -> None: robot_uid=robot.uid, world=CuroboWorldCfg( rigid_objects=obstacles, - obstacle_representation="cuboid", + obstacle_representation=("sphere"), dynamic_obstacle_names=( [obstacle.uid for obstacle in obstacles] if use_independent_worlds @@ -747,6 +760,11 @@ def main() -> None: ) ) ) + if visualize_collision_models: + # This opens one blocking Open3D window. The spheres are loaded from + # the exact robot/world YAML caches consumed by cuRobo; close the + # window to continue with planner backend creation and execution. + motion_generator.planner.visualize_collision_models(control_part) engine = AtomicActionEngine(motion_generator) engine.register( MoveEndEffector( diff --git a/tests/sim/planners/test_curobo_planner.py b/tests/sim/planners/test_curobo_planner.py index f679809a7..687f8c6a5 100644 --- a/tests/sim/planners/test_curobo_planner.py +++ b/tests/sim/planners/test_curobo_planner.py @@ -27,6 +27,8 @@ import importlib import logging import math +from contextlib import nullcontext +from types import SimpleNamespace import pytest import torch @@ -38,6 +40,7 @@ CuroboPlanner, CuroboPlannerCfg as CuroboPlannerCfgDirect, CuroboWorldCfg, + _CuroboProfile, _configure_curobo_logging, _matrix_to_position_quaternion, _require_curobo, @@ -48,8 +51,12 @@ from embodichain.lab.sim.planners.curobo.curobo_yaml import ( _mesh_to_obstacle_entry, _parse_mimic_joint_names, + generate_curobo_robot_yaml, generate_curobo_world_yaml, + visualize_curobo_robot_collision_model, + visualize_curobo_world_collision_model, ) +from embodichain.lab.sim.planners.utils import MoveType _SIM_ROBOT_UID = "curobo_franka_inprocess_test" _SIM_CONTROL_PART = "arm" @@ -218,11 +225,113 @@ def test_curobo_world_cfg_uses_v2_safe_default_collision_cache(): assert cfg.obstacle_representation == "sphere" -def test_auto_gen_defaults_keep_sphere_count_low(): - """The voxel sphere estimate must be scaled down so planning stays fast.""" +def test_auto_gen_defaults_keep_sphere_count_low_and_fit_type_fixed(): + """MorphIt is fixed by the generator while density remains configurable.""" auto = CuroboPlannerCfg(robot_uid="franka").auto_gen - assert auto.fit_type == "voxel" assert auto.sphere_density == 0.1 + assert not hasattr(auto, "fit_type") + + +def test_runtime_scene_records_spheres_without_changing_yaml_or_cache(tmp_path): + class FakeScene: + def __init__(self, *, sphere=None, mesh=None): + self.sphere = sphere or [] + self.mesh = mesh or [] + + @classmethod + def create(cls, data): + return cls( + sphere=[SimpleNamespace(name=name) for name in data.get("sphere", {})], + mesh=[SimpleNamespace(name=name) for name in data.get("mesh", {})], + ) + + scene_path = tmp_path / "sphere_world.yml" + scene_path.write_text( + yaml.safe_dump( + { + "sphere": { + "block_0": {"position": [0.0, 0.0, 0.0], "radius": 0.1}, + "block_1": {"position": [0.1, 0.0, 0.0], "radius": 0.1}, + "block_2": {"position": [0.2, 0.0, 0.0], "radius": 0.1}, + } + } + ), + encoding="utf-8", + ) + planner = CuroboPlanner.__new__(CuroboPlanner) + planner._bindings = SimpleNamespace(Scene=FakeScene) + + runtime_scene, runtime_cache, expected_names = planner._prepare_runtime_scene_model( + str(scene_path), {"cuboid": 8, "mesh": 2} + ) + + assert runtime_scene == str(scene_path) + assert runtime_cache == {"cuboid": 8, "mesh": 2} + assert expected_names == [["block_0", "block_1", "block_2"]] + + +def test_runtime_scene_records_independent_sphere_worlds(): + class FakeScene: + def __init__(self, names): + self.sphere = [SimpleNamespace(name=name) for name in names] + self.mesh = [] + + @classmethod + def create(cls, data): + return cls(list(data.get("sphere", {}))) + + planner = CuroboPlanner.__new__(CuroboPlanner) + planner._bindings = SimpleNamespace(Scene=FakeScene) + + runtime_scene, runtime_cache, expected_names = planner._prepare_runtime_scene_model( + [{"sphere": {"a": {}}}, {"sphere": {"b": {}, "c": {}}}], + {"mesh": 1}, + ) + + assert runtime_scene == [ + {"sphere": {"a": {}}}, + {"sphere": {"b": {}, "c": {}}}, + ] + assert runtime_cache == {"mesh": 1} + assert expected_names == [["a"], ["b", "c"]] + + +def test_runtime_sphere_validation_rejects_missing_collision_objects(): + checker = SimpleNamespace( + get_obstacle_names=lambda env_idx: ["block_0"] if env_idx == 0 else [] + ) + planner = SimpleNamespace(scene_collision_checker=checker) + + with pytest.raises(RuntimeError, match="block_1"): + CuroboPlanner._validate_runtime_sphere_obstacles( + planner, [["block_0", "block_1"]] + ) + + +def test_analytic_sphere_storage_preserves_center_radius_and_name(): + pytest.importorskip("curobo") + from curobo.scene import Scene + from curobo.types import DeviceCfg + + from embodichain.lab.sim.planners.curobo.curobo_sphere_data import SphereData + + scene = Scene.create( + { + "sphere": { + "block_0": { + "position": [1.0, 2.0, 3.0], + "radius": 0.4, + } + } + } + ) + storage = SphereData.from_scene_cfg(scene, DeviceCfg(device="cpu")) + + assert storage.get_names() == ["block_0"] + assert storage.radius[0, 0].item() == pytest.approx(0.4) + assert storage.inv_pose[0, 0, :7].tolist() == pytest.approx( + [-1.0, -2.0, -3.0, 1.0, 0.0, 0.0, 0.0] + ) def test_curobo_planner_class_is_lazy_import_safe(): @@ -234,6 +343,150 @@ def test_curobo_planner_class_is_lazy_import_safe(): assert "curobo" not in sys.modules +def test_backend_disables_curobo_self_collision(monkeypatch): + create_kwargs = {} + + class FakeMotionPlannerCfg: + @staticmethod + def create(**kwargs): + create_kwargs.update(kwargs) + return SimpleNamespace( + trajopt_solver_config=SimpleNamespace(interpolation_dt=None) + ) + + class FakeMotionPlanner: + joint_names = ["joint"] + + def __init__(self, cfg): + self.cfg = cfg + + planner = CuroboPlanner.__new__(CuroboPlanner) + planner.cfg = SimpleNamespace( + world=SimpleNamespace(multi_env=False), + collision_activation_distance=0.01, + interpolation_dt=0.025, + ) + planner._curobo_device = torch.device("cuda:0") + planner._bindings = SimpleNamespace( + MotionPlannerCfg=FakeMotionPlannerCfg, + DeviceCfg=lambda device: device, + MotionPlanner=FakeMotionPlanner, + BatchMotionPlanner=FakeMotionPlanner, + ) + planner._validate_profile_joint_names = lambda *args: None + planner._validate_base_link_name = lambda *args: None + planner._resolve_tool_frame = lambda *args: "tool" + planner._load_runtime_robot_config = lambda path: { + "robot_cfg": { + "kinematics": { + "source": path, + "self_collision_buffer": {}, + "self_collision_ignore": {}, + } + } + } + monkeypatch.setattr(torch.cuda, "device", lambda device: nullcontext()) + + planner._build_backend( + control_part="arm", + batch_size=1, + profile=_CuroboProfile( + robot_config_path="robot.yml", + sim_to_curobo_joint_names={"joint": "joint"}, + ), + sim_joint_names=["joint"], + scene_model=None, + collision_cache=None, + use_cuda_graph=False, + planning_mode=MoveType.EEF_MOVE, + ) + + assert create_kwargs["self_collision_check"] is False + assert create_kwargs["robot"]["robot_cfg"]["kinematics"] == { + "source": "robot.yml", + "self_collision_buffer": {}, + "self_collision_ignore": {}, + } + + +def test_runtime_robot_config_adds_only_curobo_compatibility_placeholders(tmp_path): + config_path = tmp_path / "robot.yml" + config_path.write_text( + yaml.safe_dump( + { + "robot_cfg": { + "kinematics": { + "base_link": "base", + "collision_spheres": { + "base": [{"center": [0.0, 0.0, 0.0], "radius": 0.1}] + }, + } + } + } + ), + encoding="utf-8", + ) + + runtime_config = CuroboPlanner._load_runtime_robot_config(str(config_path)) + kinematics = runtime_config["robot_cfg"]["kinematics"] + + assert kinematics["self_collision_buffer"] == {} + assert kinematics["self_collision_ignore"] == {} + persisted = yaml.safe_load(config_path.read_text(encoding="utf-8")) + assert "self_collision_buffer" not in persisted["robot_cfg"]["kinematics"] + assert "self_collision_ignore" not in persisted["robot_cfg"]["kinematics"] + + +def test_disable_self_collision_reaches_all_curobo_rollouts(): + class FakeCostCfg: + def __init__(self): + self.disable_calls = 0 + + def disable_self_collision(self): + self.disable_calls += 1 + + class FakeRollout: + def __init__(self): + self.cost_cfg = FakeCostCfg() + + def get_cost_manager_configs(self): + return [self.cost_cfg] + + ik_metrics = FakeRollout() + ik_optimizer = FakeRollout() + trajopt_metrics = FakeRollout() + trajopt_optimizer = FakeRollout() + graph_rollout = FakeRollout() + planner_cfg = SimpleNamespace( + ik_solver_config=SimpleNamespace( + core_cfg=SimpleNamespace( + metrics_rollout_config=ik_metrics, + optimizer_rollout_configs=[ik_optimizer], + ) + ), + trajopt_solver_config=SimpleNamespace( + core_cfg=SimpleNamespace( + metrics_rollout_config=trajopt_metrics, + optimizer_rollout_configs=[trajopt_optimizer], + ) + ), + graph_planner_config=SimpleNamespace(rollout_config=graph_rollout), + ) + + CuroboPlanner._disable_curobo_self_collision_rollouts(planner_cfg) + + assert all( + rollout.cost_cfg.disable_calls == 1 + for rollout in ( + ik_metrics, + ik_optimizer, + trajopt_metrics, + trajopt_optimizer, + graph_rollout, + ) + ) + + def test_cpu_sim_resolves_current_cuda_device(monkeypatch): """A CPU simulation defaults cuRobo to the current CUDA device.""" monkeypatch.setattr(torch.cuda, "is_available", lambda: True) @@ -298,6 +551,109 @@ def test_parse_mimic_joint_names_handles_missing_file(tmp_path): assert _parse_mimic_joint_names(str(tmp_path / "does_not_exist.urdf")) == set() +def test_robot_spheres_use_dexsim_morphit_with_two_hulls(tmp_path, monkeypatch): + pytest.importorskip("curobo") + import dexsim.kit.meshproc as meshproc + + urdf_path = tmp_path / "robot.urdf" + urdf_path.write_text( + '', + encoding="utf-8", + ) + + class FakeRobot: + cfg = type( + "Cfg", + (), + {"fpath": str(urdf_path), "init_qpos": [], "base_link_name": "base"}, + )() + joint_names = [] + control_parts = {"arm": []} + + def get_link_names(self): + return ["base"] + + def get_link_vert_face(self, link_name): # noqa: ARG002 + return _unit_cube_vertices(), _cube_faces() + + def get_control_part_link_names(self, control_part): # noqa: ARG002 + return ["base"] + + calls = [] + + def fake_sphere_fit(mesh, **kwargs): + calls.append((mesh, kwargs)) + return ( + True, + torch.tensor([[0.0, 0.0, 0.0]], dtype=torch.float32), + torch.tensor([0.25], dtype=torch.float32), + ) + + monkeypatch.setattr(torch.cuda, "is_available", lambda: True) + monkeypatch.setattr(meshproc, "sphere_fit", fake_sphere_fit) + output_path = tmp_path / "robot.yml" + + generate_curobo_robot_yaml( + FakeRobot(), + "arm", + str(output_path), + urdf_path=str(urdf_path), + device="cuda:0", + ) + + assert len(calls) == 1 + _, kwargs = calls[0] + assert kwargs["fit_type"] is meshproc.SphereFitType.MORPHIT + assert kwargs["max_convex_hull_num"] == 2 + kinematics = yaml.safe_load(output_path.read_text(encoding="utf-8"))["robot_cfg"][ + "kinematics" + ] + assert kinematics["collision_spheres"]["base"][0]["radius"] == pytest.approx(0.25) + assert "self_collision_buffer" not in kinematics + assert "self_collision_ignore" not in kinematics + + +def test_robot_collision_visualization_reads_cache_and_live_link_pose(tmp_path): + robot_yaml_path = tmp_path / "robot_visual.yml" + robot_yaml_path.write_text( + yaml.safe_dump( + { + "robot_cfg": { + "kinematics": { + "collision_sphere_buffer": 0.0, + "collision_spheres": { + "base": [{"center": [0.0, 0.0, 0.0], "radius": 0.1}] + }, + } + } + } + ), + encoding="utf-8", + ) + + class FakeRobot: + def get_link_vert_face(self, link_name): # noqa: ARG002 + return _unit_cube_vertices(), _cube_faces() + + def get_link_pose( + self, link_name, env_ids=None, to_matrix=False # noqa: ARG002 + ): + pose = torch.eye(4, dtype=torch.float32) + pose[:3, 3] = torch.tensor([1.0, 2.0, 3.0]) + return pose.unsqueeze(0) + + geometries = visualize_curobo_robot_collision_model( + FakeRobot(), str(robot_yaml_path), draw=False + ) + + assert [geometry["name"] for geometry in geometries] == [ + "robot_mesh/base", + "robot_spheres", + ] + sphere_bounds = geometries[-1]["geometry"].get_axis_aligned_bounding_box() + assert sphere_bounds.get_center() == pytest.approx([1.0, 2.0, 3.0]) + + # World YAML generation @@ -473,6 +829,71 @@ def test_empty_mesh_raises_for_cuboid(): ) +def test_sphere_obstacle_uses_dexsim_morphit_with_sixteen_hulls(monkeypatch): + import dexsim.kit.meshproc as meshproc + + calls = [] + + def fake_sphere_fit(mesh, **kwargs): + calls.append((mesh, kwargs)) + return ( + True, + torch.tensor([[0.0, 0.0, 0.0]], dtype=torch.float32), + torch.tensor([0.25], dtype=torch.float32), + ) + + monkeypatch.setattr(torch.cuda, "is_available", lambda: True) + monkeypatch.setattr(meshproc, "sphere_fit", fake_sphere_fit) + + entries = _mesh_to_obstacle_entry( + "block", + _unit_cube_vertices(), + _cube_faces(), + _identity_pose(), + representation="sphere", + device="cuda:0", + ) + + assert len(calls) == 1 + _, kwargs = calls[0] + assert kwargs["fit_type"] is meshproc.SphereFitType.MORPHIT + assert kwargs["max_convex_hull_num"] == 16 + assert entries[0][0:2] == ("sphere", "block_0") + assert entries[0][2]["position"] == pytest.approx([0.45, 0.0, 0.18]) + + +def test_obstacle_collision_visualization_reads_cached_spheres(tmp_path): + world_yaml_path = tmp_path / "world_visual.yml" + world_yaml_path.write_text( + yaml.safe_dump( + {"sphere": {"block_0": {"position": [1.0, 2.0, 3.0], "radius": 0.1}}} + ), + encoding="utf-8", + ) + + class FakeVisualRigidObject(_FakeRigidObject): + def get_local_pose(self, to_matrix=False): + if not to_matrix: + return super().get_local_pose(to_matrix=False) + pose = torch.eye(4, dtype=torch.float32) + pose[:3, 3] = self._pose[:3] + return pose.unsqueeze(0) + + rigid_object = FakeVisualRigidObject( + "block", _unit_cube_vertices(), _cube_faces(), _identity_pose() + ) + geometries = visualize_curobo_world_collision_model( + [rigid_object], str(world_yaml_path), draw=False + ) + + assert [geometry["name"] for geometry in geometries] == [ + "obstacle_mesh/block", + "obstacle_spheres", + ] + sphere_bounds = geometries[-1]["geometry"].get_axis_aligned_bounding_box() + assert sphere_bounds.get_center() == pytest.approx([1.0, 2.0, 3.0]) + + def test_generate_cuboid_world_yaml_assembles_schema(tmp_path): rigid_object = _FakeRigidObject( "demo_block", From 602d080d09795c22267e53d811bf23a8dae0c8f5 Mon Sep 17 00:00:00 2001 From: matafela Date: Fri, 7 Aug 2026 17:42:18 +0800 Subject: [PATCH 2/6] fix --- .../overview/sim/planners/curobo_planner.md | 39 +- .../lab/sim/planners/curobo/curobo_planner.py | 502 +++--------------- .../sim/planners/curobo/curobo_sphere_data.py | 275 ---------- .../lab/sim/planners/curobo/curobo_yaml.py | 355 +++++-------- examples/sim/planners/curobo_planner.py | 8 +- .../motion_generation/BENCHMARK_DESIGN.md | 5 +- .../motion_generation/planners/curobo.py | 8 +- .../motion_generation/suites/coverage.yaml | 2 - .../motion_generation/suites/smoke.yaml | 2 - tests/sim/planners/test_curobo_integration.py | 22 +- tests/sim/planners/test_curobo_planner.py | 403 +++----------- 11 files changed, 327 insertions(+), 1294 deletions(-) delete mode 100644 embodichain/lab/sim/planners/curobo/curobo_sphere_data.py diff --git a/docs/source/overview/sim/planners/curobo_planner.md b/docs/source/overview/sim/planners/curobo_planner.md index 75df7c6c6..2cac6f9e2 100644 --- a/docs/source/overview/sim/planners/curobo_planner.md +++ b/docs/source/overview/sim/planners/curobo_planner.md @@ -142,26 +142,19 @@ second CUDA context. The collision world is always auto-generated from live `RigidObject` meshes via `CuroboWorldCfg.rigid_objects`: the adapter reads each object's mesh -(`get_vertices` / `get_triangles`) and world pose (`get_local_pose`) and writes a -cached cuRobo scene YAML on the first plan, using -`CuroboWorldCfg.obstacle_representation` (`"sphere"` by default; use -`"cuboid"` for a local-frame AABB placed as an OBB via the object pose, or -`"mesh"` for the exact triangle mesh). Sphere worlds remain sphere-based in -the generated YAML, cache, collision-model visualization, and runtime checker. -cuRobo V2 can parse those spheres but omits sphere storage from its generic -world checker. EmbodiChain registers an analytic sphere obstacle type with that -checker before constructing the backend; its signed distance is evaluated as -the center distance minus the obstacle radius. The runtime sphere cache is -sized automatically from the scene, and backend initialization verifies that -no fitted sphere was silently dropped. +(`get_vertices` / `get_triangles`) and world pose (`get_local_pose`), decomposes +the mesh into at most 16 convex hulls with DexSim +`convex_decomposition_visacd`, and computes their union as an ESDF voxel grid. +The tensor-backed voxel scene is cached on the first plan and loaded directly +into cuRobo's `SceneData`; there are no cuboid, triangle-mesh, or sphere world +representation branches. `CuroboWorldCfg.voxel_size` controls resolution and +`voxel_padding` keeps collision queries inside the ESDF grid near its boundary. Generated poses are authored in the cuRobo base/world frame, so this is exact when the robot base sits at the simulator world origin. For obstacles that move or live in an offset base frame, also declare their names in `CuroboWorldCfg.dynamic_obstacle_names` and update poses at plan time through -`CuroboPlanOptions.dynamic_obstacle_poses` (provision -`CuroboWorldCfg.collision_cache` before planning). Dynamic updates require the -`"cuboid"` or `"mesh"` representation because sphere fitting expands one object -into multiple independently named obstacles. +`CuroboPlanOptions.dynamic_obstacle_poses`. Every source object remains one +same-named voxel layer, so pose updates use the original `RigidObject` name. ### Shared and per-environment collision worlds @@ -185,14 +178,13 @@ differ, the adapter rejects the update and instructs the caller to enable With `multi_env=True`, cuRobo allocates one collision world per batch row and EmbodiChain sends row `i` of each dynamic obstacle pose to world `i`. The -auto-generated YAML still reads the static scene from env 0 and clones that +auto-generated voxel cache still reads the static scene from env 0 and clones that scene for every row; setting `multi_env=True` does not by itself discover each environment's distinct initial object poses. Any object whose robot-relative pose differs by environment must also: -1. Use `obstacle_representation="cuboid"` or `"mesh"`. -2. Be listed in `CuroboWorldCfg.dynamic_obstacle_names`. -3. Have its current `(B, 4, 4)` simulator-world poses passed through +1. Be listed in `CuroboWorldCfg.dynamic_obstacle_names`. +2. Have its current `(B, 4, 4)` simulator-world poses passed through `CuroboPlanOptions.dynamic_obstacle_poses` when planning. For example: @@ -200,7 +192,6 @@ For example: ```python world_cfg = CuroboWorldCfg( rigid_objects=[block], - obstacle_representation="cuboid", dynamic_obstacle_names=["block"], multi_env=True, ) @@ -328,9 +319,9 @@ python examples/sim/planners/curobo_planner.py --headless --sim-device cpu ~~~ The demo exports the DexSim `demo_block` into the cuRobo collision world via -`CuroboWorldCfg.rigid_objects` (the robot and world YAMLs are both -auto-generated), prints the result status and trajectory shape, then replays the -returned full-DoF trajectory. CUDA graph capture is enabled by default with the +`CuroboWorldCfg.rigid_objects` (the robot YAML and voxel world cache are +auto-generated), prints the result status and trajectory shape, then replays +the returned full-DoF trajectory. CUDA graph capture is enabled by default with the renderer-compatible `"thread_local"` mode; pass `--no-cuda-graph` to disable it. Headless runs automatically record this fixed offscreen camera view to an MP4. Set an explicit diff --git a/embodichain/lab/sim/planners/curobo/curobo_planner.py b/embodichain/lab/sim/planners/curobo/curobo_planner.py index d5cb6ea56..3d8b1cbbb 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_planner.py +++ b/embodichain/lab/sim/planners/curobo/curobo_planner.py @@ -38,7 +38,6 @@ from contextlib import contextmanager, nullcontext from copy import deepcopy from dataclasses import dataclass -from pathlib import Path from types import SimpleNamespace from typing import TYPE_CHECKING @@ -83,6 +82,10 @@ # cuRobo self-collision checking. _CUROBO_ROBOT_YAML_GENERATOR_VERSION = "v4-no-self-collision" +# World caches contain tensor-backed ESDF voxel grids and are intentionally +# versioned independently from robot sphere YAMLs. +_CUROBO_WORLD_CACHE_VERSION = "v1-visacd16-voxel" + # cuRobo 0.8 does not expose PyTorch's CUDA stream-capture error mode. The # temporary adapter below therefore replaces ``torch.cuda.graph`` only while # cuRobo can lazily record graphs. Serialize that small process-wide patch. @@ -147,40 +150,26 @@ class CuroboWorldCfg: """ rigid_objects: list[RigidObject] | None = None - """Live :class:`RigidObject` obstacles to bake into the auto-generated world YAML. + """Live objects to bake into the auto-generated voxel collision scene. - The adapter reads each object's mesh (``get_vertices`` / ``get_triangles``) - and world pose (``get_local_pose``) and writes a cuRobo V2 scene YAML (cached - on disk by content hash). Poses are written in the cuRobo world/base frame, + The adapter reads each object's mesh (``get_vertices`` / ``get_triangles``), + decomposes it with DexSim VisACD, and builds a cuRobo ESDF voxel layer cached + on disk by content hash. Poses are expressed in the cuRobo world/base frame, so this is exact when the robot base sits at the simulator world origin. For obstacles that move or live in an offset base frame, also list their names in :attr:`dynamic_obstacle_names` to update poses at plan time. ``None`` yields an initially empty collision world. """ - obstacle_representation: str = "sphere" - """Collision representation used when generating the YAML from :attr:`rigid_objects`. - - ``"sphere"`` (default) fits spheres with DexSim's MorphIt implementation - (approximate, and requires CUDA + Open3D). cuRobo V2 can parse sphere - obstacles but omits their collision storage; EmbodiChain registers an - analytic sphere SDF with the generic Warp checker at backend creation. - ``"cuboid"`` emits a local-frame AABB per object, - placed as an OBB via the object pose. ``"mesh"`` emits the full triangle - mesh (exact, no CUDA). - """ + voxel_size: float = 0.01 + """ESDF voxel edge length in meters for every world collision object.""" - collision_cache: dict[str, int | dict[str, int | float | list[float]]] = { - "cuboid": 8, - "mesh": 2, - } - """Per-geometry cache capacity created before world updates. + voxel_padding: float = 0.1 + """Free-space padding around each object-local voxel grid in meters. - cuRobo V2 accepts integer ``cuboid`` and ``mesh`` capacities. A ``voxel`` - cache, when needed for dynamic voxel worlds, must instead be a dictionary - with V2's ``layers``, ``dims``, and ``voxel_size`` fields. - - Analytic sphere obstacles use a separate runtime cache sized from the scene. + The padding must cover the largest robot collision-sphere radius plus the + planner's collision activation distance so queries do not leave the grid + while the robot is still close enough to collide. """ dynamic_obstacle_names: list[str] = [] @@ -208,8 +197,7 @@ class CuroboWorldCfg: initial pose from every simulator environment. Per-env pose differences must therefore be declared in :attr:`dynamic_obstacle_names` and supplied as batched ``(B, 4, 4)`` poses through - :attr:`CuroboPlanOptions.dynamic_obstacle_poses`. Dynamic updates require - ``obstacle_representation="cuboid"`` or ``"mesh"``. + :attr:`CuroboPlanOptions.dynamic_obstacle_poses`. Prefer the shared default when the rebased layouts are identical because independent worlds replicate scene data and collision caches across the @@ -238,7 +226,7 @@ class CuroboAutoGenCfg: """ cache_dir: str | None = None - """Directory for cached robot YAMLs. + """Directory for cached robot YAMLs and voxel collision worlds. ``None`` (default) uses ``$XDG_CACHE_HOME/embodichain_curobo`` or ``~/.cache/embodichain_curobo``. The cache key hashes the generator version, @@ -271,7 +259,7 @@ class CuroboAutoGenCfg: """Padding added to every fitted sphere's radius (m).""" force: bool = False - """Bypass the cache and regenerate the robot YAML on the next plan.""" + """Regenerate the robot YAML and voxel world on the next plan.""" @configclass @@ -281,10 +269,10 @@ class CuroboPlannerCfg(BasePlannerCfg): cuRobo runs in the simulator process so it reuses the existing CUDA context instead of keeping a spawned Python process and a second CUDA context alive. CUDA graphs are enabled by default with renderer-compatible thread-local - capture. Both the cuRobo robot YAML and the collision-world YAML are - auto-generated internally (from the robot's URDF and from - :attr:`world.rigid_objects` respectively); no external YAML is used. The - per-control-part profile is auto-derived from the robot's solver at plan time. + capture. The robot YAML and tensor-backed voxel world are auto-generated + internally from the robot URDF and :attr:`world.rigid_objects`; no external + collision-world YAML is used. The per-control-part profile is auto-derived + from the robot's solver at plan time. """ planner_type: str = "curobo" @@ -630,182 +618,6 @@ def _configure_curobo_logging(log_level: str) -> None: logging.getLogger("curobo").setLevel(levels[normalized]) -def _enable_curobo_sphere_collision_support() -> None: - """Register analytic sphere obstacles with cuRobo V2's generic checker. - - cuRobo 0.8 publishes ``Sphere`` as a scene type but omits sphere storage - from ``SceneData``. Its Warp checker is intentionally extensible through - ``OBSTACLE_SDF_MODULES``; register EmbodiChain's analytic sphere storage and - extend the aggregate scene container before importing the motion planner. - """ - import sys - - kernel_module_names = ( - "curobo._src.geom.collision.wp_collision_kernel", - "curobo._src.geom.collision.wp_sweep_collision_kernel", - ) - preloaded_kernel_modules = { - name for name in kernel_module_names if name in sys.modules - } - data_package = importlib.import_module("curobo._src.geom.data") - sphere_module_path = "embodichain.lab.sim.planners.curobo.curobo_sphere_data" - if sphere_module_path not in data_package.OBSTACLE_SDF_MODULES: - data_package.OBSTACLE_SDF_MODULES.append(sphere_module_path) - sphere_module = importlib.import_module(sphere_module_path) - scene_data_module = importlib.import_module("curobo._src.geom.data.data_scene") - scene_data_cls = scene_data_module.SceneData - if not getattr(scene_data_cls, "_embodichain_sphere_support", False): - original_from_scene = scene_data_cls.from_scene_cfg - original_from_batch = scene_data_cls.from_batch_scene_cfg - original_create_cache = scene_data_cls.create_cache - original_get_valid_data = scene_data_cls.get_valid_data - original_get_names = scene_data_cls.get_obstacle_names - original_load_scene = scene_data_cls.load_from_scene_cfg - original_update_pose = scene_data_cls.update_obstacle_pose - original_enable_obstacle = scene_data_cls.enable_obstacle - original_clear = scene_data_cls.clear - - @classmethod - def from_scene_cfg_with_spheres(cls, scene_cfg, device_cfg, *args, **kwargs): - data = original_from_scene(scene_cfg, device_cfg, *args, **kwargs) - num_envs = kwargs.get("num_envs", args[0] if args else 1) - env_idx = kwargs.get("env_idx", args[1] if len(args) > 1 else 0) - data.spheres = ( - sphere_module.SphereData.from_scene_cfg( - scene_cfg, - device_cfg, - env_idx=env_idx, - num_envs=num_envs, - ) - if scene_cfg.sphere - else None - ) - return data - - @classmethod - def from_batch_scene_cfg_with_spheres( - cls, scene_cfg_list, device_cfg, *args, **kwargs - ): - data = original_from_batch(scene_cfg_list, device_cfg, *args, **kwargs) - data.spheres = ( - sphere_module.SphereData.from_batch_scene_cfg( - scene_cfg_list, device_cfg - ) - if any(scene.sphere for scene in scene_cfg_list) - else None - ) - return data - - @classmethod - def create_cache_with_spheres(cls, *args, **kwargs): - data = original_create_cache(*args, **kwargs) - data.spheres = None - return data - - def get_valid_data_with_spheres(self): - valid_data = original_get_valid_data(self) - spheres = getattr(self, "spheres", None) - if spheres is not None: - valid_data.append(spheres) - return valid_data - - def get_obstacle_names_with_spheres(self, env_idx=0): - names = original_get_names(self, env_idx) - spheres = getattr(self, "spheres", None) - if spheres is not None: - names.extend(spheres.get_names(env_idx)) - return names - - def load_from_scene_cfg_with_spheres( - self, scene_cfg, env_idx=0, store_reference=True - ): - original_load_scene(self, scene_cfg, env_idx, store_reference) - if scene_cfg.sphere: - spheres = getattr(self, "spheres", None) - if spheres is None or len(scene_cfg.sphere) > spheres.max_n: - self.spheres = sphere_module.SphereData.create_cache( - len(scene_cfg.sphere), self.num_envs, self.device_cfg - ) - self.spheres.load_batch(scene_cfg.sphere, env_idx) - - def update_obstacle_pose_with_spheres(self, name, pose, env_idx=0): - spheres = getattr(self, "spheres", None) - if spheres is not None and name in spheres.get_names(env_idx): - spheres.update_pose(name, pose, env_idx) - return - original_update_pose(self, name, pose, env_idx) - - def enable_obstacle_with_spheres(self, name, enabled=True, env_idx=0): - spheres = getattr(self, "spheres", None) - if spheres is not None and name in spheres.get_names(env_idx): - spheres.set_enabled(name, enabled, env_idx) - return - original_enable_obstacle(self, name, enabled, env_idx) - - def clear_with_spheres(self, env_idx=None): - original_clear(self, env_idx) - spheres = getattr(self, "spheres", None) - if spheres is not None: - spheres.clear(env_idx) - - def has_spheres(self): - return getattr(self, "spheres", None) is not None - - scene_data_cls.from_scene_cfg = from_scene_cfg_with_spheres - scene_data_cls.from_batch_scene_cfg = from_batch_scene_cfg_with_spheres - scene_data_cls.create_cache = create_cache_with_spheres - scene_data_cls.get_valid_data = get_valid_data_with_spheres - scene_data_cls.get_obstacle_names = get_obstacle_names_with_spheres - scene_data_cls.load_from_scene_cfg = load_from_scene_cfg_with_spheres - scene_data_cls.update_obstacle_pose = update_obstacle_pose_with_spheres - scene_data_cls.enable_obstacle = enable_obstacle_with_spheres - scene_data_cls.clear = clear_with_spheres - scene_data_cls.has_spheres = has_spheres - scene_data_cls._embodichain_sphere_support = True - - collision_scene_module = importlib.import_module( - "curobo._src.geom.collision.collision_scene" - ) - collision_scene_cls = collision_scene_module.SceneCollision - if not getattr(collision_scene_cls, "_embodichain_sphere_support", False): - original_collision_types = collision_scene_cls.collision_types.fget - - def collision_types_with_spheres(self): - collision_types = original_collision_types(self) - collision_types["sphere"] = self.data.has_spheres() - return collision_types - - collision_scene_cls.collision_types = property(collision_types_with_spheres) - collision_scene_cls._embodichain_sphere_support = True - - # If another package imported the generic kernels first, add the sphere - # overloads to their existing Warp function sets as well. - import warp as wp - - for module_name, include_sdf_only in ( - ("curobo._src.geom.collision.wp_collision_kernel", False), - ("curobo._src.geom.collision.wp_sweep_collision_kernel", True), - ): - if module_name not in preloaded_kernel_modules: - continue - kernel_module = sys.modules.get(module_name) - if kernel_module is None: - continue - kernel_module.is_obs_enabled = wp.func( - sphere_module.is_obs_enabled, module=module_name - ) - kernel_module.load_obstacle_transform = wp.func( - sphere_module.load_obstacle_transform, module=module_name - ) - if include_sdf_only: - kernel_module.compute_local_sdf = wp.func( - sphere_module.compute_local_sdf, module=module_name - ) - kernel_module.compute_local_sdf_with_grad = wp.func( - sphere_module.compute_local_sdf_with_grad, module=module_name - ) - - def _require_curobo(log_level: str = "error") -> "Any": """Lazily import and bundle the cuRobo V2 public facade types. @@ -824,7 +636,6 @@ def _require_curobo(log_level: str = "error") -> "Any": # cuRobo 0.8 references ``wp.torch.*``, which Warp >= 1.13 relocated. _ensure_warp_torch_compat() try: - _enable_curobo_sphere_collision_support() planner_mod = importlib.import_module("curobo.motion_planner") batch_mod = importlib.import_module("curobo.batch_motion_planner") scene_mod = importlib.import_module("curobo.scene") @@ -958,21 +769,16 @@ def __init__(self, cfg: CuroboPlannerCfg) -> None: {} ) world_cfg = cfg.world - if world_cfg.obstacle_representation not in ("cuboid", "mesh", "sphere"): + if world_cfg.voxel_size <= 0.0: logger.log_error( - "CuroboWorldCfg.obstacle_representation must be 'cuboid', 'mesh', " - f"or 'sphere', got {world_cfg.obstacle_representation!r}.", + f"CuroboWorldCfg.voxel_size must be positive, got " + f"{world_cfg.voxel_size}.", ValueError, ) - if ( - world_cfg.dynamic_obstacle_names - and world_cfg.obstacle_representation == "sphere" - ): + if world_cfg.voxel_padding < 0.0: logger.log_error( - "Dynamic obstacle updates require the 'cuboid' or 'mesh' world " - "representation. Sphere fitting expands one RigidObject into " - "multiple independent obstacles that cannot be updated by the " - "original object name.", + f"CuroboWorldCfg.voxel_padding must be non-negative, got " + f"{world_cfg.voxel_padding}.", ValueError, ) if cfg.warmup_iterations < 0: @@ -1031,7 +837,7 @@ def prepare_backend( """Materialize and warm one lazy cuRobo backend without planning a case. This explicit lifecycle hook lets deployment tooling and benchmarks - separate one-time robot/world YAML generation, collision-sphere setup, + separate one-time robot/voxel-world generation and collision setup, CUDA graph capture, and cuRobo warmup from the first real planning call. Repeated calls for the same backend key reuse the cached backend. @@ -1177,63 +983,17 @@ def _resolve_start_qpos( # ------------------------------------------------------------------ def _materialize_multi_env_scene_model( - self, world_config_path: str | None, batch_size: int - ) -> list[dict]: - """Return one independent cuRobo scene mapping for every batch row. - - The auto-generated YAML contains env 0's static scene. Cloning it makes - the collision worlds independently addressable but does not discover - per-env simulator poses; dynamic-obstacle updates apply those later. - """ + self, scene_model: "Any | None", batch_size: int + ) -> list["Any"]: + """Clone env 0's voxel scene for every independent collision world.""" if batch_size < 1: logger.log_error( f"multi-env cuRobo batch_size must be positive, got {batch_size}.", ValueError, ) - if world_config_path is None: - return [{} for _ in range(batch_size)] - - scene_path = Path(world_config_path) - if not scene_path.is_absolute(): - content_mod = importlib.import_module("curobo.content") - scene_path = Path(content_mod.get_scene_configs_path()) / scene_path - try: - with scene_path.open(encoding="utf-8") as scene_file: - scene_model = yaml.safe_load(scene_file) - except (OSError, yaml.YAMLError) as exc: - logger.log_error( - f"Unable to load cuRobo V2 scene configuration " - f"'{world_config_path}': {exc}", - ValueError, - ) - raise AssertionError("unreachable") from exc - - if isinstance(scene_model, dict): - return [deepcopy(scene_model) for _ in range(batch_size)] - if isinstance(scene_model, list): - if not scene_model or not all( - isinstance(scene, dict) for scene in scene_model - ): - logger.log_error( - "A multi-env cuRobo scene YAML list must contain one or more " - "mapping worlds.", - ValueError, - ) - if len(scene_model) == 1: - return [deepcopy(scene_model[0]) for _ in range(batch_size)] - if len(scene_model) == batch_size: - return [deepcopy(scene) for scene in scene_model] - logger.log_error( - "A multi-env cuRobo scene YAML list must have one world to clone " - f"or exactly batch_size={batch_size} worlds; got {len(scene_model)}.", - ValueError, - ) - logger.log_error( - "A cuRobo V2 scene YAML must contain a mapping world or a list of " - f"mapping worlds, got {type(scene_model).__name__}.", - ValueError, - ) - raise AssertionError("unreachable") + if scene_model is None: + return [self._bindings.Scene.create({}) for _ in range(batch_size)] + return [scene_model.clone() for _ in range(batch_size)] def _get_backend( self, @@ -1250,18 +1010,14 @@ def _get_backend( profile = self._materialize_profile(control_part) sim_joint_names = self._resolve_sim_joint_names(control_part) world_cfg = self.cfg.world - collision_cache = ( - dict(world_cfg.collision_cache) if world_cfg.collision_cache else None - ) - world_config_path = ( - self._auto_generate_world_yaml(world_cfg) + scene_model = ( + self._auto_generate_world_scene(world_cfg) if world_cfg.rigid_objects else None ) - scene_model: str | list[dict] | None = world_config_path if multi_env: scene_model = self._materialize_multi_env_scene_model( - world_config_path, int(batch_size) + scene_model, int(batch_size) ) use_cuda_graph = bool(self.cfg.use_cuda_graph) @@ -1311,7 +1067,6 @@ def _get_backend( profile=profile, sim_joint_names=sim_joint_names, scene_model=scene_model, - collision_cache=collision_cache, use_cuda_graph=use_cuda_graph, planning_mode=planning_mode, ) @@ -1354,23 +1109,16 @@ def _build_backend( batch_size: int, profile: _CuroboProfile, sim_joint_names: list[str], - scene_model: str | list[dict] | None, - collision_cache: dict[str, int | dict[str, int | float | list[float]]] | None, + scene_model: "Any | list[Any] | None", use_cuda_graph: bool, planning_mode: MoveType, ) -> "_CuroboBackend": """Construct and validate one cuRobo planner on the selected CUDA device.""" robot_config = self._load_runtime_robot_config(profile.robot_config_path) - ( - runtime_scene_model, - runtime_collision_cache, - expected_sphere_names, - ) = self._prepare_runtime_scene_model(scene_model, collision_cache) with torch.cuda.device(self._curobo_device): planner_cfg = self._bindings.MotionPlannerCfg.create( robot=robot_config, - scene_model=runtime_scene_model, - collision_cache=runtime_collision_cache, + scene_model=scene_model, self_collision_check=False, device_cfg=self._bindings.DeviceCfg(device=self._curobo_device), max_batch_size=batch_size, @@ -1398,7 +1146,6 @@ def _build_backend( ) self._validate_base_link_name(profile, planner) tool_frame = self._resolve_tool_frame(profile, planner) - self._validate_runtime_sphere_obstacles(planner, expected_sphere_names) except Exception: self._close_planner(planner) raise @@ -1413,77 +1160,6 @@ def _build_backend( planning_mode=planning_mode, ) - def _prepare_runtime_scene_model( - self, - scene_model: str | list[dict] | None, - collision_cache: dict[str, int | dict[str, int | float | list[float]]] | None, - ) -> tuple["Any", dict | None, list[list[str]]]: - """Inspect sphere obstacles before cuRobo constructs its runtime scene. - - cuRobo V2 exposes ``Sphere`` in its public scene model, but its world - collision data omits sphere storage. EmbodiChain registers analytic - sphere storage with the generic Warp checker before this method runs; - this inspection records the names that must reach that storage. - - Returns: - Runtime scene model, collision cache, and sphere names expected in - each collision environment. - """ - if scene_model is None: - return None, collision_cache, [] - - raw_scene_model: "Any" = scene_model - if isinstance(scene_model, str): - scene_path = Path(scene_model) - if not scene_path.is_absolute(): - content_mod = importlib.import_module("curobo.content") - scene_path = Path(content_mod.get_scene_configs_path()) / scene_path - try: - with scene_path.open(encoding="utf-8") as scene_file: - raw_scene_model = yaml.safe_load(scene_file) - except (OSError, yaml.YAMLError) as exc: - raise ValueError( - f"Unable to load cuRobo V2 scene configuration " - f"'{scene_model}': {exc}" - ) from exc - - raw_scenes = ( - raw_scene_model if isinstance(raw_scene_model, list) else [raw_scene_model] - ) - parsed_scenes = [ - self._bindings.Scene.create(scene) if isinstance(scene, dict) else scene - for scene in raw_scenes - ] - expected_names = [ - [sphere.name for sphere in (scene.sphere or [])] for scene in parsed_scenes - ] - if not any(expected_names): - return scene_model, collision_cache, [] - - sphere_count = sum(len(names) for names in expected_names) - logger.log_info( - f"Loaded {sphere_count} cuRobo scene sphere(s) into EmbodiChain's " - "analytic sphere collision storage." - ) - return scene_model, collision_cache, expected_names - - @staticmethod - def _validate_runtime_sphere_obstacles( - planner: "Any", expected_names: list[list[str]] - ) -> None: - """Ensure every sphere reached cuRobo's collision checker.""" - if not expected_names: - return - checker = planner.scene_collision_checker - for env_idx, names in enumerate(expected_names): - loaded_names = set(checker.get_obstacle_names(env_idx)) - missing = sorted(set(names) - loaded_names) - if missing: - raise RuntimeError( - "cuRobo parsed sphere obstacles but did not register them " - f"in environment {env_idx}: {missing}." - ) - @staticmethod def _load_runtime_robot_config(robot_config_path: str) -> dict: """Load robot YAML and add cuRobo 0.8's required empty placeholders. @@ -1863,20 +1539,14 @@ def _robot_yaml_cache_key( hasher.update(str(auto.collision_sphere_buffer).encode("utf-8")) return hasher.hexdigest() - def _auto_generate_world_yaml(self, world_cfg: CuroboWorldCfg) -> str: - """Return a cached cuRobo world YAML path generated from ``rigid_objects``. - - Mirrors :meth:`_auto_generate_robot_yaml`: a content-hashed YAML is written - to the cuRobo cache directory (reusing :attr:`CuroboAutoGenCfg.cache_dir`) - on the first plan and reused thereafter. Sphere-fit parameters come from - :class:`CuroboAutoGenCfg` so robot and world fitting are configured together. - """ - from .curobo_yaml import generate_curobo_world_yaml + def _auto_generate_world_scene(self, world_cfg: CuroboWorldCfg) -> "Any": + """Load or generate a tensor-backed cuRobo voxel scene.""" + from .curobo_yaml import generate_curobo_world_scene rigid_objects = world_cfg.rigid_objects if not rigid_objects: logger.log_error( - "_auto_generate_world_yaml requires non-empty rigid_objects.", + "_auto_generate_world_scene requires non-empty rigid_objects.", ValueError, ) assert rigid_objects is not None # log_error raises above; narrows type @@ -1885,43 +1555,37 @@ def _auto_generate_world_yaml(self, world_cfg: CuroboWorldCfg) -> str: os.environ.get("XDG_CACHE_HOME", os.path.expanduser("~/.cache")), "embodichain_curobo", ) - cache_key = self._world_yaml_cache_key(world_cfg) - cache_path = os.path.join(cache_dir, f"world_{cache_key}.yml") + cache_key = self._world_scene_cache_key(world_cfg) + cache_path = os.path.join(cache_dir, f"world_{cache_key}.pt") if not auto.force and os.path.exists(cache_path): - logger.log_info(f"cuRobo world YAML cache hit: {cache_path}") - return cache_path - logger.log_info( - f"Auto-generating cuRobo world YAML from {len(rigid_objects)} " - f"RigidObject(s) ({world_cfg.obstacle_representation}) -> {cache_path}" - ) - return generate_curobo_world_yaml( - rigid_objects, - cache_path, - representation=world_cfg.obstacle_representation, - num_spheres=auto.num_spheres, - sphere_density=auto.sphere_density, - surface_radius=auto.surface_radius, - iterations=auto.iterations, - collision_sphere_buffer=auto.collision_sphere_buffer, - device=str(self._curobo_device), - ) + logger.log_info(f"cuRobo voxel world cache hit: {cache_path}") + scene_data = torch.load(cache_path, map_location="cpu", weights_only=True) + else: + logger.log_info( + f"Generating VisACD voxel collision data from " + f"{len(rigid_objects)} RigidObject(s) -> {cache_path}" + ) + scene_data = generate_curobo_world_scene( + rigid_objects, + voxel_size=world_cfg.voxel_size, + voxel_padding=world_cfg.voxel_padding, + ) + os.makedirs(cache_dir, exist_ok=True) + torch.save(scene_data, cache_path) - def _world_yaml_cache_key(self, world_cfg: CuroboWorldCfg) -> str: - """Hash per-object mesh/pose + representation + fit params into a cache key. + runtime_data = deepcopy(scene_data) + for voxel in runtime_data.get("voxel", {}).values(): + voxel["feature_tensor"] = voxel["feature_tensor"].to( + device=self._curobo_device, dtype=torch.float16 + ) + return self._bindings.Scene.create(runtime_data) - Includes each object's vertex/face/pose bytes so editing the simulator - geometry or moving a static obstacle regenerates the YAML, matching the - robot-YAML cache's URDF-content inclusion. - """ + def _world_scene_cache_key(self, world_cfg: CuroboWorldCfg) -> str: + """Hash object geometry, initial poses, and voxel settings.""" hasher = hashlib.md5() - hasher.update(world_cfg.obstacle_representation.encode("utf-8")) - auto = self.cfg.auto_gen - hasher.update(_CUROBO_ROBOT_YAML_GENERATOR_VERSION.encode("utf-8")) - hasher.update(str(auto.num_spheres).encode("utf-8")) - hasher.update(str(auto.sphere_density).encode("utf-8")) - hasher.update(str(auto.surface_radius).encode("utf-8")) - hasher.update(str(auto.iterations).encode("utf-8")) - hasher.update(str(auto.collision_sphere_buffer).encode("utf-8")) + hasher.update(_CUROBO_WORLD_CACHE_VERSION.encode("utf-8")) + hasher.update(str(world_cfg.voxel_size).encode("utf-8")) + hasher.update(str(world_cfg.voxel_padding).encode("utf-8")) for idx, obj in enumerate(world_cfg.rigid_objects or []): name = getattr(obj, "uid", None) or f"obstacle_{idx}" hasher.update(name.encode("utf-8")) @@ -1940,41 +1604,31 @@ def visualize_collision_models( control_part: str, env_id: int = 0, ) -> None: - """Visualize cached robot and obstacle spheres at their simulator poses. + """Visualize cached robot spheres and world collision voxels. - This materializes the same content-addressed YAML caches used by the - planner, then reads sphere centers and radii back from those files. The + This materializes the same content-addressed caches used by the planner. The robot spheres are transformed by each link's live - :meth:`~embodichain.lab.sim.objects.Articulation.get_link_pose`; static - obstacle spheres retain the exact world positions serialized in the - world cache. + :meth:`~embodichain.lab.sim.objects.Articulation.get_link_pose`; obstacle + voxels retain the poses and ESDF values consumed by cuRobo. Args: control_part: Robot control part whose cuRobo profile/cache is used. env_id: Simulator environment instance to visualize. - Raises: - ValueError: If obstacles are configured with a non-sphere - representation. """ from .curobo_yaml import visualize_curobo_collision_models profile = self._materialize_profile(control_part) world_cfg = self.cfg.world rigid_objects = world_cfg.rigid_objects - world_yaml_path = None + world_scene = None if rigid_objects: - if world_cfg.obstacle_representation != "sphere": - raise ValueError( - "Obstacle collision-model visualization requires " - "CuroboWorldCfg.obstacle_representation='sphere'." - ) - world_yaml_path = self._auto_generate_world_yaml(world_cfg) + world_scene = self._auto_generate_world_scene(world_cfg) visualize_curobo_collision_models( self.robot, profile.robot_config_path, rigid_objects, - world_yaml_path, + world_scene, env_id, ) diff --git a/embodichain/lab/sim/planners/curobo/curobo_sphere_data.py b/embodichain/lab/sim/planners/curobo/curobo_sphere_data.py deleted file mode 100644 index ad4f34b09..000000000 --- a/embodichain/lab/sim/planners/curobo/curobo_sphere_data.py +++ /dev/null @@ -1,275 +0,0 @@ -# ---------------------------------------------------------------------------- -# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. -# -# 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. -# ---------------------------------------------------------------------------- - -"""Analytic sphere-obstacle storage for cuRobo V2's generic Warp checker.""" - -from __future__ import annotations - -from dataclasses import dataclass -from typing import TYPE_CHECKING - -import torch -import warp as wp - -from curobo._src.geom.data.helper_pose import ( - get_obs_idx, - load_transform_from_inv_pose, -) -from curobo._src.util.logging import log_and_raise - -if TYPE_CHECKING: - from curobo._src.geom.types import SceneCfg, Sphere - from curobo._src.types.device_cfg import DeviceCfg - from curobo._src.types.pose import Pose - -__all__ = [ - "SphereData", - "SphereDataWarp", - "compute_local_sdf", - "compute_local_sdf_with_grad", - "is_obs_enabled", - "load_obstacle_transform", -] - -_SDF_EPS = 1.0e-8 - - -@wp.struct -class SphereDataWarp: - """Warp view of batched analytic sphere obstacles.""" - - radius: wp.array(dtype=wp.float32) - inv_pose: wp.array2d(dtype=wp.float32) - enable: wp.array(dtype=wp.uint8) - n_per_env: wp.array(dtype=wp.int32) - max_n: wp.int32 - num_envs: wp.int32 - - -@dataclass -class SphereData: - """GPU tensor storage for analytic sphere obstacles.""" - - radius: torch.Tensor - inv_pose: torch.Tensor - enable: torch.Tensor - count: torch.Tensor - names: list[list[str | None]] - max_n: int - num_envs: int - device_cfg: "DeviceCfg" - - @classmethod - def create_cache( - cls, max_n: int, num_envs: int, device_cfg: "DeviceCfg" - ) -> "SphereData": - """Create an empty fixed-capacity sphere cache.""" - radius = torch.zeros( - (num_envs, max_n), - dtype=device_cfg.dtype, - device=device_cfg.device, - ) - inv_pose = torch.zeros( - (num_envs, max_n, 8), - dtype=device_cfg.dtype, - device=device_cfg.device, - ) - inv_pose[..., 3] = 1.0 - enable = torch.zeros( - (num_envs, max_n), dtype=torch.uint8, device=device_cfg.device - ) - count = torch.zeros((num_envs,), dtype=torch.int32, device=device_cfg.device) - return cls( - radius=radius, - inv_pose=inv_pose, - enable=enable, - count=count, - names=[[None for _ in range(max_n)] for _ in range(num_envs)], - max_n=max_n, - num_envs=num_envs, - device_cfg=device_cfg, - ) - - @classmethod - def from_scene_cfg( - cls, - scene_cfg: "SceneCfg", - device_cfg: "DeviceCfg", - env_idx: int = 0, - num_envs: int = 1, - max_n: int | None = None, - ) -> "SphereData": - """Create storage from one cuRobo scene.""" - spheres = scene_cfg.sphere or [] - capacity = max_n if max_n is not None else max(len(spheres), 1) - instance = cls.create_cache(capacity, num_envs, device_cfg) - if spheres: - instance.load_batch(spheres, env_idx) - return instance - - @classmethod - def from_batch_scene_cfg( - cls, - scene_cfg_list: list["SceneCfg"], - device_cfg: "DeviceCfg", - max_n: int | None = None, - ) -> "SphereData": - """Create storage from independent batched scenes.""" - num_envs = len(scene_cfg_list) - counts = [len(scene.sphere or []) for scene in scene_cfg_list] - capacity = max_n if max_n is not None else max(max(counts), 1) - instance = cls.create_cache(capacity, num_envs, device_cfg) - for env_idx, scene in enumerate(scene_cfg_list): - if scene.sphere: - instance.load_batch(scene.sphere, env_idx) - return instance - - def load_batch(self, spheres: list["Sphere"], env_idx: int) -> None: - """Replace one environment's sphere obstacles.""" - if len(spheres) > self.max_n: - log_and_raise( - f"Cannot load {len(spheres)} spheres, max cache size is {self.max_n}" - ) - if not spheres: - self.clear(env_idx) - return - num_spheres = len(spheres) - centers = torch.as_tensor( - [sphere.pose[:3] for sphere in spheres], - dtype=self.device_cfg.dtype, - device=self.device_cfg.device, - ) - inverse_poses = torch.zeros( - (num_spheres, 7), - dtype=self.device_cfg.dtype, - device=self.device_cfg.device, - ) - # Sphere orientation is immaterial. Identity rotation plus translated - # origin is the exact world-to-local transform and avoids cuRobo's - # CUDA-only generic pose-inverse kernel. - inverse_poses[:, :3] = -centers - inverse_poses[:, 3] = 1.0 - self.radius[env_idx, :num_spheres] = torch.as_tensor( - [sphere.radius for sphere in spheres], - dtype=self.device_cfg.dtype, - device=self.device_cfg.device, - ) - self.inv_pose[env_idx, :num_spheres, :7] = inverse_poses - self.enable[env_idx, :num_spheres] = 1 - self.enable[env_idx, num_spheres:] = 0 - self.names[env_idx][:num_spheres] = [sphere.name for sphere in spheres] - self.names[env_idx][num_spheres:] = [None] * (self.max_n - num_spheres) - self.count[env_idx] = num_spheres - - def update_pose(self, name: str, pose: Pose, env_idx: int = 0) -> None: - """Update a named sphere pose.""" - idx = self.get_idx(name, env_idx) - position = pose.position.reshape(-1, 3)[0].to(self.inv_pose) - self.inv_pose[env_idx, idx, :3] = -position - self.inv_pose[env_idx, idx, 3:7] = torch.as_tensor( - [1.0, 0.0, 0.0, 0.0], - dtype=self.device_cfg.dtype, - device=self.device_cfg.device, - ) - - def set_enabled(self, name: str, enabled: bool, env_idx: int = 0) -> None: - """Enable or disable a named sphere.""" - self.enable[env_idx, self.get_idx(name, env_idx)] = int(enabled) - - def get_idx(self, name: str, env_idx: int = 0) -> int: - """Return a named sphere's local index.""" - try: - return self.names[env_idx].index(name) - except ValueError: - log_and_raise( - f"Sphere with name '{name}' not found in environment {env_idx}" - ) - raise AssertionError("unreachable") - - def get_names(self, env_idx: int = 0) -> list[str]: - """Return active sphere names.""" - return self.names[env_idx][: int(self.count[env_idx].item())] - - def clear(self, env_idx: int | None = None) -> None: - """Disable all spheres in one or every environment.""" - if env_idx is None: - self.enable.zero_() - self.count.zero_() - self.names = [ - [None for _ in range(self.max_n)] for _ in range(self.num_envs) - ] - else: - self.enable[env_idx].zero_() - self.count[env_idx] = 0 - self.names[env_idx] = [None for _ in range(self.max_n)] - - def to_warp(self) -> SphereDataWarp: - """Return the Warp view consumed by cuRobo's generic kernels.""" - data = SphereDataWarp() - data.radius = wp.from_torch(self.radius.view(-1), dtype=wp.float32) - data.inv_pose = wp.from_torch(self.inv_pose.view(-1, 8), dtype=wp.float32) - data.enable = wp.from_torch(self.enable.view(-1), dtype=wp.uint8) - data.n_per_env = wp.from_torch(self.count.view(-1), dtype=wp.int32) - data.max_n = self.max_n - data.num_envs = self.num_envs - return data - - -def is_obs_enabled( - obs_set: SphereDataWarp, env_idx: wp.int32, local_idx: wp.int32 -) -> wp.bool: - """Return whether a sphere slot is active.""" - flat_idx = get_obs_idx(env_idx, local_idx, obs_set.max_n) - return obs_set.enable[flat_idx] == wp.uint8(1) - - -def load_obstacle_transform( - obs_set: SphereDataWarp, env_idx: wp.int32, local_idx: wp.int32 -) -> wp.transform: - """Load a sphere's world-to-local transform.""" - flat_idx = get_obs_idx(env_idx, local_idx, obs_set.max_n) - return load_transform_from_inv_pose(obs_set.inv_pose, flat_idx) - - -def compute_local_sdf( - obs_set: SphereDataWarp, - env_idx: wp.int32, - local_idx: wp.int32, - local_pt: wp.vec3, -) -> wp.float32: - """Return analytic signed distance to a sphere surface.""" - flat_idx = get_obs_idx(env_idx, local_idx, obs_set.max_n) - return wp.length(local_pt) - obs_set.radius[flat_idx] - - -def compute_local_sdf_with_grad( - obs_set: SphereDataWarp, - env_idx: wp.int32, - local_idx: wp.int32, - local_pt: wp.vec3, -) -> wp.vec4: - """Return analytic sphere SDF and negative spatial gradient.""" - flat_idx = get_obs_idx(env_idx, local_idx, obs_set.max_n) - distance = wp.length(local_pt) - gx = wp.float32(0.0) - gy = wp.float32(0.0) - gz = wp.float32(0.0) - if distance > _SDF_EPS: - inverse_distance = -1.0 / distance - gx = local_pt[0] * inverse_distance - gy = local_pt[1] * inverse_distance - gz = local_pt[2] * inverse_distance - return wp.vec4(distance - obs_set.radius[flat_idx], gx, gy, gz) diff --git a/embodichain/lab/sim/planners/curobo/curobo_yaml.py b/embodichain/lab/sim/planners/curobo/curobo_yaml.py index 9e2b0f175..488faa357 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_yaml.py +++ b/embodichain/lab/sim/planners/curobo/curobo_yaml.py @@ -22,8 +22,8 @@ this automatically (with on-disk caching) on the first plan; see :class:`~embodichain.lab.sim.planners.curobo.curobo_planner.CuroboAutoGenCfg`. -:func:`generate_curobo_world_yaml` builds the cuRobo collision-world YAML from -live :class:`~embodichain.lab.sim.objects.RigidObject` meshes. +:func:`generate_curobo_world_scene` builds cuRobo voxel collision data from live +:class:`~embodichain.lab.sim.objects.RigidObject` meshes. """ from __future__ import annotations @@ -40,7 +40,7 @@ __all__ = [ "generate_curobo_robot_yaml", - "generate_curobo_world_yaml", + "generate_curobo_world_scene", "visualize_curobo_collision_models", "visualize_curobo_robot_collision_model", "visualize_curobo_world_collision_model", @@ -111,7 +111,7 @@ def _to_open3d_tensor_mesh( faces: torch.Tensor, o3d: Any, ) -> Any: - """Create the Open3D tensor mesh expected by DexSim's ``sphere_fit``.""" + """Create an Open3D tensor triangle mesh from tensor-like geometry.""" return o3d.t.geometry.TriangleMesh.from_legacy( _to_open3d_legacy_mesh(vertices, faces, o3d) ) @@ -324,68 +324,46 @@ def generate_curobo_robot_yaml( # ============================================================================= -# World (obstacle) YAML generation from RigidObject meshes +# World voxel generation from RigidObject meshes # ============================================================================= -_REPRESENTATIONS = ("cuboid", "mesh", "sphere") +def _voxel_grid_coordinates( + grid_shape: tuple[int, int, int], voxel_size: float +) -> torch.Tensor: + """Return voxel centers in cuRobo's X/Y/Z flattening order.""" + axes = [ + (torch.arange(size, dtype=torch.float32) - (size - 1) / 2.0) * voxel_size + for size in grid_shape + ] + return torch.stack(torch.meshgrid(*axes, indexing="ij"), dim=-1).reshape(-1, 3) -def _mesh_to_obstacle_entry( +def _convex_hulls_to_voxel_entry( name: str, vertices: torch.Tensor, faces: torch.Tensor, pose: torch.Tensor, *, - representation: str = "cuboid", - num_spheres: int | None = None, - sphere_density: float = 1.0, - surface_radius: float = 0.005, - iterations: int = 200, - collision_sphere_buffer: float = 0.0, - device: str = "cuda:0", -) -> list[tuple[str, str, dict]]: - """Convert one mesh + pose into cuRobo world-YAML obstacle entry/entries. - - Pure tensor helper (no simulator import for ``cuboid``/``mesh``) so it is - unit-testable without CUDA. ``sphere`` lazily imports DexSim + Open3D. - - Args: - name: Obstacle name (cuRobo key under ``cuboid``/``mesh``/``sphere``). - vertices: Mesh vertices ``(V, 3)`` in the object's local frame. - faces: Triangle indices ``(F, 3)`` (any integer dtype). - pose: Object pose as ``(x, y, z, qw, qx, qy, qz)`` ``(7,)`` or a - homogeneous ``(4, 4)`` matrix, expressed in the cuRobo world/base - frame (the same frame static collision YAMLs are authored in). - representation: ``"cuboid"`` (local-frame AABB -> OBB via ``pose``, - default), ``"mesh"`` (exact triangle mesh), or ``"sphere"`` (fit - spheres with DexSim's :func:`sphere_fit`). - num_spheres: Per-mesh sphere count; ``None`` auto-estimates (sphere only). - sphere_density: Multiplier on the auto sphere count (sphere only). - surface_radius: Fixed radius for MorphIt's surface fallback (sphere only). - iterations: Adam iterations for MorphIt (sphere only). - collision_sphere_buffer: Padding added to each fitted radius (sphere only). - device: CUDA device for sphere fitting (sphere only). + voxel_size: float = 0.01, + voxel_padding: float = 0.1, +) -> tuple[str, dict[str, object]]: + """Decompose one mesh with VisACD and convert its union to an ESDF grid. - Returns: - A list of ``(top_level_key, obstacle_name, fields)`` tuples. ``cuboid``/ - ``mesh`` return one entry; ``sphere`` returns one entry per fitted sphere. - - Raises: - ValueError: If ``representation`` is unsupported, ``pose`` is malformed, - or the mesh has no geometry for the requested representation. - RuntimeError: If ``"sphere"`` is requested without CUDA. - ImportError: If ``"sphere"`` is requested without DexSim/Open3D. + The grid is centered at the object's local origin, so the voxel obstacle's + pose stays identical to the source object's pose during dynamic updates. """ - if representation not in _REPRESENTATIONS: - raise ValueError( - f"representation must be one of {_REPRESENTATIONS}, got {representation!r}." - ) - vertices = ( torch.as_tensor(vertices, dtype=torch.float32).detach().to("cpu").reshape(-1, 3) ) - faces = torch.as_tensor(faces).detach().to("cpu") + faces = torch.as_tensor(faces).detach().to("cpu").reshape(-1, 3) + if vertices.numel() == 0 or faces.numel() == 0: + raise ValueError(f"object {name!r} has no mesh geometry for voxelization.") + if voxel_size <= 0.0: + raise ValueError(f"voxel_size must be positive, got {voxel_size}.") + if voxel_padding < 0.0: + raise ValueError(f"voxel_padding must be non-negative, got {voxel_padding}.") + pose = torch.as_tensor(pose, dtype=torch.float32).detach().to("cpu") if pose.shape == (4, 4): position = pose[:3, 3] @@ -396,192 +374,119 @@ def _mesh_to_obstacle_entry( f"pose must be (7,) [x,y,z,qw,qx,qy,qz] or (4, 4), got {tuple(pose.shape)}." ) - if representation == "mesh": - if vertices.numel() == 0 or faces.numel() == 0: - raise ValueError( - f"object {name!r} has no mesh geometry for the 'mesh' representation." - ) - return [ - ( - "mesh", - name, - { - "vertices": vertices.tolist(), - "faces": faces.reshape(-1).to(torch.int64).tolist(), - "pose": pose.tolist(), - }, - ) - ] - - if representation == "cuboid": - if vertices.numel() == 0: - raise ValueError( - f"object {name!r} has no vertices for the 'cuboid' representation." - ) - # Local-frame AABB, emitted as an OBB via the object pose: cuRobo's - # Cuboid is centered at ``pose[:3]`` with ``dims`` along the pose axes. - vmin = vertices.amin(dim=0) - vmax = vertices.amax(dim=0) - dims = vmax - vmin - center_local = (vmin + vmax) / 2.0 - rotation = matrix_from_quat(pose[3:7]) # (3, 3), wxyz - center_world = rotation @ center_local + pose[:3] - cuboid_pose = torch.cat([center_world, pose[3:7]]) - return [("cuboid", name, {"dims": dims.tolist(), "pose": cuboid_pose.tolist()})] - - # representation == "sphere": fit spheres in the local frame, then transform - # centers into the cuRobo world/base frame (Sphere obstacles have no pose/FK). - if vertices.numel() == 0 or faces.numel() == 0: - raise ValueError( - f"object {name!r} has no mesh geometry for the 'sphere' representation." - ) - if not torch.cuda.is_available(): - raise RuntimeError( - "The 'sphere' representation requires CUDA for DexSim MorphIt fitting." - ) - import open3d as o3d - from dexsim.kit.meshproc import SphereFitType, sphere_fit + from dexsim.kit.meshproc import convex_decomposition_visacd mesh = _to_open3d_tensor_mesh(vertices, faces, o3d) - is_success, centers, fitted_radii = sphere_fit( + is_success, convex_hulls = convex_decomposition_visacd( mesh, - num_spheres=num_spheres, - sphere_density=sphere_density, - surface_radius=surface_radius, - fit_type=SphereFitType.MORPHIT, - iterations=iterations, max_convex_hull_num=_OBSTACLE_MAX_CONVEX_HULL_NUM, - device=device, + is_visual=False, ) - if not is_success: - raise RuntimeError(f"No spheres could be fitted for object {name!r}.") + if not is_success or not convex_hulls: + raise RuntimeError(f"VisACD decomposition failed for object {name!r}.") - centers_local = centers.detach().to("cpu").reshape(-1, 3).to(torch.float32) - radii = fitted_radii.detach().to("cpu").reshape(-1).to(torch.float32) + float( - collision_sphere_buffer + local_half_extent = torch.maximum( + vertices.amin(dim=0).abs(), vertices.amax(dim=0).abs() ) - rotation = matrix_from_quat(pose[3:7]) - centers_world = centers_local @ rotation.T + pose[:3] - entries: list[tuple[str, str, dict]] = [] - for i in range(centers_world.shape[0]): - entries.append( - ( - "sphere", - f"{name}_{i}", - { - "position": centers_world[i].tolist(), - "radius": float(radii[i].item()), - }, - ) - ) - return entries + requested_dims = 2.0 * (local_half_extent + float(voxel_padding)) + grid_shape_tensor = torch.ceil(requested_dims / float(voxel_size)).to(torch.int64) + grid_shape_tensor = torch.clamp(grid_shape_tensor, min=2) + grid_shape = tuple(int(value) for value in grid_shape_tensor.tolist()) + dims = grid_shape_tensor.to(torch.float32) * float(voxel_size) + query_points = _voxel_grid_coordinates(grid_shape, float(voxel_size)) + query_o3d = o3d.core.Tensor(query_points.numpy(), dtype=o3d.core.Dtype.Float32) + + union_sdf = torch.full((query_points.shape[0],), torch.inf, dtype=torch.float32) + for hull in convex_hulls: + hull_cpu = hull.cpu() if hasattr(hull, "cpu") else hull + scene = o3d.t.geometry.RaycastingScene() + scene.add_triangles(hull_cpu) + hull_sdf = torch.from_numpy( + scene.compute_signed_distance(query_o3d).numpy() + ).to(torch.float32) + union_sdf = torch.minimum(union_sdf, hull_sdf) + + feature_tensor = union_sdf.reshape(grid_shape).to(torch.float16).contiguous() + return name, { + "pose": pose.tolist(), + "dims": dims.tolist(), + "voxel_size": float(voxel_size), + "feature_tensor": feature_tensor, + } -def generate_curobo_world_yaml( +def generate_curobo_world_scene( rigid_objects: Sequence[RigidObject], - output_path: str, *, - representation: str = "cuboid", env_id: int = 0, - num_spheres: int | None = None, - sphere_density: float = 1.0, - surface_radius: float = 0.005, - iterations: int = 200, - collision_sphere_buffer: float = 0.0, - device: str = "cuda:0", -) -> str: - """Generate a cuRobo V2 scene (world) YAML from a sequence of ``RigidObject``. - - Each object's mesh (``get_vertices`` / ``get_triangles``) and world pose - (``get_local_pose``) are converted into cuRobo obstacle entries under a single - top-level key (``cuboid`` / ``mesh`` / ``sphere``). The cuRobo planner loads - the resulting YAML as its collision world. + voxel_size: float = 0.01, + voxel_padding: float = 0.1, +) -> dict[str, dict[str, dict[str, object]]]: + """Build a VisACD-decomposed ESDF voxel scene for cuRobo. - .. attention:: - Poses are written in the cuRobo world/base frame - the same convention as - a hand-authored static collision YAML. When the robot base is offset from - the simulator world origin, rebase the object poses first, or register the - obstacle name in ``CuroboWorldCfg.dynamic_obstacle_names`` and update its - pose at plan time via - :meth:`~embodichain.lab.sim.planners.curobo.curobo_planner.CuroboPlanner.update_dynamic_obstacles`. + Every source object produces one same-named voxel layer. The SDF is the + union of at most 16 convex hulls returned by DexSim's + :func:`convex_decomposition_visacd`. Args: - rigid_objects: ``RigidObject`` instances to bake into the collision world. - output_path: Destination YAML file path. - representation: ``"cuboid"`` (default, AABB->OBB, no CUDA), ``"mesh"`` - (exact triangle mesh, no CUDA), or ``"sphere"`` (DexSim MorphIt - sphere fit, requiring CUDA + DexSim + Open3D). - env_id: Environment instance index to read geometry/pose from (the static - world is shared, so env 0 is representative). - num_spheres: Per-object sphere count; ``None`` auto-estimates (sphere only). - sphere_density: Multiplier on the auto sphere count (sphere only). - surface_radius: Fixed radius for MorphIt's surface fallback (sphere only). - iterations: Adam iterations for MorphIt (sphere only). - collision_sphere_buffer: Padding added to each fitted radius (sphere only). - device: CUDA device for sphere fitting (sphere only). + rigid_objects: Live obstacles whose meshes define the collision world. + env_id: Environment row used for geometry and initial object poses. + voxel_size: ESDF voxel edge length in meters. + voxel_padding: Free-space padding around each object-local mesh. Returns: - The ``output_path`` that was written. + A tensor-backed ``{"voxel": ...}`` mapping accepted by cuRobo + :meth:`Scene.create`. Raises: - ValueError: If ``rigid_objects`` is empty or a representation/pose is - invalid. + ValueError: If no usable uniquely named objects are provided. + RuntimeError: If DexSim VisACD decomposition fails. """ - import os - - import yaml - rigid_objects = list(rigid_objects) if not rigid_objects: raise ValueError("rigid_objects must contain at least one RigidObject.") - data: dict[str, dict[str, object]] = {} - used_names: set[str] = set() + voxels: dict[str, dict[str, object]] = {} for idx, obj in enumerate(rigid_objects): name = getattr(obj, "uid", None) or f"obstacle_{idx}" - if name in used_names: + if name in voxels: raise ValueError( f"Duplicate obstacle name {name!r}; RigidObject uids must be unique." ) - used_names.add(name) - vertices = obj.get_vertices(env_ids=[env_id], scale=True)[0] faces = obj.get_triangles(env_ids=[env_id])[0] pose = obj.get_local_pose(to_matrix=False)[env_id] - if vertices is None or faces is None or vertices.numel() == 0: logger.log_warning( f"RigidObject {name!r} has no mesh geometry; skipping collision export." ) continue - - entries = _mesh_to_obstacle_entry( + obstacle_name, fields = _convex_hulls_to_voxel_entry( name, vertices, faces, pose, - representation=representation, - num_spheres=num_spheres, - sphere_density=sphere_density, - surface_radius=surface_radius, - iterations=iterations, - collision_sphere_buffer=collision_sphere_buffer, - device=device, + voxel_size=voxel_size, + voxel_padding=voxel_padding, ) - for top_key, obstacle_name, fields in entries: - data.setdefault(top_key, {})[obstacle_name] = fields + voxels[obstacle_name] = fields - if not data: + if not voxels: raise ValueError( "No collision obstacles could be generated from the given RigidObjects." ) - - os.makedirs(os.path.dirname(os.path.abspath(output_path)), exist_ok=True) - with open(output_path, "w") as yaml_file: - yaml.dump(data, yaml_file, default_flow_style=False, sort_keys=False) - return output_path + # VoxelData allocates each layer with the first layer's capacity. Keep the + # largest layer first so differently-sized object grids all fit the cache. + voxels = dict( + sorted( + voxels.items(), + key=lambda item: int(item[1]["feature_tensor"].numel()), + reverse=True, + ) + ) + return {"voxel": voxels} # ============================================================================= @@ -713,17 +618,16 @@ def visualize_curobo_robot_collision_model( def visualize_curobo_world_collision_model( rigid_objects: Sequence[RigidObject], - world_yaml_path: str, + world_scene: Any, env_id: int = 0, *, draw: bool = True, ) -> list[dict[str, Any]]: - """Visualize live obstacle meshes and spheres loaded from a cached world YAML. + """Visualize live obstacle meshes and cuRobo ESDF collision voxels. Args: rigid_objects: Live simulator obstacles represented by the cache. - world_yaml_path: Cached auto-generated cuRobo world YAML. It must use - the ``sphere`` representation. + world_scene: Tensor-backed scene mapping or a cuRobo ``Scene`` instance. env_id: Simulator environment instance whose live meshes are shown. draw: Open an Open3D window immediately. ``False`` returns draw entries for composition with the robot collision model. @@ -732,15 +636,15 @@ def visualize_curobo_world_collision_model( Open3D geometry dictionaries suitable for :func:`open3d.visualization.draw`. """ import open3d as o3d - import yaml - with open(world_yaml_path, encoding="utf-8") as yaml_file: - data = yaml.safe_load(yaml_file) - sphere_entries = data.get("sphere", {}) if isinstance(data, dict) else {} - if not sphere_entries: - raise ValueError( - f"World cache {world_yaml_path!r} contains no sphere representation." - ) + if isinstance(world_scene, dict): + voxel_entries = list(world_scene.get("voxel", {}).items()) + else: + voxel_entries = [ + (voxel.name, voxel) for voxel in (getattr(world_scene, "voxel", None) or []) + ] + if not voxel_entries: + raise ValueError("The cuRobo world scene contains no voxel collision data.") meshes: list[tuple[str, Any]] = [] for idx, obj in enumerate(rigid_objects): @@ -756,20 +660,33 @@ def visualize_curobo_world_collision_model( mesh.transform(pose.numpy()) meshes.append((f"obstacle_mesh/{name}", mesh)) - centers = torch.as_tensor( - [entry["position"] for entry in sphere_entries.values()], dtype=torch.float32 - ).reshape(-1, 3) - radii = torch.as_tensor( - [entry["radius"] for entry in sphere_entries.values()], dtype=torch.float32 - ).reshape(-1) - geometries = _collision_visualization_geometries( - meshes, - centers, - radii, - sphere_name="obstacle_spheres", - sphere_color=[0.8, 0.15, 0.0, 0.5], - mesh_color=[0.45, 0.55, 0.45, 1.0], - ) + mesh_material = o3d.visualization.rendering.MaterialRecord() + mesh_material.shader = "defaultLit" + mesh_material.base_color = [0.45, 0.55, 0.45, 1.0] + geometries = [ + {"name": name, "geometry": mesh, "material": mesh_material} + for name, mesh in meshes + ] + for name, entry in voxel_entries: + get_value = ( + entry.get if isinstance(entry, dict) else lambda key: getattr(entry, key) + ) + features = torch.as_tensor(get_value("feature_tensor")).detach().cpu() + voxel_size = float(get_value("voxel_size")) + local_points = _voxel_grid_coordinates(tuple(features.shape), voxel_size) + occupied = features.reshape(-1) <= 0.5 * voxel_size + if not torch.any(occupied): + continue + pose = torch.as_tensor(get_value("pose"), dtype=torch.float32) + rotation = matrix_from_quat(pose[3:7]) + world_points = local_points[occupied] @ rotation.T + pose[:3] + point_cloud = o3d.geometry.PointCloud() + point_cloud.points = o3d.utility.Vector3dVector(world_points.numpy()) + point_cloud.paint_uniform_color([0.8, 0.15, 0.0]) + voxel_grid = o3d.geometry.VoxelGrid.create_from_point_cloud( + point_cloud, voxel_size=voxel_size + ) + geometries.append({"name": f"obstacle_voxels/{name}", "geometry": voxel_grid}) if draw: o3d.visualization.draw(geometries, title="cuRobo obstacle collision model") return geometries @@ -779,19 +696,19 @@ def visualize_curobo_collision_models( robot: Robot, robot_yaml_path: str, rigid_objects: Sequence[RigidObject] | None = None, - world_yaml_path: str | None = None, + world_scene: Any | None = None, env_id: int = 0, ) -> None: - """Draw cached robot and obstacle collision spheres in one Open3D window.""" + """Draw cached robot spheres and obstacle collision voxels together.""" import open3d as o3d geometries = visualize_curobo_robot_collision_model( robot, robot_yaml_path, env_id, draw=False ) - if rigid_objects and world_yaml_path is not None: + if rigid_objects and world_scene is not None: geometries.extend( visualize_curobo_world_collision_model( - rigid_objects, world_yaml_path, env_id, draw=False + rigid_objects, world_scene, env_id, draw=False ) ) o3d.visualization.draw(geometries, title="cuRobo collision models") diff --git a/examples/sim/planners/curobo_planner.py b/examples/sim/planners/curobo_planner.py index 86c216ccd..5430273d0 100644 --- a/examples/sim/planners/curobo_planner.py +++ b/examples/sim/planners/curobo_planner.py @@ -17,7 +17,8 @@ """cuRobo V2 collision-aware planning through the atomic-action interface. The demo creates one or more copies of the selected robot and a kinematic -cuboid represented in both DexSim and cuRobo. With multiple environments, each +DexSim cuboid converted through VisACD into a cuRobo ESDF voxel obstacle. With +multiple environments, each obstacle receives a small reproducible XY/yaw perturbation and cuRobo allocates an independent collision world for each environment. The demo then executes a batched ``MoveEndEffector`` action through :class:`AtomicActionEngine`, replays @@ -746,7 +747,6 @@ def main() -> None: robot_uid=robot.uid, world=CuroboWorldCfg( rigid_objects=obstacles, - obstacle_representation=("sphere"), dynamic_obstacle_names=( [obstacle.uid for obstacle in obstacles] if use_independent_worlds @@ -761,8 +761,8 @@ def main() -> None: ) ) if visualize_collision_models: - # This opens one blocking Open3D window. The spheres are loaded from - # the exact robot/world YAML caches consumed by cuRobo; close the + # This opens one blocking Open3D window. Robot spheres and obstacle + # voxels come from the exact caches consumed by cuRobo; close the # window to continue with planner backend creation and execution. motion_generator.planner.visualize_collision_models(control_part) engine = AtomicActionEngine(motion_generator) diff --git a/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md b/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md index 75bcef44c..331bef559 100644 --- a/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md +++ b/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md @@ -337,8 +337,8 @@ planner supports interruption. cuRobo lazily creates and caches a backend for each `(control_part, batch_size, multi_env, move_type)`. First use may include -robot/world YAML generation, sphere fitting, collision-cache allocation, CUDA -graph capture, and warmup. NMG has checkpoint loading, actor construction, and +robot sphere fitting, voxel-world generation, CUDA graph capture, and warmup. +NMG has checkpoint loading, actor construction, and device transfer. Report the following separately for both: @@ -654,7 +654,6 @@ planners: warmup_iterations: 1 preserve_plan_samples: true world: - obstacle_representation: mesh multi_env: false tracks: diff --git a/scripts/benchmark/motion_generation/planners/curobo.py b/scripts/benchmark/motion_generation/planners/curobo.py index 2b4e34716..cb5b47a76 100644 --- a/scripts/benchmark/motion_generation/planners/curobo.py +++ b/scripts/benchmark/motion_generation/planners/curobo.py @@ -74,12 +74,8 @@ def build(self) -> None: ) world = CuroboWorldCfg( rigid_objects=None, - obstacle_representation=str( - world_values.get("obstacle_representation", "sphere") - ), - collision_cache=dict( - world_values.get("collision_cache", {"cuboid": 8, "mesh": 2}) - ), + voxel_size=float(world_values.get("voxel_size", 0.01)), + voxel_padding=float(world_values.get("voxel_padding", 0.1)), dynamic_obstacle_names=[], multi_env=False, ) diff --git a/scripts/benchmark/motion_generation/suites/coverage.yaml b/scripts/benchmark/motion_generation/suites/coverage.yaml index ebcd8faa9..dd5d8386e 100644 --- a/scripts/benchmark/motion_generation/suites/coverage.yaml +++ b/scripts/benchmark/motion_generation/suites/coverage.yaml @@ -18,10 +18,8 @@ planners: warmup_iterations: 1 preserve_plan_samples: true world: - obstacle_representation: sphere multi_env: false auto_gen: - fit_type: voxel sphere_density: 0.1 collision_sphere_buffer: 0.0 - id: ik_interpolate diff --git a/scripts/benchmark/motion_generation/suites/smoke.yaml b/scripts/benchmark/motion_generation/suites/smoke.yaml index eafbbc5a6..aae619952 100644 --- a/scripts/benchmark/motion_generation/suites/smoke.yaml +++ b/scripts/benchmark/motion_generation/suites/smoke.yaml @@ -18,10 +18,8 @@ planners: warmup_iterations: 1 preserve_plan_samples: true world: - obstacle_representation: sphere multi_env: false auto_gen: - fit_type: voxel sphere_density: 0.1 collision_sphere_buffer: 0.0 - id: ik_interpolate diff --git a/tests/sim/planners/test_curobo_integration.py b/tests/sim/planners/test_curobo_integration.py index 5c941900c..1efa69207 100644 --- a/tests/sim/planners/test_curobo_integration.py +++ b/tests/sim/planners/test_curobo_integration.py @@ -84,14 +84,12 @@ def _make_sim_robot(num_envs: int = 1): @pytest.mark.slow -def test_curobo_v2_plans_around_a_static_cuboid(): +def test_curobo_v2_plans_around_a_static_voxel_obstacle(): sim, robot, block = _make_sim_robot() try: cfg = CuroboPlannerCfg( robot_uid=ROBOT_UID, - world=CuroboWorldCfg( - rigid_objects=[block], obstacle_representation="cuboid" - ), + world=CuroboWorldCfg(rigid_objects=[block]), # Skipping optional non-graph warmup keeps fresh CI runs practical. warmup_iterations=0, ) @@ -136,18 +134,13 @@ def test_curobo_v2_plans_around_a_static_cuboid(): @pytest.mark.slow -def test_curobo_v2_plans_around_rigid_object_mesh_world(): - """Auto-generate the collision world from a live RigidObject mesh and plan. - - Uses the ``mesh`` representation (exact triangle mesh) to exercise the full - mesh -> cuRobo world-YAML path end-to-end, complementing the default - ``cuboid`` path in :func:`test_curobo_v2_plans_around_a_static_cuboid`. - """ +def test_curobo_v2_plans_around_rigid_object_voxel_world(): + """Exercise mesh -> VisACD convex hulls -> voxel ESDF planning end to end.""" sim, robot, block = _make_sim_robot() try: cfg = CuroboPlannerCfg( robot_uid=ROBOT_UID, - world=CuroboWorldCfg(rigid_objects=[block], obstacle_representation="mesh"), + world=CuroboWorldCfg(rigid_objects=[block]), warmup_iterations=0, ) mg = MotionGenerator(MotionGenCfg(planner_cfg=cfg)) @@ -191,9 +184,7 @@ def test_curobo_v2_plans_a_joint_space_move(): try: cfg = CuroboPlannerCfg( robot_uid=ROBOT_UID, - world=CuroboWorldCfg( - rigid_objects=[block], obstacle_representation="cuboid" - ), + world=CuroboWorldCfg(rigid_objects=[block]), warmup_iterations=0, ) mg = MotionGenerator(MotionGenCfg(planner_cfg=cfg)) @@ -232,7 +223,6 @@ def test_curobo_v2_multi_env_worlds_are_independent(): robot_uid=ROBOT_UID, world=CuroboWorldCfg( rigid_objects=[block], - obstacle_representation="cuboid", dynamic_obstacle_names=["demo_block"], multi_env=True, ), diff --git a/tests/sim/planners/test_curobo_planner.py b/tests/sim/planners/test_curobo_planner.py index 687f8c6a5..08ce00699 100644 --- a/tests/sim/planners/test_curobo_planner.py +++ b/tests/sim/planners/test_curobo_planner.py @@ -17,7 +17,7 @@ """Unit and smoke tests for the optional cuRobo planner. Most tests are dependency-free and cover planner configuration, conversion, -validation, and generated robot/world YAML. The two GPU-marked smoke tests +validation, generated robot YAML, and voxel-world data. The GPU-marked smoke tests exercise cached in-process planning and CPU-physics interoperability. Full collision-planning coverage remains in ``test_curobo_integration.py``. """ @@ -26,7 +26,6 @@ import importlib import logging -import math from contextlib import nullcontext from types import SimpleNamespace @@ -49,10 +48,10 @@ _validate_dynamic_obstacles, ) from embodichain.lab.sim.planners.curobo.curobo_yaml import ( - _mesh_to_obstacle_entry, + _convex_hulls_to_voxel_entry, _parse_mimic_joint_names, generate_curobo_robot_yaml, - generate_curobo_world_yaml, + generate_curobo_world_scene, visualize_curobo_robot_collision_model, visualize_curobo_world_collision_model, ) @@ -218,11 +217,11 @@ def test_configure_curobo_logging_rejects_unknown_level(): _configure_curobo_logging("silent") -def test_curobo_world_cfg_uses_v2_safe_default_collision_cache(): +def test_curobo_world_cfg_has_single_voxel_collision_path(): cfg = CuroboWorldCfg() - assert cfg.collision_cache == {"cuboid": 8, "mesh": 2} - assert cfg.obstacle_representation == "sphere" + assert cfg.voxel_size == pytest.approx(0.01) + assert cfg.voxel_padding == pytest.approx(0.1) def test_auto_gen_defaults_keep_sphere_count_low_and_fit_type_fixed(): @@ -232,108 +231,6 @@ def test_auto_gen_defaults_keep_sphere_count_low_and_fit_type_fixed(): assert not hasattr(auto, "fit_type") -def test_runtime_scene_records_spheres_without_changing_yaml_or_cache(tmp_path): - class FakeScene: - def __init__(self, *, sphere=None, mesh=None): - self.sphere = sphere or [] - self.mesh = mesh or [] - - @classmethod - def create(cls, data): - return cls( - sphere=[SimpleNamespace(name=name) for name in data.get("sphere", {})], - mesh=[SimpleNamespace(name=name) for name in data.get("mesh", {})], - ) - - scene_path = tmp_path / "sphere_world.yml" - scene_path.write_text( - yaml.safe_dump( - { - "sphere": { - "block_0": {"position": [0.0, 0.0, 0.0], "radius": 0.1}, - "block_1": {"position": [0.1, 0.0, 0.0], "radius": 0.1}, - "block_2": {"position": [0.2, 0.0, 0.0], "radius": 0.1}, - } - } - ), - encoding="utf-8", - ) - planner = CuroboPlanner.__new__(CuroboPlanner) - planner._bindings = SimpleNamespace(Scene=FakeScene) - - runtime_scene, runtime_cache, expected_names = planner._prepare_runtime_scene_model( - str(scene_path), {"cuboid": 8, "mesh": 2} - ) - - assert runtime_scene == str(scene_path) - assert runtime_cache == {"cuboid": 8, "mesh": 2} - assert expected_names == [["block_0", "block_1", "block_2"]] - - -def test_runtime_scene_records_independent_sphere_worlds(): - class FakeScene: - def __init__(self, names): - self.sphere = [SimpleNamespace(name=name) for name in names] - self.mesh = [] - - @classmethod - def create(cls, data): - return cls(list(data.get("sphere", {}))) - - planner = CuroboPlanner.__new__(CuroboPlanner) - planner._bindings = SimpleNamespace(Scene=FakeScene) - - runtime_scene, runtime_cache, expected_names = planner._prepare_runtime_scene_model( - [{"sphere": {"a": {}}}, {"sphere": {"b": {}, "c": {}}}], - {"mesh": 1}, - ) - - assert runtime_scene == [ - {"sphere": {"a": {}}}, - {"sphere": {"b": {}, "c": {}}}, - ] - assert runtime_cache == {"mesh": 1} - assert expected_names == [["a"], ["b", "c"]] - - -def test_runtime_sphere_validation_rejects_missing_collision_objects(): - checker = SimpleNamespace( - get_obstacle_names=lambda env_idx: ["block_0"] if env_idx == 0 else [] - ) - planner = SimpleNamespace(scene_collision_checker=checker) - - with pytest.raises(RuntimeError, match="block_1"): - CuroboPlanner._validate_runtime_sphere_obstacles( - planner, [["block_0", "block_1"]] - ) - - -def test_analytic_sphere_storage_preserves_center_radius_and_name(): - pytest.importorskip("curobo") - from curobo.scene import Scene - from curobo.types import DeviceCfg - - from embodichain.lab.sim.planners.curobo.curobo_sphere_data import SphereData - - scene = Scene.create( - { - "sphere": { - "block_0": { - "position": [1.0, 2.0, 3.0], - "radius": 0.4, - } - } - } - ) - storage = SphereData.from_scene_cfg(scene, DeviceCfg(device="cpu")) - - assert storage.get_names() == ["block_0"] - assert storage.radius[0, 0].item() == pytest.approx(0.4) - assert storage.inv_pose[0, 0, :7].tolist() == pytest.approx( - [-1.0, -2.0, -3.0, 1.0, 0.0, 0.0, 0.0] - ) - - def test_curobo_planner_class_is_lazy_import_safe(): """Referencing the class must not import curobo.""" import sys @@ -396,7 +293,6 @@ def __init__(self, cfg): ), sim_joint_names=["joint"], scene_model=None, - collision_cache=None, use_cuda_graph=False, planning_mode=MoveType.EEF_MOVE, ) @@ -730,147 +626,75 @@ def get_local_pose(self, to_matrix=False): # noqa: ARG002 return self._pose.unsqueeze(0) -def test_cuboid_entry_centered_mesh_matches_aabb_and_pose(): - entries = _mesh_to_obstacle_entry( - "demo_block", - _unit_cube_vertices(), - _cube_faces(), - _identity_pose(), - representation="cuboid", - ) - - assert len(entries) == 1 - top_key, name, fields = entries[0] - assert (top_key, name) == ("cuboid", "demo_block") - assert fields["dims"] == pytest.approx([1.0, 1.0, 1.0]) - assert fields["pose"] == pytest.approx([0.45, 0.0, 0.18, 1.0, 0.0, 0.0, 0.0]) +def _mock_visacd_as_identity(monkeypatch, calls=None): + import dexsim.kit.meshproc as meshproc + def fake_visacd(mesh, **kwargs): + if calls is not None: + calls.append((mesh, kwargs)) + return True, (mesh,) -def test_cuboid_entry_off_origin_mesh_offsets_center(): - vertices = _unit_cube_vertices() + 0.5 - _, _, fields = _mesh_to_obstacle_entry( - "block", - vertices, - _cube_faces(), - _identity_pose(), - representation="cuboid", - )[0] + monkeypatch.setattr(meshproc, "convex_decomposition_visacd", fake_visacd) - assert fields["dims"] == pytest.approx([1.0, 1.0, 1.0]) - assert fields["pose"][:3] == pytest.approx([0.95, 0.5, 0.68]) +def test_voxel_entry_uses_visacd_with_sixteen_hulls(monkeypatch): + calls = [] + _mock_visacd_as_identity(monkeypatch, calls) -def test_cuboid_entry_rotated_pose_preserves_center(): - quaternion = torch.tensor( - [math.cos(math.pi / 4), 0.0, 0.0, math.sin(math.pi / 4)], - dtype=torch.float32, - ) - pose = torch.cat([torch.tensor([0.45, 0.0, 0.18]), quaternion]) - _, _, fields = _mesh_to_obstacle_entry( + name, fields = _convex_hulls_to_voxel_entry( "block", _unit_cube_vertices(), _cube_faces(), - pose, - representation="cuboid", - )[0] + _identity_pose(), + voxel_size=0.25, + voxel_padding=0.25, + ) - assert fields["pose"][:3] == pytest.approx([0.45, 0.0, 0.18]) - assert fields["pose"][3:] == pytest.approx(quaternion.tolist()) + assert len(calls) == 1 + _, kwargs = calls[0] + assert kwargs["max_convex_hull_num"] == 16 + assert name == "block" + assert fields["pose"] == pytest.approx(_identity_pose().tolist()) + assert fields["dims"] == pytest.approx([1.5, 1.5, 1.5]) + assert tuple(fields["feature_tensor"].shape) == (6, 6, 6) + assert fields["feature_tensor"].amin() < 0.0 + assert fields["feature_tensor"].amax() > 0.0 -def test_cuboid_entry_accepts_homogeneous_pose(): +def test_voxel_entry_preserves_homogeneous_object_pose(monkeypatch): + _mock_visacd_as_identity(monkeypatch) pose = torch.eye(4, dtype=torch.float32) pose[:3, 3] = torch.tensor([0.45, 0.0, 0.18]) - _, _, fields = _mesh_to_obstacle_entry( + + _, fields = _convex_hulls_to_voxel_entry( "block", _unit_cube_vertices(), _cube_faces(), pose, - representation="cuboid", - )[0] - - assert fields["pose"] == pytest.approx([0.45, 0.0, 0.18, 1.0, 0.0, 0.0, 0.0]) - - -def test_mesh_entry_serializes_flat_face_buffer(): - top_key, name, fields = _mesh_to_obstacle_entry( - "demo_block", - _unit_cube_vertices(), - _cube_faces(), - _identity_pose(), - representation="mesh", - )[0] + voxel_size=0.5, + voxel_padding=0.0, + ) - assert (top_key, name) == ("mesh", "demo_block") - assert len(fields["vertices"]) == 8 - assert len(fields["faces"]) == 36 assert fields["pose"] == pytest.approx(_identity_pose().tolist()) -def test_invalid_obstacle_representation_raises(): - with pytest.raises(ValueError, match="representation"): - _mesh_to_obstacle_entry( +@pytest.mark.parametrize( + ("voxel_size", "voxel_padding", "match"), + [(0.0, 0.1, "voxel_size"), (0.1, -0.1, "voxel_padding")], +) +def test_voxel_entry_rejects_invalid_settings(voxel_size, voxel_padding, match): + with pytest.raises(ValueError, match=match): + _convex_hulls_to_voxel_entry( "block", _unit_cube_vertices(), _cube_faces(), _identity_pose(), - representation="banana", + voxel_size=voxel_size, + voxel_padding=voxel_padding, ) -def test_empty_mesh_raises_for_cuboid(): - with pytest.raises(ValueError, match="no vertices"): - _mesh_to_obstacle_entry( - "block", - torch.zeros((0, 3), dtype=torch.float32), - torch.zeros((0, 3), dtype=torch.int32), - _identity_pose(), - representation="cuboid", - ) - - -def test_sphere_obstacle_uses_dexsim_morphit_with_sixteen_hulls(monkeypatch): - import dexsim.kit.meshproc as meshproc - - calls = [] - - def fake_sphere_fit(mesh, **kwargs): - calls.append((mesh, kwargs)) - return ( - True, - torch.tensor([[0.0, 0.0, 0.0]], dtype=torch.float32), - torch.tensor([0.25], dtype=torch.float32), - ) - - monkeypatch.setattr(torch.cuda, "is_available", lambda: True) - monkeypatch.setattr(meshproc, "sphere_fit", fake_sphere_fit) - - entries = _mesh_to_obstacle_entry( - "block", - _unit_cube_vertices(), - _cube_faces(), - _identity_pose(), - representation="sphere", - device="cuda:0", - ) - - assert len(calls) == 1 - _, kwargs = calls[0] - assert kwargs["fit_type"] is meshproc.SphereFitType.MORPHIT - assert kwargs["max_convex_hull_num"] == 16 - assert entries[0][0:2] == ("sphere", "block_0") - assert entries[0][2]["position"] == pytest.approx([0.45, 0.0, 0.18]) - - -def test_obstacle_collision_visualization_reads_cached_spheres(tmp_path): - world_yaml_path = tmp_path / "world_visual.yml" - world_yaml_path.write_text( - yaml.safe_dump( - {"sphere": {"block_0": {"position": [1.0, 2.0, 3.0], "radius": 0.1}}} - ), - encoding="utf-8", - ) - +def test_obstacle_collision_visualization_reads_voxels(): class FakeVisualRigidObject(_FakeRigidObject): def get_local_pose(self, to_matrix=False): if not to_matrix: @@ -882,61 +706,32 @@ def get_local_pose(self, to_matrix=False): rigid_object = FakeVisualRigidObject( "block", _unit_cube_vertices(), _cube_faces(), _identity_pose() ) + features = torch.ones((3, 3, 3), dtype=torch.float16) + features[1, 1, 1] = -0.1 + world_scene = { + "voxel": { + "block": { + "pose": [1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0], + "dims": [0.3, 0.3, 0.3], + "voxel_size": 0.1, + "feature_tensor": features, + } + } + } geometries = visualize_curobo_world_collision_model( - [rigid_object], str(world_yaml_path), draw=False + [rigid_object], world_scene, draw=False ) assert [geometry["name"] for geometry in geometries] == [ "obstacle_mesh/block", - "obstacle_spheres", + "obstacle_voxels/block", ] - sphere_bounds = geometries[-1]["geometry"].get_axis_aligned_bounding_box() - assert sphere_bounds.get_center() == pytest.approx([1.0, 2.0, 3.0]) - - -def test_generate_cuboid_world_yaml_assembles_schema(tmp_path): - rigid_object = _FakeRigidObject( - "demo_block", - _unit_cube_vertices(), - _cube_faces(), - _identity_pose(), - ) - output_path = tmp_path / "world.yml" - - result = generate_curobo_world_yaml( - [rigid_object], - str(output_path), - representation="cuboid", - ) - data = yaml.safe_load(output_path.read_text(encoding="utf-8")) - - assert result == str(output_path) - assert list(data) == ["cuboid"] - assert data["cuboid"]["demo_block"]["dims"] == pytest.approx([1.0, 1.0, 1.0]) - assert data["cuboid"]["demo_block"]["pose"][:3] == pytest.approx([0.45, 0.0, 0.18]) - - -def test_generate_mesh_world_yaml_assembles_schema(tmp_path): - rigid_object = _FakeRigidObject( - "demo_block", - _unit_cube_vertices(), - _cube_faces(), - _identity_pose(), - ) - output_path = tmp_path / "world_mesh.yml" - - generate_curobo_world_yaml( - [rigid_object], - str(output_path), - representation="mesh", - ) - data = yaml.safe_load(output_path.read_text(encoding="utf-8")) + voxel_bounds = geometries[-1]["geometry"].get_axis_aligned_bounding_box() + assert voxel_bounds.get_center() == pytest.approx([1.0, 2.0, 3.0]) - assert list(data) == ["mesh"] - assert len(data["mesh"]["demo_block"]["vertices"]) == 8 - -def test_generate_world_yaml_supports_multiple_objects(tmp_path): +def test_generate_world_scene_supports_multiple_objects(monkeypatch): + _mock_visacd_as_identity(monkeypatch) rigid_objects = [ _FakeRigidObject( "block_a", @@ -951,25 +746,22 @@ def test_generate_world_yaml_supports_multiple_objects(tmp_path): _identity_pose((0.0, 0.3, 0.1)), ), ] - output_path = tmp_path / "multi.yml" - - generate_curobo_world_yaml( - rigid_objects, - str(output_path), - representation="cuboid", + scene_data = generate_curobo_world_scene( + rigid_objects, voxel_size=0.5, voxel_padding=0.0 ) - data = yaml.safe_load(output_path.read_text(encoding="utf-8")) - assert set(data["cuboid"]) == {"block_a", "block_b"} - assert data["cuboid"]["block_b"]["pose"][:3] == pytest.approx([0.0, 0.3, 0.1]) + assert list(scene_data) == ["voxel"] + assert set(scene_data["voxel"]) == {"block_a", "block_b"} + assert scene_data["voxel"]["block_b"]["pose"][:3] == pytest.approx([0.0, 0.3, 0.1]) -def test_generate_world_yaml_rejects_empty_input(tmp_path): +def test_generate_world_scene_rejects_empty_input(): with pytest.raises(ValueError, match="at least one"): - generate_curobo_world_yaml([], str(tmp_path / "world.yml")) + generate_curobo_world_scene([]) -def test_generate_world_yaml_rejects_duplicate_names(tmp_path): +def test_generate_world_scene_rejects_duplicate_names(monkeypatch): + _mock_visacd_as_identity(monkeypatch) pose = _identity_pose() first = _FakeRigidObject( "block", @@ -985,39 +777,14 @@ def test_generate_world_yaml_rejects_duplicate_names(tmp_path): ) with pytest.raises(ValueError, match="Duplicate"): - generate_curobo_world_yaml( - [first, second], - str(tmp_path / "world.yml"), - ) + generate_curobo_world_scene([first, second], voxel_size=0.5) -def test_generated_cuboid_yaml_loads_in_curobo_scene_cfg(tmp_path): +def test_generated_voxel_data_loads_in_curobo_scene_cfg(monkeypatch): pytest.importorskip("curobo") from curobo._src.geom.types import SceneCfg - rigid_object = _FakeRigidObject( - "demo_block", - _unit_cube_vertices(), - _cube_faces(), - _identity_pose(), - ) - output_path = tmp_path / "world.yml" - generate_curobo_world_yaml( - [rigid_object], - str(output_path), - representation="cuboid", - ) - - scene = SceneCfg.create(yaml.safe_load(output_path.read_text(encoding="utf-8"))) - - assert len(scene.cuboid) == 1 - assert scene.cuboid[0].name == "demo_block" - assert scene.cuboid[0].dims == pytest.approx([1.0, 1.0, 1.0]) - - -def test_generated_mesh_yaml_loads_in_curobo_scene_cfg(tmp_path): - pytest.importorskip("curobo") - from curobo._src.geom.types import SceneCfg + _mock_visacd_as_identity(monkeypatch) rigid_object = _FakeRigidObject( "demo_block", @@ -1025,18 +792,16 @@ def test_generated_mesh_yaml_loads_in_curobo_scene_cfg(tmp_path): _cube_faces(), _identity_pose(), ) - output_path = tmp_path / "world_mesh.yml" - generate_curobo_world_yaml( - [rigid_object], - str(output_path), - representation="mesh", + scene_data = generate_curobo_world_scene( + [rigid_object], voxel_size=0.5, voxel_padding=0.0 ) - scene = SceneCfg.create(yaml.safe_load(output_path.read_text(encoding="utf-8"))) + scene = SceneCfg.create(scene_data) - assert len(scene.mesh) == 1 - assert scene.mesh[0].name == "demo_block" - assert len(scene.mesh[0].vertices) == 8 + assert len(scene.voxel) == 1 + assert scene.voxel[0].name == "demo_block" + assert scene.voxel[0].voxel_size == pytest.approx(0.5) + assert tuple(scene.voxel[0].feature_tensor.shape) == (2, 2, 2) # Simulator smoke coverage From 78a542eb0ae96491b256e7f79760511ea444093d Mon Sep 17 00:00:00 2001 From: matafela Date: Fri, 7 Aug 2026 19:07:38 +0800 Subject: [PATCH 3/6] update --- .../lab/sim/planners/curobo/curobo_yaml.py | 319 ++++++++++++++---- examples/sim/planners/curobo_planner.py | 6 +- tests/sim/planners/test_curobo_planner.py | 150 +++++++- 3 files changed, 394 insertions(+), 81 deletions(-) diff --git a/embodichain/lab/sim/planners/curobo/curobo_yaml.py b/embodichain/lab/sim/planners/curobo/curobo_yaml.py index 488faa357..4a82b2151 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_yaml.py +++ b/embodichain/lab/sim/planners/curobo/curobo_yaml.py @@ -616,27 +616,124 @@ def visualize_curobo_robot_collision_model( return geometries -def visualize_curobo_world_collision_model( - rigid_objects: Sequence[RigidObject], - world_scene: Any, - env_id: int = 0, - *, - draw: bool = True, -) -> list[dict[str, Any]]: - """Visualize live obstacle meshes and cuRobo ESDF collision voxels. +def _get_or_create_dexsim_material( + env: Any, + name: str, + color: list[float], +) -> Any: + """Return a named DexSim material without accumulating duplicates.""" + material = env.find_material(name) + if material is None: + return env.create_color_material(color, name, has_alpha=len(color) == 4) + material.set_base_color(color) + return material - Args: - rigid_objects: Live simulator obstacles represented by the cache. - world_scene: Tensor-backed scene mapping or a cuRobo ``Scene`` instance. - env_id: Simulator environment instance whose live meshes are shown. - draw: Open an Open3D window immediately. ``False`` returns draw entries - for composition with the robot collision model. - Returns: - Open3D geometry dictionaries suitable for :func:`open3d.visualization.draw`. - """ +def _create_open3d_sphere_mesh( + centers: torch.Tensor, + radii: torch.Tensor, +) -> Any: + """Build one Open3D mesh containing all requested collision spheres.""" + import numpy as np + import open3d as o3d + + centers = ( + torch.as_tensor(centers, dtype=torch.float32).detach().cpu().reshape(-1, 3) + ) + radii = torch.as_tensor(radii, dtype=torch.float32).detach().cpu().reshape(-1) + if centers.shape[0] != radii.shape[0]: + raise ValueError( + "Visualization sphere centers and radii must have the same length, got " + f"{centers.shape[0]} and {radii.shape[0]}." + ) + if torch.any(radii <= 0.0): + raise ValueError("Visualization sphere radii must all be positive.") + if centers.shape[0] == 0: + raise ValueError("At least one visualization sphere is required.") + + sphere_template = o3d.geometry.TriangleMesh.create_sphere(radius=1.0, resolution=8) + sphere_template.compute_vertex_normals() + template_vertices = np.asarray(sphere_template.vertices) + template_triangles = np.asarray(sphere_template.triangles) + template_normals = np.asarray(sphere_template.vertex_normals) + centers_np = centers.numpy() + radii_np = radii.numpy() + + # Vectorized assembly avoids repeated ``combined_mesh += sphere`` reallocations, + # which become quadratic for a dense obstacle surface. + sphere_count = centers_np.shape[0] + vertices_per_sphere = template_vertices.shape[0] + vertices = ( + template_vertices[None, :, :] * radii_np[:, None, None] + centers_np[:, None, :] + ).reshape(-1, 3) + triangle_offsets = (np.arange(sphere_count, dtype=np.int64) * vertices_per_sphere)[ + :, None, None + ] + triangles = (template_triangles[None, :, :] + triangle_offsets).reshape(-1, 3) + + mesh = o3d.geometry.TriangleMesh() + mesh.vertices = o3d.utility.Vector3dVector(vertices) + mesh.triangles = o3d.utility.Vector3iVector(triangles) + mesh.vertex_normals = o3d.utility.Vector3dVector( + np.tile(template_normals, (sphere_count, 1)) + ) + return mesh + + +def _load_dexsim_sphere_mesh( + env: Any, + centers: torch.Tensor, + radii: torch.Tensor, + material: Any, +) -> Any: + """Write one combined sphere mesh to ``/tmp`` and load it into DexSim.""" + import os + import tempfile + import open3d as o3d + mesh = _create_open3d_sphere_mesh(centers, radii) + with tempfile.NamedTemporaryFile( + prefix="curobo_collision_spheres_", + suffix=".ply", + dir="/tmp", + delete=False, + ) as temp_file: + mesh_path = temp_file.name + + actor = None + try: + if not o3d.io.write_triangle_mesh(mesh_path, mesh, write_ascii=False): + raise RuntimeError( + f"Could not write collision sphere mesh to {mesh_path!r}." + ) + actor = env.load_actor(mesh_path) + if actor is None: + raise RuntimeError(f"DexSim could not load collision mesh {mesh_path!r}.") + actor.set_material(material) + return actor + except Exception: + if actor is not None: + env.remove_actor(actor) + raise + finally: + try: + os.unlink(mesh_path) + except FileNotFoundError: + pass + + +def _remove_dexsim_visualization_actors(env: Any, actors: Sequence[Any]) -> None: + """Remove every temporary actor, continuing if an individual removal fails.""" + for actor in reversed(actors): + try: + env.remove_actor(actor) + except Exception as exc: # noqa: BLE001 + logger.log_warning(f"Could not remove a cuRobo visualization actor: {exc}") + + +def _world_collision_sphere_data(world_scene: Any) -> tuple[torch.Tensor, torch.Tensor]: + """Return world-space centers and radii for the ESDF surface overlay.""" if isinstance(world_scene, dict): voxel_entries = list(world_scene.get("voxel", {}).items()) else: @@ -646,27 +743,8 @@ def visualize_curobo_world_collision_model( if not voxel_entries: raise ValueError("The cuRobo world scene contains no voxel collision data.") - meshes: list[tuple[str, Any]] = [] - for idx, obj in enumerate(rigid_objects): - name = getattr(obj, "uid", None) or f"obstacle_{idx}" - vertices = obj.get_vertices(env_ids=[env_id], scale=True)[0] - faces = obj.get_triangles(env_ids=[env_id])[0] - if vertices is None or faces is None or vertices.numel() == 0: - continue - pose = torch.as_tensor( - obj.get_local_pose(to_matrix=True)[env_id], dtype=torch.float32 - ).cpu() - mesh = _to_open3d_legacy_mesh(vertices, faces, o3d) - mesh.transform(pose.numpy()) - meshes.append((f"obstacle_mesh/{name}", mesh)) - - mesh_material = o3d.visualization.rendering.MaterialRecord() - mesh_material.shader = "defaultLit" - mesh_material.base_color = [0.45, 0.55, 0.45, 1.0] - geometries = [ - {"name": name, "geometry": mesh, "material": mesh_material} - for name, mesh in meshes - ] + centers: list[torch.Tensor] = [] + radii: list[torch.Tensor] = [] for name, entry in voxel_entries: get_value = ( entry.get if isinstance(entry, dict) else lambda key: getattr(entry, key) @@ -674,22 +752,72 @@ def visualize_curobo_world_collision_model( features = torch.as_tensor(get_value("feature_tensor")).detach().cpu() voxel_size = float(get_value("voxel_size")) local_points = _voxel_grid_coordinates(tuple(features.shape), voxel_size) - occupied = features.reshape(-1) <= 0.5 * voxel_size - if not torch.any(occupied): + surface = torch.abs(features.reshape(-1)) <= 0.5 * voxel_size + if not torch.any(surface): + logger.log_warning( + f"Voxel collision entry {name!r} has no samples near its zero level set." + ) continue - pose = torch.as_tensor(get_value("pose"), dtype=torch.float32) + pose = torch.as_tensor(get_value("pose"), dtype=torch.float32).detach().cpu() rotation = matrix_from_quat(pose[3:7]) - world_points = local_points[occupied] @ rotation.T + pose[:3] - point_cloud = o3d.geometry.PointCloud() - point_cloud.points = o3d.utility.Vector3dVector(world_points.numpy()) - point_cloud.paint_uniform_color([0.8, 0.15, 0.0]) - voxel_grid = o3d.geometry.VoxelGrid.create_from_point_cloud( - point_cloud, voxel_size=voxel_size + world_points = local_points[surface] @ rotation.T + pose[:3] + centers.append(world_points) + radii.append(torch.full((world_points.shape[0],), 0.5 * voxel_size)) + + if not centers: + raise ValueError( + "The cuRobo world scene contains no visible collision surface." ) - geometries.append({"name": f"obstacle_voxels/{name}", "geometry": voxel_grid}) - if draw: - o3d.visualization.draw(geometries, title="cuRobo obstacle collision model") - return geometries + return torch.cat(centers), torch.cat(radii) + + +def visualize_curobo_world_collision_model( + rigid_objects: Sequence[RigidObject], + world_scene: Any, + env_id: int = 0, + *, + env: Any | None = None, + material: Any | None = None, +) -> list[Any]: + """Add the cuRobo ESDF collision surface to the DexSim scene. + + The rigid objects are already present in the live DexSim scene, so this + function only adds an overlay for the voxel collision data consumed by + cuRobo. Voxels within half a voxel of the zero level set are rendered as + half-voxel-radius spheres. All spheres are merged into one Open3D mesh, + written temporarily under ``/tmp``, and imported as one DexSim actor. + + Args: + rigid_objects: Live simulator obstacles represented by the cache. + world_scene: Tensor-backed scene mapping or a cuRobo ``Scene`` instance. + env_id: Simulator environment instance represented by ``world_scene``. + Retained for API consistency; world-scene poses are already in the + selected environment's world frame. + env: DexSim environment that receives the visualization actors. Uses + the environment of :func:`dexsim.default_world` when omitted. + material: Optional DexSim material for the collision-surface spheres. + + Returns: + A one-element list containing the combined DexSim actor. The caller owns + this actor and must remove it with + :meth:`dexsim.environment.Env.remove_actor`. + """ + import dexsim + + # Keep these parameters in the public API because the collision cache is + # associated with the supplied live objects and simulator environment. + _ = rigid_objects, env_id + if env is None: + env = dexsim.default_world().get_env() + if material is None: + material = _get_or_create_dexsim_material( + env, + "curobo_world_collision_material", + [1.0, 0.0, 0.0, 0.45], + ) + + centers, radii = _world_collision_sphere_data(world_scene) + return [_load_dexsim_sphere_mesh(env, centers, radii, material)] def visualize_curobo_collision_models( @@ -699,16 +827,87 @@ def visualize_curobo_collision_models( world_scene: Any | None = None, env_id: int = 0, ) -> None: - """Draw cached robot spheres and obstacle collision voxels together.""" - import open3d as o3d + """Show robot and world collision models in DexSim until Enter is pressed. + + Robot spheres are loaded from the generated cuRobo YAML and transformed by + the current simulator link poses. Obstacle spheres show the zero level set + of the ESDF voxel data passed to cuRobo. Robot and obstacle spheres are each + merged into one temporary DexSim actor so they can use blue and red + materials respectively. Both actors are removed before the function returns, + including when ``input`` is interrupted. + """ + import dexsim + import yaml - geometries = visualize_curobo_robot_collision_model( - robot, robot_yaml_path, env_id, draw=False + world = dexsim.default_world() + env = world.get_env() + robot_material = _get_or_create_dexsim_material( + env, + "curobo_robot_collision_material", + [0.0, 0.0, 1.0, 0.45], + ) + obstacle_material = _get_or_create_dexsim_material( + env, + "curobo_world_collision_material", + [1.0, 0.0, 0.0, 0.45], ) - if rigid_objects and world_scene is not None: - geometries.extend( - visualize_curobo_world_collision_model( - rigid_objects, world_scene, env_id, draw=False + + with open(robot_yaml_path, encoding="utf-8") as yaml_file: + data = yaml.safe_load(yaml_file) + kinematics = data["robot_cfg"]["kinematics"] + cached_spheres = kinematics.get("collision_spheres", {}) + sphere_buffer = float(kinematics.get("collision_sphere_buffer", 0.0)) + + robot_center_batches: list[torch.Tensor] = [] + robot_radius_batches: list[torch.Tensor] = [] + visualization_actors: list[Any] = [] + try: + for link_name, link_spheres in cached_spheres.items(): + if not link_spheres: + continue + link_pose = ( + torch.as_tensor( + robot.get_link_pose(link_name, env_ids=[env_id], to_matrix=True)[0], + dtype=torch.float32, + ) + .detach() + .cpu() + ) + centers_local = torch.as_tensor( + [sphere["center"] for sphere in link_spheres], dtype=torch.float32 + ).reshape(-1, 3) + centers_world = centers_local @ link_pose[:3, :3].T + link_pose[:3, 3] + radii = torch.as_tensor( + [float(sphere["radius"]) + sphere_buffer for sphere in link_spheres], + dtype=torch.float32, ) + robot_center_batches.append(centers_world) + robot_radius_batches.append(radii) + + sphere_count = 0 + if robot_center_batches: + robot_centers = torch.cat(robot_center_batches) + robot_radii = torch.cat(robot_radius_batches) + visualization_actors.append( + _load_dexsim_sphere_mesh( + env, robot_centers, robot_radii, robot_material + ) + ) + sphere_count += robot_centers.shape[0] + if rigid_objects and world_scene is not None: + world_centers, world_radii = _world_collision_sphere_data(world_scene) + visualization_actors.append( + _load_dexsim_sphere_mesh( + env, world_centers, world_radii, obstacle_material + ) + ) + sphere_count += world_centers.shape[0] + if not visualization_actors: + raise ValueError("The cuRobo caches contain no collision geometry.") + + input( + f"Showing {sphere_count} cuRobo collision spheres in " + "DexSim. Press Enter to remove them and continue..." ) - o3d.visualization.draw(geometries, title="cuRobo collision models") + finally: + _remove_dexsim_visualization_actors(env, visualization_actors) diff --git a/examples/sim/planners/curobo_planner.py b/examples/sim/planners/curobo_planner.py index 5430273d0..19f947984 100644 --- a/examples/sim/planners/curobo_planner.py +++ b/examples/sim/planners/curobo_planner.py @@ -761,9 +761,9 @@ def main() -> None: ) ) if visualize_collision_models: - # This opens one blocking Open3D window. Robot spheres and obstacle - # voxels come from the exact caches consumed by cuRobo; close the - # window to continue with planner backend creation and execution. + # This overlays the exact cached robot spheres and obstacle ESDF + # surface used by cuRobo in the DexSim window. Press Enter in the + # terminal to remove the overlay and continue planner creation. motion_generator.planner.visualize_collision_models(control_part) engine = AtomicActionEngine(motion_generator) engine.register( diff --git a/tests/sim/planners/test_curobo_planner.py b/tests/sim/planners/test_curobo_planner.py index 08ce00699..537ef8b8f 100644 --- a/tests/sim/planners/test_curobo_planner.py +++ b/tests/sim/planners/test_curobo_planner.py @@ -27,6 +27,7 @@ import importlib import logging from contextlib import nullcontext +from pathlib import Path from types import SimpleNamespace import pytest @@ -52,6 +53,7 @@ _parse_mimic_joint_names, generate_curobo_robot_yaml, generate_curobo_world_scene, + visualize_curobo_collision_models, visualize_curobo_robot_collision_model, visualize_curobo_world_collision_model, ) @@ -694,20 +696,59 @@ def test_voxel_entry_rejects_invalid_settings(voxel_size, voxel_padding, match): ) -def test_obstacle_collision_visualization_reads_voxels(): - class FakeVisualRigidObject(_FakeRigidObject): - def get_local_pose(self, to_matrix=False): - if not to_matrix: - return super().get_local_pose(to_matrix=False) - pose = torch.eye(4, dtype=torch.float32) - pose[:3, 3] = self._pose[:3] - return pose.unsqueeze(0) +class _FakeDexsimMaterial: + def __init__(self, name, color): + self.name = name + self.color = color + + def set_base_color(self, color): + self.color = color + + +class _FakeDexsimActor: + def __init__(self, mesh): + self.mesh = mesh + self.material = None + + def set_material(self, material): + self.material = material + + +class _FakeDexsimEnv: + def __init__(self): + self.materials = {} + self.actors = [] + self.loaded_paths = [] + self.removed_actors = [] - rigid_object = FakeVisualRigidObject( + def find_material(self, name): + return self.materials.get(name) + + def create_color_material(self, color, name, has_alpha=False): # noqa: ARG002 + material = _FakeDexsimMaterial(name, color) + self.materials[name] = material + return material + + def load_actor(self, mesh_path): + import open3d as o3d + + self.loaded_paths.append(mesh_path) + actor = _FakeDexsimActor(o3d.io.read_triangle_mesh(mesh_path)) + self.actors.append(actor) + return actor + + def remove_actor(self, actor): + self.removed_actors.append(actor) + + +def test_obstacle_collision_visualization_loads_one_combined_dexsim_actor(): + rigid_object = _FakeRigidObject( "block", _unit_cube_vertices(), _cube_faces(), _identity_pose() ) + env = _FakeDexsimEnv() + features = torch.ones((3, 3, 3), dtype=torch.float16) - features[1, 1, 1] = -0.1 + features[1, 1, 1] = 0.0 world_scene = { "voxel": { "block": { @@ -718,16 +759,89 @@ def get_local_pose(self, to_matrix=False): } } } - geometries = visualize_curobo_world_collision_model( - [rigid_object], world_scene, draw=False + actors = visualize_curobo_world_collision_model( + [rigid_object], world_scene, env=env ) - assert [geometry["name"] for geometry in geometries] == [ - "obstacle_mesh/block", - "obstacle_voxels/block", - ] - voxel_bounds = geometries[-1]["geometry"].get_axis_aligned_bounding_box() - assert voxel_bounds.get_center() == pytest.approx([1.0, 2.0, 3.0]) + assert actors == env.actors + assert len(env.loaded_paths) == 1 + assert not Path(env.loaded_paths[0]).exists() + bounds = actors[0].mesh.get_axis_aligned_bounding_box() + assert bounds.get_center() == pytest.approx([1.0, 2.0, 3.0]) + assert bounds.get_extent() == pytest.approx([0.1, 0.1, 0.1]) + assert actors[0].material.name == "curobo_world_collision_material" + assert actors[0].material.color == [1.0, 0.0, 0.0, 0.45] + + +def test_combined_collision_visualization_colors_and_cleans_two_actors( + tmp_path, monkeypatch +): + import dexsim + + robot_yaml_path = tmp_path / "robot_visual.yml" + robot_yaml_path.write_text( + yaml.safe_dump( + { + "robot_cfg": { + "kinematics": { + "collision_sphere_buffer": 0.01, + "collision_spheres": { + "hand": [{"center": [0.1, 0.0, 0.0], "radius": 0.1}] + }, + } + } + } + ), + encoding="utf-8", + ) + + class FakeRobot: + def get_link_pose( + self, link_name, env_ids=None, to_matrix=False # noqa: ARG002 + ): + pose = torch.eye(4, dtype=torch.float32) + pose[:3, 3] = torch.tensor([1.0, 2.0, 3.0]) + return pose.unsqueeze(0) + + env = _FakeDexsimEnv() + world = SimpleNamespace(get_env=lambda: env) + prompts = [] + monkeypatch.setattr(dexsim, "default_world", lambda: world) + monkeypatch.setattr("builtins.input", lambda prompt: prompts.append(prompt) or "") + + features = torch.ones((3, 3, 3), dtype=torch.float16) + features[1, 1, 1] = 0.0 + world_scene = { + "voxel": { + "block": { + "pose": [2.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0], + "voxel_size": 0.1, + "feature_tensor": features, + } + } + } + rigid_object = _FakeRigidObject( + "block", _unit_cube_vertices(), _cube_faces(), _identity_pose() + ) + visualize_curobo_collision_models( + FakeRobot(), str(robot_yaml_path), [rigid_object], world_scene + ) + + assert len(env.actors) == 2 + robot_actor, obstacle_actor = env.actors + robot_bounds = robot_actor.mesh.get_axis_aligned_bounding_box() + assert robot_bounds.get_center() == pytest.approx([1.1, 2.0, 3.0]) + assert robot_bounds.get_extent() == pytest.approx([0.22, 0.22, 0.22]) + assert robot_actor.material.name == "curobo_robot_collision_material" + assert robot_actor.material.color == [0.0, 0.0, 1.0, 0.45] + obstacle_bounds = obstacle_actor.mesh.get_axis_aligned_bounding_box() + assert obstacle_bounds.get_center() == pytest.approx([2.0, 2.0, 3.0]) + assert obstacle_bounds.get_extent() == pytest.approx([0.1, 0.1, 0.1]) + assert obstacle_actor.material.name == "curobo_world_collision_material" + assert obstacle_actor.material.color == [1.0, 0.0, 0.0, 0.45] + assert env.removed_actors == list(reversed(env.actors)) + assert all(not Path(path).exists() for path in env.loaded_paths) + assert "Showing 2 cuRobo collision spheres" in prompts[0] def test_generate_world_scene_supports_multiple_objects(monkeypatch): From 34b1093c179cdae188b1bf7a705ea9106423f89a Mon Sep 17 00:00:00 2001 From: matafela Date: Tue, 11 Aug 2026 15:42:44 +0800 Subject: [PATCH 4/6] update visual api --- .../overview/sim/planners/curobo_planner.md | 2 +- embodichain/lab/sim/planners/base_planner.py | 23 +++++++++ .../lab/sim/planners/curobo/curobo_planner.py | 2 +- examples/sim/planners/curobo_planner.py | 8 +-- tests/sim/planners/test_base_planner.py | 50 +++++++++++++++++++ 5 files changed, 80 insertions(+), 5 deletions(-) create mode 100644 tests/sim/planners/test_base_planner.py diff --git a/docs/source/overview/sim/planners/curobo_planner.md b/docs/source/overview/sim/planners/curobo_planner.md index 2cac6f9e2..b5c16c57b 100644 --- a/docs/source/overview/sim/planners/curobo_planner.md +++ b/docs/source/overview/sim/planners/curobo_planner.md @@ -237,7 +237,7 @@ tighter collision coverage, or set `force=True` to bypass the cache. For an Open3D overlay of the live robot/obstacle meshes and the exact spheres read back from those YAML caches, call -`planner.visualize_collision_models(control_part)`. Robot sphere centers are +`planner.visualize_robot_collision_models(control_part)`. Robot sphere centers are transformed by the simulator's live link poses. The interactive cuRobo example calls this once after planner initialization; close the Open3D window to continue. diff --git a/embodichain/lab/sim/planners/base_planner.py b/embodichain/lab/sim/planners/base_planner.py index ab66cbae9..0bddc7504 100644 --- a/embodichain/lab/sim/planners/base_planner.py +++ b/embodichain/lab/sim/planners/base_planner.py @@ -186,6 +186,29 @@ def supports_move_type(self, move_type: MoveType) -> bool: """ return move_type in self.supported_move_types + def visualize_robot_collision_models( + self, + control_part: str, + env_id: int = 0, + ) -> None: + """Visualize the robot collision models used by this planner. + + Planners that support collision avoidance should override this method + with their backend-specific visualization. + + Args: + control_part: Robot control part whose collision models are visualized. + env_id: Simulator environment instance to visualize. + + Raises: + NotImplementedError: If the planner does not support collision avoidance. + """ + logger.log_error( + f"{type(self).__name__} does not support collision avoidance or robot " + "collision model visualization.", + NotImplementedError, + ) + def default_plan_options(self) -> PlanOptions: """Return backend-default planning options.""" return PlanOptions() diff --git a/embodichain/lab/sim/planners/curobo/curobo_planner.py b/embodichain/lab/sim/planners/curobo/curobo_planner.py index 3d8b1cbbb..b7a9fd9d6 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_planner.py +++ b/embodichain/lab/sim/planners/curobo/curobo_planner.py @@ -1599,7 +1599,7 @@ def _world_scene_cache_key(self, world_cfg: CuroboWorldCfg) -> str: hasher.update(pose.detach().to("cpu").to(torch.float32).numpy().tobytes()) return hasher.hexdigest() - def visualize_collision_models( + def visualize_robot_collision_models( self, control_part: str, env_id: int = 0, diff --git a/examples/sim/planners/curobo_planner.py b/examples/sim/planners/curobo_planner.py index 19f947984..f8bc6ba0d 100644 --- a/examples/sim/planners/curobo_planner.py +++ b/examples/sim/planners/curobo_planner.py @@ -723,7 +723,9 @@ def main() -> None: seed=args.seed, ) use_independent_worlds = args.num_envs > 1 - visualize_collision_models = not args.headless and not use_independent_worlds + visualize_robot_collision_models = ( + not args.headless and not use_independent_worlds + ) if use_independent_worlds: for name, poses in obstacle_poses.items(): yaw_deg = torch.rad2deg(torch.atan2(poses[:, 1, 0], poses[:, 0, 0])) @@ -760,11 +762,11 @@ def main() -> None: ) ) ) - if visualize_collision_models: + if visualize_robot_collision_models: # This overlays the exact cached robot spheres and obstacle ESDF # surface used by cuRobo in the DexSim window. Press Enter in the # terminal to remove the overlay and continue planner creation. - motion_generator.planner.visualize_collision_models(control_part) + motion_generator.planner.visualize_robot_collision_models(control_part) engine = AtomicActionEngine(motion_generator) engine.register( MoveEndEffector( diff --git a/tests/sim/planners/test_base_planner.py b/tests/sim/planners/test_base_planner.py new file mode 100644 index 000000000..9b68491ea --- /dev/null +++ b/tests/sim/planners/test_base_planner.py @@ -0,0 +1,50 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# 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. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import pytest + +from embodichain.lab.sim.planners.base_planner import BasePlanner, PlanOptions +from embodichain.lab.sim.planners.curobo.curobo_planner import CuroboPlanner +from embodichain.lab.sim.planners.utils import PlanResult, PlanState + + +class _PlannerWithoutCollisionAvoidance(BasePlanner): + def plan( + self, + target_states: list[PlanState], + options: PlanOptions = PlanOptions(), + ) -> PlanResult: + raise NotImplementedError + + +def test_collision_model_visualization_is_unsupported_by_default(): + planner = _PlannerWithoutCollisionAvoidance.__new__( + _PlannerWithoutCollisionAvoidance + ) + + with pytest.raises( + NotImplementedError, match="does not support collision avoidance" + ): + planner.visualize_robot_collision_models("arm") + + +def test_curobo_overrides_robot_collision_model_visualization(): + assert ( + CuroboPlanner.visualize_robot_collision_models + is not BasePlanner.visualize_robot_collision_models + ) From f985a822a542779ac47f116ce4066629c6b9fb1c Mon Sep 17 00:00:00 2001 From: matafela Date: Tue, 11 Aug 2026 15:51:38 +0800 Subject: [PATCH 5/6] update paramter --- .../lab/sim/planners/curobo/curobo_planner.py | 16 +--------------- .../lab/sim/planners/curobo/curobo_yaml.py | 4 ++-- 2 files changed, 3 insertions(+), 17 deletions(-) diff --git a/embodichain/lab/sim/planners/curobo/curobo_planner.py b/embodichain/lab/sim/planners/curobo/curobo_planner.py index b7a9fd9d6..dbf07e521 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_planner.py +++ b/embodichain/lab/sim/planners/curobo/curobo_planner.py @@ -74,18 +74,6 @@ "https://nvlabs.github.io/curobo/latest/getting-started/installation.html" ) -# Bumped whenever the auto-generated robot-YAML schema/logic changes so that -# cached YAMLs from an older generator are regenerated instead of reused. v2 -# excluded URDF mimic joints from cspace/lock_joints; v3 switched both robot -# and obstacle fitting to DexSim MorphIt with fixed convex-hull limits; v4 -# removes self-collision metadata because the backend temporarily disables -# cuRobo self-collision checking. -_CUROBO_ROBOT_YAML_GENERATOR_VERSION = "v4-no-self-collision" - -# World caches contain tensor-backed ESDF voxel grids and are intentionally -# versioned independently from robot sphere YAMLs. -_CUROBO_WORLD_CACHE_VERSION = "v1-visacd16-voxel" - # cuRobo 0.8 does not expose PyTorch's CUDA stream-capture error mode. The # temporary adapter below therefore replaces ``torch.cuda.graph`` only while # cuRobo can lazily record graphs. Serialize that small process-wide patch. @@ -164,7 +152,7 @@ class CuroboWorldCfg: voxel_size: float = 0.01 """ESDF voxel edge length in meters for every world collision object.""" - voxel_padding: float = 0.1 + voxel_padding: float = 0.005 """Free-space padding around each object-local voxel grid in meters. The padding must cover the largest robot collision-sphere radius plus the @@ -1523,7 +1511,6 @@ def _robot_yaml_cache_key( ) -> str: """Hash the URDF path/content and fit parameters into a stable cache key.""" hasher = hashlib.md5() - hasher.update(_CUROBO_ROBOT_YAML_GENERATOR_VERSION.encode("utf-8")) hasher.update(urdf_path.encode("utf-8")) try: with open(urdf_path, "rb") as urdf_file: @@ -1583,7 +1570,6 @@ def _auto_generate_world_scene(self, world_cfg: CuroboWorldCfg) -> "Any": def _world_scene_cache_key(self, world_cfg: CuroboWorldCfg) -> str: """Hash object geometry, initial poses, and voxel settings.""" hasher = hashlib.md5() - hasher.update(_CUROBO_WORLD_CACHE_VERSION.encode("utf-8")) hasher.update(str(world_cfg.voxel_size).encode("utf-8")) hasher.update(str(world_cfg.voxel_padding).encode("utf-8")) for idx, obj in enumerate(world_cfg.rigid_objects or []): diff --git a/embodichain/lab/sim/planners/curobo/curobo_yaml.py b/embodichain/lab/sim/planners/curobo/curobo_yaml.py index 4a82b2151..d9e7e1dbf 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_yaml.py +++ b/embodichain/lab/sim/planners/curobo/curobo_yaml.py @@ -346,7 +346,7 @@ def _convex_hulls_to_voxel_entry( pose: torch.Tensor, *, voxel_size: float = 0.01, - voxel_padding: float = 0.1, + voxel_padding: float = 0.005, ) -> tuple[str, dict[str, object]]: """Decompose one mesh with VisACD and convert its union to an ESDF grid. @@ -422,7 +422,7 @@ def generate_curobo_world_scene( *, env_id: int = 0, voxel_size: float = 0.01, - voxel_padding: float = 0.1, + voxel_padding: float = 0.005, ) -> dict[str, dict[str, dict[str, object]]]: """Build a VisACD-decomposed ESDF voxel scene for cuRobo. From a97f74628086d79566f3a50231351ac9cf38404f Mon Sep 17 00:00:00 2001 From: matafela Date: Tue, 11 Aug 2026 16:49:04 +0800 Subject: [PATCH 6/6] shape fetching from dexsim --- .../embodichain.lab.sim.objects.rst | 4 + .../overview/sim/planners/curobo_planner.md | 69 +++- embodichain/lab/sim/objects/__init__.py | 2 +- embodichain/lab/sim/objects/rigid_object.py | 194 ++++++++- .../lab/sim/planners/curobo/curobo_planner.py | 201 ++++++++-- .../lab/sim/planners/curobo/curobo_yaml.py | 374 +++++++++++++++--- tests/sim/objects/test_collision_shapes.py | 97 +++++ tests/sim/planners/test_curobo_planner.py | 200 +++++++++- 8 files changed, 1015 insertions(+), 126 deletions(-) create mode 100644 tests/sim/objects/test_collision_shapes.py diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst index 3af7583d5..9aaa00c66 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst @@ -22,6 +22,7 @@ bodies. Light LightCfg RigidObject + CollisionShapeDesc RigidBodyData RigidObjectCfg RigidObjectGroup @@ -67,6 +68,9 @@ Rigid Object :inherited-members: :show-inheritance: +.. autoclass:: CollisionShapeDesc + :members: + .. autoclass:: RigidBodyData :members: :inherited-members: diff --git a/docs/source/overview/sim/planners/curobo_planner.md b/docs/source/overview/sim/planners/curobo_planner.md index b5c16c57b..b6c411adc 100644 --- a/docs/source/overview/sim/planners/curobo_planner.md +++ b/docs/source/overview/sim/planners/curobo_planner.md @@ -140,21 +140,55 @@ that use only one move type retain one planner backend; using both incurs a second one-time warmup and its graph-resident memory, but still no subprocess or second CUDA context. -The collision world is always auto-generated from live `RigidObject` meshes via -`CuroboWorldCfg.rigid_objects`: the adapter reads each object's mesh -(`get_vertices` / `get_triangles`) and world pose (`get_local_pose`), decomposes -the mesh into at most 16 convex hulls with DexSim -`convex_decomposition_visacd`, and computes their union as an ESDF voxel grid. -The tensor-backed voxel scene is cached on the first plan and loaded directly -into cuRobo's `SceneData`; there are no cuboid, triangle-mesh, or sphere world -representation branches. `CuroboWorldCfg.voxel_size` controls resolution and -`voxel_padding` keeps collision queries inside the ESDF grid near its boundary. +The collision world is auto-generated from live `RigidObject` **physical +collision shapes** via `RigidObject.get_collision_shapes()`. It does not use +`get_vertices()` / `get_triangles()`, which expose combined visual meshes and may +differ from the geometry used by DexSim physics. + +`CuroboWorldCfg.representation="auto"` is the default. The policy preserves +boxes as cuboids, spheres and capsules as analytic primitives, and convex +collision shapes as meshes. Triangle meshes remain meshes up to +`mesh_triangle_threshold`; above that threshold they become voxel ESDF when the +estimated dense allocation fits `max_voxel_count`. Pose-dynamic meshes use twice +the threshold before voxelization, while static cached meshes favor ESDF sooner +for repeated collision queries. SDF descriptors fall back to +their canonical collision mesh because the current DexSim Python binding does +not expose reusable SDF grid data. Unsupported descriptors raise an explicit +error instead of silently falling back to visual geometry. + +Forced voxel mode and per-object overrides remain available: + +```python +world_cfg = CuroboWorldCfg( + rigid_objects=[room_scan, precision_fixture], + representation="auto", + overrides={ + "room_scan": "voxel", + "precision_fixture": "mesh", + }, +) +``` + +`voxel_size` and `voxel_padding` configure generated ESDF layers. `plane_dims` +bounds an infinite DexSim plane as a thin cuRobo cuboid. Compound and ACD bodies +produce stable names such as `fixture__shape_0`; callers still use the owning +`RigidObject` UID in `dynamic_obstacle_names` and +`CuroboPlanOptions.dynamic_obstacle_poses`, and the adapter fans each update out +through the sub-shapes' local poses. + +> **DexSim binding requirement:** Correct compound/USD offsets require +> `RigidBody.get_shape_geometry()` to copy each physical shape's local pose into +> `ShapeGeometry.local_pose`. Reusing a DexSim SDF as a voxel grid additionally +> requires grid metadata/data that the current Python API does not expose. Until +> those upstream bindings are available, verify compound offsets explicitly; +> SDF descriptors use their canonical collision mesh when one is exposed and +> otherwise raise an actionable error. + Generated poses are authored in the cuRobo base/world frame, so this is exact when the robot base sits at the simulator world origin. For obstacles that move -or live in an offset base frame, also declare their names in +or live in an offset base frame, declare their object UIDs in `CuroboWorldCfg.dynamic_obstacle_names` and update poses at plan time through -`CuroboPlanOptions.dynamic_obstacle_poses`. Every source object remains one -same-named voxel layer, so pose updates use the original `RigidObject` name. +`CuroboPlanOptions.dynamic_obstacle_poses`. ### Shared and per-environment collision worlds @@ -178,7 +212,7 @@ differ, the adapter rejects the update and instructs the caller to enable With `multi_env=True`, cuRobo allocates one collision world per batch row and EmbodiChain sends row `i` of each dynamic obstacle pose to world `i`. The -auto-generated voxel cache still reads the static scene from env 0 and clones that +auto-generated collision cache still reads the static scene from env 0 and clones that scene for every row; setting `multi_env=True` does not by itself discover each environment's distinct initial object poses. Any object whose robot-relative pose differs by environment must also: @@ -231,12 +265,13 @@ or `~/.cache/embodichain_curobo`) keyed by the URDF path, URDF content, control part, tool frame, and fit parameters, so editing the URDF or changing the fit settings regenerates automatically and subsequent inits reuse the cache. Sphere fitting always uses DexSim's `SphereFitType.MORPHIT`, with at most 2 convex hulls -per robot link and 16 per obstacle. The default `sphere_density=0.1` keeps the +per robot link and 16 per voxelized obstacle shape. The default +`sphere_density=0.1` keeps the per-link sphere count low (~80 for a Panda) so planning stays fast; raise it for tighter collision coverage, or set `force=True` to bypass the cache. -For an Open3D overlay of the live robot/obstacle meshes and the exact spheres -read back from those YAML caches, call +For an Open3D overlay of the robot collision spheres and sampled world collision +representations read back from those caches, call `planner.visualize_robot_collision_models(control_part)`. Robot sphere centers are transformed by the simulator's live link poses. The interactive cuRobo example calls this once after planner initialization; close the Open3D window to continue. @@ -319,7 +354,7 @@ python examples/sim/planners/curobo_planner.py --headless --sim-device cpu ~~~ The demo exports the DexSim `demo_block` into the cuRobo collision world via -`CuroboWorldCfg.rigid_objects` (the robot YAML and voxel world cache are +`CuroboWorldCfg.rigid_objects` (the robot YAML and mixed collision-world cache are auto-generated), prints the result status and trajectory shape, then replays the returned full-DoF trajectory. CUDA graph capture is enabled by default with the renderer-compatible `"thread_local"` mode; pass `--no-cuda-graph` to disable it. diff --git a/embodichain/lab/sim/objects/__init__.py b/embodichain/lab/sim/objects/__init__.py index 52c24fefe..97b6aa728 100644 --- a/embodichain/lab/sim/objects/__init__.py +++ b/embodichain/lab/sim/objects/__init__.py @@ -20,7 +20,7 @@ """ from ..common import BatchEntity -from .rigid_object import RigidObject, RigidBodyData, RigidObjectCfg +from .rigid_object import CollisionShapeDesc, RigidObject, RigidBodyData, RigidObjectCfg from .rigid_object_group import ( RigidObjectGroup, RigidBodyGroupData, diff --git a/embodichain/lab/sim/objects/rigid_object.py b/embodichain/lab/sim/objects/rigid_object.py index 5a4bd80a1..e99e14ec4 100644 --- a/embodichain/lab/sim/objects/rigid_object.py +++ b/embodichain/lab/sim/objects/rigid_object.py @@ -25,7 +25,20 @@ from functools import cached_property from dexsim.models import MeshObject -from dexsim.types import RigidBodyGPUAPIReadType, RigidBodyGPUAPIWriteType +from dexsim.types import ( + RigidBodyGPUAPIReadType, + RigidBodyGPUAPIWriteType, + RigidBodyShape, +) +from dexsim.engine import ( + BoxGeometry, + CapsuleGeometry, + ConvexMeshGeometry, + PlaneGeometry, + SDFGeometry, + SphereGeometry, + TriangleMeshGeometry, +) from dexsim.engine import CudaArray, MaterialInst, PhysicsScene from embodichain.lab.sim.cfg import RigidObjectCfg, RigidBodyAttributesCfg from embodichain.lab.sim.shapes import MeshCfg @@ -49,7 +62,36 @@ from embodichain.utils.math import matrix_from_quat, quat_from_matrix, matrix_from_euler from embodichain.utils import logger -__all__ = ["RigidBodyData", "RigidObject", "RigidObjectCfg"] +__all__ = ["CollisionShapeDesc", "RigidBodyData", "RigidObject", "RigidObjectCfg"] + + +@dataclass +class CollisionShapeDesc: + """Planner-independent snapshot of one DexSim physical collision shape. + + Geometry values are copied from DexSim's runtime collision descriptor and + therefore already include its geometry scale. Consumers must not apply + :attr:`RigidObjectCfg.body_scale` again. + + Attributes: + name: Stable shape name within the owning rigid object. + shape_type: DexSim collision-shape type. + local_pose: Shape pose relative to the rigid object's frame, as ``(4, 4)``. + half_extents: Box half-extents, when applicable. + radius: Sphere or capsule radius, when applicable. + half_height: Capsule cylinder half-height, when applicable. + vertices: Scaled collision-mesh vertices, when applicable. + triangles: Collision-mesh triangle indices, when applicable. + """ + + name: str + shape_type: RigidBodyShape + local_pose: torch.Tensor + half_extents: torch.Tensor | None = None + radius: float | None = None + half_height: float | None = None + vertices: torch.Tensor | None = None + triangles: torch.Tensor | None = None @dataclass @@ -1204,6 +1246,154 @@ def get_triangles(self, env_ids: Sequence[int] | None = None) -> torch.Tensor: device=self.device, ) + def get_collision_shapes(self, env_id: int = 0) -> list[CollisionShapeDesc]: + """Snapshot the physical collision shapes used by DexSim. + + Unlike :meth:`get_vertices` and :meth:`get_triangles`, this method reads + the physics body's collision descriptors rather than render meshes. For + batched objects, every row is checked for identical shape topology before + the requested row is returned. + + .. attention:: + The installed DexSim binding must populate ``ShapeGeometry.local_pose`` + and dispatch SDF geometry through ``get_shape_geometry``. This method + preserves the values returned by DexSim and raises an actionable error + when a descriptor cannot be retrieved. + + Args: + env_id: Environment row whose collision geometry is returned. + + Returns: + Physical collision-shape descriptors in stable shape-index order. + + Raises: + IndexError: If ``env_id`` is outside the object batch. + RuntimeError: If DexSim cannot expose a collision descriptor. + ValueError: If collision topology differs between environment rows. + """ + if env_id < 0 or env_id >= self.num_instances: + raise IndexError( + f"env_id must be in [0, {self.num_instances}), got {env_id}." + ) + + requested = self._get_collision_shapes_for_entity(env_id) + requested_topology = self._collision_shape_topology(requested) + for other_env_id in range(self.num_instances): + if other_env_id == env_id: + continue + other = self._get_collision_shapes_for_entity(other_env_id) + if self._collision_shape_topology(other) != requested_topology: + raise ValueError( + f"RigidObject {self.uid!r} has different collision-shape " + f"topology in environment rows {env_id} and {other_env_id}." + ) + return requested + + def _get_collision_shapes_for_entity(self, env_id: int) -> list[CollisionShapeDesc]: + """Return physical collision descriptors for one simulator entity.""" + physical_body = self._entities[env_id].get_physical_body() + if physical_body is None: + raise RuntimeError(f"RigidObject {self.uid!r} has no DexSim physical body.") + + shape_count = int(physical_body.get_shape_count()) + shapes: list[CollisionShapeDesc] = [] + for shape_idx in range(shape_count): + try: + geometry = physical_body.get_shape_geometry(shape_idx) + except Exception as exc: # noqa: BLE001 + raise RuntimeError( + f"DexSim could not expose collision shape {shape_idx} for " + f"RigidObject {self.uid!r}. SDF/custom shapes require a " + "geometry descriptor or canonical collision mesh." + ) from exc + if geometry is None: + raise RuntimeError( + f"DexSim returned no geometry for collision shape {shape_idx} " + f"of RigidObject {self.uid!r}." + ) + + shape_name = physical_body.get_shape_name(shape_idx) or f"shape_{shape_idx}" + local_pose = torch.tensor(geometry.local_pose, dtype=torch.float32) + if local_pose.shape != (4, 4): + raise RuntimeError( + f"DexSim collision shape {shape_idx} of {self.uid!r} returned " + f"local_pose shape {tuple(local_pose.shape)}, expected (4, 4)." + ) + desc = CollisionShapeDesc( + name=str(shape_name), + shape_type=self._collision_shape_type(geometry), + local_pose=local_pose.clone(), + ) + if isinstance(geometry, BoxGeometry): + desc.half_extents = torch.tensor( + geometry.half_extents, dtype=torch.float32 + ) + elif isinstance(geometry, SphereGeometry): + desc.radius = float(geometry.radius) + elif isinstance(geometry, CapsuleGeometry): + desc.radius = float(geometry.radius) + desc.half_height = float(geometry.half_height) + elif isinstance( + geometry, (ConvexMeshGeometry, TriangleMeshGeometry, SDFGeometry) + ): + vertices = torch.tensor(geometry.vertices, dtype=torch.float32).reshape( + -1, 3 + ) + triangles = torch.tensor(geometry.triangles, dtype=torch.int32).reshape( + -1, 3 + ) + scale = getattr(geometry, "scale", None) + if scale is not None: + vertices = vertices * torch.tensor( + scale, dtype=torch.float32 + ).reshape(1, 3) + desc.vertices = vertices + desc.triangles = triangles + shapes.append(desc) + return shapes + + @staticmethod + def _collision_shape_type(geometry: object) -> RigidBodyShape: + """Map a concrete DexSim geometry descriptor to its shape enum.""" + if isinstance(geometry, BoxGeometry): + return RigidBodyShape.BOX + if isinstance(geometry, PlaneGeometry): + return RigidBodyShape.PLANE + if isinstance(geometry, SphereGeometry): + return RigidBodyShape.SPHERE + if isinstance(geometry, CapsuleGeometry): + return RigidBodyShape.CAPSULE + if isinstance(geometry, ConvexMeshGeometry): + return RigidBodyShape.CONVEX + if isinstance(geometry, TriangleMeshGeometry): + return RigidBodyShape.MESH + if isinstance(geometry, SDFGeometry): + return RigidBodyShape.SDF + raise RuntimeError( + f"Unsupported DexSim collision geometry descriptor " + f"{type(geometry).__name__}." + ) + + @staticmethod + def _collision_shape_topology( + shapes: list[CollisionShapeDesc], + ) -> tuple[tuple[object, ...], ...]: + """Return the topology-only signature used for batched validation.""" + return tuple( + ( + shape.name, + shape.shape_type.value, + None if shape.vertices is None else tuple(shape.vertices.shape), + None if shape.triangles is None else tuple(shape.triangles.shape), + ( + None + if shape.triangles is None + else shape.triangles.contiguous().numpy().tobytes() + ), + ) + for shape in shapes + ) + def get_user_ids(self, env_ids: Sequence[int] | None = None) -> torch.Tensor: """Get the user ids of the rigid bodies. diff --git a/embodichain/lab/sim/planners/curobo/curobo_planner.py b/embodichain/lab/sim/planners/curobo/curobo_planner.py index dbf07e521..f62fa6ea9 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_planner.py +++ b/embodichain/lab/sim/planners/curobo/curobo_planner.py @@ -133,22 +133,41 @@ def __deepcopy__(self, memo: dict) -> "_RigidObjectRefList": # noqa: ARG002 class CuroboWorldCfg: """Static collision-world configuration for the cuRobo backend. - The collision world is always auto-generated from live :class:`RigidObject` - meshes (see :attr:`rigid_objects`); there is no external scene-YAML path. + The collision world is generated from live :class:`RigidObject` physical + collision shapes (see :attr:`rigid_objects`); there is no external + scene-YAML path. """ rigid_objects: list[RigidObject] | None = None - """Live objects to bake into the auto-generated voxel collision scene. - - The adapter reads each object's mesh (``get_vertices`` / ``get_triangles``), - decomposes it with DexSim VisACD, and builds a cuRobo ESDF voxel layer cached - on disk by content hash. Poses are expressed in the cuRobo world/base frame, - so this is exact when the robot base sits at the simulator world origin. For - obstacles that move or live in an offset base frame, also list their names in - :attr:`dynamic_obstacle_names` to update poses at plan time. ``None`` yields an - initially empty collision world. + """Live objects to export into the auto-generated collision scene. + + The adapter reads :meth:`RigidObject.get_collision_shapes`, so planning uses + the physical geometry seen by DexSim rather than combined visual meshes. + Poses are expressed in the cuRobo world/base frame. For obstacles that move + or live in an offset base frame, also list their names in + :attr:`dynamic_obstacle_names`. ``None`` yields an empty collision world. + """ + + representation: str = "auto" + """Collision representation policy: ``"auto"`` or forced ``"voxel"``.""" + + overrides: dict[str, str] = {} + """Per-object representation overrides keyed by :class:`RigidObject` UID. + + Supported values are ``"auto"``, ``"voxel"``, ``"mesh"``, ``"cuboid"``, + ``"sphere"``, and ``"capsule"``. A forced analytic representation must + match the source physical shape. """ + mesh_triangle_threshold: int = 5_000 + """Triangle count above which ``auto`` prefers voxel ESDF for mesh shapes.""" + + max_voxel_count: int = 2_000_000 + """Maximum estimated voxel count allowed when ``auto`` selects ESDF.""" + + plane_dims: tuple[float, float, float] = (10.0, 10.0, 0.01) + """Workspace-bounded dimensions used to represent an infinite plane.""" + voxel_size: float = 0.01 """ESDF voxel edge length in meters for every world collision object.""" @@ -466,6 +485,38 @@ def _validate_dynamic_obstacles( ) +_WORLD_REPRESENTATIONS = frozenset( + {"auto", "voxel", "mesh", "cuboid", "sphere", "capsule"} +) + + +def _validate_world_cfg(cfg: CuroboWorldCfg) -> None: + """Validate collision-world representation policy settings.""" + if cfg.representation not in _WORLD_REPRESENTATIONS: + raise ValueError( + f"CuroboWorldCfg.representation must be one of " + f"{sorted(_WORLD_REPRESENTATIONS)}, got {cfg.representation!r}." + ) + invalid_overrides = { + name: value + for name, value in cfg.overrides.items() + if value not in _WORLD_REPRESENTATIONS + } + if invalid_overrides: + raise ValueError( + f"CuroboWorldCfg.overrides contains unsupported representations: " + f"{invalid_overrides}." + ) + if cfg.mesh_triangle_threshold < 0: + raise ValueError("CuroboWorldCfg.mesh_triangle_threshold must be non-negative.") + if cfg.max_voxel_count <= 0: + raise ValueError("CuroboWorldCfg.max_voxel_count must be positive.") + if len(cfg.plane_dims) != 3 or any(value <= 0.0 for value in cfg.plane_dims): + raise ValueError( + "CuroboWorldCfg.plane_dims must contain three positive dimensions." + ) + + # ============================================================================= # Lazy cuRobo V2 binding acquisition # ============================================================================= @@ -756,7 +807,9 @@ def __init__(self, cfg: CuroboPlannerCfg) -> None: self._backend_cache: dict[tuple[str, int, bool, MoveType], "_CuroboBackend"] = ( {} ) + self._dynamic_shape_cache: dict[str, list[tuple[str, torch.Tensor]]] = {} world_cfg = cfg.world + _validate_world_cfg(world_cfg) if world_cfg.voxel_size <= 0.0: logger.log_error( f"CuroboWorldCfg.voxel_size must be positive, got " @@ -1527,7 +1580,7 @@ def _robot_yaml_cache_key( return hasher.hexdigest() def _auto_generate_world_scene(self, world_cfg: CuroboWorldCfg) -> "Any": - """Load or generate a tensor-backed cuRobo voxel scene.""" + """Load or generate a tensor-backed mixed cuRobo collision scene.""" from .curobo_yaml import generate_curobo_world_scene rigid_objects = world_cfg.rigid_objects @@ -1545,17 +1598,23 @@ def _auto_generate_world_scene(self, world_cfg: CuroboWorldCfg) -> "Any": cache_key = self._world_scene_cache_key(world_cfg) cache_path = os.path.join(cache_dir, f"world_{cache_key}.pt") if not auto.force and os.path.exists(cache_path): - logger.log_info(f"cuRobo voxel world cache hit: {cache_path}") + logger.log_info(f"cuRobo collision world cache hit: {cache_path}") scene_data = torch.load(cache_path, map_location="cpu", weights_only=True) else: logger.log_info( - f"Generating VisACD voxel collision data from " + f"Generating physical-shape collision data from " f"{len(rigid_objects)} RigidObject(s) -> {cache_path}" ) scene_data = generate_curobo_world_scene( rigid_objects, + representation=world_cfg.representation, + overrides=world_cfg.overrides, + dynamic_obstacle_names=world_cfg.dynamic_obstacle_names, voxel_size=world_cfg.voxel_size, voxel_padding=world_cfg.voxel_padding, + mesh_triangle_threshold=world_cfg.mesh_triangle_threshold, + max_voxel_count=world_cfg.max_voxel_count, + plane_dims=world_cfg.plane_dims, ) os.makedirs(cache_dir, exist_ok=True) torch.save(scene_data, cache_path) @@ -1568,20 +1627,34 @@ def _auto_generate_world_scene(self, world_cfg: CuroboWorldCfg) -> "Any": return self._bindings.Scene.create(runtime_data) def _world_scene_cache_key(self, world_cfg: CuroboWorldCfg) -> str: - """Hash object geometry, initial poses, and voxel settings.""" + """Hash physical collision geometry, initial poses, and policy settings.""" hasher = hashlib.md5() + hasher.update(b"physical-shapes-v1") + hasher.update(world_cfg.representation.encode("utf-8")) + hasher.update(repr(sorted(world_cfg.overrides.items())).encode("utf-8")) + hasher.update(repr(sorted(world_cfg.dynamic_obstacle_names)).encode("utf-8")) hasher.update(str(world_cfg.voxel_size).encode("utf-8")) hasher.update(str(world_cfg.voxel_padding).encode("utf-8")) + hasher.update(str(world_cfg.mesh_triangle_threshold).encode("utf-8")) + hasher.update(str(world_cfg.max_voxel_count).encode("utf-8")) + hasher.update(repr(world_cfg.plane_dims).encode("utf-8")) for idx, obj in enumerate(world_cfg.rigid_objects or []): name = getattr(obj, "uid", None) or f"obstacle_{idx}" hasher.update(name.encode("utf-8")) - vertices = obj.get_vertices(env_ids=[0], scale=True)[0] - faces = obj.get_triangles(env_ids=[0])[0] - pose = obj.get_local_pose(to_matrix=False)[0] - hasher.update( - vertices.detach().to("cpu").to(torch.float32).numpy().tobytes() - ) - hasher.update(faces.detach().to("cpu").numpy().tobytes()) + for shape in obj.get_collision_shapes(env_id=0): + hasher.update(shape.name.encode("utf-8")) + hasher.update(str(shape.shape_type.value).encode("utf-8")) + for value in ( + shape.local_pose, + shape.half_extents, + shape.vertices, + shape.triangles, + ): + if value is not None: + hasher.update(value.detach().cpu().numpy().tobytes()) + hasher.update(repr(shape.radius).encode("utf-8")) + hasher.update(repr(shape.half_height).encode("utf-8")) + pose = obj.get_local_pose(to_matrix=True)[0] hasher.update(pose.detach().to("cpu").to(torch.float32).numpy().tobytes()) return hasher.hexdigest() @@ -1590,12 +1663,12 @@ def visualize_robot_collision_models( control_part: str, env_id: int = 0, ) -> None: - """Visualize cached robot spheres and world collision voxels. + """Visualize cached robot spheres and world collision models. This materializes the same content-addressed caches used by the planner. The robot spheres are transformed by each link's live :meth:`~embodichain.lab.sim.objects.Articulation.get_link_pose`; obstacle - voxels retain the poses and ESDF values consumed by cuRobo. + samples retain the mixed physical-shape scene consumed by cuRobo. Args: control_part: Robot control part whose cuRobo profile/cache is used. @@ -2215,23 +2288,69 @@ def update_dynamic_obstacles( pose_tensor, device=self._curobo_device, dtype=torch.float32 ) b = pose_tensor.shape[0] - for cached_backend in backends: - key = id(cached_backend) - inv = inv_cache.get(key) - if inv is None or inv.shape[0] != b: - if ( - backend is not None - and sim_base_pose_inv is not None - and sim_base_pose_inv.shape[0] == b - ): - inv = sim_base_pose_inv - else: - inv = pose_inv(self._get_sim_base_pose(cached_backend, b)) - inv_cache[key] = inv - curobo_pose = self._sim_world_to_curobo_base_pose( - pose_tensor, cached_backend, inv - ) - self._update_backend_obstacle(name, curobo_pose, cached_backend) + for obstacle_name, shape_local_pose in self._dynamic_obstacle_shapes(name): + local_pose = shape_local_pose.to( + device=self._curobo_device, dtype=torch.float32 + ).expand(b, -1, -1) + shape_pose_tensor = pose_tensor @ local_pose + for cached_backend in backends: + key = id(cached_backend) + inv = inv_cache.get(key) + if inv is None or inv.shape[0] != b: + if ( + backend is not None + and sim_base_pose_inv is not None + and sim_base_pose_inv.shape[0] == b + ): + inv = sim_base_pose_inv + else: + inv = pose_inv(self._get_sim_base_pose(cached_backend, b)) + inv_cache[key] = inv + curobo_pose = self._sim_world_to_curobo_base_pose( + shape_pose_tensor, cached_backend, inv + ) + self._update_backend_obstacle( + obstacle_name, curobo_pose, cached_backend + ) + + def _dynamic_obstacle_shapes( + self, object_name: str + ) -> list[tuple[str, torch.Tensor]]: + """Resolve one object UID to stable cuRobo shape names and local poses.""" + shape_cache = getattr(self, "_dynamic_shape_cache", None) + if shape_cache is None: + shape_cache = {} + self._dynamic_shape_cache = shape_cache + if object_name in shape_cache: + return shape_cache[object_name] + rigid_object = next( + ( + obj + for obj in self.cfg.world.rigid_objects or [] + if getattr(obj, "uid", None) == object_name + ), + None, + ) + if rigid_object is None: + logger.log_error( + f"Dynamic obstacle {object_name!r} has no matching RigidObject in " + "CuroboWorldCfg.rigid_objects.", + ValueError, + ) + shapes = rigid_object.get_collision_shapes(env_id=0) + results: list[tuple[str, torch.Tensor]] = [] + for shape_idx, shape in enumerate(shapes): + obstacle_name = ( + object_name if len(shapes) == 1 else f"{object_name}__shape_{shape_idx}" + ) + local_pose = shape.local_pose.clone() + if shape.shape_type.name == "PLANE": + local_offset = torch.eye(4, dtype=torch.float32) + local_offset[2, 3] = -0.5 * self.cfg.world.plane_dims[2] + local_pose = local_pose @ local_offset + results.append((obstacle_name, local_pose)) + shape_cache[object_name] = results + return results def _update_backend_obstacle( self, name: str, pose_tensor: torch.Tensor, backend: "_CuroboBackend" diff --git a/embodichain/lab/sim/planners/curobo/curobo_yaml.py b/embodichain/lab/sim/planners/curobo/curobo_yaml.py index d9e7e1dbf..a2ece5f9e 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_yaml.py +++ b/embodichain/lab/sim/planners/curobo/curobo_yaml.py @@ -22,8 +22,8 @@ this automatically (with on-disk caching) on the first plan; see :class:`~embodichain.lab.sim.planners.curobo.curobo_planner.CuroboAutoGenCfg`. -:func:`generate_curobo_world_scene` builds cuRobo voxel collision data from live -:class:`~embodichain.lab.sim.objects.RigidObject` meshes. +:func:`generate_curobo_world_scene` builds mixed cuRobo collision data from live +:class:`~embodichain.lab.sim.objects.RigidObject` physical shapes. """ from __future__ import annotations @@ -31,7 +31,9 @@ from typing import TYPE_CHECKING, Any, Sequence import torch +from dexsim.types import RigidBodyShape +from embodichain.lab.sim.objects.rigid_object import CollisionShapeDesc from embodichain.utils import logger from embodichain.utils.math import matrix_from_quat, quat_from_matrix @@ -324,7 +326,7 @@ def generate_curobo_robot_yaml( # ============================================================================= -# World voxel generation from RigidObject meshes +# World collision generation from RigidObject physical shapes # ============================================================================= @@ -417,76 +419,280 @@ def _convex_hulls_to_voxel_entry( } +def _pose_matrix_to_list(pose: torch.Tensor) -> list[float]: + """Convert a homogeneous pose matrix to cuRobo ``xyz+wxyz`` format.""" + pose = torch.as_tensor(pose, dtype=torch.float32).detach().cpu() + return torch.cat([pose[:3, 3], quat_from_matrix(pose[:3, :3])]).tolist() + + +def _collision_shape_mesh( + shape: CollisionShapeDesc, + plane_dims: tuple[float, float, float], +) -> tuple[torch.Tensor, torch.Tensor]: + """Convert a physical collision descriptor to a local triangle mesh.""" + if shape.vertices is not None and shape.triangles is not None: + if shape.vertices.numel() and shape.triangles.numel(): + return shape.vertices, shape.triangles + + import trimesh + + if shape.shape_type == RigidBodyShape.BOX: + assert shape.half_extents is not None + mesh = trimesh.creation.box(extents=(2.0 * shape.half_extents).numpy()) + elif shape.shape_type == RigidBodyShape.PLANE: + mesh = trimesh.creation.box(extents=plane_dims) + elif shape.shape_type == RigidBodyShape.SPHERE: + assert shape.radius is not None + mesh = trimesh.creation.icosphere(subdivisions=2, radius=shape.radius) + elif shape.shape_type == RigidBodyShape.CAPSULE: + assert shape.radius is not None and shape.half_height is not None + mesh = trimesh.creation.capsule( + radius=shape.radius, height=2.0 * shape.half_height + ) + else: + raise ValueError( + f"Collision shape {shape.name!r} ({shape.shape_type.name}) does not " + "expose a mesh usable by cuRobo." + ) + return ( + torch.as_tensor(mesh.vertices, dtype=torch.float32), + torch.as_tensor(mesh.faces, dtype=torch.int32), + ) + + +def _estimated_voxel_count( + vertices: torch.Tensor, + voxel_size: float, + voxel_padding: float, +) -> int: + """Estimate the dense ESDF allocation for a local collision mesh.""" + extents = vertices.amax(dim=0) - vertices.amin(dim=0) + 2.0 * voxel_padding + shape = torch.clamp(torch.ceil(extents / voxel_size), min=2).to(torch.int64) + return int(torch.prod(shape).item()) + + +def _auto_collision_representation( + shape: CollisionShapeDesc, + *, + is_dynamic: bool, + voxel_size: float, + voxel_padding: float, + mesh_triangle_threshold: int, + max_voxel_count: int, + plane_dims: tuple[float, float, float], +) -> str: + """Select a cuRobo representation from one physical shape descriptor.""" + native = { + RigidBodyShape.BOX: "cuboid", + RigidBodyShape.PLANE: "cuboid", + RigidBodyShape.SPHERE: "sphere", + RigidBodyShape.CAPSULE: "capsule", + RigidBodyShape.CONVEX: "mesh", + RigidBodyShape.SDF: "mesh", + } + if shape.shape_type in native: + return native[shape.shape_type] + if shape.shape_type != RigidBodyShape.MESH: + raise ValueError( + f"No automatic cuRobo representation for DexSim shape " + f"{shape.shape_type.name}." + ) + + vertices, triangles = _collision_shape_mesh(shape, plane_dims) + effective_triangle_threshold = mesh_triangle_threshold * (2 if is_dynamic else 1) + if triangles.shape[0] <= effective_triangle_threshold: + return "mesh" + voxel_count = _estimated_voxel_count(vertices, voxel_size, voxel_padding) + if voxel_count <= max_voxel_count: + return "voxel" + logger.log_warning( + f"Keeping collision mesh {shape.name!r}: its estimated ESDF allocation " + f"({voxel_count} voxels) exceeds max_voxel_count={max_voxel_count}." + ) + return "mesh" + + +def _validate_forced_representation( + representation: str, + shape: CollisionShapeDesc, +) -> None: + """Reject analytic overrides that do not match the physics shape.""" + required_type = { + "cuboid": {RigidBodyShape.BOX, RigidBodyShape.PLANE}, + "sphere": {RigidBodyShape.SPHERE}, + "capsule": {RigidBodyShape.CAPSULE}, + } + if ( + representation in required_type + and shape.shape_type not in required_type[representation] + ): + raise ValueError( + f"Cannot represent DexSim {shape.shape_type.name} shape " + f"{shape.name!r} as {representation!r}." + ) + + def generate_curobo_world_scene( rigid_objects: Sequence[RigidObject], *, env_id: int = 0, + representation: str = "auto", + overrides: dict[str, str] | None = None, + dynamic_obstacle_names: Sequence[str] = (), voxel_size: float = 0.01, voxel_padding: float = 0.005, + mesh_triangle_threshold: int = 5_000, + max_voxel_count: int = 2_000_000, + plane_dims: tuple[float, float, float] = (10.0, 10.0, 0.01), ) -> dict[str, dict[str, dict[str, object]]]: - """Build a VisACD-decomposed ESDF voxel scene for cuRobo. + """Build a mixed cuRobo scene from DexSim physical collision shapes. - Every source object produces one same-named voxel layer. The SDF is the - union of at most 16 convex hulls returned by DexSim's - :func:`convex_decomposition_visacd`. + ``auto`` preserves primitives, exports collision meshes directly, and uses + ESDF for triangle meshes whose complexity exceeds ``mesh_triangle_threshold`` + when the estimated dense grid fits ``max_voxel_count``. Forced ``voxel`` + remains available globally or per object. Args: - rigid_objects: Live obstacles whose meshes define the collision world. - env_id: Environment row used for geometry and initial object poses. + rigid_objects: Live obstacles whose physical shapes define the world. + env_id: Environment row used for geometry and initial poses. + representation: Global ``auto`` or forced representation policy. + overrides: Per-object policies keyed by rigid-object UID. + dynamic_obstacle_names: Object UIDs whose poses change between plans. voxel_size: ESDF voxel edge length in meters. - voxel_padding: Free-space padding around each object-local mesh. + voxel_padding: Free-space padding around object-local voxel grids. + mesh_triangle_threshold: Auto-policy triangle threshold. + max_voxel_count: Auto-policy upper bound for a dense ESDF grid. + plane_dims: Workspace-bounded cuboid dimensions used for planes. Returns: - A tensor-backed ``{"voxel": ...}`` mapping accepted by cuRobo - :meth:`Scene.create`. + A mixed tensor-backed scene mapping accepted by cuRobo ``Scene.create``. Raises: - ValueError: If no usable uniquely named objects are provided. + ValueError: If configuration or collision geometry is unsupported. RuntimeError: If DexSim VisACD decomposition fails. """ rigid_objects = list(rigid_objects) if not rigid_objects: raise ValueError("rigid_objects must contain at least one RigidObject.") - - voxels: dict[str, dict[str, object]] = {} - for idx, obj in enumerate(rigid_objects): - name = getattr(obj, "uid", None) or f"obstacle_{idx}" - if name in voxels: + overrides = overrides or {} + supported = {"auto", "voxel", "mesh", "cuboid", "sphere", "capsule"} + if representation not in supported or any( + value not in supported for value in overrides.values() + ): + raise ValueError(f"representation policies must be one of {sorted(supported)}.") + if voxel_size <= 0.0: + raise ValueError(f"voxel_size must be positive, got {voxel_size}.") + if voxel_padding < 0.0: + raise ValueError(f"voxel_padding must be non-negative, got {voxel_padding}.") + if mesh_triangle_threshold < 0: + raise ValueError("mesh_triangle_threshold must be non-negative.") + if max_voxel_count <= 0: + raise ValueError("max_voxel_count must be positive.") + if len(plane_dims) != 3 or any(value <= 0.0 for value in plane_dims): + raise ValueError("plane_dims must contain three positive dimensions.") + + scene: dict[str, dict[str, dict[str, object]]] = {} + object_names: set[str] = set() + for object_idx, obj in enumerate(rigid_objects): + object_name = getattr(obj, "uid", None) or f"obstacle_{object_idx}" + if object_name in object_names: raise ValueError( - f"Duplicate obstacle name {name!r}; RigidObject uids must be unique." + f"Duplicate obstacle name {object_name!r}; RigidObject uids must be unique." ) - vertices = obj.get_vertices(env_ids=[env_id], scale=True)[0] - faces = obj.get_triangles(env_ids=[env_id])[0] - pose = obj.get_local_pose(to_matrix=False)[env_id] - if vertices is None or faces is None or vertices.numel() == 0: - logger.log_warning( - f"RigidObject {name!r} has no mesh geometry; skipping collision export." + object_names.add(object_name) + shapes = obj.get_collision_shapes(env_id=env_id) + object_pose = ( + torch.as_tensor( + obj.get_local_pose(to_matrix=True)[env_id], dtype=torch.float32 ) - continue - obstacle_name, fields = _convex_hulls_to_voxel_entry( - name, - vertices, - faces, - pose, - voxel_size=voxel_size, - voxel_padding=voxel_padding, + .detach() + .cpu() + ) + for shape_idx, shape in enumerate(shapes): + obstacle_name = ( + object_name if len(shapes) == 1 else f"{object_name}__shape_{shape_idx}" + ) + shape_pose = object_pose @ shape.local_pose + policy = overrides.get(object_name, representation) + if policy == "auto": + policy = _auto_collision_representation( + shape, + is_dynamic=object_name in dynamic_obstacle_names, + voxel_size=voxel_size, + voxel_padding=voxel_padding, + mesh_triangle_threshold=mesh_triangle_threshold, + max_voxel_count=max_voxel_count, + plane_dims=plane_dims, + ) + _validate_forced_representation(policy, shape) + if shape.shape_type == RigidBodyShape.PLANE: + offset = torch.eye(4, dtype=torch.float32) + offset[2, 3] = -0.5 * plane_dims[2] + shape_pose = shape_pose @ offset + + fields: dict[str, object] + if policy == "cuboid": + if shape.shape_type == RigidBodyShape.PLANE: + dims = list(plane_dims) + else: + assert shape.half_extents is not None + dims = (2.0 * shape.half_extents).tolist() + fields = {"pose": _pose_matrix_to_list(shape_pose), "dims": dims} + elif policy == "sphere": + assert shape.radius is not None + fields = { + "pose": _pose_matrix_to_list(shape_pose), + "radius": shape.radius, + } + elif policy == "capsule": + assert shape.radius is not None and shape.half_height is not None + fields = { + "pose": _pose_matrix_to_list(shape_pose), + "radius": shape.radius, + "base": [0.0, 0.0, -shape.half_height], + "tip": [0.0, 0.0, shape.half_height], + } + elif policy == "mesh": + vertices, triangles = _collision_shape_mesh(shape, plane_dims) + fields = { + "pose": _pose_matrix_to_list(shape_pose), + "vertices": vertices.tolist(), + "faces": triangles.reshape(-1).tolist(), + } + elif policy == "voxel": + vertices, triangles = _collision_shape_mesh(shape, plane_dims) + _, fields = _convex_hulls_to_voxel_entry( + obstacle_name, + vertices, + triangles, + shape_pose, + voxel_size=voxel_size, + voxel_padding=voxel_padding, + ) + else: # pragma: no cover - policy is validated above + raise AssertionError(f"Unhandled collision policy {policy!r}.") + scene.setdefault(policy, {})[obstacle_name] = fields + + unknown_overrides = sorted(set(overrides) - object_names) + if unknown_overrides: + raise ValueError( + f"representation overrides reference unknown RigidObject UIDs: " + f"{unknown_overrides}." ) - voxels[obstacle_name] = fields - if not voxels: + if not scene: raise ValueError( "No collision obstacles could be generated from the given RigidObjects." ) - # VoxelData allocates each layer with the first layer's capacity. Keep the - # largest layer first so differently-sized object grids all fit the cache. - voxels = dict( - sorted( - voxels.items(), - key=lambda item: int(item[1]["feature_tensor"].numel()), - reverse=True, + if "voxel" in scene: + scene["voxel"] = dict( + sorted( + scene["voxel"].items(), + key=lambda item: int(item[1]["feature_tensor"].numel()), + reverse=True, + ) ) - ) - return {"voxel": voxels} + return scene # ============================================================================= @@ -733,16 +939,13 @@ def _remove_dexsim_visualization_actors(env: Any, actors: Sequence[Any]) -> None def _world_collision_sphere_data(world_scene: Any) -> tuple[torch.Tensor, torch.Tensor]: - """Return world-space centers and radii for the ESDF surface overlay.""" + """Return world-space samples and radii for the collision-world overlay.""" if isinstance(world_scene, dict): voxel_entries = list(world_scene.get("voxel", {}).items()) else: voxel_entries = [ (voxel.name, voxel) for voxel in (getattr(world_scene, "voxel", None) or []) ] - if not voxel_entries: - raise ValueError("The cuRobo world scene contains no voxel collision data.") - centers: list[torch.Tensor] = [] radii: list[torch.Tensor] = [] for name, entry in voxel_entries: @@ -764,6 +967,61 @@ def _world_collision_sphere_data(world_scene: Any) -> tuple[torch.Tensor, torch. centers.append(world_points) radii.append(torch.full((world_points.shape[0],), 0.5 * voxel_size)) + representation_names = ("cuboid", "sphere", "capsule", "mesh") + for representation in representation_names: + if isinstance(world_scene, dict): + entries = list(world_scene.get(representation, {}).items()) + else: + entries = [ + (entry.name, entry) + for entry in (getattr(world_scene, representation, None) or []) + ] + for _, entry in entries: + get_value = ( + entry.get + if isinstance(entry, dict) + else lambda key: getattr(entry, key) + ) + pose = torch.as_tensor(get_value("pose"), dtype=torch.float32) + rotation = matrix_from_quat(pose[3:7]) + if representation == "sphere": + local_points = torch.zeros((1, 3), dtype=torch.float32) + sample_radii = torch.tensor([float(get_value("radius"))]) + elif representation == "capsule": + base = torch.as_tensor(get_value("base"), dtype=torch.float32) + tip = torch.as_tensor(get_value("tip"), dtype=torch.float32) + steps = torch.linspace(0.0, 1.0, 9).unsqueeze(-1) + local_points = base + steps * (tip - base) + sample_radii = torch.full( + (local_points.shape[0],), float(get_value("radius")) + ) + elif representation == "cuboid": + dims = torch.as_tensor(get_value("dims"), dtype=torch.float32) + signs = torch.tensor( + [ + [x, y, z] + for x in (-0.5, 0.5) + for y in (-0.5, 0.5) + for z in (-0.5, 0.5) + ], + dtype=torch.float32, + ) + local_points = signs * dims + sample_radii = torch.full( + (local_points.shape[0],), max(0.005, float(dims.amin()) * 0.1) + ) + else: + local_points = torch.as_tensor( + get_value("vertices"), dtype=torch.float32 + ).reshape(-1, 3) + if local_points.shape[0] > 10_000: + stride = (local_points.shape[0] + 9_999) // 10_000 + local_points = local_points[::stride] + sample_radii = torch.full((local_points.shape[0],), 0.005) + world_points = local_points @ rotation.T + pose[:3] + centers.append(world_points) + radii.append(sample_radii) + if not centers: raise ValueError( "The cuRobo world scene contains no visible collision surface." @@ -779,13 +1037,13 @@ def visualize_curobo_world_collision_model( env: Any | None = None, material: Any | None = None, ) -> list[Any]: - """Add the cuRobo ESDF collision surface to the DexSim scene. + """Add a sampled cuRobo collision-world overlay to the DexSim scene. The rigid objects are already present in the live DexSim scene, so this - function only adds an overlay for the voxel collision data consumed by - cuRobo. Voxels within half a voxel of the zero level set are rendered as - half-voxel-radius spheres. All spheres are merged into one Open3D mesh, - written temporarily under ``/tmp``, and imported as one DexSim actor. + function only adds an overlay for the collision data consumed by cuRobo. + Voxel zero-level samples, analytic primitives, and mesh vertices are rendered + as spheres. All samples are merged into one Open3D mesh, written temporarily + under ``/tmp``, and imported as one DexSim actor. Args: rigid_objects: Live simulator obstacles represented by the cache. @@ -830,11 +1088,11 @@ def visualize_curobo_collision_models( """Show robot and world collision models in DexSim until Enter is pressed. Robot spheres are loaded from the generated cuRobo YAML and transformed by - the current simulator link poses. Obstacle spheres show the zero level set - of the ESDF voxel data passed to cuRobo. Robot and obstacle spheres are each - merged into one temporary DexSim actor so they can use blue and red - materials respectively. Both actors are removed before the function returns, - including when ``input`` is interrupted. + the current simulator link poses. Obstacle samples show the mixed scene data + passed to cuRobo. Robot and obstacle samples are each merged into one + temporary DexSim actor so they can use blue and red materials respectively. + Both actors are removed before the function returns, including when ``input`` + is interrupted. """ import dexsim import yaml diff --git a/tests/sim/objects/test_collision_shapes.py b/tests/sim/objects/test_collision_shapes.py new file mode 100644 index 000000000..684bddcdd --- /dev/null +++ b/tests/sim/objects/test_collision_shapes.py @@ -0,0 +1,97 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# 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. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import pytest +from dexsim.engine import BoxGeometry +from dexsim.types import RigidBodyShape + +from embodichain.lab.sim.objects import RigidObject + + +class _FakePhysicalBody: + def __init__(self, geometries): + self.geometries = geometries + + def get_shape_count(self): + return len(self.geometries) + + def get_shape_geometry(self, shape_idx): + return self.geometries[shape_idx] + + def get_shape_name(self, shape_idx): + return f"collision_{shape_idx}" + + +class _UnavailableGeometryBody(_FakePhysicalBody): + def get_shape_geometry(self, shape_idx): + raise RuntimeError("SDF dispatch is unavailable") + + +class _FakePhysicalEntity: + def __init__(self, geometries): + self.physical_body = _FakePhysicalBody(geometries) + + def get_physical_body(self): + return self.physical_body + + +def _box_geometry(half_extents): + geometry = BoxGeometry() + geometry.half_extents = half_extents + return geometry + + +def test_get_collision_shapes_snapshots_physical_box_geometry(): + rigid_object = RigidObject.__new__(RigidObject) + rigid_object.uid = "fixture" + rigid_object._entities = [_FakePhysicalEntity([_box_geometry([0.1, 0.2, 0.3])])] + + shapes = rigid_object.get_collision_shapes() + + assert len(shapes) == 1 + assert shapes[0].name == "collision_0" + assert shapes[0].shape_type == RigidBodyShape.BOX + assert shapes[0].half_extents.tolist() == pytest.approx([0.1, 0.2, 0.3]) + + +def test_get_collision_shapes_rejects_batched_topology_mismatch(): + rigid_object = RigidObject.__new__(RigidObject) + rigid_object.uid = "fixture" + rigid_object._entities = [ + _FakePhysicalEntity([_box_geometry([0.1, 0.2, 0.3])]), + _FakePhysicalEntity( + [ + _box_geometry([0.1, 0.2, 0.3]), + _box_geometry([0.4, 0.5, 0.6]), + ] + ), + ] + + with pytest.raises(ValueError, match="different collision-shape topology"): + rigid_object.get_collision_shapes() + + +def test_get_collision_shapes_reports_unavailable_sdf_geometry(): + rigid_object = RigidObject.__new__(RigidObject) + rigid_object.uid = "sdf_object" + entity = _FakePhysicalEntity([]) + entity.physical_body = _UnavailableGeometryBody([object()]) + rigid_object._entities = [entity] + + with pytest.raises(RuntimeError, match="canonical collision mesh"): + rigid_object.get_collision_shapes() diff --git a/tests/sim/planners/test_curobo_planner.py b/tests/sim/planners/test_curobo_planner.py index 537ef8b8f..dfa335afc 100644 --- a/tests/sim/planners/test_curobo_planner.py +++ b/tests/sim/planners/test_curobo_planner.py @@ -17,7 +17,7 @@ """Unit and smoke tests for the optional cuRobo planner. Most tests are dependency-free and cover planner configuration, conversion, -validation, generated robot YAML, and voxel-world data. The GPU-marked smoke tests +validation, generated robot YAML, and mixed collision-world data. The GPU-marked smoke tests exercise cached in-process planning and CPU-physics interoperability. Full collision-planning coverage remains in ``test_curobo_integration.py``. """ @@ -33,7 +33,9 @@ import pytest import torch import yaml +from dexsim.types import RigidBodyShape +from embodichain.lab.sim.objects import CollisionShapeDesc from embodichain.lab.sim.planners import CuroboPlannerCfg from embodichain.lab.sim.planners.curobo.curobo_planner import ( CuroboPlanOptions, @@ -51,6 +53,7 @@ from embodichain.lab.sim.planners.curobo.curobo_yaml import ( _convex_hulls_to_voxel_entry, _parse_mimic_joint_names, + _world_collision_sphere_data, generate_curobo_robot_yaml, generate_curobo_world_scene, visualize_curobo_collision_models, @@ -219,11 +222,13 @@ def test_configure_curobo_logging_rejects_unknown_level(): _configure_curobo_logging("silent") -def test_curobo_world_cfg_has_single_voxel_collision_path(): +def test_curobo_world_cfg_defaults_to_auto_collision_policy(): cfg = CuroboWorldCfg() + assert cfg.representation == "auto" + assert cfg.overrides == {} assert cfg.voxel_size == pytest.approx(0.01) - assert cfg.voxel_padding == pytest.approx(0.1) + assert cfg.voxel_padding == pytest.approx(0.005) def test_auto_gen_defaults_keep_sphere_count_low_and_fit_type_fixed(): @@ -604,7 +609,7 @@ def _identity_pose( class _FakeRigidObject: - """Expose the mesh and pose API required by the world generator.""" + """Expose the physical-shape and pose API required by the world generator.""" def __init__( self, @@ -612,11 +617,21 @@ def __init__( vertices: torch.Tensor, faces: torch.Tensor, pose: torch.Tensor, + collision_shapes: list[CollisionShapeDesc] | None = None, ) -> None: self.uid = uid self._vertices = vertices self._faces = faces self._pose = pose + self._collision_shapes = collision_shapes or [ + CollisionShapeDesc( + name="shape_0", + shape_type=RigidBodyShape.MESH, + local_pose=torch.eye(4), + vertices=vertices, + triangles=faces, + ) + ] def get_vertices(self, env_ids=None, scale=False): # noqa: ARG002 return self._vertices.unsqueeze(0) @@ -624,9 +639,16 @@ def get_vertices(self, env_ids=None, scale=False): # noqa: ARG002 def get_triangles(self, env_ids=None): # noqa: ARG002 return self._faces.unsqueeze(0) - def get_local_pose(self, to_matrix=False): # noqa: ARG002 + def get_local_pose(self, to_matrix=False): + if to_matrix: + pose = torch.eye(4, dtype=torch.float32) + pose[:3, 3] = self._pose[:3] + return pose.unsqueeze(0) return self._pose.unsqueeze(0) + def get_collision_shapes(self, env_id=0): # noqa: ARG002 + return self._collision_shapes + def _mock_visacd_as_identity(monkeypatch, calls=None): import dexsim.kit.meshproc as meshproc @@ -844,6 +866,146 @@ def get_link_pose( assert "Showing 2 cuRobo collision spheres" in prompts[0] +def test_auto_world_scene_preserves_physical_box_as_cuboid(): + box = CollisionShapeDesc( + name="physics_box", + shape_type=RigidBodyShape.BOX, + local_pose=torch.eye(4), + half_extents=torch.tensor([0.1, 0.2, 0.3]), + ) + rigid_object = _FakeRigidObject( + "fixture", + _unit_cube_vertices(), + _cube_faces(), + _identity_pose((1.0, 2.0, 3.0)), + [box], + ) + + scene_data = generate_curobo_world_scene([rigid_object]) + + assert list(scene_data) == ["cuboid"] + assert scene_data["cuboid"]["fixture"]["dims"] == pytest.approx([0.2, 0.4, 0.6]) + assert scene_data["cuboid"]["fixture"]["pose"][:3] == pytest.approx([1.0, 2.0, 3.0]) + + +def test_mixed_collision_visualization_supports_cuboid(): + centers, radii = _world_collision_sphere_data( + { + "cuboid": { + "fixture": { + "pose": [1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0], + "dims": [0.2, 0.4, 0.6], + } + } + } + ) + + assert centers.shape == (8, 3) + assert radii.shape == (8,) + + +def test_world_scene_object_override_can_force_voxel(monkeypatch): + _mock_visacd_as_identity(monkeypatch) + box = CollisionShapeDesc( + name="physics_box", + shape_type=RigidBodyShape.BOX, + local_pose=torch.eye(4), + half_extents=torch.tensor([0.5, 0.5, 0.5]), + ) + rigid_object = _FakeRigidObject( + "room_scan", + _unit_cube_vertices(), + _cube_faces(), + _identity_pose(), + [box], + ) + + scene_data = generate_curobo_world_scene( + [rigid_object], + overrides={"room_scan": "voxel"}, + voxel_size=0.5, + voxel_padding=0.0, + ) + + assert list(scene_data) == ["voxel"] + assert set(scene_data["voxel"]) == {"room_scan"} + + +def test_auto_world_scene_preserves_compound_shape_names_and_local_poses(): + box_pose = torch.eye(4) + box_pose[0, 3] = 0.25 + shapes = [ + CollisionShapeDesc( + name="box", + shape_type=RigidBodyShape.BOX, + local_pose=box_pose, + half_extents=torch.tensor([0.1, 0.1, 0.1]), + ), + CollisionShapeDesc( + name="sphere", + shape_type=RigidBodyShape.SPHERE, + local_pose=torch.eye(4), + radius=0.15, + ), + ] + rigid_object = _FakeRigidObject( + "compound", + _unit_cube_vertices(), + _cube_faces(), + _identity_pose((1.0, 0.0, 0.0)), + shapes, + ) + + scene_data = generate_curobo_world_scene([rigid_object]) + + assert set(scene_data) == {"cuboid", "sphere"} + assert set(scene_data["cuboid"]) == {"compound__shape_0"} + assert set(scene_data["sphere"]) == {"compound__shape_1"} + assert scene_data["cuboid"]["compound__shape_0"]["pose"][:3] == pytest.approx( + [1.25, 0.0, 0.0] + ) + + +def test_dynamic_compound_object_fans_out_to_shape_local_poses(): + first_pose = torch.eye(4) + first_pose[0, 3] = 0.25 + shapes = [ + CollisionShapeDesc( + name="first", + shape_type=RigidBodyShape.BOX, + local_pose=first_pose, + half_extents=torch.ones(3), + ), + CollisionShapeDesc( + name="second", + shape_type=RigidBodyShape.SPHERE, + local_pose=torch.eye(4), + radius=0.1, + ), + ] + rigid_object = _FakeRigidObject( + "compound", + _unit_cube_vertices(), + _cube_faces(), + _identity_pose(), + shapes, + ) + planner = CuroboPlanner.__new__(CuroboPlanner) + planner.cfg = SimpleNamespace( + world=CuroboWorldCfg( + rigid_objects=[rigid_object], dynamic_obstacle_names=["compound"] + ) + ) + + obstacle_shapes = planner._dynamic_obstacle_shapes("compound") + + assert [name for name, _ in obstacle_shapes] == [ + "compound__shape_0", + "compound__shape_1", + ] + assert obstacle_shapes[0][1][:3, 3].tolist() == pytest.approx([0.25, 0.0, 0.0]) + + def test_generate_world_scene_supports_multiple_objects(monkeypatch): _mock_visacd_as_identity(monkeypatch) rigid_objects = [ @@ -861,7 +1023,10 @@ def test_generate_world_scene_supports_multiple_objects(monkeypatch): ), ] scene_data = generate_curobo_world_scene( - rigid_objects, voxel_size=0.5, voxel_padding=0.0 + rigid_objects, + representation="voxel", + voxel_size=0.5, + voxel_padding=0.0, ) assert list(scene_data) == ["voxel"] @@ -907,7 +1072,10 @@ def test_generated_voxel_data_loads_in_curobo_scene_cfg(monkeypatch): _identity_pose(), ) scene_data = generate_curobo_world_scene( - [rigid_object], voxel_size=0.5, voxel_padding=0.0 + [rigid_object], + representation="voxel", + voxel_size=0.5, + voxel_padding=0.0, ) scene = SceneCfg.create(scene_data) @@ -918,6 +1086,24 @@ def test_generated_voxel_data_loads_in_curobo_scene_cfg(monkeypatch): assert tuple(scene.voxel[0].feature_tensor.shape) == (2, 2, 2) +def test_generated_physical_mesh_loads_in_curobo_scene_cfg(): + pytest.importorskip("curobo") + from curobo._src.geom.types import SceneCfg + + rigid_object = _FakeRigidObject( + "collision_mesh", + _unit_cube_vertices(), + _cube_faces(), + _identity_pose(), + ) + + scene = SceneCfg.create(generate_curobo_world_scene([rigid_object])) + + assert len(scene.mesh) == 1 + assert scene.mesh[0].name == "collision_mesh" + assert len(scene.mesh[0].vertices) == _unit_cube_vertices().shape[0] + + # Simulator smoke coverage