Mesh-based Non-collision Constraints

isaaclab_arena/assets/dummy_object.py

@@ -2,31 +2,37 @@
 # All rights reserved.
 #
 # SPDX-License-Identifier: Apache-2.0
+from __future__ import annotations
+
 import torch
+from typing import TYPE_CHECKING
 
 from isaaclab_arena.relations.relations import Relation, RelationBase, UnaryRelation
 from isaaclab_arena.utils.bounding_box import AxisAlignedBoundingBox, quaternion_to_90_deg_z_quarters
 from isaaclab_arena.utils.pose import Pose
 
+if TYPE_CHECKING:
+    import trimesh
+
 
 class DummyObject:
-    """
-    Dummy object for testing purposes without Isaac Sim dependencies.
-    """
+    """Dummy object for testing purposes without Isaac Sim dependencies."""
 
     def __init__(
         self,
         name: str,
         bounding_box: AxisAlignedBoundingBox,
         initial_pose: Pose | None = None,
         relations: list[RelationBase] = [],
+        collision_mesh: trimesh.Trimesh | None = None,
         **kwargs,
     ):
         self.name = name
         self.initial_pose = initial_pose
         self.bounding_box = bounding_box
         assert self.bounding_box is not None
         self.relations = list(relations)
+        self._collision_mesh = collision_mesh
 
     def add_relation(self, relation: RelationBase) -> None:
         self.relations.append(relation)
@@ -63,3 +69,7 @@ def get_initial_pose(self) -> Pose | None:
 
     def is_initial_pose_set(self) -> bool:
         return self.initial_pose is not None
+
+    def get_collision_mesh(self) -> trimesh.Trimesh | None:
+        """Return the collision mesh, or None to fall back to AABB."""
+        return self._collision_mesh

isaaclab_arena/assets/object_base.py

@@ -7,9 +7,13 @@
 
 import torch
 from abc import ABC, abstractmethod
+from typing import TYPE_CHECKING
 
 import warp as wp
 from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg
+
+if TYPE_CHECKING:
+    import trimesh
 from isaaclab.envs import ManagerBasedEnv
 from isaaclab.managers import EventTermCfg, SceneEntityCfg
 from isaaclab.sensors.contact_sensor.contact_sensor_cfg import ContactSensorCfg
@@ -73,6 +77,14 @@ def get_world_bounding_box(self) -> AxisAlignedBoundingBox:
         """Get bounding box in world coordinates (local bbox rotated and translated)."""
         ...
 
