update curobo sphere fit

docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst

@@ -22,6 +22,7 @@ bodies.
     Light
     LightCfg
     RigidObject
+    CollisionShapeDesc
     RigidBodyData
     RigidObjectCfg
     RigidObjectGroup
@@ -67,6 +68,9 @@ Rigid Object
     :inherited-members:
     :show-inheritance:
 
+.. autoclass:: CollisionShapeDesc
+    :members:
+
 .. autoclass:: RigidBodyData
     :members:
     :inherited-members:

docs/source/overview/sim/planners/curobo_planner.md

@@ -89,9 +89,15 @@ different planning GPU. A CPU value is rejected because cuRobo itself has no
 CPU backend.
 
 The robot configuration must be a cuRobo V2 robot profile with collision
-spheres and self-collision data; the adapter generates this from the robot's
-URDF automatically. A plain URDF alone is not sufficient for collision planning
-without that sphere-fitting step.
+spheres; the adapter generates this from the robot's URDF automatically. A plain
+URDF alone is not sufficient for robot-to-world collision planning without that
+sphere-fitting step.
+
+:::{warning}
+cuRobo self-collision checking is temporarily disabled in this backend.
+Robot-to-world collision checking remains enabled, but planned trajectories
+are not currently rejected when two robot links collide with each other.
+:::
 
 The adapter automatically rebases simulator-world Cartesian goals and dynamic
 obstacle poses through the live simulator control-part base, so parallel arena
@@ -134,21 +140,55 @@ that use only one move type retain one planner backend; using both incurs a
 second one-time warmup and its graph-resident memory, but still no subprocess or
 second CUDA context.
 
-The collision world is always auto-generated from live `RigidObject` meshes via
-`CuroboWorldCfg.rigid_objects`: the adapter reads each object's mesh
-(`get_vertices` / `get_triangles`) and world pose (`get_local_pose`) and writes a
-cached cuRobo scene YAML on the first plan, using
-`CuroboWorldCfg.obstacle_representation` (`"sphere"` by default for fast
-collision queries; use `"cuboid"` for a local-frame AABB placed as an OBB via
-the object pose, or `"mesh"` for the exact triangle mesh).
+The collision world is auto-generated from live `RigidObject` **physical
+collision shapes** via `RigidObject.get_collision_shapes()`. It does not use
+`get_vertices()` / `get_triangles()`, which expose combined visual meshes and may
+differ from the geometry used by DexSim physics.
+
+`CuroboWorldCfg.representation="auto"` is the default. The policy preserves
+boxes as cuboids, spheres and capsules as analytic primitives, and convex
+collision shapes as meshes. Triangle meshes remain meshes up to
+`mesh_triangle_threshold`; above that threshold they become voxel ESDF when the
+estimated dense allocation fits `max_voxel_count`. Pose-dynamic meshes use twice
+the threshold before voxelization, while static cached meshes favor ESDF sooner
+for repeated collision queries. SDF descriptors fall back to
+their canonical collision mesh because the current DexSim Python binding does
+not expose reusable SDF grid data. Unsupported descriptors raise an explicit
+error instead of silently falling back to visual geometry.
+
+Forced voxel mode and per-object overrides remain available:
+
+```python
+world_cfg = CuroboWorldCfg(
+    rigid_objects=[room_scan, precision_fixture],
+    representation="auto",
+    overrides={
+        "room_scan": "voxel",
+        "precision_fixture": "mesh",
+    },
+)
+```
+
+`voxel_size` and `voxel_padding` configure generated ESDF layers. `plane_dims`
+bounds an infinite DexSim plane as a thin cuRobo cuboid. Compound and ACD bodies
+produce stable names such as `fixture__shape_0`; callers still use the owning
+`RigidObject` UID in `dynamic_obstacle_names` and
+`CuroboPlanOptions.dynamic_obstacle_poses`, and the adapter fans each update out
+through the sub-shapes' local poses.
+
+> **DexSim binding requirement:** Correct compound/USD offsets require
+> `RigidBody.get_shape_geometry()` to copy each physical shape's local pose into
+> `ShapeGeometry.local_pose`. Reusing a DexSim SDF as a voxel grid additionally
+> requires grid metadata/data that the current Python API does not expose. Until
+> those upstream bindings are available, verify compound offsets explicitly;
+> SDF descriptors use their canonical collision mesh when one is exposed and
+> otherwise raise an actionable error.
+
 Generated poses are authored in the cuRobo base/world frame, so this is exact
 when the robot base sits at the simulator world origin. For obstacles that move
