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 3af7583d..9aaa00c6 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 915f7f09..ed897815 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 @@ -134,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`) 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). +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` (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`. ### Shared and per-environment collision worlds @@ -172,14 +212,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 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: -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: @@ -187,7 +226,6 @@ For example: ```python world_cfg = CuroboWorldCfg( rigid_objects=[block], - obstacle_representation="cuboid", dynamic_obstacle_names=["block"], multi_env=True, ) @@ -225,11 +263,18 @@ 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 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 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. ## Generate a motion @@ -309,9 +354,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 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. Headless runs automatically record this fixed offscreen camera view to an MP4. Set an explicit diff --git a/embodichain/lab/sim/objects/__init__.py b/embodichain/lab/sim/objects/__init__.py index 52c24fef..97b6aa72 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 5a4bd80a..e99e14ec 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/base_planner.py b/embodichain/lab/sim/planners/base_planner.py index c6f0eeca..291c47c9 100644 --- a/embodichain/lab/sim/planners/base_planner.py +++ b/embodichain/lab/sim/planners/base_planner.py @@ -190,6 +190,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 f9cf5fce..667a6862 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 @@ -75,12 +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: -# 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" - # 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. @@ -140,41 +133,50 @@ 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 :class:`RigidObject` obstacles to bake into the auto-generated world YAML. - - 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, - 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. """ - obstacle_representation: str = "sphere" - """Collision representation used when generating the YAML from :attr:`rigid_objects`. + representation: str = "auto" + """Collision representation policy: ``"auto"`` or forced ``"voxel"``.""" + + overrides: dict[str, str] = {} + """Per-object representation overrides keyed by :class:`RigidObject` UID. - ``"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, - placed as an OBB via the object pose. ``"mesh"`` emits the full triangle - mesh (exact, no CUDA). + Supported values are ``"auto"``, ``"voxel"``, ``"mesh"``, ``"cuboid"``, + ``"sphere"``, and ``"capsule"``. A forced analytic representation must + match the source physical shape. """ - collision_cache: dict[str, int | dict[str, int | float | list[float]]] = { - "cuboid": 8, - "mesh": 2, - } - """Per-geometry cache capacity created before world updates. + 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.""" - 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. + voxel_size: float = 0.01 + """ESDF voxel edge length in meters for every world collision object.""" + + 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 + 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] = [] @@ -202,8 +204,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 @@ -232,7 +233,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, @@ -241,10 +242,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,23 +250,23 @@ 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).""" 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 @@ -279,10 +276,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" @@ -489,6 +486,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 # ============================================================================= @@ -649,6 +678,7 @@ def _require_curobo(log_level: str = "error") -> "Any": try: 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( @@ -667,6 +697,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, ) @@ -732,7 +763,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 @@ -776,22 +808,19 @@ 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 - if world_cfg.obstacle_representation not in ("cuboid", "mesh", "sphere"): + _validate_world_cfg(world_cfg) + 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: @@ -850,7 +879,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. @@ -1022,63 +1051,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, @@ -1095,18 +1078,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) @@ -1156,7 +1135,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, ) @@ -1199,17 +1177,17 @@ 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) with torch.cuda.device(self._curobo_device): planner_cfg = self._bindings.MotionPlannerCfg.create( - robot=profile.robot_config_path, + robot=robot_config, scene_model=scene_model, - collision_cache=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), @@ -1218,6 +1196,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( @@ -1249,6 +1228,58 @@ def _build_backend( planning_mode=planning_mode, ) + @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. @@ -1543,7 +1574,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, @@ -1561,7 +1591,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: @@ -1570,7 +1599,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")) @@ -1578,20 +1606,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 mixed cuRobo collision 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 @@ -1600,57 +1622,102 @@ 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, - fit_type=auto.fit_type, - 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 collision world cache hit: {cache_path}") + scene_data = torch.load(cache_path, map_location="cpu", weights_only=True) + else: + logger.log_info( + 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) - 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 physical collision geometry, initial poses, and policy settings.""" 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(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(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() + def visualize_robot_collision_models( + self, + control_part: str, + env_id: int = 0, + ) -> None: + """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 + samples retain the mixed physical-shape scene consumed by cuRobo. + + Args: + control_part: Robot control part whose cuRobo profile/cache is used. + env_id: Simulator environment instance to visualize. + + """ + 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_scene = None + if rigid_objects: + world_scene = self._auto_generate_world_scene(world_cfg) + visualize_curobo_collision_models( + self.robot, + profile.robot_config_path, + rigid_objects, + world_scene, + 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) @@ -2248,23 +2315,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 1b24eec6..a2ece5f9 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_yaml.py +++ b/embodichain/lab/sim/planners/curobo/curobo_yaml.py @@ -17,28 +17,40 @@ 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`. -: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 mixed cuRobo collision data from live +:class:`~embodichain.lab.sim.objects.RigidObject` physical shapes. """ from __future__ import annotations -from typing import TYPE_CHECKING, Sequence +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 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_scene", + "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 +91,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 an Open3D tensor triangle mesh from tensor-like geometry.""" + 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 +126,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 +138,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 +162,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 +177,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 +198,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 +226,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 +257,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 +283,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 +299,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 +312,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, @@ -359,72 +326,46 @@ def _two_hop_neighbors(link: str) -> set[str]: # ============================================================================= -# World (obstacle) YAML generation from RigidObject meshes +# World collision generation from RigidObject physical shapes # ============================================================================= -_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", - fit_type: str = "voxel", - 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 / cuRobo import for ``cuboid``/``mesh``) so - it is unit-testable without CUDA. ``sphere`` lazily imports cuRobo + trimesh - and runs on CUDA. + voxel_size: float = 0.01, + voxel_padding: float = 0.005, +) -> tuple[str, dict[str, object]]: + """Decompose one mesh with VisACD and convert its union to an ESDF grid. - 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 cuRobo's :func:`fit_spheres_to_mesh`). - fit_type: cuRobo sphere-fit strategy (``"voxel"``/``"morphit"``/ - ``"surface"``); only used by ``"sphere"``. - 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). - collision_sphere_buffer: Padding added to each fitted radius (sphere only). - device: CUDA device for sphere fitting (sphere only). - - 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 cuRobo/trimesh. + 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] @@ -435,212 +376,796 @@ 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(), - }, - ) - ] + import open3d as o3d - 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." + from dexsim.kit.meshproc import convex_decomposition_visacd + + mesh = _to_open3d_tensor_mesh(vertices, faces, o3d) + is_success, convex_hulls = convex_decomposition_visacd( + mesh, + max_convex_hull_num=_OBSTACLE_MAX_CONVEX_HULL_NUM, + is_visual=False, + ) + if not is_success or not convex_hulls: + raise RuntimeError(f"VisACD decomposition failed for object {name!r}.") + + local_half_extent = torch.maximum( + vertices.amin(dim=0).abs(), vertices.amax(dim=0).abs() + ) + 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 _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 ) - if not torch.cuda.is_available(): - raise RuntimeError( - "The 'sphere' representation requires CUDA for cuRobo sphere fitting." + 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), + ) - import trimesh - 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 +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()) + - fit_type_map = { - "morphit": SphereFitType.MORPHIT, - "voxel": SphereFitType.VOXEL, - "surface": SphereFitType.SURFACE, +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 fit_type not in fit_type_map: + if shape.shape_type in native: + return native[shape.shape_type] + if shape.shape_type != RigidBodyShape.MESH: raise ValueError( - f"fit_type must be one of {list(fit_type_map)}, got {fit_type!r}." + f"No automatic cuRobo representation for DexSim shape " + f"{shape.shape_type.name}." ) - 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, - num_spheres=num_spheres, - sphere_density=sphere_density, - surface_radius=surface_radius, - fit_type=fit_type_map[fit_type], - iterations=iterations, - device_cfg=DeviceCfg(device=device), - ) - if fit_result.num_spheres == 0: - 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( - collision_sphere_buffer + 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}." ) - 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 "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}." ) - return entries -def generate_curobo_world_yaml( +def generate_curobo_world_scene( rigid_objects: Sequence[RigidObject], - output_path: str, *, - 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, - 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. - - .. 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`. + 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 mixed cuRobo scene from DexSim physical collision shapes. + + ``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: ``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). - 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). - collision_sphere_buffer: Padding added to each fitted radius (sphere only). - device: CUDA device for sphere fitting (sphere only). + 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 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: - The ``output_path`` that was written. + A mixed tensor-backed scene mapping accepted by cuRobo ``Scene.create``. Raises: - ValueError: If ``rigid_objects`` is empty or a representation/pose is - invalid. + ValueError: If configuration or collision geometry is unsupported. + 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() - for idx, obj in enumerate(rigid_objects): - name = getattr(obj, "uid", None) or f"obstacle_{idx}" - if name in used_names: + 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." + ) + 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 + ) + .detach() + .cpu() + ) + for shape_idx, shape in enumerate(shapes): + obstacle_name = ( + object_name if len(shapes) == 1 else f"{object_name}__shape_{shape_idx}" ) - used_names.add(name) + 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}." + ) + + if not scene: + raise ValueError( + "No collision obstacles could be generated from the given RigidObjects." + ) + if "voxel" in scene: + scene["voxel"] = dict( + sorted( + scene["voxel"].items(), + key=lambda item: int(item[1]["feature_tensor"].numel()), + reverse=True, + ) + ) + return scene + + +# ============================================================================= +# 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. - 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] + 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: - logger.log_warning( - f"RigidObject {name!r} has no mesh geometry; skipping collision export." - ) 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 + - entries = _mesh_to_obstacle_entry( - name, - vertices, - faces, - pose, - representation=representation, - fit_type=fit_type, - num_spheres=num_spheres, - sphere_density=sphere_density, - surface_radius=surface_radius, - iterations=iterations, - collision_sphere_buffer=collision_sphere_buffer, - device=device, +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 + + +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]}." ) - for top_key, obstacle_name, fields in entries: - data.setdefault(top_key, {})[obstacle_name] = fields + 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 - if not data: + 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 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 []) + ] + 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) + ) + 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) + 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).detach().cpu() + rotation = matrix_from_quat(pose[3:7]) + 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)) + + 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( - "No collision obstacles could be generated from the given RigidObjects." + "The cuRobo world scene contains no visible collision surface." ) + return torch.cat(centers), torch.cat(radii) - 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 + +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 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 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. + 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( + robot: Robot, + robot_yaml_path: str, + rigid_objects: Sequence[RigidObject] | None = None, + world_scene: Any | None = None, + env_id: int = 0, +) -> None: + """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 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 + + 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], + ) + + 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..." + ) + 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 b3105f24..969affe6 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 @@ -458,11 +459,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 @@ -711,6 +724,9 @@ def main() -> None: seed=args.seed, ) use_independent_worlds = args.num_envs > 1 + 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])) @@ -734,7 +750,6 @@ def main() -> None: robot_uid=robot.uid, world=CuroboWorldCfg( rigid_objects=obstacles, - obstacle_representation="cuboid", dynamic_obstacle_names=( [obstacle.uid for obstacle in obstacles] if use_independent_worlds @@ -748,6 +763,11 @@ def main() -> None: ) ) ) + 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_robot_collision_models(control_part) engine = AtomicActionEngine(motion_generator) binding = ActionBinding(manipulators={"primary": control_part}) motion_policy = MotionPolicy( diff --git a/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md b/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md index 66463bb2..5e5d5835 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 2b4e3471..cb5b47a7 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 ebcd8faa..dd5d8386 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 eafbbc5a..aae61995 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/objects/test_collision_shapes.py b/tests/sim/objects/test_collision_shapes.py new file mode 100644 index 00000000..684bddcd --- /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_base_planner.py b/tests/sim/planners/test_base_planner.py new file mode 100644 index 00000000..9b68491e --- /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 + ) diff --git a/tests/sim/planners/test_curobo_integration.py b/tests/sim/planners/test_curobo_integration.py index 5c941900..1efa6920 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 f62563d2..9046214b 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 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``. """ @@ -26,18 +26,23 @@ import importlib import logging -import math +from contextlib import nullcontext +from pathlib import Path +from types import SimpleNamespace 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, CuroboPlanner, CuroboPlannerCfg as CuroboPlannerCfgDirect, CuroboWorldCfg, + _CuroboProfile, _configure_curobo_logging, _matrix_to_position_quaternion, _require_curobo, @@ -46,10 +51,16 @@ _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_world_yaml, + _world_collision_sphere_data, + generate_curobo_robot_yaml, + generate_curobo_world_scene, + visualize_curobo_collision_models, + 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" @@ -211,36 +222,20 @@ 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_defaults_to_auto_collision_policy(): cfg = CuroboWorldCfg() - assert cfg.collision_cache == {"cuboid": 8, "mesh": 2} - assert cfg.obstacle_representation == "sphere" + assert cfg.representation == "auto" + assert cfg.overrides == {} + assert cfg.voxel_size == pytest.approx(0.01) + assert cfg.voxel_padding == pytest.approx(0.005) -def test_curobo_collision_world_binding_merges_owned_obstacle_poses(): - planner = object.__new__(CuroboPlanner) - configured_pose = torch.eye(4).unsqueeze(0) - observed_pose = torch.eye(4).unsqueeze(0) - observed_pose[:, 0, 3] = 0.5 - options = CuroboPlanOptions(dynamic_obstacle_poses={"configured": configured_pose}) - - bound = planner.with_collision_world( - options, - obstacle_poses={"observed": observed_pose}, - ) - - assert bound is options - assert set(bound.dynamic_obstacle_poses) == {"configured", "observed"} - assert torch.equal(bound.dynamic_obstacle_poses["observed"], observed_pose) - assert bound.dynamic_obstacle_poses["observed"] is not observed_pose - - -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_curobo_planner_class_is_lazy_import_safe(): @@ -252,6 +247,149 @@ 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, + 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) @@ -316,6 +454,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 @@ -368,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, @@ -376,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) @@ -388,152 +639,375 @@ 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 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", - ) +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 test_generate_cuboid_world_yaml_assembles_schema(tmp_path): + def set_material(self, material): + self.material = material + + +class _FakeDexsimEnv: + def __init__(self): + self.materials = {} + self.actors = [] + self.loaded_paths = [] + self.removed_actors = [] + + 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( - "demo_block", + "block", _unit_cube_vertices(), _cube_faces(), _identity_pose() + ) + env = _FakeDexsimEnv() + + features = torch.ones((3, 3, 3), dtype=torch.float16) + features[1, 1, 1] = 0.0 + 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, + } + } + } + actors = visualize_curobo_world_collision_model( + [rigid_object], world_scene, env=env + ) + + 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_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(), + _identity_pose((1.0, 2.0, 3.0)), + [box], ) - output_path = tmp_path / "world.yml" - result = generate_curobo_world_yaml( - [rigid_object], - str(output_path), - representation="cuboid", + 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], + } + } + } ) - 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]) + assert centers.shape == (8, 3) + assert radii.shape == (8,) -def test_generate_mesh_world_yaml_assembles_schema(tmp_path): +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( - "demo_block", + "room_scan", _unit_cube_vertices(), _cube_faces(), _identity_pose(), + [box], ) - output_path = tmp_path / "world_mesh.yml" - generate_curobo_world_yaml( + scene_data = generate_curobo_world_scene( [rigid_object], - str(output_path), - representation="mesh", + 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, ) - data = yaml.safe_load(output_path.read_text(encoding="utf-8")) + 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 list(data) == ["mesh"] - assert len(data["mesh"]["demo_block"]["vertices"]) == 8 + 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_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", @@ -548,25 +1022,25 @@ 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( + scene_data = generate_curobo_world_scene( rigid_objects, - str(output_path), - representation="cuboid", + representation="voxel", + 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", @@ -582,58 +1056,52 @@ 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 + _mock_visacd_as_identity(monkeypatch) + rigid_object = _FakeRigidObject( "demo_block", _unit_cube_vertices(), _cube_faces(), _identity_pose(), ) - output_path = tmp_path / "world.yml" - generate_curobo_world_yaml( + scene_data = generate_curobo_world_scene( [rigid_object], - str(output_path), - representation="cuboid", + representation="voxel", + 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.cuboid) == 1 - assert scene.cuboid[0].name == "demo_block" - assert scene.cuboid[0].dims == pytest.approx([1.0, 1.0, 1.0]) + 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) -def test_generated_mesh_yaml_loads_in_curobo_scene_cfg(tmp_path): +def test_generated_physical_mesh_loads_in_curobo_scene_cfg(): pytest.importorskip("curobo") from curobo._src.geom.types import SceneCfg rigid_object = _FakeRigidObject( - "demo_block", + "collision_mesh", _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", - ) - scene = SceneCfg.create(yaml.safe_load(output_path.read_text(encoding="utf-8"))) + scene = SceneCfg.create(generate_curobo_world_scene([rigid_object])) assert len(scene.mesh) == 1 - assert scene.mesh[0].name == "demo_block" - assert len(scene.mesh[0].vertices) == 8 + assert scene.mesh[0].name == "collision_mesh" + assert len(scene.mesh[0].vertices) == _unit_cube_vertices().shape[0] # Simulator smoke coverage