Fix Stand Mesh Lifecycle

isaaclab_arena/assets/background_library.py

@@ -13,6 +13,7 @@
 from isaaclab_arena.assets.lightwheel_utils import acquire_lightwheel_asset
 from isaaclab_arena.assets.nucleus import ARENA_NUCLEUS_DIR
 from isaaclab_arena.assets.register import register_asset
+from isaaclab_arena.relations.collision_mode import CollisionMode
 from isaaclab_arena.utils.pose import Pose
 
 
@@ -189,6 +190,9 @@ def __init__(self, layout_id: int = 1, style_id: int = 1):
             )[0]
         )
         super().__init__()
+        # PlaceableAsset.__init__ resets these; raw kitchen mesh keeps counter/floor concavities.
+        self.collision_mode = CollisionMode.MESH
+        self.repair_collision_mesh_non_watertight = False
 
     def get_viewer_cfg(self) -> ViewerCfg:
         # Looking in through the open front.

isaaclab_arena/assets/object_reference.py

@@ -95,6 +95,11 @@ def get_world_bounding_box(self) -> AxisAlignedBoundingBox:
         quarters = quaternion_to_90_deg_z_quarters(parent_pose.rotation_xyzw)
         return box.rotated_90_around_z(quarters).translated(world_position)
 
+    def get_prim_path_in_parent_usd(self) -> str:
+        """Return the referenced prim's absolute path in its parent USD stage."""
+        with open_stage(self.parent_asset.usd_path) as parent_stage:
+            return self.isaaclab_prim_path_to_original_prim_path(self.prim_path, self.parent_asset, parent_stage)
+
     def get_collision_mesh(self) -> trimesh.Trimesh | None:
         """Return the referenced prim's collision mesh in its local frame, or None if unavailable."""
         if not self._collision_mesh_loaded:

isaaclab_arena/embodiments/droid/droid.py

@@ -36,6 +36,7 @@
 from isaaclab_arena.embodiments.embodiment_base import EmbodimentBase
 from isaaclab_arena.embodiments.franka.franka import franka_stack_events
 from isaaclab_arena.embodiments.robot_on_stand_utils import RobotPrimSpec, StandPrimSpec, compose_on_stand_usd
+from isaaclab_arena.relations.collision_mode import CollisionMode
 from isaaclab_arena.utils.bounding_box import AxisAlignedBoundingBox
 from isaaclab_arena.utils.cameras import ArenaCameraCfg
 from isaaclab_arena.utils.pose import Pose
@@ -67,8 +68,8 @@ class DroidEmbodimentBase(EmbodimentBase, ABC):
     which changes how far the stand extends below the root link.
     When manually placing the robot on floor, ``set_initial_pose`` z value and
     ``stand_height_m`` should be adjusted together to keep the bottom of stand fixed.