-or live in an offset base frame, also declare their names in
+or live in an offset base frame, declare their object UIDs in
 `CuroboWorldCfg.dynamic_obstacle_names` and update poses at plan time through
-`CuroboPlanOptions.dynamic_obstacle_poses` (provision
-`CuroboWorldCfg.collision_cache` before planning). Dynamic updates require the
-`"cuboid"` or `"mesh"` representation because sphere fitting expands one object
-into multiple independently named obstacles.
+`CuroboPlanOptions.dynamic_obstacle_poses`.
 
 ### Shared and per-environment collision worlds
 
@@ -172,22 +212,20 @@ differ, the adapter rejects the update and instructs the caller to enable
 
 With `multi_env=True`, cuRobo allocates one collision world per batch row and
 EmbodiChain sends row `i` of each dynamic obstacle pose to world `i`. The
-auto-generated YAML still reads the static scene from env 0 and clones that
+auto-generated collision cache still reads the static scene from env 0 and clones that
 scene for every row; setting `multi_env=True` does not by itself discover each
 environment's distinct initial object poses. Any object whose robot-relative
 pose differs by environment must also:
 
-1. Use `obstacle_representation="cuboid"` or `"mesh"`.
-2. Be listed in `CuroboWorldCfg.dynamic_obstacle_names`.
-3. Have its current `(B, 4, 4)` simulator-world poses passed through
+1. Be listed in `CuroboWorldCfg.dynamic_obstacle_names`.
+2. Have its current `(B, 4, 4)` simulator-world poses passed through
    `CuroboPlanOptions.dynamic_obstacle_poses` when planning.
 
 For example:
 
 ```python
 world_cfg = CuroboWorldCfg(
     rigid_objects=[block],
-    obstacle_representation="cuboid",
     dynamic_obstacle_names=["block"],
     multi_env=True,
 )
@@ -225,11 +263,18 @@ robot's URDF and solver, so nothing robot-specific needs to be hardcoded:
 The generated YAML is cached on disk (default `$XDG_CACHE_HOME/embodichain_curobo`
 or `~/.cache/embodichain_curobo`) keyed by the URDF path, URDF content, control
 part, tool frame, and fit parameters, so editing the URDF or changing the fit
-settings regenerates automatically and subsequent inits reuse the cache. Tune the
-fit with `CuroboPlannerCfg.auto_gen` (`fit_type="voxel"` by default for fast
-first-generation; `"morphit"` for best quality; `force=True` to bypass the cache).
-The default `sphere_density=0.1` keeps the per-link sphere count low (~80 for a
-Panda) so planning stays fast; raise it for tighter collision coverage.
+settings regenerates automatically and subsequent inits reuse the cache. Sphere
+fitting always uses DexSim's `SphereFitType.MORPHIT`, with at most 2 convex hulls
+per robot link and 16 per voxelized obstacle shape. The default
+`sphere_density=0.1` keeps the
+per-link sphere count low (~80 for a Panda) so planning stays fast; raise it for
+tighter collision coverage, or set `force=True` to bypass the cache.
+
+For an Open3D overlay of the robot collision spheres and sampled world collision
+representations read back from those caches, call
+`planner.visualize_robot_collision_models(control_part)`. Robot sphere centers are
+transformed by the simulator's live link poses. The interactive cuRobo example
+calls this once after planner initialization; close the Open3D window to continue.
 
 ## Generate a motion
 
@@ -309,9 +354,9 @@ python examples/sim/planners/curobo_planner.py --headless --sim-device cpu
 ~~~
 
 The demo exports the DexSim `demo_block` into the cuRobo collision world via
-`CuroboWorldCfg.rigid_objects` (the robot and world YAMLs are both
-auto-generated), prints the result status and trajectory shape, then replays the
-returned full-DoF trajectory. CUDA graph capture is enabled by default with the
+`CuroboWorldCfg.rigid_objects` (the robot YAML and mixed collision-world cache are
+auto-generated), prints the result status and trajectory shape, then replays
+the returned full-DoF trajectory. CUDA graph capture is enabled by default with the
 renderer-compatible `"thread_local"` mode; pass `--no-cuda-graph` to disable it.
 Headless runs
 automatically record this fixed offscreen camera view to an MP4. Set an explicit

embodichain/lab/sim/objects/__init__.py

@@ -20,7 +20,7 @@
 """
 
 from ..common import BatchEntity
-from .rigid_object import RigidObject, RigidBodyData, RigidObjectCfg
+from .rigid_object import CollisionShapeDesc, RigidObject, RigidBodyData, RigidObjectCfg
 from .rigid_object_group import (
     RigidObjectGroup,
     RigidBodyGroupData,

embodichain/lab/sim/objects/rigid_object.py

@@ -25,7 +25,20 @@
 from functools import cached_property
 
 from dexsim.models import MeshObject
-from dexsim.types import RigidBodyGPUAPIReadType, RigidBodyGPUAPIWriteType
+from dexsim.types import (
+    RigidBodyGPUAPIReadType,
+    RigidBodyGPUAPIWriteType,
+    RigidBodyShape,
+)
+from dexsim.engine import (
+    BoxGeometry,
+    CapsuleGeometry,
+    ConvexMeshGeometry,
+    PlaneGeometry,
+    SDFGeometry,
+    SphereGeometry,
+    TriangleMeshGeometry,
+)
 from dexsim.engine import CudaArray, MaterialInst, PhysicsScene
 from embodichain.lab.sim.cfg import RigidObjectCfg, RigidBodyAttributesCfg
 from embodichain.lab.sim.shapes import MeshCfg
@@ -49,7 +62,36 @@
 from embodichain.utils.math import matrix_from_quat, quat_from_matrix, matrix_from_euler
 from embodichain.utils import logger
 
-__all__ = ["RigidBodyData", "RigidObject", "RigidObjectCfg"]
+__all__ = ["CollisionShapeDesc", "RigidBodyData", "RigidObject", "RigidObjectCfg"]
+
+
+@dataclass
+class CollisionShapeDesc:
+    """Planner-independent snapshot of one DexSim physical collision shape.
+
+    Geometry values are copied from DexSim's runtime collision descriptor and
+    therefore already include its geometry scale. Consumers must not apply
+    :attr:`RigidObjectCfg.body_scale` again.
+
+    Attributes:
+        name: Stable shape name within the owning rigid object.
+        shape_type: DexSim collision-shape type.
+        local_pose: Shape pose relative to the rigid object's frame, as ``(4, 4)``.
+        half_extents: Box half-extents, when applicable.
+        radius: Sphere or capsule radius, when applicable.
+        half_height: Capsule cylinder half-height, when applicable.
+        vertices: Scaled collision-mesh vertices, when applicable.
+        triangles: Collision-mesh triangle indices, when applicable.
+    """
+
+    name: str
+    shape_type: RigidBodyShape
+    local_pose: torch.Tensor
+    half_extents: torch.Tensor | None = None
+    radius: float | None = None
+    half_height: float | None = None
+    vertices: torch.Tensor | None = None
+    triangles: torch.Tensor | None = None
 
 
 @dataclass
@@ -1204,6 +1246,154 @@ def get_triangles(self, env_ids: Sequence[int] | None = None) -> torch.Tensor:
             device=self.device,
         )
 
+    def get_collision_shapes(self, env_id: int = 0) -> list[CollisionShapeDesc]:
+        """Snapshot the physical collision shapes used by DexSim.
+
+        Unlike :meth:`get_vertices` and :meth:`get_triangles`, this method reads
+        the physics body's collision descriptors rather than render meshes. For
+        batched objects, every row is checked for identical shape topology before
+        the requested row is returned.
+
+        .. attention::
+            The installed DexSim binding must populate ``ShapeGeometry.local_pose``
+            and dispatch SDF geometry through ``get_shape_geometry``. This method
+            preserves the values returned by DexSim and raises an actionable error
+            when a descriptor cannot be retrieved.
+
+        Args:
+            env_id: Environment row whose collision geometry is returned.
+
+        Returns:
+            Physical collision-shape descriptors in stable shape-index order.
+
+        Raises:
+            IndexError: If ``env_id`` is outside the object batch.
+            RuntimeError: If DexSim cannot expose a collision descriptor.
+            ValueError: If collision topology differs between environment rows.
+        """
+        if env_id < 0 or env_id >= self.num_instances:
+            raise IndexError(
+                f"env_id must be in [0, {self.num_instances}), got {env_id}."
+            )
+
+        requested = self._get_collision_shapes_for_entity(env_id)
+        requested_topology = self._collision_shape_topology(requested)
+        for other_env_id in range(self.num_instances):
+            if other_env_id == env_id:
+                continue
+            other = self._get_collision_shapes_for_entity(other_env_id)
+            if self._collision_shape_topology(other) != requested_topology:
+                raise ValueError(
+                    f"RigidObject {self.uid!r} has different collision-shape "
+                    f"topology in environment rows {env_id} and {other_env_id}."
+                )
+        return requested
+
+    def _get_collision_shapes_for_entity(self, env_id: int) -> list[CollisionShapeDesc]:
+        """Return physical collision descriptors for one simulator entity."""
+        physical_body = self._entities[env_id].get_physical_body()
+        if physical_body is None:
+            raise RuntimeError(f"RigidObject {self.uid!r} has no DexSim physical body.")
+
+        shape_count = int(physical_body.get_shape_count())
+        shapes: list[CollisionShapeDesc] = []
+        for shape_idx in range(shape_count):
+            try:
+                geometry = physical_body.get_shape_geometry(shape_idx)
+            except Exception as exc:  # noqa: BLE001
+                raise RuntimeError(
+                    f"DexSim could not expose collision shape {shape_idx} for "
+                    f"RigidObject {self.uid!r}. SDF/custom shapes require a "
+                    "geometry descriptor or canonical collision mesh."
+                ) from exc
+            if geometry is None:
+                raise RuntimeError(
+                    f"DexSim returned no geometry for collision shape {shape_idx} "
+                    f"of RigidObject {self.uid!r}."
+                )
+
+            shape_name = physical_body.get_shape_name(shape_idx) or f"shape_{shape_idx}"
+            local_pose = torch.tensor(geometry.local_pose, dtype=torch.float32)
+            if local_pose.shape != (4, 4):
+                raise RuntimeError(
+                    f"DexSim collision shape {shape_idx} of {self.uid!r} returned "
+                    f"local_pose shape {tuple(local_pose.shape)}, expected (4, 4)."
+                )
+            desc = CollisionShapeDesc(
+                name=str(shape_name),
+                shape_type=self._collision_shape_type(geometry),
+                local_pose=local_pose.clone(),
+            )
+            if isinstance(geometry, BoxGeometry):
+                desc.half_extents = torch.tensor(
+                    geometry.half_extents, dtype=torch.float32
+                )
+            elif isinstance(geometry, SphereGeometry):
+                desc.radius = float(geometry.radius)
+            elif isinstance(geometry, CapsuleGeometry):
+                desc.radius = float(geometry.radius)
+                desc.half_height = float(geometry.half_height)
+            elif isinstance(
+                geometry, (ConvexMeshGeometry, TriangleMeshGeometry, SDFGeometry)
+            ):
+                vertices = torch.tensor(geometry.vertices, dtype=torch.float32).reshape(
+                    -1, 3
+                )
+                triangles = torch.tensor(geometry.triangles, dtype=torch.int32).reshape(
+                    -1, 3
+                )
+                scale = getattr(geometry, "scale", None)
+                if scale is not None:
+                    vertices = vertices * torch.tensor(
+                        scale, dtype=torch.float32
+                    ).reshape(1, 3)
+                desc.vertices = vertices
+                desc.triangles = triangles
+            shapes.append(desc)
+        return shapes
+
+    @staticmethod
+    def _collision_shape_type(geometry: object) -> RigidBodyShape:
+        """Map a concrete DexSim geometry descriptor to its shape enum."""
+        if isinstance(geometry, BoxGeometry):
+            return RigidBodyShape.BOX
+        if isinstance(geometry, PlaneGeometry):
+            return RigidBodyShape.PLANE
+        if isinstance(geometry, SphereGeometry):
+            return RigidBodyShape.SPHERE
+        if isinstance(geometry, CapsuleGeometry):
+            return RigidBodyShape.CAPSULE
+        if isinstance(geometry, ConvexMeshGeometry):
+            return RigidBodyShape.CONVEX
+        if isinstance(geometry, TriangleMeshGeometry):
+            return RigidBodyShape.MESH
+        if isinstance(geometry, SDFGeometry):
+            return RigidBodyShape.SDF
+        raise RuntimeError(
+            f"Unsupported DexSim collision geometry descriptor "
+            f"{type(geometry).__name__}."
+        )
+
+    @staticmethod
+    def _collision_shape_topology(
+        shapes: list[CollisionShapeDesc],
+    ) -> tuple[tuple[object, ...], ...]:
+        """Return the topology-only signature used for batched validation."""
+        return tuple(
+            (
+                shape.name,
+                shape.shape_type.value,
+                None if shape.vertices is None else tuple(shape.vertices.shape),
+                None if shape.triangles is None else tuple(shape.triangles.shape),
+                (
+                    None
+                    if shape.triangles is None
+                    else shape.triangles.contiguous().numpy().tobytes()
+                ),
+            )
+            for shape in shapes
+        )
+
     def get_user_ids(self, env_ids: Sequence[int] | None = None) -> torch.Tensor:
         """Get the user ids of the rigid bodies.