+    def get_collision_mesh(self) -> trimesh.Trimesh | None:
+        """Return the collision mesh for this object, or None.
+
+        When None, the mesh-based collision system falls back to AABB overlap
+        for any pair involving this object. Subclasses with mesh geometry
+        should override this method.
+        """
+
     def _get_initial_pose_as_pose(self) -> Pose | None:
         """Return a single ``Pose`` suitable for *init_state* and bounding-box calculations.
 

isaaclab_arena/relations/object_placer.py

@@ -13,6 +13,7 @@
 from isaaclab_arena.relations.object_placer_params import ObjectPlacerParams
 from isaaclab_arena.relations.placement_result import MultiEnvPlacementResult, PlacementResult
 from isaaclab_arena.relations.relation_solver import RelationSolver
+from isaaclab_arena.relations.relation_solver_params import CollisionMode
 from isaaclab_arena.relations.relations import (
     IsAnchor,
     On,
@@ -161,6 +162,11 @@ def _prepare_placement(
                 "Call anchor_object.set_initial_pose(...) before placing."
             )
 
+        assert not (self.params.random_yaw_init and self.params.solver_params.collision_mode == CollisionMode.MESH), (
+            "random_yaw_init is not yet supported with CollisionMode.MESH -- "
+            "sphere centers are not rotated by candidate yaw."
+        )
+
         generator: torch.Generator | None = None
         if self.params.placement_seed is not None:
             generator = torch.Generator()
@@ -598,6 +604,88 @@ def _validate_no_overlap(
                     return False
         return True
 
+    def _get_cpu_mesh_manager(self):
+        """Lazily create a CPU WarpMeshManager, cached across validation calls."""
+        if not hasattr(self, "_cpu_mesh_manager"):
+            from isaaclab_arena.relations.warp_mesh_manager import WarpMeshManager
+
+            self._cpu_mesh_manager = WarpMeshManager(
+                num_spheres=self.params.solver_params.num_spheres,
+                device="cpu",
+            )
+        return self._cpu_mesh_manager
+
+    def _validate_no_overlap_mesh(
+        self,
+        positions: dict[ObjectBase, tuple[float, float, float]],
+    ) -> bool:
+        """Validate no-overlap using sphere-to-SDF mesh queries.
+
+        Mirrors the AABB validator's pair-skipping logic (On pairs, anchor-anchor).
+        Skips pairs where either object lacks a collision mesh (the solver's loss
+        path still penalizes those via AABB fallback during optimization).
+        """
+        from isaaclab_arena.relations.warp_sdf_kernels import mesh_sdf
+
+        on_pairs: set[tuple] = set()
+        anchor_ids: set[int] = set()
+        for obj in positions:
+            for rel in obj.get_relations():
+                if isinstance(rel, On) and rel.parent in positions:
+                    on_pairs.add((id(obj), id(rel.parent)))
+                    on_pairs.add((id(rel.parent), id(obj)))
+            if any(isinstance(r, IsAnchor) for r in obj.get_relations()):
+                anchor_ids.add(id(obj))
+
+        clearance_m = self.params.solver_params.clearance_m
+        tolerance = max(0.0, clearance_m - 1e-6)
+        manager = self._get_cpu_mesh_manager()
+
+        warned_no_mesh: set[str] = set()
+        objects = list(positions.keys())
+        for i in range(len(objects)):
+            for j in range(i + 1, len(objects)):
+                a, b = objects[i], objects[j]
+                if id(a) in anchor_ids and id(b) in anchor_ids:
+                    continue
+                if (id(a), id(b)) in on_pairs:
+                    continue
+
+                a_mesh = a.get_collision_mesh()
+                b_mesh = b.get_collision_mesh()
+                if a_mesh is None or b_mesh is None:
+                    for obj, mesh in [(a, a_mesh), (b, b_mesh)]:
+                        if mesh is None and obj.name not in warned_no_mesh:
+                            warned_no_mesh.add(obj.name)
+                            print(
+                                f"  [NoCollision] MESH mode: '{obj.name}' has no collision mesh, skipping mesh"
+                                " validation"
+                            )
+                    continue
+
+                a_pos = torch.tensor(positions[a], dtype=torch.float32)
+                b_pos = torch.tensor(positions[b], dtype=torch.float32)
+
+                # Forward: a's spheres against b's mesh
+                spheres_a = manager.get_query_spheres(a_mesh, obj=a)
+                warp_b = manager.get_warp_mesh(b_mesh, obj=b)
+                centers_a_in_b = spheres_a[:, :3] + a_pos - b_pos
+                if (mesh_sdf(centers_a_in_b, warp_b) < spheres_a[:, 3] + tolerance).any():
+                    if self.params.verbose:
+                        print(f"  Mesh overlap between '{a.name}' and '{b.name}'")
+                    return False
+
+                # Reverse: b's spheres against a's mesh
+                spheres_b = manager.get_query_spheres(b_mesh, obj=b)
+                warp_a = manager.get_warp_mesh(a_mesh, obj=a)
+                centers_b_in_a = spheres_b[:, :3] + b_pos - a_pos
+                if (mesh_sdf(centers_b_in_a, warp_a) < spheres_b[:, 3] + tolerance).any():
+                    if self.params.verbose:
+                        print(f"  Mesh overlap between '{b.name}' and '{a.name}'")
+                    return False
+
+        return True
+
     def _validate_placement(
         self,
         positions: dict[ObjectBase, tuple[float, float, float]],
@@ -612,7 +700,13 @@ def _validate_placement(
         Returns:
             True if no overlaps exist and On relations hold, False otherwise.
         """
-        return self._validate_no_overlap(positions, env_bboxes) and self._validate_on_relations(positions, env_bboxes)
+        if self.params.solver_params.collision_mode == CollisionMode.MESH:
+            if not self._validate_no_overlap_mesh(positions):
+                return False
+        else:
+            if not self._validate_no_overlap(positions, env_bboxes):
+                return False
+        return self._validate_on_relations(positions, env_bboxes)
 
     def _apply_poses(
         self,

isaaclab_arena/relations/object_placer_params.py

@@ -5,7 +5,9 @@
 
 from dataclasses import dataclass, field
 
-from isaaclab_arena.relations.relation_solver_params import RelationSolverParams
+from isaaclab_arena.relations.relation_solver_params import CollisionMode, RelationSolverParams
+
+__all__ = ["CollisionMode", "ObjectPlacerParams"]
 
 
 @dataclass