-    ``placement_bbox_stand_only`` uses the stand footprint for ``On`` / ``NextTo`` placement
-    instead of the full robot+stand USD bounds.
+    ``placement_bbox_stand_only`` uses the stand footprint for relation and collision placement
+    instead of the full robot+stand USD geometry.
     """
 
     name = "droid"
@@ -87,6 +88,8 @@ def __init__(
         super().__init__(enable_cameras, initial_pose, concatenate_observation_terms, arm_mode)
         self.stand_height_m = stand_height_m
         self.placement_bbox_stand_only = placement_bbox_stand_only
+        if placement_bbox_stand_only:
+            self.collision_mode = CollisionMode.BBOX
         self.scene_config = DroidSceneCfg()
         self.scene_config.robot.spawn.usd_path = compose_on_stand_usd(
             _DROID_ROBOT_PRIM,

isaaclab_arena/environments/relation_solver_interface.py

@@ -11,7 +11,7 @@
 
 from isaaclab_arena.relations.collision_mode import CollisionMode, get_object_collision_mode
 from isaaclab_arena.relations.object_placer_params import ObjectPlacerParams
-from isaaclab_arena.relations.placement_events import get_pose_from_layout, solve_and_place_objects
+from isaaclab_arena.relations.placement_events import PlacementPoolHandle, get_pose_from_layout, solve_and_place_objects
 from isaaclab_arena.relations.pooled_object_placer import PooledObjectPlacer
 from isaaclab_arena.relations.relations import get_anchor_objects
 from isaaclab_arena.utils.pose import PosePerEnv
@@ -20,6 +20,7 @@
     from isaaclab.managers import EventTermCfg
 
     from isaaclab_arena.assets.asset import Asset
+    from isaaclab_arena.assets.object_reference import ObjectReference
     from isaaclab_arena.assets.object_set import RigidObjectSet
     from isaaclab_arena.relations.collision_object import CollisionObject
     from isaaclab_arena.relations.placement_asset import PlaceableAsset
@@ -29,11 +30,16 @@
 def _get_passive_collision_objects(
     assets: Iterable[Asset | RigidObjectSet],
     include_background: bool = False,
+    background_mesh_exclusions: Iterable[ObjectReference] = (),
 ) -> list[CollisionObject]:
     """Load passive collision discovery only when relation placement needs it."""
     from isaaclab_arena.relations.passive_collision_objects import get_passive_collision_objects
 
-    return get_passive_collision_objects(assets, include_background=include_background)
+    return get_passive_collision_objects(
+        assets,
+        include_background=include_background,
+        background_mesh_exclusions=background_mesh_exclusions,
+    )
 
 
 def solve_and_apply_relation_placement(
@@ -77,12 +83,18 @@ def solve_and_apply_relation_placement(
     # mutating the caller.
     placer_params.reachability_config = copy.copy(placer_params.reachability_config)
     if collision_objects is None and scene_assets is not None:
+        from isaaclab_arena.assets.object_reference import ObjectReference
+
         scene_assets = list(scene_assets)
+        background_mesh_exclusions = [
+            asset for asset in get_anchor_objects(assets) if isinstance(asset, ObjectReference)
+        ]
         collision_objects = _get_passive_collision_objects(
             scene_assets,
             include_background=_should_include_background_mesh(
                 assets, scene_assets, placer_params.solver_params.collision_mode
             ),
+            background_mesh_exclusions=background_mesh_exclusions,
         )
     placement_pool = PooledObjectPlacer(
         objects=assets,
@@ -184,8 +196,7 @@ def _apply_dynamic_spawn_pose(
         func=solve_and_place_objects,
         mode="reset",
         params={
-            "assets": assets,
-            "placement_pool": placement_pool,
+            "placement_pool": PlacementPoolHandle(placement_pool),
         },
     )
 

isaaclab_arena/relations/background_collision_object.py

@@ -67,15 +67,23 @@ def get_collision_mesh(self) -> trimesh.Trimesh:
         return self._mesh
 
 
-def make_fixed_collision_objects(objects: Sequence[CollisionObject]) -> list[CollisionObject]:
+def make_fixed_collision_objects(
+    objects: Sequence[CollisionObject],
+    excluded_prim_paths_by_object: dict[CollisionObject, Sequence[str]] | None = None,
+) -> list[CollisionObject]:
     """Combine the objects' collision meshes into one FixedCollisionObject.
 
