diff --git a/agent_context/topics/motion-planning/collision-worlds.md b/agent_context/topics/motion-planning/collision-worlds.md index 74b57a62a..8efd22e03 100644 --- a/agent_context/topics/motion-planning/collision-worlds.md +++ b/agent_context/topics/motion-planning/collision-worlds.md @@ -7,16 +7,28 @@ Read this when the request needs these details. [Topic overview](motion-planning `CuroboWorldCfg.rigid_objects` accepts either a mapping or a sequence. Use `Mapping[registry_id, RigidObject]` for a registry-backed integration. The mapping key is the authoritative logical/source obstacle ID used by the -content-cache key, `collision_world_entity_ids`, and registry validation. For -`cuboid` and `mesh`, it is also the physical YAML obstacle name and dynamic -update key. For `sphere`, one static source expands to physical YAML names such -as `registry_id_0`; dynamic sphere configuration is rejected, while cache and -full-world identity remain keyed by `registry_id`. A registry mapping whose -source lacks mesh geometry required by the selected representation fails fast -instead of silently dropping the source. The sequence form is an advanced -direct-core path that derives names from each object's `uid` or an +content-cache key, `collision_world_entity_ids`, generated obstacle-name prefix, +dynamic update key, and registry validation. Scene generation reads physical +collision descriptors through `RigidObject.get_collision_shapes()` and emits a +mixed tensor-backed cuRobo scene. A compound source expands to physical names +such as `registry_id__shape_0`; cache and full-world identity remain keyed by +the unexpanded `registry_id`, and dynamic updates fan out through the physical +shapes' local poses. A registry mapping whose source lacks physical collision +shapes fails fast instead of silently dropping the source. The sequence form is +an advanced direct-core path that derives names from each object's `uid` or an `obstacle_` fallback. +Analytic box, plane, sphere, and capsule shapes retain their native cuRobo +representations. Mesh-backed `MESH`, `CONVEX`, and `SDF` shapes never become +direct cuRobo `Mesh` entries: scene generation computes one Open3D convex hull, +then samples its signed distance into a dense ESDF `VoxelGrid`. The global or +per-object `"voxel"` policy can apply the same conversion to an analytic shape; +the direct `"mesh"` policy is unsupported. `max_voxel_count` guards every ESDF +allocation and fails fast with an actionable error. `mesh_triangle_threshold` +remains accepted only for configuration compatibility and no longer changes +representation selection. The world cache format is versioned so caches that +may contain direct mesh entries are not reused. + `CuroboWorldCfg.multi_env` controls collision-world batching, not whether robot states or goals are batched: @@ -28,10 +40,13 @@ states or goals are batched: different poses relative to their local robot bases, such as per-env pose randomization. -The multi-env scene is cloned from the YAML generated using env 0; enabling the -flag does not load distinct initial simulator poses for other rows. Per-env -differences require `"cuboid"` or `"mesh"` representation, registration in -`dynamic_obstacle_names`, and current `(B, 4, 4)` world poses in +The multi-env scene is cloned from the cached tensor-backed scene dictionary +generated using env 0; enabling the flag does not load distinct initial +simulator poses for other rows. Each clone remains a dictionary until cuRobo +0.8 constructs its own `SceneCfg`, because its multi-env list parser expects a +dictionary per environment rather than pre-built `SceneCfg` instances. Per-env +differences require registration in `dynamic_obstacle_names` and current +`(B, 4, 4)` world poses in `CuroboPlanOptions.dynamic_obstacle_poses`. Independent worlds replicate scene data and collision caches, so retain the shared default for identical rebased layouts. @@ -53,8 +68,9 @@ contract. It requires unique canonical IDs and requires the dynamic subset to belong to the complete world. `MotionGenerator.collision_world_info` forwards that contract and retains derived ID/mode properties for callers. For cuRobo, the complete set is every mapping key (or inferred sequence name), while the -dynamic set is exactly `CuroboWorldCfg.dynamic_obstacle_names`. Sphere-expanded -physical YAML names are not part of either logical ID declaration. +dynamic set is exactly `CuroboWorldCfg.dynamic_obstacle_names`. +Compound-expanded physical shape names are not part of either logical ID +declaration. `CuroboWorldCfg` rejects duplicate obstacle names and requires every `dynamic_obstacle_name` to match an object registered in `rigid_objects`, so a planner-local mismatch fails before backend construction. 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 fc0a040fd..e0698a628 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 @@ -69,6 +70,9 @@ Rigid Object :inherited-members: :show-inheritance: +.. autoclass:: CollisionShapeDesc + :members: + .. autoclass:: RigidBodyData :members: :inherited-members: diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 253938d7a..f0c35aa99 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -882,6 +882,7 @@ embodichain.lab.sim.objects.rigid_object .. autosummary:: + CollisionShapeDesc RigidBodyData RigidObject RigidObjectCfg @@ -952,7 +953,10 @@ embodichain.lab.sim.motion.planners.curobo.curobo_yaml .. autosummary:: generate_curobo_robot_yaml - generate_curobo_world_yaml + generate_curobo_world_scene + visualize_curobo_collision_models + visualize_curobo_robot_collision_model + visualize_curobo_world_collision_model embodichain.lab.sim.motion.planners.neural_planner -------------------------------------------------- diff --git a/docs/source/overview/sim/motion/planners/curobo_planner.md b/docs/source/overview/sim/motion/planners/curobo_planner.md index eefd3066a..566d3a415 100644 --- a/docs/source/overview/sim/motion/planners/curobo_planner.md +++ b/docs/source/overview/sim/motion/planners/curobo_planner.md @@ -85,7 +85,6 @@ planner_cfg = CuroboPlannerCfg( planner_type="curobo", world=CuroboWorldCfg( rigid_objects=registry.collision_geometry_by_id(), - obstacle_representation="cuboid", dynamic_obstacle_names=list(registry.dynamic_collision_entity_ids), multi_env=collision_mode is SceneCollisionWorldMode.PER_ENV, ), @@ -111,9 +110,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 @@ -156,42 +161,77 @@ 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 canonical, registry-backed form is a mapping -from authoritative registry ID to live object; 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. The canonical, registry-backed +form of `CuroboWorldCfg.rigid_objects` is a mapping from authoritative registry +ID to live object. The mapping key, rather than `RigidObject.uid`, becomes the +logical collision ID, cache identity, generated obstacle-name prefix, and +runtime-update key. The sequence form remains available for advanced callers +and derives IDs from each `uid` or an `obstacle_` fallback. + +`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": room_scan, + "precision_fixture": 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 +canonical ID 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. The mapping key, rather -than `RigidObject.uid`, is the canonical logical/source ID used by cache -identity and collision-world validation. For `"cuboid"` and `"mesh"`, that ID -is also used unchanged as the physical YAML obstacle name and runtime update -key. For obstacles that move or live in an offset base frame, also declare their -canonical IDs in +when the robot base sits at the simulator world origin. For obstacles that move +or live in an offset base frame, declare their canonical IDs 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 physical YAML obstacles named `_0`, `_1`, and -so on; dynamic sphere configuration is rejected. These derived names are -backend details. The cache and registry/planner full-world contract continue to -use the unexpanded canonical source ID. - -Registry-backed mappings fail fast if a selected source has no mesh geometry -required by the chosen representation. This prevents a canonical collision ID -from being silently skipped during YAML generation. The advanced sequence form -retains its lower-level behavior independently of this registry contract. - -`CuroboPlanner.collision_world_entity_ids` reports every configured logical +`CuroboPlanOptions.dynamic_obstacle_poses`. + +Registry-backed mappings fail fast if a selected source has no physical +collision shapes. This prevents a canonical collision ID from being silently +omitted from the generated tensor-backed scene. The advanced sequence form may +skip such an object with a warning, independently of the registry contract. + +`CuroboPlanner.collision_world_info.entity_ids` reports every configured logical source ID: each mapping key on the registry path, or each inferred name on the -advanced sequence path. It deliberately does not expose sphere-expanded -physical YAML names. `dynamic_collision_entity_ids` reports exactly the -configured dynamic subset. Static entries therefore participate in -construction-time identity validation even though they do not receive per-plan -pose updates. +advanced sequence path. It deliberately does not expose compound-expanded +physical shape names. `collision_world_info.dynamic_entity_ids` reports exactly +the configured dynamic subset. `MotionGenerator` forwards these through its +`collision_world_entity_ids` and `dynamic_collision_entity_ids` properties. +Static entries therefore participate in construction-time identity validation +even though they do not receive per-plan pose updates. `CuroboWorldCfg` validates this planner-local registration at construction: obstacle IDs must be unique, and every dynamic obstacle ID must match an entry @@ -230,14 +270,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 -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 +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. 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 a registry-backed world, derive both the geometry mapping and dynamic ID @@ -246,7 +285,6 @@ list from the same catalog: ```python world_cfg = CuroboWorldCfg( rigid_objects=registry.collision_geometry_by_id(), - obstacle_representation="cuboid", dynamic_obstacle_names=list(registry.dynamic_collision_entity_ids), multi_env=True, ) @@ -293,11 +331,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 @@ -374,9 +419,9 @@ python examples/sim/motion/planners/curobo_planner.py --headless --sim-device cp ~~~ 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/docs/source/overview/task_program/scene_registry.md b/docs/source/overview/task_program/scene_registry.md index 86b8e535c..6d18f3d94 100644 --- a/docs/source/overview/task_program/scene_registry.md +++ b/docs/source/overview/task_program/scene_registry.md @@ -292,7 +292,6 @@ registry = SceneRegistry.from_simulation( world = CuroboWorldCfg( rigid_objects=registry.collision_geometry_by_id(), - obstacle_representation="cuboid", dynamic_obstacle_names=list(registry.dynamic_collision_entity_ids), multi_env=True, ) diff --git a/embodichain/lab/sim/motion/planners/base_planner.py b/embodichain/lab/sim/motion/planners/base_planner.py index 5dabfc930..7a84b876f 100644 --- a/embodichain/lab/sim/motion/planners/base_planner.py +++ b/embodichain/lab/sim/motion/planners/base_planner.py @@ -256,6 +256,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/motion/planners/curobo/curobo_planner.py b/embodichain/lab/sim/motion/planners/curobo/curobo_planner.py index c9869d84b..061e8ca74 100644 --- a/embodichain/lab/sim/motion/planners/curobo/curobo_planner.py +++ b/embodichain/lab/sim/motion/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 @@ -77,12 +76,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. @@ -149,44 +142,54 @@ def __deepcopy__(self, memo: dict) -> "_RigidObjectRefMapping": # 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] | Mapping[str, RigidObject] | None = None - """Live :class:`RigidObject` obstacles to bake into the 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). A mapping is the registry-backed path: its keys are - authoritative obstacle IDs even when they differ from ``RigidObject.uid``. - The list form remains available for advanced callers and derives names from - ``uid`` (or ``obstacle_`` when absent). 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 canonical 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. + A mapping is the registry-backed form: each mapping key is the authoritative + obstacle ID even when it differs from :attr:`RigidObject.uid`. The list form + derives IDs from ``uid`` (or ``obstacle_`` when absent). Poses are + expressed in the cuRobo world/base frame. For obstacles that move or live in + an offset base frame, also list their canonical IDs 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 canonical obstacle ID. - ``"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"``, ``"cuboid"``, ``"sphere"``, + and ``"capsule"``. Mesh-backed collision shapes always become convex-hull + ESDF voxels; direct cuRobo ``Mesh`` obstacles are not supported. 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 + """Deprecated compatibility setting; mesh-backed shapes always use ESDF.""" + + max_voxel_count: int = 2_000_000 + """Maximum estimated voxel count allowed for one ESDF obstacle.""" + + plane_dims: tuple[float, float, float] = (10.0, 10.0, 0.01) + """Workspace-bounded dimensions used to represent an infinite plane.""" + + voxel_size: float = 0.01 + """ESDF voxel edge length in meters for every world collision object.""" - 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_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] = [] @@ -214,8 +217,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 @@ -299,7 +301,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, @@ -308,10 +310,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`.""" @@ -320,23 +318,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 @@ -346,10 +344,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" @@ -556,6 +554,36 @@ def _validate_dynamic_obstacles( ) +_WORLD_REPRESENTATIONS = frozenset({"auto", "voxel", "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 # ============================================================================= @@ -717,6 +745,7 @@ def _require_curobo(log_level: str = "error") -> "Any": planner_mod = importlib.import_module("curobo.motion_planner") batch_mod = importlib.import_module("curobo.batch_motion_planner") collision_mod = importlib.import_module("curobo.collision_checking") + scene_mod = importlib.import_module("curobo.scene") types_mod = importlib.import_module("curobo.types") except ModuleNotFoundError as exc: raise ImportError( @@ -737,6 +766,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, ) @@ -802,7 +832,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 @@ -958,22 +989,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: @@ -1032,7 +1060,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. @@ -1204,63 +1232,28 @@ 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 scene dictionary for each independent collision world. + + cuRobo 0.8 accepts one instantiated ``SceneCfg`` for a shared world, but + its multi-environment list branch calls ``SceneCfg.create`` on every + element. Keep the entries as dictionaries so both analytic and voxel + obstacles follow that supported construction path. """ 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: + if scene_model 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, + if not isinstance(scene_model, Mapping): + raise TypeError( + "multi-env cuRobo scene_model must be a scene dictionary, got " + f"{type(scene_model).__name__}." ) - 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") + return [deepcopy(scene_model) for _ in range(batch_size)] def _get_backend( self, @@ -1277,18 +1270,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) @@ -1338,7 +1327,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, ) @@ -1381,17 +1369,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), @@ -1400,6 +1388,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( @@ -1431,6 +1420,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. @@ -1725,7 +1766,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, @@ -1743,7 +1783,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: @@ -1752,7 +1791,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")) @@ -1760,20 +1798,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 @@ -1782,56 +1814,106 @@ 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 + ) + # MotionPlannerCfg.create() accepts this dictionary directly. Keeping + # the raw representation is required for multi_env, where cuRobo 0.8 + # applies SceneCfg.create() separately to every list element. + return 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")) + # V2 makes mesh-backed collision shapes convex-hull ESDF voxels and + # intentionally invalidates caches that may contain direct Mesh entries. + hasher.update(b"physical-shapes-v2") + 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 name, obj in _named_rigid_objects(world_cfg.rigid_objects): 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) @@ -2427,23 +2509,64 @@ 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 canonical object ID to 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 = dict(_named_rigid_objects(self.cfg.world.rigid_objects)).get( + object_name + ) + 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/motion/planners/curobo/curobo_yaml.py b/embodichain/lab/sim/motion/planners/curobo/curobo_yaml.py index 55029e6f8..cefe5466d 100644 --- a/embodichain/lab/sim/motion/planners/curobo/curobo_yaml.py +++ b/embodichain/lab/sim/motion/planners/curobo/curobo_yaml.py @@ -17,29 +17,42 @@ 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.motion.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. Analytic +primitives stay analytic, while mesh-backed shapes are reduced to a convex hull +before being voxelized into an ESDF grid. """ from __future__ import annotations from collections.abc import Mapping, Sequence -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, Any 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 def _named_rigid_objects( @@ -92,6 +105,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, @@ -99,7 +140,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, @@ -112,12 +152,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. @@ -136,14 +176,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. @@ -153,33 +191,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 = {} @@ -188,32 +212,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: @@ -228,35 +240,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(), ) ] @@ -266,42 +271,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 [] @@ -327,14 +297,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: @@ -343,7 +313,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": { @@ -356,8 +326,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, @@ -372,72 +340,51 @@ 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 _compute_convex_hull(mesh: Any) -> Any: + """Return the Open3D convex hull used as the voxelization surface.""" + return mesh.compute_convex_hull() -def _mesh_to_obstacle_entry( +def _convex_hull_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. - - 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. + voxel_size: float = 0.01, + voxel_padding: float = 0.005, +) -> tuple[str, dict[str, object]]: + """Reduce one mesh to its convex hull and convert it to an ESDF grid. - 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] @@ -448,227 +395,817 @@ 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." - ) - if not torch.cuda.is_available(): + mesh = _to_open3d_tensor_mesh(vertices, faces, o3d) + try: + convex_hull = _compute_convex_hull(mesh) + except Exception as exc: # noqa: BLE001 - normalize Open3D/QHull failures + raise RuntimeError( + f"Convex-hull preprocessing failed for object {name!r}." + ) from exc + if convex_hull.is_empty(): raise RuntimeError( - "The 'sphere' representation requires CUDA for cuRobo sphere fitting." + f"Convex-hull preprocessing produced no geometry for object {name!r}." ) + # Open3D's QHull bridge returns Float64 vertices even for a Float32 input, + # while RaycastingScene requires Float32 triangle positions. + convex_hull.vertex.positions = convex_hull.vertex.positions.to( + o3d.core.Dtype.Float32 + ) - import trimesh + 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) + + convex_hull = convex_hull.cpu() if hasattr(convex_hull, "cpu") else convex_hull + scene = o3d.t.geometry.RaycastingScene() + scene.add_triangles(convex_hull) + signed_distance = torch.from_numpy( + scene.compute_signed_distance(query_o3d).numpy() + ).to(torch.float32) + + feature_tensor = signed_distance.reshape(grid_shape).to(torch.float16).contiguous() + return name, { + "pose": pose.tolist(), + "dims": dims.tolist(), + "voxel_size": float(voxel_size), + "feature_tensor": feature_tensor, + } - 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 - fit_type_map = { - "morphit": SphereFitType.MORPHIT, - "voxel": SphereFitType.VOXEL, - "surface": SphereFitType.SURFACE, - } - if fit_type not in fit_type_map: +def _pose_matrix_to_list(pose: torch.Tensor) -> list[float]: + """Convert a homogeneous pose matrix to cuRobo ``xyz+wxyz`` format.""" + pose = torch.as_tensor(pose, dtype=torch.float32).detach().cpu() + return torch.cat([pose[:3, 3], quat_from_matrix(pose[:3, :3])]).tolist() + + +def _collision_shape_mesh( + shape: CollisionShapeDesc, + plane_dims: tuple[float, float, float], +) -> tuple[torch.Tensor, torch.Tensor]: + """Convert a physical collision descriptor to a local triangle mesh.""" + if shape.vertices is not None and shape.triangles is not None: + if shape.vertices.numel() and shape.triangles.numel(): + return shape.vertices, shape.triangles + + import trimesh + + if shape.shape_type == RigidBodyShape.BOX: + assert shape.half_extents is not None + mesh = trimesh.creation.box(extents=(2.0 * shape.half_extents).numpy()) + elif shape.shape_type == RigidBodyShape.PLANE: + mesh = trimesh.creation.box(extents=plane_dims) + elif shape.shape_type == RigidBodyShape.SPHERE: + assert shape.radius is not None + mesh = trimesh.creation.icosphere(subdivisions=2, radius=shape.radius) + elif shape.shape_type == RigidBodyShape.CAPSULE: + assert shape.radius is not None and shape.half_height is not None + mesh = trimesh.creation.capsule( + radius=shape.radius, height=2.0 * shape.half_height + ) + else: raise ValueError( - f"fit_type must be one of {list(fit_type_map)}, got {fit_type!r}." + f"Collision shape {shape.name!r} ({shape.shape_type.name}) does not " + "expose a mesh usable by cuRobo." ) - 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), + return ( + torch.as_tensor(mesh.vertices, dtype=torch.float32), + torch.as_tensor(mesh.faces, dtype=torch.int32), ) - 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) + +def _estimated_voxel_count( + vertices: torch.Tensor, + voxel_size: float, + voxel_padding: float, +) -> int: + """Estimate the dense ESDF allocation for a local collision mesh.""" + local_half_extent = torch.maximum( + vertices.amin(dim=0).abs(), vertices.amax(dim=0).abs() ) - radii = fit_result.radii.detach().to("cpu").reshape(-1).to(torch.float32) + float( - collision_sphere_buffer + requested_dims = 2.0 * (local_half_extent + float(voxel_padding)) + shape = torch.clamp(torch.ceil(requested_dims / voxel_size), min=2).to(torch.int64) + return int(torch.prod(shape).item()) + + +def _auto_collision_representation( + shape: CollisionShapeDesc, +) -> 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: "voxel", + RigidBodyShape.SDF: "voxel", + RigidBodyShape.MESH: "voxel", + } + if shape.shape_type in native: + return native[shape.shape_type] + raise ValueError( + f"No automatic cuRobo representation for DexSim shape " + f"{shape.shape_type.name}." ) - 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()), - }, - ) + + +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] | Mapping[str, 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 live ``RigidObject`` handles. - - 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.motion.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 analytic primitives and converts every mesh-backed + physical shape (triangle mesh, convex mesh, or SDF mesh) exclusively to an + ESDF voxel grid. Meshes are reduced to an Open3D convex hull before sampling + signed distances. Direct cuRobo ``Mesh`` obstacles are intentionally not + supported. Args: - rigid_objects: Objects to bake into the collision world. Mapping keys are - authoritative obstacle IDs. A sequence derives each name from the - object's ``uid`` (or ``obstacle_`` when absent). - 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. + Mapping keys are authoritative obstacle IDs. A sequence derives + each ID from the object's ``uid`` (or ``obstacle_``). + env_id: Environment row used for geometry and initial poses. + representation: Global ``auto`` or forced representation policy. + overrides: Per-object policies keyed by canonical obstacle ID. + dynamic_obstacle_names: Canonical obstacle IDs 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: Deprecated compatibility parameter. Mesh-backed + shapes are always voxelized regardless of triangle count. + max_voxel_count: Upper bound for any generated 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 Open3D convex-hull preprocessing fails. """ - import os - - import yaml - registry_backed = isinstance(rigid_objects, Mapping) named_rigid_objects = _named_rigid_objects(rigid_objects) if not named_rigid_objects: raise ValueError("rigid_objects must contain at least one RigidObject.") - - data: dict[str, dict[str, object]] = {} - used_names: set[str] = set() - for name, obj in named_rigid_objects: - if not isinstance(name, str) or not name or name != name.strip(): + overrides = overrides or {} + supported = {"auto", "voxel", "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_name, obj in named_rigid_objects: + if ( + not isinstance(object_name, str) + or not object_name + or object_name != object_name.strip() + ): raise ValueError( "Obstacle IDs must be non-empty strings without outer whitespace." ) - if name in used_names: + if object_name in object_names: raise ValueError( - f"Duplicate obstacle name {name!r}; obstacle IDs must be unique." + f"Duplicate obstacle name {object_name!r}; obstacle IDs must be unique." ) - used_names.add(name) - - vertices = obj.get_vertices(env_ids=[env_id], scale=True)[0] - faces = obj.get_triangles(env_ids=[env_id])[0] - if ( - vertices is None - or faces is None - or vertices.numel() == 0 - or faces.numel() == 0 - ): + object_names.add(object_name) + shapes = obj.get_collision_shapes(env_id=env_id) + if not shapes: if registry_backed: raise ValueError( - f"Registry-backed obstacle {name!r} has no mesh geometry; " - "the declared collision world cannot omit it." + f"Registry-backed obstacle {object_name!r} has no physical " + "collision shapes; the declared collision world cannot omit it." ) logger.log_warning( - f"RigidObject {name!r} has no mesh geometry; skipping collision export." + f"RigidObject {object_name!r} has no physical collision shapes; " + "skipping collision export." ) continue - pose = obj.get_local_pose(to_matrix=False)[env_id] - - 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, + 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}" + ) + shape_pose = object_pose @ shape.local_pose + policy = overrides.get(object_name, representation) + if policy == "auto": + policy = _auto_collision_representation(shape) + _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 == "voxel": + vertices, triangles = _collision_shape_mesh(shape, plane_dims) + voxel_count = _estimated_voxel_count( + vertices, + voxel_size, + voxel_padding, + ) + if voxel_count > max_voxel_count: + raise ValueError( + f"Voxelizing collision shape {obstacle_name!r} requires " + f"{voxel_count} voxels, exceeding " + f"max_voxel_count={max_voxel_count}. Increase voxel_size " + "or max_voxel_count." + ) + _, fields = _convex_hull_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 obstacle IDs: " + f"{unknown_overrides}." ) - for top_key, obstacle_name, fields in entries: - data.setdefault(top_key, {})[obstacle_name] = fields - if not data: + 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 - 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 + +# ============================================================================= +# Cached collision-model visualization +# ============================================================================= + + +def _collision_visualization_geometries( + meshes: list[tuple[str, Any]], + centers: torch.Tensor, + radii: torch.Tensor, + *, + sphere_name: str, + sphere_color: list[float], + mesh_color: list[float], +) -> list[dict[str, Any]]: + """Build Open3D draw entries in the style of DexSim's ``sphere_fit_visual``.""" + import open3d as o3d + + mesh_material = o3d.visualization.rendering.MaterialRecord() + mesh_material.shader = "defaultLit" + mesh_material.base_color = mesh_color + + geometries = [ + {"name": name, "geometry": mesh, "material": mesh_material} + for name, mesh in meshes + ] + + spheres_mesh = o3d.geometry.TriangleMesh() + centers_np = centers.detach().cpu().numpy().reshape(-1, 3) + radii_np = radii.detach().cpu().numpy().reshape(-1) + for center, radius in zip(centers_np, radii_np): + sphere = o3d.geometry.TriangleMesh.create_sphere(float(radius)) + sphere.translate(center) + spheres_mesh += sphere + spheres_mesh.compute_vertex_normals() + + sphere_material = o3d.visualization.rendering.MaterialRecord() + sphere_material.shader = "defaultLitSSR" + sphere_material.base_color = sphere_color + sphere_material.base_roughness = 0.05 + sphere_material.base_reflectance = 0.0 + sphere_material.base_clearcoat = 1.0 + sphere_material.thickness = 1.0 + sphere_material.transmission = 0.2 + sphere_material.absorption_distance = 10.0 + sphere_material.absorption_color = sphere_color[:3] + geometries.append( + { + "name": sphere_name, + "geometry": spheres_mesh, + "material": sphere_material, + } + ) + return geometries + + +def visualize_curobo_robot_collision_model( + robot: Robot, + robot_yaml_path: str, + env_id: int = 0, + *, + draw: bool = True, +) -> list[dict[str, Any]]: + """Visualize a robot's live link meshes and cached collision spheres. + + Sphere centers and radii are always loaded from ``robot_yaml_path``. Each + link-local cached center is transformed by the link's live simulator pose + from :meth:`Articulation.get_link_pose`, making frame errors directly + visible against the corresponding world-space mesh. + + Args: + robot: Live simulator robot. + robot_yaml_path: Cached auto-generated cuRobo robot YAML. + env_id: Simulator environment instance to visualize. + draw: Open an Open3D window immediately. ``False`` returns draw entries + for composition with another collision model. + + Returns: + Open3D geometry dictionaries suitable for :func:`open3d.visualization.draw`. + """ + import open3d as o3d + import yaml + + with open(robot_yaml_path, encoding="utf-8") as yaml_file: + data = yaml.safe_load(yaml_file) + kinematics = data["robot_cfg"]["kinematics"] + cached_spheres = kinematics.get("collision_spheres", {}) + sphere_buffer = float(kinematics.get("collision_sphere_buffer", 0.0)) + + meshes: list[tuple[str, Any]] = [] + world_centers: list[torch.Tensor] = [] + radii: list[float] = [] + for link_name, link_spheres in cached_spheres.items(): + vertices, faces = robot.get_link_vert_face(link_name) + if vertices is None or faces is None or vertices.numel() == 0: + continue + link_pose = torch.as_tensor( + robot.get_link_pose(link_name, env_ids=[env_id], to_matrix=True)[0], + dtype=torch.float32, + ).cpu() + mesh = _to_open3d_legacy_mesh(vertices, faces, o3d) + mesh.transform(link_pose.numpy()) + meshes.append((f"robot_mesh/{link_name}", mesh)) + + centers_local = torch.as_tensor( + [sphere["center"] for sphere in link_spheres], dtype=torch.float32 + ).reshape(-1, 3) + centers_world = centers_local @ link_pose[:3, :3].T + link_pose[:3, 3] + world_centers.extend(centers_world.unbind()) + radii.extend(float(sphere["radius"]) + sphere_buffer for sphere in link_spheres) + + if not world_centers: + raise ValueError( + f"Robot cache {robot_yaml_path!r} contains no collision spheres." + ) + geometries = _collision_visualization_geometries( + meshes, + torch.stack(world_centers), + torch.tensor(radii, dtype=torch.float32), + sphere_name="robot_spheres", + sphere_color=[0.0, 0.2, 0.8, 0.5], + mesh_color=[0.5, 0.5, 0.5, 1.0], + ) + if draw: + o3d.visualization.draw(geometries, title="cuRobo robot collision model") + return geometries + + +def _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: + material = env.create_pbr_material(name) + material_inst = material.get_inst() + material_inst.update_pbr_material_type("BSDF_GGX_SMITH") + material_inst.set_pbr_param("baseColor", color[0], color[1], color[2]) + material_inst.set_pbr_param("roughness", 0.02) + material_inst.set_pbr_param("ior", 1.12) + material_inst.set_pbr_param("colorAbsorption", 0.005, 0.005, 0.005) + material_inst.set_pbr_param("scaleAbsorption", 0.01) + return material + 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]}." + ) + if torch.any(radii <= 0.0): + raise ValueError("Visualization sphere radii must all be positive.") + if centers.shape[0] == 0: + raise ValueError("At least one visualization sphere is required.") + + sphere_template = o3d.geometry.TriangleMesh.create_sphere(radius=1.0, resolution=8) + sphere_template.compute_vertex_normals() + template_vertices = np.asarray(sphere_template.vertices) + template_triangles = np.asarray(sphere_template.triangles) + template_normals = np.asarray(sphere_template.vertex_normals) + centers_np = centers.numpy() + radii_np = radii.numpy() + + # Vectorized assembly avoids repeated ``combined_mesh += sphere`` reallocations, + # which become quadratic for a dense obstacle surface. + sphere_count = centers_np.shape[0] + vertices_per_sphere = template_vertices.shape[0] + vertices = ( + template_vertices[None, :, :] * radii_np[:, None, None] + centers_np[:, None, :] + ).reshape(-1, 3) + triangle_offsets = (np.arange(sphere_count, dtype=np.int64) * vertices_per_sphere)[ + :, None, None + ] + triangles = (template_triangles[None, :, :] + triangle_offsets).reshape(-1, 3) + + mesh = o3d.geometry.TriangleMesh() + mesh.vertices = o3d.utility.Vector3dVector(vertices) + mesh.triangles = o3d.utility.Vector3iVector(triangles) + mesh.vertex_normals = o3d.utility.Vector3dVector( + np.tile(template_normals, (sphere_count, 1)) + ) + return mesh + + +def _load_dexsim_sphere_mesh( + env: Any, + centers: torch.Tensor, + radii: torch.Tensor, + material: Any, +) -> Any: + """Write one combined sphere mesh to ``/tmp`` and load it into DexSim.""" + import os + import tempfile + + import open3d as o3d + + mesh = _create_open3d_sphere_mesh(centers, radii) + with tempfile.NamedTemporaryFile( + prefix="curobo_collision_spheres_", + suffix=".ply", + dir="/tmp", + delete=False, + ) as temp_file: + mesh_path = temp_file.name + + actor = None + try: + if not o3d.io.write_triangle_mesh(mesh_path, mesh, write_ascii=False): + raise RuntimeError( + f"Could not write collision sphere mesh to {mesh_path!r}." + ) + actor = env.load_actor(mesh_path) + if actor is None: + raise RuntimeError(f"DexSim could not load collision mesh {mesh_path!r}.") + actor.set_material(material) + return actor + except Exception: + if actor is not None: + env.remove_actor(actor) + raise + finally: + try: + os.unlink(mesh_path) + except FileNotFoundError: + pass + + +def _remove_dexsim_visualization_actors(env: Any, actors: Sequence[Any]) -> None: + """Remove every temporary actor, continuing if an individual removal fails.""" + for actor in reversed(actors): + try: + env.remove_actor(actor) + except Exception as exc: # noqa: BLE001 + logger.log_warning(f"Could not remove a cuRobo visualization actor: {exc}") + + +def _world_collision_sphere_data(world_scene: Any) -> tuple[torch.Tensor, torch.Tensor]: + """Return world-space 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( + "The cuRobo world scene contains no visible collision surface." + ) + return torch.cat(centers), torch.cat(radii) + + +def visualize_curobo_world_collision_model( + rigid_objects: Sequence[RigidObject] | Mapping[str, 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, + optionally keyed by canonical registry ID. + 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] | Mapping[str, 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. + + Args: + robot: Live simulator robot whose link poses position the cached spheres. + robot_yaml_path: Generated cuRobo robot YAML containing collision spheres. + rigid_objects: Optional live world obstacles represented by ``world_scene``. + world_scene: Optional tensor-backed scene or cuRobo ``Scene`` instance. + env_id: Simulator environment instance to visualize. + """ + 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.75, 0.75, 1.0], + ) + obstacle_material = _get_or_create_dexsim_material( + env, + "curobo_world_collision_material", + [1.0, 0.75, 0.75], + ) + + 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/embodichain/lab/sim/objects/__init__.py b/embodichain/lab/sim/objects/__init__.py index a6220b1d3..a1d91b7b0 100644 --- a/embodichain/lab/sim/objects/__init__.py +++ b/embodichain/lab/sim/objects/__init__.py @@ -22,7 +22,7 @@ from __future__ import annotations 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 39185a8a0..d58092604 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 @@ -1211,6 +1253,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/examples/sim/motion/planners/curobo_planner.py b/examples/sim/motion/planners/curobo_planner.py index 0600548c4..8ab76a260 100644 --- a/examples/sim/motion/planners/curobo_planner.py +++ b/examples/sim/motion/planners/curobo_planner.py @@ -16,8 +16,10 @@ """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 +The demo creates one or more copies of the selected robot and an Open3D box +mesh stored in a temporary file. Its DexSim collision mesh is reduced to a +convex hull and converted 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 @@ -39,6 +41,7 @@ import argparse import sys +import tempfile import time from pathlib import Path @@ -75,7 +78,7 @@ ) import numpy as np from embodichain.lab.sim.robots import FrankaPandaCfg, URRobotCfg, DexforceW1Cfg -from embodichain.lab.sim.shapes import CubeCfg +from embodichain.lab.sim.shapes import MeshCfg __all__ = ["main"] @@ -105,7 +108,11 @@ def parse_args() -> argparse.Namespace: # This standalone example does not merge a gym config after parsing, so # override the launcher's ``None`` sentinel with a concrete single-world # default. - parser.set_defaults(arena_space=2.0, num_envs=1) + parser.set_defaults( + arena_space=2.0, + num_envs=1, + seed=DEFAULT_RANDOM_SEED, + ) # Backward-compatible aliases used by older versions of this example. parser.add_argument( "--step-repeat", @@ -169,12 +176,6 @@ def parse_args() -> argparse.Namespace: "num_envs > 1." ), ) - parser.add_argument( - "--seed", - type=int, - default=DEFAULT_RANDOM_SEED, - help="Random seed used for per-environment obstacle perturbations.", - ) parser.add_argument( "--cuda-graph", action=argparse.BooleanOptionalAction, @@ -244,7 +245,7 @@ def _build_scene( gpu_id: int = 0, visualization: VisualizationCfg | None = None, ) -> tuple[SimulationManager, Robot, RigidObject, torch.Tensor, str]: - """Create the batched robot scene with an identical cuboid in each arena.""" + """Create the batched robot scene with an identical box mesh in each arena.""" sim = SimulationManager( SimulationManagerCfg( headless=True, @@ -457,29 +458,86 @@ 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}") - - # This object is also exported into the cuRobo collision world below via - # CuroboWorldCfg.rigid_objects, so the simulator and planner share geometry - # automatically (no hand-authored collision YAML to keep in sync). - demo_block = sim.add_rigid_object( - cfg=RigidObjectCfg( - uid="demo_block", - shape=CubeCfg(size=demo_block_size), - attrs=RigidBodyAttributesCfg(), - body_type="kinematic", - init_pos=demo_block_position, - init_rot=(0.0, 0.0, 0.0), - ) + # 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}") + + # Load an Open3D-generated box mesh through DexSim, then remove the source + # file: cuRobo reads the live physical collision descriptor rather than the + # temporary render asset. The mesh-backed descriptor is always converted to + # a convex-hull ESDF voxel by generate_curobo_world_scene(). + demo_block_mesh_path = _create_temporary_box_mesh(demo_block_size) + try: + demo_block = sim.add_rigid_object( + cfg=RigidObjectCfg( + uid="demo_block", + shape=MeshCfg(fpath=str(demo_block_mesh_path)), + attrs=RigidBodyAttributesCfg(), + body_type="kinematic", + init_pos=demo_block_position, + init_rot=(0.0, 0.0, 0.0), + ) + ) + finally: + demo_block_mesh_path.unlink(missing_ok=True) return sim, robot, demo_block, target_xpos, control_part +def _create_temporary_box_mesh(size: list[float]) -> Path: + """Write a centered Open3D box mesh to a temporary OBJ file. + + Args: + size: Box dimensions in metres as ``[length, width, height]``. + + Returns: + Path to the generated temporary mesh. The caller owns its deletion. + + Raises: + ValueError: If ``size`` does not contain three positive finite values. + RuntimeError: If Open3D cannot write the temporary mesh. + """ + dimensions = np.asarray(size, dtype=np.float64) + if dimensions.shape != (3,) or not np.isfinite(dimensions).all(): + raise ValueError("Box mesh size must contain three finite dimensions.") + if np.any(dimensions <= 0.0): + raise ValueError("Box mesh dimensions must be positive.") + + import open3d as o3d + + mesh = o3d.geometry.TriangleMesh.create_box( + width=float(dimensions[0]), + height=float(dimensions[1]), + depth=float(dimensions[2]), + ) + mesh.translate((-0.5 * dimensions).tolist()) + mesh.compute_vertex_normals() + with tempfile.NamedTemporaryFile( + prefix="embodichain_curobo_box_", + suffix=".obj", + delete=False, + ) as temporary_file: + mesh_path = Path(temporary_file.name) + if not o3d.io.write_triangle_mesh(str(mesh_path), mesh): + mesh_path.unlink(missing_ok=True) + raise RuntimeError(f"Open3D failed to write temporary box mesh {mesh_path}.") + return mesh_path + + def _resolve_batched_target(target: torch.Tensor, num_envs: int) -> torch.Tensor: """Return a homogeneous target pose for every simulation environment.""" if num_envs < 1: @@ -710,6 +768,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])) @@ -733,7 +794,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 @@ -747,6 +807,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 = engine.bind_control_parts( "move_end_effector", diff --git a/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md b/scripts/benchmark/motion_generation/BENCHMARK_DESIGN.md index 348248bfb..34b88412e 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: @@ -653,7 +653,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 c52ffdf2a..1cc2ae843 100644 --- a/scripts/benchmark/motion_generation/planners/curobo.py +++ b/scripts/benchmark/motion_generation/planners/curobo.py @@ -76,12 +76,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 6cc331254..ff1be231e 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 459fe3f49..a5c955143 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/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py index 3397e1aeb..d2209423a 100644 --- a/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py +++ b/scripts/tutorials/atomic_action/dynamic_obstacle_recovery.py @@ -77,7 +77,6 @@ CONTROL_PART = "arm" SAMPLE_COUNT = 80 COMMAND_CYCLE_TIME = 0.1 -COLLISION_SPHERE_FIT_TYPE = "morphit" COLLISION_SPHERE_FIT_DENSITY = 0.3 ROBOT_COLLISION_BUFFER = 0.0 MOVE_AFTER_COMMAND = 12 @@ -431,17 +430,14 @@ def main() -> None: MotionGenCfg( planner_cfg=CuroboPlannerCfg( robot_uid=robot.uid, - # The coarse default voxel fit under-covers the hand and - # fingertips. Keep the denser morphit fit, but no extra radius + # Keep MorphIt's fitted sphere set dense, but add no extra radius # padding: 5 mm makes this tutorial's initial pose infeasible. auto_gen=CuroboAutoGenCfg( - fit_type=COLLISION_SPHERE_FIT_TYPE, sphere_density=COLLISION_SPHERE_FIT_DENSITY, collision_sphere_buffer=ROBOT_COLLISION_BUFFER, ), world=CuroboWorldCfg( rigid_objects=[obstacle], - obstacle_representation="cuboid", dynamic_obstacle_names=[OBSTACLE_UID], multi_env=args.num_envs > 1, ), diff --git a/scripts/tutorials/atomic_action/pickup.py b/scripts/tutorials/atomic_action/pickup.py index d67a154dd..c931092b6 100644 --- a/scripts/tutorials/atomic_action/pickup.py +++ b/scripts/tutorials/atomic_action/pickup.py @@ -130,7 +130,6 @@ def main() -> None: hand_open, hand_close = get_hand_open_close_qpos(robot) initialize_pre_pick_robot_pose(robot, obj, hand_open) motion_gen = create_curobo_motion_generator(robot) - engine = create_simulation_atomic_action_engine( motion_generator=motion_gen, scene_entities=(obj,), diff --git a/tests/lab/task_program/semantics/test_scene_curobo_integration.py b/tests/lab/task_program/semantics/test_scene_curobo_integration.py index ee6257c31..11c6405dc 100644 --- a/tests/lab/task_program/semantics/test_scene_curobo_integration.py +++ b/tests/lab/task_program/semantics/test_scene_curobo_integration.py @@ -82,7 +82,6 @@ def test_registry_id_remains_authoritative_through_curobo_binding() -> None: geometry = registry.collision_geometry_by_id() world_cfg = CuroboWorldCfg( rigid_objects=geometry, # type: ignore[arg-type] - obstacle_representation="cuboid", dynamic_obstacle_names=list(registry.dynamic_collision_entity_ids), multi_env=mode is SceneCollisionWorldMode.PER_ENV, ) diff --git a/tests/lab/task_program/test_semantic_executor_curobo_gpu.py b/tests/lab/task_program/test_semantic_executor_curobo_gpu.py index 0d76b7b9f..d9322e919 100644 --- a/tests/lab/task_program/test_semantic_executor_curobo_gpu.py +++ b/tests/lab/task_program/test_semantic_executor_curobo_gpu.py @@ -262,13 +262,11 @@ def test_semantic_runtime_replans_after_dynamic_curobo_world_change() -> None: planner_cfg=CuroboPlannerCfg( robot_uid=ROBOT_UID, auto_gen=CuroboAutoGenCfg( - fit_type="morphit", sphere_density=0.3, collision_sphere_buffer=0.005, ), world=CuroboWorldCfg( rigid_objects=[obstacle], - obstacle_representation="cuboid", dynamic_obstacle_names=[OBSTACLE_UID], multi_env=False, ), diff --git a/tests/sim/atomic_actions/test_tutorial_utils.py b/tests/sim/atomic_actions/test_tutorial_utils.py index 139b1be9b..7093722d4 100644 --- a/tests/sim/atomic_actions/test_tutorial_utils.py +++ b/tests/sim/atomic_actions/test_tutorial_utils.py @@ -948,13 +948,15 @@ def test_dynamic_obstacle_recovery_keeps_strict_collision_contract() -> None: assert module.TRACKING_ERROR_THRESHOLD == pytest.approx( STRICT_RECOVERY_TRACKING_ERROR ) - assert module.COLLISION_SPHERE_FIT_TYPE == "morphit" assert module.COLLISION_SPHERE_FIT_DENSITY == pytest.approx( STRICT_RECOVERY_SPHERE_DENSITY ) assert module.MINIMUM_REPLAN_CLEARANCE == pytest.approx( STRICT_RECOVERY_MINIMUM_CLEARANCE ) + assert "fit_type=" not in main_source + assert "sphere_density=COLLISION_SPHERE_FIT_DENSITY" in main_source + assert "collision_sphere_buffer=ROBOT_COLLISION_BUFFER" in main_source assert "blocked_path_clearance > MAXIMUM_BLOCKED_PATH_CLEARANCE" in main_source assert "replan_clearance < MINIMUM_REPLAN_CLEARANCE" in main_source diff --git a/tests/sim/motion/planners/test_base_planner.py b/tests/sim/motion/planners/test_base_planner.py index 8ae69443f..473c8e28b 100644 --- a/tests/sim/motion/planners/test_base_planner.py +++ b/tests/sim/motion/planners/test_base_planner.py @@ -20,7 +20,40 @@ import pytest -from embodichain.lab.sim.motion.planners.base_planner import CollisionWorldInfo +from embodichain.lab.sim.motion.planners.base_planner import ( + BasePlanner, + CollisionWorldInfo, + PlanOptions, +) +from embodichain.lab.sim.motion.planners.curobo.curobo_planner import CuroboPlanner +from embodichain.lab.sim.motion.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 + ) def test_collision_world_info_represents_one_contract() -> None: diff --git a/tests/sim/motion/planners/test_curobo_example.py b/tests/sim/motion/planners/test_curobo_example.py new file mode 100644 index 000000000..c6b38663a --- /dev/null +++ b/tests/sim/motion/planners/test_curobo_example.py @@ -0,0 +1,80 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Regression tests for the standalone cuRobo planner example.""" + +from __future__ import annotations + +from argparse import Namespace +from pathlib import Path +import runpy +import sys +from typing import Callable, cast + +import pytest + +PROJECT_ROOT = Path(__file__).resolve().parents[4] +EXAMPLE_PATH = PROJECT_ROOT / "examples/sim/motion/planners/curobo_planner.py" +EXPECTED_DEFAULT_SEED = 0 +BOX_MESH_SIZE = (0.2, 0.4, 0.6) + + +@pytest.fixture(scope="module") +def example_namespace() -> dict[str, object]: + """Load the example module without starting its simulation.""" + return runpy.run_path( + str(EXAMPLE_PATH), + run_name="__curobo_planner_example_test__", + ) + + +@pytest.fixture(scope="module") +def parse_example_args(example_namespace: dict[str, object]) -> Callable[[], Namespace]: + """Return the example argument parser.""" + return cast(Callable[[], Namespace], example_namespace["parse_args"]) + + +def test_standalone_parser_uses_a_concrete_seed( + monkeypatch: pytest.MonkeyPatch, + parse_example_args: Callable[[], Namespace], +) -> None: + """The standalone example must not inherit the launcher's None seed.""" + monkeypatch.setattr(sys, "argv", [str(EXAMPLE_PATH)]) + + args = parse_example_args() + + assert args.seed == EXPECTED_DEFAULT_SEED + + +def test_temporary_box_mesh_has_requested_centered_bounds( + example_namespace: dict[str, object], +) -> None: + """The generated obstacle mesh must match its simulator pose convention.""" + import open3d as o3d + + create_box_mesh = cast( + Callable[[list[float]], Path], + example_namespace["_create_temporary_box_mesh"], + ) + mesh_path = create_box_mesh(list(BOX_MESH_SIZE)) + try: + mesh = o3d.io.read_triangle_mesh(str(mesh_path)) + bounds = mesh.get_axis_aligned_bounding_box() + + assert bounds.get_center() == pytest.approx((0.0, 0.0, 0.0)) + assert bounds.get_extent() == pytest.approx(BOX_MESH_SIZE) + finally: + mesh_path.unlink(missing_ok=True) diff --git a/tests/sim/motion/planners/test_curobo_integration.py b/tests/sim/motion/planners/test_curobo_integration.py index df12238fe..f1ba5ba6f 100644 --- a/tests/sim/motion/planners/test_curobo_integration.py +++ b/tests/sim/motion/planners/test_curobo_integration.py @@ -17,9 +17,9 @@ """Optional cuRobo V2 + CUDA integration test. Skipped entirely when cuRobo or CUDA is unavailable. When both are present, -it builds a Panda profile + static cuboid world, plans a collision-aware EEF -move through the EmbodiChain ``MotionGenerator`` API, and verifies the -``PlanResult`` contract. +it builds a Panda profile with shared and per-environment collision worlds, +plans Cartesian and joint-space moves through the EmbodiChain motion APIs, and +verifies the resulting trajectories and dynamic obstacle updates. """ from __future__ import annotations @@ -33,6 +33,8 @@ if not torch.cuda.is_available(): pytest.skip("cuRobo V2 requires CUDA", allow_module_level=True) +pytestmark = [pytest.mark.requires_sim, pytest.mark.gpu] + from embodichain.lab.sim import SimulationManager, SimulationManagerCfg # noqa: E402 from embodichain.lab.sim.objects import RigidObjectCfg # noqa: E402 from embodichain.lab.sim.robots import FrankaPandaCfg # noqa: E402 @@ -83,15 +85,12 @@ def _make_sim_robot(num_envs: int = 1): return sim, robot, block -@pytest.mark.slow -def test_curobo_v2_plans_around_a_static_cuboid(): +def test_curobo_v2_plans_around_a_static_cuboid_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, ) @@ -135,19 +134,13 @@ def test_curobo_v2_plans_around_a_static_cuboid(): SimulationManager.flush_cleanup_queue() -@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 convex-hull preprocessing and 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], representation="voxel"), warmup_iterations=0, ) mg = MotionGenerator(MotionGenCfg(planner_cfg=cfg)) @@ -184,16 +177,13 @@ def test_curobo_v2_plans_around_rigid_object_mesh_world(): SimulationManager.flush_cleanup_queue() -@pytest.mark.slow def test_curobo_v2_plans_a_joint_space_move(): """Route a ``JOINT_MOVE`` through V2 ``plan_cspace`` on CUDA.""" 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]), warmup_iterations=0, ) mg = MotionGenerator(MotionGenCfg(planner_cfg=cfg)) @@ -222,7 +212,6 @@ def test_curobo_v2_plans_a_joint_space_move(): SimulationManager.flush_cleanup_queue() -@pytest.mark.slow def test_curobo_v2_multi_env_worlds_are_independent(): """Apply the first dynamic update to both in-process collision worlds.""" sim, robot, block = _make_sim_robot(num_envs=2) @@ -232,7 +221,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/motion/planners/test_curobo_planner.py b/tests/sim/motion/planners/test_curobo_planner.py index 4609677f1..3c7515057 100644 --- a/tests/sim/motion/planners/test_curobo_planner.py +++ b/tests/sim/motion/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,19 +26,24 @@ 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.motion.planners import CuroboPlannerCfg +from embodichain.lab.sim.motion.planners.curobo import curobo_yaml from embodichain.lab.sim.motion.planners.curobo.curobo_planner import ( CuroboPlanOptions, CuroboPlanner, CuroboPlannerCfg as CuroboPlannerCfgDirect, CuroboWorldCfg, + _CuroboProfile, _configure_curobo_logging, _matrix_to_position_quaternion, _require_curobo, @@ -47,10 +52,16 @@ _validate_dynamic_obstacles, ) from embodichain.lab.sim.motion.planners.curobo.curobo_yaml import ( - _mesh_to_obstacle_entry, + _convex_hull_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.motion.planners.utils import MoveType _SIM_ROBOT_UID = "curobo_franka_inprocess_test" _SIM_CONTROL_PART = "arm" @@ -212,11 +223,13 @@ 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_world_cfg_accepts_registered_dynamic_obstacle(): @@ -342,11 +355,11 @@ def test_curobo_collision_world_binding_merges_owned_obstacle_poses(): 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(): @@ -358,6 +371,167 @@ 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_multi_env_scene_model_clones_scene_dictionaries_independently(): + planner = object.__new__(CuroboPlanner) + scene_model = { + "voxel": { + "block": { + "feature_tensor": torch.ones((2, 2, 2), dtype=torch.float16), + } + } + } + + copies = planner._materialize_multi_env_scene_model(scene_model, batch_size=2) + + assert all(isinstance(scene, dict) for scene in copies) + assert copies[0] is not copies[1] + copies[0]["voxel"]["block"]["feature_tensor"].zero_() + assert torch.all(copies[1]["voxel"]["block"]["feature_tensor"] == 1.0) + + +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) @@ -422,6 +596,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 @@ -474,7 +751,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, @@ -482,11 +759,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) @@ -494,151 +781,459 @@ 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 _track_convex_hull_preprocessing(monkeypatch, calls=None): + original_compute_convex_hull = curobo_yaml._compute_convex_hull + def tracked_compute_convex_hull(mesh): + if calls is not None: + calls.append(mesh) + return original_compute_convex_hull(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( + curobo_yaml, + "_compute_convex_hull", + tracked_compute_convex_hull, + ) - 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_computes_convex_hull_before_signed_distance(monkeypatch): + calls = [] + _track_convex_hull_preprocessing(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_hull_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 + 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): + _track_convex_hull_preprocessing(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_hull_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_hull_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 + self.pbr_type = None + self.pbr_params = {} + + def set_base_color(self, color): + self.color = color + + def get_inst(self): + return self + + def update_pbr_material_type(self, material_type): + self.pbr_type = material_type + + def set_pbr_param(self, name, *values): + self.pbr_params[name] = values + if name == "baseColor": + self.color = list(values) + + +class _FakeDexsimActor: + def __init__(self, mesh): + self.mesh = mesh + self.material = None + + def set_material(self, material): + self.material = material + + +class _FakeDexsimEnv: + def __init__(self): + self.materials = {} + self.actors = [] + self.loaded_paths = [] + self.removed_actors = [] + + 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 create_pbr_material(self, name): + material = _FakeDexsimMaterial(name, None) + 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 test_generate_cuboid_world_yaml_assembles_schema(tmp_path): + 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] + + +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.75, 0.75, 1.0] + 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.75, 0.75] + 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((1.0, 2.0, 3.0)), + [box], + ) + + scene_data = generate_curobo_world_scene([rigid_object]) + + assert list(scene_data) == ["cuboid"] + assert scene_data["cuboid"]["fixture"]["dims"] == pytest.approx([0.2, 0.4, 0.6]) + assert scene_data["cuboid"]["fixture"]["pose"][:3] == pytest.approx([1.0, 2.0, 3.0]) + + +def test_mixed_collision_visualization_supports_cuboid(): + centers, radii = _world_collision_sphere_data( + { + "cuboid": { + "fixture": { + "pose": [1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0], + "dims": [0.2, 0.4, 0.6], + } + } + } + ) + + assert centers.shape == (8, 3) + assert radii.shape == (8,) + + +def test_world_scene_object_override_can_force_voxel(monkeypatch): + _track_convex_hull_preprocessing(monkeypatch) + box = CollisionShapeDesc( + name="physics_box", + shape_type=RigidBodyShape.BOX, + local_pose=torch.eye(4), + half_extents=torch.tensor([0.5, 0.5, 0.5]), + ) + rigid_object = _FakeRigidObject( + "room_scan", _unit_cube_vertices(), _cube_faces(), _identity_pose(), + [box], ) - output_path = tmp_path / "world.yml" - result = generate_curobo_world_yaml( + scene_data = generate_curobo_world_scene( [rigid_object], - str(output_path), - representation="cuboid", + overrides={"room_scan": "voxel"}, + voxel_size=0.5, + voxel_padding=0.0, ) - 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 list(scene_data) == ["voxel"] + assert set(scene_data["voxel"]) == {"room_scan"} -def test_generate_world_yaml_uses_mapping_key_instead_of_object_uid(tmp_path): +def test_auto_world_scene_preserves_compound_shape_names_and_local_poses(): + box_pose = torch.eye(4) + box_pose[0, 3] = 0.25 + shapes = [ + CollisionShapeDesc( + name="box", + shape_type=RigidBodyShape.BOX, + local_pose=box_pose, + half_extents=torch.tensor([0.1, 0.1, 0.1]), + ), + CollisionShapeDesc( + name="sphere", + shape_type=RigidBodyShape.SPHERE, + local_pose=torch.eye(4), + radius=0.15, + ), + ] + rigid_object = _FakeRigidObject( + "compound", + _unit_cube_vertices(), + _cube_faces(), + _identity_pose((1.0, 0.0, 0.0)), + shapes, + ) + + scene_data = generate_curobo_world_scene([rigid_object]) + + assert set(scene_data) == {"cuboid", "sphere"} + assert set(scene_data["cuboid"]) == {"compound__shape_0"} + assert set(scene_data["sphere"]) == {"compound__shape_1"} + assert scene_data["cuboid"]["compound__shape_0"]["pose"][:3] == pytest.approx( + [1.25, 0.0, 0.0] + ) + + +def test_dynamic_compound_object_fans_out_to_shape_local_poses(): + first_pose = torch.eye(4) + first_pose[0, 3] = 0.25 + shapes = [ + CollisionShapeDesc( + name="first", + shape_type=RigidBodyShape.BOX, + local_pose=first_pose, + half_extents=torch.ones(3), + ), + CollisionShapeDesc( + name="second", + shape_type=RigidBodyShape.SPHERE, + local_pose=torch.eye(4), + radius=0.1, + ), + ] + rigid_object = _FakeRigidObject( + "compound", + _unit_cube_vertices(), + _cube_faces(), + _identity_pose(), + shapes, + ) + planner = CuroboPlanner.__new__(CuroboPlanner) + planner.cfg = SimpleNamespace( + world=CuroboWorldCfg( + rigid_objects=[rigid_object], dynamic_obstacle_names=["compound"] + ) + ) + + obstacle_shapes = planner._dynamic_obstacle_shapes("compound") + + assert [name for name, _ in obstacle_shapes] == [ + "compound__shape_0", + "compound__shape_1", + ] + assert obstacle_shapes[0][1][:3, 3].tolist() == pytest.approx([0.25, 0.0, 0.0]) + + +def test_generate_world_scene_uses_mapping_key_instead_of_object_uid(monkeypatch): + _track_convex_hull_preprocessing(monkeypatch) rigid_object = _FakeRigidObject( "legacy_uid", _unit_cube_vertices(), _cube_faces(), _identity_pose(), ) - output_path = tmp_path / "registry_world.yml" - generate_curobo_world_yaml( + scene_data = generate_curobo_world_scene( {"registry_cube": rigid_object}, - str(output_path), - representation="cuboid", + voxel_size=0.5, + voxel_padding=0.0, + ) + + assert set(scene_data["voxel"]) == {"registry_cube"} + + +@pytest.mark.parametrize( + "shape_type", + [RigidBodyShape.MESH, RigidBodyShape.CONVEX, RigidBodyShape.SDF], +) +def test_auto_world_scene_voxelizes_every_mesh_backed_shape(monkeypatch, shape_type): + _track_convex_hull_preprocessing(monkeypatch) + mesh_shape = CollisionShapeDesc( + name="mesh_backed_shape", + shape_type=shape_type, + local_pose=torch.eye(4), + vertices=_unit_cube_vertices(), + triangles=_cube_faces(), + ) + rigid_object = _FakeRigidObject( + "mesh_obstacle", + _unit_cube_vertices(), + _cube_faces(), + _identity_pose(), + [mesh_shape], + ) + + scene_data = generate_curobo_world_scene( + [rigid_object], + voxel_size=0.5, + voxel_padding=0.0, + ) + + assert list(scene_data) == ["voxel"] + + +def test_mesh_backed_world_scene_rejects_oversized_voxel_grid(): + rigid_object = _FakeRigidObject( + "mesh_obstacle", + _unit_cube_vertices(), + _cube_faces(), + _identity_pose(), ) - data = yaml.safe_load(output_path.read_text(encoding="utf-8")) - assert set(data["cuboid"]) == {"registry_cube"} + with pytest.raises(ValueError, match="max_voxel_count"): + generate_curobo_world_scene( + [rigid_object], + voxel_size=0.1, + voxel_padding=0.0, + max_voxel_count=8, + ) -def test_world_yaml_cache_key_includes_registry_id(): +def test_world_scene_cache_key_includes_registry_id(): rigid_object = _FakeRigidObject( "legacy_uid", _unit_cube_vertices(), @@ -650,12 +1245,12 @@ def test_world_yaml_cache_key_includes_registry_id(): robot_uid="robot", world=CuroboWorldCfg(rigid_objects={"registry_cube": rigid_object}), ) - registry_key = planner._world_yaml_cache_key(planner.cfg.world) + registry_key = planner._world_scene_cache_key(planner.cfg.world) planner.cfg.world = CuroboWorldCfg( rigid_objects={"renamed_registry_cube": rigid_object} ) - renamed_key = planner._world_yaml_cache_key(planner.cfg.world) + renamed_key = planner._world_scene_cache_key(planner.cfg.world) assert registry_key != renamed_key @@ -672,7 +1267,6 @@ def test_dynamic_update_uses_registry_id_in_curobo_backend(): robot_uid="robot", world=CuroboWorldCfg( rigid_objects={"registry_cube": rigid_object}, - obstacle_representation="cuboid", dynamic_obstacle_names=["registry_cube"], ), ) @@ -769,27 +1363,22 @@ def validate(sample, *, env_query_idx): assert joint_states[0][1] == ("curobo_right", "curobo_left") -def test_generate_mesh_world_yaml_assembles_schema(tmp_path): +def test_generate_world_scene_rejects_direct_mesh_representation(): rigid_object = _FakeRigidObject( "demo_block", _unit_cube_vertices(), _cube_faces(), _identity_pose(), ) - output_path = tmp_path / "world_mesh.yml" - - generate_curobo_world_yaml( - [rigid_object], - str(output_path), - representation="mesh", - ) - data = yaml.safe_load(output_path.read_text(encoding="utf-8")) - - assert list(data) == ["mesh"] - assert len(data["mesh"]["demo_block"]["vertices"]) == 8 + with pytest.raises(ValueError, match="representation policies"): + generate_curobo_world_scene( + [rigid_object], + representation="mesh", + ) -def test_generate_world_yaml_supports_multiple_objects(tmp_path): +def test_generate_world_scene_supports_multiple_objects(monkeypatch): + _track_convex_hull_preprocessing(monkeypatch) rigid_objects = [ _FakeRigidObject( "block_a", @@ -804,40 +1393,40 @@ 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_registry_world_yaml_rejects_empty_geometry_instead_of_skipping(tmp_path): +def test_registry_world_scene_rejects_missing_physical_shapes(): rigid_object = _FakeRigidObject( "legacy_uid", torch.zeros((0, 3), dtype=torch.float32), torch.zeros((0, 3), dtype=torch.int64), _identity_pose(), ) + rigid_object._collision_shapes = [] - with pytest.raises(ValueError, match="Registry-backed obstacle.*no mesh"): - generate_curobo_world_yaml( + with pytest.raises(ValueError, match="Registry-backed obstacle.*no physical"): + generate_curobo_world_scene( {"registry_cube": rigid_object}, - str(tmp_path / "world.yml"), ) -def test_generate_world_yaml_rejects_duplicate_names(tmp_path): +def test_generate_world_scene_rejects_duplicate_names(monkeypatch): + _track_convex_hull_preprocessing(monkeypatch) pose = _identity_pose() first = _FakeRigidObject( "block", @@ -853,13 +1442,10 @@ 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_generate_world_yaml_rejects_outer_whitespace_in_mapping_id(tmp_path): +def test_generate_world_scene_rejects_outer_whitespace_in_mapping_id(): rigid_object = _FakeRigidObject( "legacy_uid", _unit_cube_vertices(), @@ -868,58 +1454,61 @@ def test_generate_world_yaml_rejects_outer_whitespace_in_mapping_id(tmp_path): ) with pytest.raises(ValueError, match="without outer whitespace"): - generate_curobo_world_yaml( + generate_curobo_world_scene( {" registry_cube": rigid_object}, - str(tmp_path / "world.yml"), ) -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 + _track_convex_hull_preprocessing(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_as_voxel_in_curobo_scene_cfg(monkeypatch): pytest.importorskip("curobo") from curobo._src.geom.types import SceneCfg + _track_convex_hull_preprocessing(monkeypatch) 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], + voxel_size=0.5, + voxel_padding=0.0, + ) + ) - assert len(scene.mesh) == 1 - assert scene.mesh[0].name == "demo_block" - assert len(scene.mesh[0].vertices) == 8 + assert not scene.mesh + assert len(scene.voxel) == 1 + assert scene.voxel[0].name == "collision_mesh" # Simulator smoke coverage diff --git a/tests/sim/objects/test_collision_shapes.py b/tests/sim/objects/test_collision_shapes.py new file mode 100644 index 000000000..684bddcdd --- /dev/null +++ b/tests/sim/objects/test_collision_shapes.py @@ -0,0 +1,97 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import pytest +from dexsim.engine import BoxGeometry +from dexsim.types import RigidBodyShape + +from embodichain.lab.sim.objects import RigidObject + + +class _FakePhysicalBody: + def __init__(self, geometries): + self.geometries = geometries + + def get_shape_count(self): + return len(self.geometries) + + def get_shape_geometry(self, shape_idx): + return self.geometries[shape_idx] + + def get_shape_name(self, shape_idx): + return f"collision_{shape_idx}" + + +class _UnavailableGeometryBody(_FakePhysicalBody): + def get_shape_geometry(self, shape_idx): + raise RuntimeError("SDF dispatch is unavailable") + + +class _FakePhysicalEntity: + def __init__(self, geometries): + self.physical_body = _FakePhysicalBody(geometries) + + def get_physical_body(self): + return self.physical_body + + +def _box_geometry(half_extents): + geometry = BoxGeometry() + geometry.half_extents = half_extents + return geometry + + +def test_get_collision_shapes_snapshots_physical_box_geometry(): + rigid_object = RigidObject.__new__(RigidObject) + rigid_object.uid = "fixture" + rigid_object._entities = [_FakePhysicalEntity([_box_geometry([0.1, 0.2, 0.3])])] + + shapes = rigid_object.get_collision_shapes() + + assert len(shapes) == 1 + assert shapes[0].name == "collision_0" + assert shapes[0].shape_type == RigidBodyShape.BOX + assert shapes[0].half_extents.tolist() == pytest.approx([0.1, 0.2, 0.3]) + + +def test_get_collision_shapes_rejects_batched_topology_mismatch(): + rigid_object = RigidObject.__new__(RigidObject) + rigid_object.uid = "fixture" + rigid_object._entities = [ + _FakePhysicalEntity([_box_geometry([0.1, 0.2, 0.3])]), + _FakePhysicalEntity( + [ + _box_geometry([0.1, 0.2, 0.3]), + _box_geometry([0.4, 0.5, 0.6]), + ] + ), + ] + + with pytest.raises(ValueError, match="different collision-shape topology"): + rigid_object.get_collision_shapes() + + +def test_get_collision_shapes_reports_unavailable_sdf_geometry(): + rigid_object = RigidObject.__new__(RigidObject) + rigid_object.uid = "sdf_object" + entity = _FakePhysicalEntity([]) + entity.physical_body = _UnavailableGeometryBody([object()]) + rigid_object._entities = [entity] + + with pytest.raises(RuntimeError, match="canonical collision mesh"): + rigid_object.get_collision_shapes()