-    Objects in BBOX mode or without an extractable mesh are returned unchanged;
-    a whole-scene Background that cannot aggregate is an error.
+    Objects in BBOX mode or without an extractable mesh are returned unchanged.
+    Background mesh extraction failures are errors.
+
+    Args:
+        objects: Fixed collision objects to aggregate.
+        excluded_prim_paths_by_object: USD prim subtrees omitted from individual objects'
+            extracted meshes, keyed by source object.
     """
     from isaaclab_arena.assets.background import Background
 
-    mesh, skipped_objects = _combine_fixed_meshes(objects)
+    mesh, skipped_objects = _combine_fixed_meshes(objects, excluded_prim_paths_by_object)
     collision_objects: list[CollisionObject] = []
     if mesh is not None:
         collision_objects.append(FixedCollisionObject(mesh))
@@ -104,18 +112,27 @@ def make_fixed_collision_objects(objects: Sequence[CollisionObject]) -> list[Col
     return collision_objects
 
 
-def _combine_fixed_meshes(objects: Sequence[CollisionObject]) -> tuple[trimesh.Trimesh | None, list[CollisionObject]]:
+def _combine_fixed_meshes(
+    objects: Sequence[CollisionObject],
+    excluded_prim_paths_by_object: dict[CollisionObject, Sequence[str]] | None = None,
+) -> tuple[trimesh.Trimesh | None, list[CollisionObject]]:
     from isaaclab_arena.assets.background import Background
     from isaaclab_arena.relations.warp_mesh_manager import WarpMeshAndSphereCache
+    from isaaclab_arena.utils.usd_helpers import AllCollisionMeshesExcludedError
 
     manager = WarpMeshAndSphereCache(device="cpu")
+    excluded_prim_paths_by_object = excluded_prim_paths_by_object or {}
     meshes = []
     skipped_objects = []
     for obj in objects:
         if obj.collision_mode == CollisionMode.BBOX:
             skipped_objects.append(obj)
             continue
-        mesh = manager.get_collision_mesh(obj)
+        excluded_prim_paths = excluded_prim_paths_by_object.get(obj, ())
+        try:
+            mesh = manager.get_collision_mesh(obj, excluded_prim_paths=excluded_prim_paths)
+        except AllCollisionMeshesExcludedError:
+            continue
         if mesh is None:
             skipped_objects.append(obj)
             continue

isaaclab_arena/relations/collision_mode.py

@@ -10,13 +10,15 @@
 
 if TYPE_CHECKING:
     from isaaclab_arena.relations.collision_object import CollisionObject
+    from isaaclab_arena.relations.warp_mesh_manager import WarpMeshAndSphereCache
+    from isaaclab_arena.utils.bounding_box import AxisAlignedBoundingBox
 
 
 class CollisionMode(Enum):
     """Collision-detection method for no-overlap constraints."""
 
     BBOX = "bbox"
-    """Axis-aligned bounding box overlap volume (fast, conservative)."""
+    """Axis-aligned bounding box penetration (fast, conservative)."""
 
     MESH = "mesh"
     """Sphere-to-SDF queries against actual mesh geometry (accurate, slower)."""
@@ -30,3 +32,32 @@ def get_object_collision_mode(obj: CollisionObject, default: CollisionMode) -> C
 def object_uses_mesh_collision(obj: CollisionObject, default: CollisionMode) -> bool:
     """Return True when the object's effective collision mode is MESH."""
     return get_object_collision_mode(obj, default) == CollisionMode.MESH
+
+
+def pair_is_covered_by_mesh_collision(
+    subject: CollisionObject,
+    obstacle: CollisionObject,
+    subject_bbox: AxisAlignedBoundingBox,
+    obstacle_bbox: AxisAlignedBoundingBox,
+    mesh_manager: WarpMeshAndSphereCache,
+    default_collision_mode: CollisionMode,
+    obstacle_is_fixed: bool,
+) -> bool:
+    """Return whether the mesh loss represents a collision pair."""
+    subject_has_mesh = (
+        object_uses_mesh_collision(subject, default_collision_mode)
+        and mesh_manager.get_collision_mesh(subject) is not None
+    )
+    obstacle_has_mesh = (
+        object_uses_mesh_collision(obstacle, default_collision_mode)
+        and mesh_manager.get_collision_mesh(obstacle) is not None
+    )
+    subject_has_mesh_or_invariant_bbox = subject_has_mesh or subject_bbox.is_batch_invariant()
+    if obstacle_is_fixed:
+        return obstacle_has_mesh and subject_has_mesh_or_invariant_bbox
+    obstacle_has_mesh_or_invariant_bbox = obstacle_has_mesh or obstacle_bbox.is_batch_invariant()
+    return (
+        (subject_has_mesh or obstacle_has_mesh)
+        and subject_has_mesh_or_invariant_bbox
+        and obstacle_has_mesh_or_invariant_bbox
+    )

isaaclab_arena/relations/no_overlap_aabb.py

@@ -11,7 +11,7 @@
 from dataclasses import dataclass
 from typing import TYPE_CHECKING
 
-from isaaclab_arena.relations.collision_mode import CollisionMode, object_uses_mesh_collision
+from isaaclab_arena.relations.collision_mode import CollisionMode, pair_is_covered_by_mesh_collision
 from isaaclab_arena.relations.relation_loss_strategies import NoCollisionLossStrategy
 from isaaclab_arena.relations.relation_solver_state import RelationSolverState
 from isaaclab_arena.relations.relations import On
@@ -26,20 +26,20 @@
 class NoOverlapPair:
     """One directed overlap penalty: the subject box is pushed off the (detached) obstacle box.
 
-    Dimensions: B = batch_size (num envs).
+    B is the batch size.
     """
 
     subject_min: torch.Tensor
-    """(B, 3) world-space min corner of the subject box."""
+    """World-space subject minimum. Shape: [B, 3]."""
 
     subject_max: torch.Tensor
-    """(B, 3) world-space max corner of the subject box."""
+    """World-space subject maximum. Shape: [B, 3]."""
 
     obstacle_min: torch.Tensor
-    """(B, 3) world-space min corner of the obstacle box."""
+    """World-space obstacle minimum. Shape: [B, 3]."""
 
     obstacle_max: torch.Tensor
-    """(B, 3) world-space max corner of the obstacle box."""
+    """World-space obstacle maximum. Shape: [B, 3]."""
 
 
 def compute_no_overlap_loss_aabb(
@@ -163,13 +163,14 @@ def _fixed_pair_is_covered_by_mesh_collision(
     default_collision_mode: CollisionMode,
 ) -> bool:
     """Return True when MESH loss handles subject vs fixed obstacle."""
-    obstacle_mesh = (
-        mesh_manager.get_collision_mesh(obstacle)
-        if object_uses_mesh_collision(obstacle, default_collision_mode)
-        else None
-    )
-    return obstacle_mesh is not None and _has_mesh_or_invariant_bbox(
-        state, subject, mesh_manager, default_collision_mode
+    return pair_is_covered_by_mesh_collision(
+        subject,
+        obstacle,
+        state.get_bbox(subject),
+        obstacle.get_bounding_box(),
+        mesh_manager,
+        default_collision_mode,
+        obstacle_is_fixed=True,
     )
 
 
@@ -181,23 +182,12 @@ def _dynamic_pair_is_covered_by_mesh_collision(
     default_collision_mode: CollisionMode,
 ) -> bool:
     """Return True when MESH loss handles a non-anchor object pair."""
-    a_mesh = mesh_manager.get_collision_mesh(a) if object_uses_mesh_collision(a, default_collision_mode) else None
-    b_mesh = mesh_manager.get_collision_mesh(b) if object_uses_mesh_collision(b, default_collision_mode) else None
-    if a_mesh is None and b_mesh is None:
-        return False
-    return _has_mesh_or_invariant_bbox(state, a, mesh_manager, default_collision_mode) and _has_mesh_or_invariant_bbox(
-        state, b, mesh_manager, default_collision_mode
+    return pair_is_covered_by_mesh_collision(
+        a,
+        b,
+        state.get_bbox(a),
+        state.get_bbox(b),
+        mesh_manager,
+        default_collision_mode,
+        obstacle_is_fixed=False,
     )
-
-
-def _has_mesh_or_invariant_bbox(
-    state: RelationSolverState,
-    obj: PlaceableAsset,
-    mesh_manager: WarpMeshAndSphereCache,
-    default_collision_mode: CollisionMode,
-) -> bool:
-    """Return True when MESH loss can represent obj as mesh or one bbox proxy."""
-    mesh = mesh_manager.get_collision_mesh(obj) if object_uses_mesh_collision(obj, default_collision_mode) else None
-    if mesh is not None:
-        return True
-    return state.get_bbox(obj).is_batch_invariant()

isaaclab_arena/relations/no_overlap_mesh.py

@@ -154,12 +154,22 @@ def compute_no_overlap_loss_mesh(
         mesh_manager.warn_sdf_sentinel(sdf_values)
         sdf_values = clamp_sdf_sentinel(sdf_values)
         penetration = torch.relu(active_radii + clearance_m - sdf_values)
-
-        pair_sum = torch.zeros(num_pairs, device=device, dtype=penetration.dtype)
-        pair_sum.index_add_(0, active_sphere_pair_id, penetration)
-        pair_mean = pair_sum / mesh_cache.pair_sphere_count
+        pair_subject_scale = (
+            mesh_cache.pair_subject_bbox_max[:, b, :] - mesh_cache.pair_subject_bbox_min[:, b, :]
+        ).amax(dim=1)
+        assert torch.all(pair_subject_scale > 0), "Subject bounding boxes must have positive size."
+        normalized_penetration = penetration / pair_subject_scale[active_sphere_pair_id]
+
+        pair_penetration = torch.zeros(num_pairs, device=device, dtype=penetration.dtype)
+        pair_penetration = pair_penetration.scatter_reduce(
+            0,
+            active_sphere_pair_id,
+            normalized_penetration,
+            reduce="amax",
+            include_self=True,
+        )
         active_pair_idx = active_pair.nonzero(as_tuple=True)[0]
-        total_loss[b] = total_loss[b] + slope * pair_mean[active_pair_idx].sum()
+        total_loss[b] = total_loss[b] + slope * pair_penetration[active_pair_idx].sum()
 
     if debug:
         print(f"  [NoOverlap MESH] total_loss={total_loss.tolist()}")

isaaclab_arena/relations/passive_collision_objects.py

@@ -23,7 +23,9 @@
 
 
 def get_passive_collision_objects(
-    assets: Iterable[Asset | RigidObjectSet], include_background: bool = False
+    assets: Iterable[Asset | RigidObjectSet],
+    include_background: bool = False,
+    background_mesh_exclusions: Iterable[ObjectReference] = (),
 ) -> list[CollisionObject]:
     """Return relation-free scene assets that qualify as passive collision obstacles.
 
@@ -34,6 +36,8 @@ def get_passive_collision_objects(
         assets: Scene assets to scan for relation-free fixed objects.
         include_background: If True, include Background assets and aggregate all
             mesh-capable objects into a single FixedCollisionObject.
+        background_mesh_exclusions: Object references whose USD subtrees are omitted
+            from an aggregated parent Background mesh.
     """
     collision_objects: list[CollisionObject] = []
     for asset in assets:
@@ -76,5 +80,14 @@ def get_passive_collision_objects(
     ]
 
     if include_background:
-        return make_fixed_collision_objects(collision_objects)
-    return list(collision_objects)
+        excluded_prim_paths_by_object: dict[CollisionObject, list[str]] = {}
+        for reference in background_mesh_exclusions:
+            if reference.parent_asset in collision_object_set:
+                excluded_prim_paths_by_object.setdefault(reference.parent_asset, []).append(
+                    reference.get_prim_path_in_parent_usd()
+                )
+        return make_fixed_collision_objects(
+            collision_objects,
+            excluded_prim_paths_by_object=excluded_prim_paths_by_object,
+        )
+    return collision_objects