diff --git a/docs/pages/concepts/concept_object_placement.rst b/docs/pages/concepts/concept_object_placement.rst index 57ad1c0e9..14bea96cf 100644 --- a/docs/pages/concepts/concept_object_placement.rst +++ b/docs/pages/concepts/concept_object_placement.rst @@ -327,7 +327,7 @@ and add them to the scene: The builder automatically: - Collects all objects with at least one relation from the scene -- Adds pairwise ``NoCollision`` constraints between all non-anchor objects (if not already present) +- Automatically prevents overlap between placed objects (controlled by ``clearance_m`` in ``RelationSolverParams``) - Creates an ``ObjectPlacer`` and runs placement - Sets the solved poses on the objects before handing them to Isaac Lab @@ -405,15 +405,13 @@ Configuration placer = ObjectPlacer(params=ObjectPlacerParams( max_placement_attempts=10, - min_separation_m=0.01, placement_seed=42, verbose=True, )) Key parameters: -- ``max_placement_attempts`` (default ``5``): Number of solver restarts before giving up -- ``min_separation_m`` (default ``0.0``): Extra separation enforced during overlap validation +- ``max_placement_attempts`` (default ``10``): Number of solver restarts before giving up - ``placement_seed`` (default ``None``): Random seed for reproducible placements - ``on_relation_z_tolerance_m`` (default ``5e-3``): Tolerance for Z validation of ``On`` relations diff --git a/isaaclab_arena/environments/arena_env_builder.py b/isaaclab_arena/environments/arena_env_builder.py index 0dfad8298..147be50fa 100644 --- a/isaaclab_arena/environments/arena_env_builder.py +++ b/isaaclab_arena/environments/arena_env_builder.py @@ -29,7 +29,6 @@ from isaaclab_arena.metrics.recorder_manager_utils import metrics_to_recorder_manager_cfg from isaaclab_arena.relations.object_placer import ObjectPlacer from isaaclab_arena.relations.object_placer_params import ObjectPlacerParams -from isaaclab_arena.relations.relations import IsAnchor, NoCollision from isaaclab_arena.tasks.no_task import NoTask from isaaclab_arena.utils.configclass import combine_configclass_instances from isaaclab_arena.utils.isaaclab_utils.simulation_app import reapply_viewer_cfg @@ -72,25 +71,13 @@ def _get_objects_with_relations(self) -> list[Object | ObjectReference]: objects_with_relations.append(asset) return objects_with_relations - def _add_pairwise_no_collision(self, objects_with_relations: list[Object | ObjectReference]) -> None: - """Add NoCollision between every pair of non-anchor objects (if not already present).""" - non_anchors = [ - obj for obj in objects_with_relations if not any(isinstance(r, IsAnchor) for r in obj.get_relations()) - ] - for i, obj_a in enumerate(non_anchors): - for obj_b in non_anchors[i + 1 :]: - has_no_collision = any(isinstance(r, NoCollision) and r.parent is obj_b for r in obj_a.get_relations()) - if not has_no_collision: - obj_a.add_relation(NoCollision(obj_b)) - def _solve_relations(self) -> None: """Solve spatial relations for objects in the scene. This method: 1. Collects all objects from the scene that have relations - 2. Adds NoCollision between every pair of non-anchor objects (if not already present) - 3. Runs the ObjectPlacer to solve spatial constraints - 4. Applies solved positions to objects + 2. Runs the ObjectPlacer to solve spatial constraints + 3. Applies solved positions to objects """ # All objects with relations are subjects of the relation solving. objects_with_relations = self._get_objects_with_relations() @@ -99,8 +86,6 @@ def _solve_relations(self) -> None: print("No objects with relations found in scene. Skipping relation solving.") return - self._add_pairwise_no_collision(objects_with_relations) - # Run the ObjectPlacer (default on_relation_z_tolerance_m accommodates solver residual). placement_seed = getattr(self.args, "placement_seed", None) placer = ObjectPlacer(params=ObjectPlacerParams(placement_seed=placement_seed)) diff --git a/isaaclab_arena/relations/object_placer.py b/isaaclab_arena/relations/object_placer.py index f76a7db83..4edb7a42e 100644 --- a/isaaclab_arena/relations/object_placer.py +++ b/isaaclab_arena/relations/object_placer.py @@ -298,16 +298,29 @@ def _validate_no_overlap( self, positions: dict[Object | ObjectReference, tuple[float, float, float]], ) -> bool: - """Check that no two objects overlap in 3D (axis-aligned bbox with margin).""" + """Check that no two objects overlap in 3D (axis-aligned bbox with clearance margin). + + All pairs must be separated by at least ``clearance_m``. + Anchor-to-anchor pairs are skipped (both fixed, user's responsibility). + """ + anchor_objects = get_anchor_objects(list(positions.keys())) + anchor_set = set(anchor_objects) objects = list(positions.keys()) + # Small tolerance to avoid false positives at floating-point boundaries + # (e.g., On relation places child exactly at clearance_m above parent). + margin = max(0.0, self.params.solver_params.clearance_m - 1e-6) + for i in range(len(objects)): for j in range(i + 1, len(objects)): a, b = objects[i], objects[j] + if a in anchor_set and b in anchor_set: + continue + a_world = a.get_bounding_box().translated(positions[a]) b_world = b.get_bounding_box().translated(positions[b]) - if a_world.overlaps(b_world, margin=self.params.min_separation_m): + if a_world.overlaps(b_world, margin=margin): if self.params.verbose: print(f" Overlap between '{a.name}' and '{b.name}'") return False diff --git a/isaaclab_arena/relations/object_placer_params.py b/isaaclab_arena/relations/object_placer_params.py index 2acfc163e..ef40e9add 100644 --- a/isaaclab_arena/relations/object_placer_params.py +++ b/isaaclab_arena/relations/object_placer_params.py @@ -27,11 +27,6 @@ class ObjectPlacerParams: placement_seed: int | None = None """Random seed for reproducible placement. If None, uses current RNG state.""" - min_separation_m: float = 0.0 - """Minimum separation (meters) required between object bounding boxes. - Set to 0.0 to only reject actual overlaps. A small positive value (e.g. 0.005) - adds a safety margin between objects.""" - on_relation_z_tolerance_m: float = 5e-3 """Tolerance (meters) for On-relation Z validation. Valid Z band is extended to (parent_top - tolerance, parent_top + clearance_m + tolerance]. Default 5e-3 accommodates solver residual.""" diff --git a/isaaclab_arena/relations/relation_loss_strategies.py b/isaaclab_arena/relations/relation_loss_strategies.py index 320818e24..2739d3b9f 100644 --- a/isaaclab_arena/relations/relation_loss_strategies.py +++ b/isaaclab_arena/relations/relation_loss_strategies.py @@ -18,7 +18,7 @@ from isaaclab_arena.utils.bounding_box import AxisAlignedBoundingBox if TYPE_CHECKING: - from isaaclab_arena.relations.relations import AtPosition, NextTo, On, Relation, NoCollision + from isaaclab_arena.relations.relations import AtPosition, NextTo, On, Relation from isaaclab_arena.relations.relations import Side @@ -306,8 +306,10 @@ def compute_loss( return relation.relation_loss_weight * total_loss -class NoCollisionLossStrategy(RelationLossStrategy): - """Loss strategy for NoCollision relations. +class NoCollisionLossStrategy: + """Computes no-overlap loss between two bounding boxes. + + Internal utility used by RelationSolver. Not part of the public API. Computes loss based on: 1. X overlap: zero when child and parent are separated along X; else overlap length @@ -328,24 +330,24 @@ def __init__(self, slope: float = 10.0, debug: bool = False): def compute_loss( self, - relation: "NoCollision", + clearance_m: float, child_pos: torch.Tensor, child_bbox: AxisAlignedBoundingBox, parent_world_bbox: AxisAlignedBoundingBox, ) -> torch.Tensor: - """Compute loss for NoCollision relation. + """Compute no-overlap loss between child and parent bounding boxes. Args: - relation: NoCollision relation with relation_loss_weight. + clearance_m: Minimum required gap between child and parent bounding boxes. child_pos: Child object position tensor (x, y, z) in world coords. child_bbox: Child object local bounding box. parent_world_bbox: Parent bounding box in world coordinates. Returns: - Weighted loss tensor. + Loss tensor (scalar); positive when boxes overlap within clearance, zero otherwise. """ # Parent world extents from the world bounding box, expanded by clearance_m - c = relation.clearance_m + c = clearance_m parent_x_min = parent_world_bbox.min_point[0] - c parent_x_max = parent_world_bbox.max_point[0] + c parent_y_min = parent_world_bbox.min_point[1] - c @@ -381,7 +383,7 @@ def compute_loss( ) print(f" [NoCollision] volume={overlap_volume.item():.6f}, loss={total_loss.item():.6f}") - return relation.relation_loss_weight * total_loss + return total_loss class AtPositionLossStrategy(UnaryRelationLossStrategy): diff --git a/isaaclab_arena/relations/relation_solver.py b/isaaclab_arena/relations/relation_solver.py index 6515b89b5..6a208338e 100644 --- a/isaaclab_arena/relations/relation_solver.py +++ b/isaaclab_arena/relations/relation_solver.py @@ -8,7 +8,11 @@ import torch from typing import TYPE_CHECKING -from isaaclab_arena.relations.relation_loss_strategies import RelationLossStrategy, UnaryRelationLossStrategy +from isaaclab_arena.relations.relation_loss_strategies import ( + NoCollisionLossStrategy, + RelationLossStrategy, + UnaryRelationLossStrategy, +) from isaaclab_arena.relations.relation_solver_params import RelationSolverParams from isaaclab_arena.relations.relation_solver_state import RelationSolverState from isaaclab_arena.relations.relations import AtPosition, Relation, RelationBase @@ -37,6 +41,7 @@ def __init__( params: Solver configuration parameters. If None, uses defaults. """ self.params = params or RelationSolverParams() + self._no_collision_strategy = NoCollisionLossStrategy(slope=10000.0) self._last_loss_history: list[float] = [] self._last_position_history: list = [] @@ -60,6 +65,57 @@ def _get_strategy(self, relation: RelationBase) -> RelationLossStrategy | UnaryR ) return strategy + def _compute_no_overlap_loss(self, state: RelationSolverState, debug: bool = False) -> torch.Tensor: + """Compute pairwise no-overlap loss for all object pairs. + + For each non-anchor object, computes overlap loss against every other object. + Anchor-to-anchor pairs are skipped (neither moves). Gradient flows to the + non-anchor object in each pair. + + Args: + state: Current optimization state with object positions. + debug: If True, print detailed loss breakdown. + + Returns: + Total no-overlap loss tensor. + """ + total = torch.tensor(0.0) + clearance = self.params.clearance_m + all_objects = state.optimizable_objects + list(state.anchor_objects) + + # Each non-anchor appears as "child" (receives gradient). For two non-anchors A and B, + # both (A-as-child, B-as-other) and (B-as-child, A-as-other) are computed so both get + # gradient. The .item() calls below detach the "other" position from the graph -- this + # is intentional. Do not switch to single-pair (i < j) iteration without also making + # the loss differentiable w.r.t. both positions. + for child in state.optimizable_objects: + child_pos = state.get_position(child) + child_bbox = child.get_bounding_box() + + for other in all_objects: + if other is child: + continue + + if other in state.anchor_objects: + other_world_bbox = other.get_world_bounding_box() + else: + other_pos = state.get_position(other) + other_world_bbox = other.get_bounding_box().translated( + (other_pos[0].item(), other_pos[1].item(), other_pos[2].item()) + ) + + loss = self._no_collision_strategy.compute_loss( + clearance_m=clearance, + child_pos=child_pos, + child_bbox=child_bbox, + parent_world_bbox=other_world_bbox, + ) + if debug and loss.item() > 0: + print(f" NoOverlap({child.name}, {other.name}): {loss.item():.6f}") + total = total + loss + + return total + def _compute_total_loss(self, state: RelationSolverState, debug: bool = False) -> torch.Tensor: """Compute total loss from all relations using registered strategies. @@ -113,6 +169,9 @@ def _compute_total_loss(self, state: RelationSolverState, debug: bool = False) - total_loss = total_loss + loss + # Built-in pairwise no-overlap loss + total_loss = total_loss + self._compute_no_overlap_loss(state, debug=debug) + return total_loss def solve( diff --git a/isaaclab_arena/relations/relation_solver_params.py b/isaaclab_arena/relations/relation_solver_params.py index 13d9c31a4..a2a591aae 100644 --- a/isaaclab_arena/relations/relation_solver_params.py +++ b/isaaclab_arena/relations/relation_solver_params.py @@ -8,12 +8,11 @@ from isaaclab_arena.relations.relation_loss_strategies import ( AtPositionLossStrategy, NextToLossStrategy, - NoCollisionLossStrategy, OnLossStrategy, RelationLossStrategy, UnaryRelationLossStrategy, ) -from isaaclab_arena.relations.relations import AtPosition, NextTo, NoCollision, On, RelationBase +from isaaclab_arena.relations.relations import AtPosition, NextTo, On, RelationBase def _default_strategies() -> dict[type[RelationBase], RelationLossStrategy | UnaryRelationLossStrategy]: @@ -21,7 +20,6 @@ def _default_strategies() -> dict[type[RelationBase], RelationLossStrategy | Una return { NextTo: NextToLossStrategy(slope=10.0), On: OnLossStrategy(slope=100.0), - NoCollision: NoCollisionLossStrategy(slope=10000.0), AtPosition: AtPositionLossStrategy(slope=100.0), } @@ -45,8 +43,16 @@ class RelationSolverParams: save_position_history: bool = True """Save position snapshots during optimization for visualization/debugging. Disable to reduce memory.""" + clearance_m: float = 0.01 + """Minimum clearance (meters) between all object bounding boxes. The solver adds + no-overlap loss for every non-anchor pair and every non-anchor-to-anchor pair. + Set to 0.0 to only prevent actual overlaps (no safety margin).""" + # default_factory ensures each instance gets its own dict (mutable defaults are shared across instances) strategies: dict[type[RelationBase], RelationLossStrategy | UnaryRelationLossStrategy] = field( default_factory=_default_strategies ) """Loss strategies for each relation type. Override to customize loss computation.""" + + def __post_init__(self): + assert self.clearance_m >= 0.0, f"clearance_m must be non-negative, got {self.clearance_m}" diff --git a/isaaclab_arena/relations/relations.py b/isaaclab_arena/relations/relations.py index 76b0e502d..c7f8644ba 100644 --- a/isaaclab_arena/relations/relations.py +++ b/isaaclab_arena/relations/relations.py @@ -117,33 +117,6 @@ def __init__( self.clearance_m = clearance_m -class NoCollision(Relation): - """Represents a 'no collision' relationship between two objects. - - This relation specifies that the child and parent bounding boxes must not - overlap. Adding NoCollision on one side is enough; the solver counts each - unordered pair once. - - Note: Loss computation is handled by NoCollisionLossStrategy in relation_loss_strategies.py. - """ - - def __init__( - self, - parent: Object | ObjectReference, - relation_loss_weight: float = 1.0, - clearance_m: float = 0.01, - ): - """ - Args: - parent: The other object that this object must not collide with. - relation_loss_weight: Weight for the relationship loss function. - clearance_m: Minimum clearance between bounding boxes in meters (default: 1cm). - """ - super().__init__(parent, relation_loss_weight) - assert clearance_m >= 0.0, f"clearance_m must be non-negative, got {clearance_m}" - self.clearance_m = clearance_m - - class IsAnchor(RelationBase): """Marker indicating this object is an anchor for relation solving. diff --git a/isaaclab_arena/tests/test_no_collision_loss.py b/isaaclab_arena/tests/test_no_collision_loss.py index af380180e..5268dac76 100644 --- a/isaaclab_arena/tests/test_no_collision_loss.py +++ b/isaaclab_arena/tests/test_no_collision_loss.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: Apache-2.0 -"""Tests for NoCollision loss strategy and RelationSolver with NoCollision relations.""" +"""Tests for NoCollisionLossStrategy and built-in solver no-overlap behavior.""" import torch @@ -11,7 +11,7 @@ from isaaclab_arena.relations.relation_loss_strategies import NoCollisionLossStrategy from isaaclab_arena.relations.relation_solver import RelationSolver from isaaclab_arena.relations.relation_solver_params import RelationSolverParams -from isaaclab_arena.relations.relations import IsAnchor, NoCollision, On +from isaaclab_arena.relations.relations import IsAnchor, On from isaaclab_arena.utils.bounding_box import AxisAlignedBoundingBox from isaaclab_arena.utils.pose import Pose @@ -32,8 +32,8 @@ def _create_table() -> DummyObject: ) -def _create_no_collision_scene() -> tuple[DummyObject, DummyObject, DummyObject]: - """Create table + two boxes with On(table) and NoCollision between boxes (for solver tests).""" +def _create_scene_no_explicit_collision_relations() -> tuple[DummyObject, DummyObject, DummyObject]: + """Create table + two boxes with On(table) but NO explicit NoCollision. Solver should handle it.""" table = _create_table() table.set_initial_pose(Pose(position_xyz=(0.0, 0.0, 0.0), rotation_xyzw=(0.0, 0.0, 0.0, 1.0))) table.add_relation(IsAnchor()) @@ -41,7 +41,6 @@ def _create_no_collision_scene() -> tuple[DummyObject, DummyObject, DummyObject] box_b = _create_box("box_b") box_a.add_relation(On(table, clearance_m=0.01)) box_b.add_relation(On(table, clearance_m=0.01)) - box_a.add_relation(NoCollision(box_b)) return table, box_a, box_b @@ -54,59 +53,47 @@ def test_no_collision_zero_loss_when_fully_separated(): """Test that NoCollision loss is zero when AABBs do not overlap on any axis.""" box_a = _create_box("box_a") box_b = _create_box("box_b") - relation = NoCollision(box_b) strategy = NoCollisionLossStrategy(slope=10.0) # Child at origin -> world X [0, 0.2]. Parent at x=1 -> world X [1, 1.2]. No X overlap => volume 0. child_pos = torch.tensor([0.0, 0.0, 0.0]) parent_world_bbox = box_b.get_bounding_box().translated((1.0, 0.0, 0.0)) - loss = strategy.compute_loss(relation, child_pos, box_a.bounding_box, parent_world_bbox) - assert torch.isclose(loss, torch.tensor(0.0), atol=1e-5) - - -def test_no_collision_zero_loss_when_separated_on_one_axis_only(): - """Test that NoCollision loss is zero when separated on one axis (overlap_x=0 => volume=0).""" - box_a = _create_box("box_a") - box_b = _create_box("box_b") - relation = NoCollision(box_b) - strategy = NoCollisionLossStrategy(slope=10.0) - - # Child X [0, 0.2], parent X [0.5, 0.7] -> no X overlap. Y and Z overlapping. - child_pos = torch.tensor([0.0, 0.0, 0.0]) - parent_world_bbox = box_b.get_bounding_box().translated((0.5, 0.0, 0.0)) - - loss = strategy.compute_loss(relation, child_pos, box_a.bounding_box, parent_world_bbox) + loss = strategy.compute_loss( + clearance_m=0.01, child_pos=child_pos, child_bbox=box_a.bounding_box, parent_world_bbox=parent_world_bbox + ) assert torch.isclose(loss, torch.tensor(0.0), atol=1e-5) -def test_no_collision_zero_loss_when_just_touching(): - """Test that NoCollision loss is zero when intervals just touch (clearance_m=0).""" +def test_no_collision_zero_loss_when_just_touching_no_clearance(): + """Test that NoCollision loss is zero when intervals just touch and clearance_m=0.0.""" box_a = _create_box("box_a") box_b = _create_box("box_b") - relation = NoCollision(box_b, clearance_m=0.0) strategy = NoCollisionLossStrategy(slope=10.0) # Child X [0, 0.2], parent X [0.2, 0.4]. Just touching. child_pos = torch.tensor([0.0, 0.0, 0.0]) parent_world_bbox = box_b.get_bounding_box().translated((0.2, 0.0, 0.0)) - loss = strategy.compute_loss(relation, child_pos, box_a.bounding_box, parent_world_bbox) + loss = strategy.compute_loss( + clearance_m=0.0, child_pos=child_pos, child_bbox=box_a.bounding_box, parent_world_bbox=parent_world_bbox + ) assert torch.isclose(loss, torch.tensor(0.0), atol=1e-5) -def test_no_collision_positive_loss_when_just_touching(): - """Test that NoCollision loss is positive when just touching with default clearance.""" +def test_no_collision_positive_loss_when_just_touching_with_clearance(): + """Test that NoCollision loss is positive when just touching with clearance_m=0.01.""" box_a = _create_box("box_a") box_b = _create_box("box_b") - relation = NoCollision(box_b) strategy = NoCollisionLossStrategy(slope=10.0) - # Child X [0, 0.2], parent X [0.2, 0.4]. Just touching; default clearance expands parent so overlap > 0. + # Child X [0, 0.2], parent X [0.2, 0.4]. Just touching; clearance expands parent so overlap > 0. child_pos = torch.tensor([0.0, 0.0, 0.0]) parent_world_bbox = box_b.get_bounding_box().translated((0.2, 0.0, 0.0)) - loss = strategy.compute_loss(relation, child_pos, box_a.bounding_box, parent_world_bbox) + loss = strategy.compute_loss( + clearance_m=0.01, child_pos=child_pos, child_bbox=box_a.bounding_box, parent_world_bbox=parent_world_bbox + ) assert loss > 0.0 @@ -114,14 +101,15 @@ def test_no_collision_positive_loss_when_3d_overlap(): """Test that NoCollision loss is positive when AABBs overlap in all three axes.""" box_a = _create_box("box_a") box_b = _create_box("box_b") - relation = NoCollision(box_b) strategy = NoCollisionLossStrategy(slope=10.0) # Child at (0.1, 0.1, 0), parent at (0.05, 0.05, 0) -> overlap in X, Y, Z. child_pos = torch.tensor([0.1, 0.1, 0.0]) parent_world_bbox = box_b.get_bounding_box().translated((0.05, 0.05, 0.0)) - loss = strategy.compute_loss(relation, child_pos, box_a.bounding_box, parent_world_bbox) + loss = strategy.compute_loss( + clearance_m=0.01, child_pos=child_pos, child_bbox=box_a.bounding_box, parent_world_bbox=parent_world_bbox + ) assert loss > 0.0 @@ -129,39 +117,25 @@ def test_no_collision_loss_scales_with_slope(): """Test that NoCollision loss scales with slope (loss = slope * overlap_volume).""" box_a = _create_box("box_a") box_b = _create_box("box_b") - relation = NoCollision(box_b) strategy_slope_10 = NoCollisionLossStrategy(slope=10.0) strategy_slope_20 = NoCollisionLossStrategy(slope=20.0) child_pos = torch.tensor([0.1, 0.1, 0.0]) parent_world_bbox = box_b.get_bounding_box().translated((0.05, 0.05, 0.0)) - loss_10 = strategy_slope_10.compute_loss(relation, child_pos, box_a.bounding_box, parent_world_bbox) - loss_20 = strategy_slope_20.compute_loss(relation, child_pos, box_a.bounding_box, parent_world_bbox) + loss_10 = strategy_slope_10.compute_loss( + clearance_m=0.01, child_pos=child_pos, child_bbox=box_a.bounding_box, parent_world_bbox=parent_world_bbox + ) + loss_20 = strategy_slope_20.compute_loss( + clearance_m=0.01, child_pos=child_pos, child_bbox=box_a.bounding_box, parent_world_bbox=parent_world_bbox + ) assert torch.isclose(loss_20, 2.0 * loss_10, rtol=1e-5) -def test_no_collision_loss_scales_with_relation_weight(): - """Test that NoCollision loss is scaled by relation_loss_weight.""" - box_a = _create_box("box_a") - box_b = _create_box("box_b") - relation_1 = NoCollision(box_b, relation_loss_weight=1.0) - relation_2 = NoCollision(box_b, relation_loss_weight=2.0) - strategy = NoCollisionLossStrategy(slope=10.0) - - child_pos = torch.tensor([0.1, 0.1, 0.0]) - parent_world_bbox = box_b.get_bounding_box().translated((0.05, 0.05, 0.0)) - - loss_1 = strategy.compute_loss(relation_1, child_pos, box_a.bounding_box, parent_world_bbox) - loss_2 = strategy.compute_loss(relation_2, child_pos, box_a.bounding_box, parent_world_bbox) - assert torch.isclose(loss_2, 2.0 * loss_1, rtol=1e-5) - - def test_no_collision_loss_volume_formula(): """Test that NoCollision loss equals slope * overlap volume for known overlap (clearance_m=0).""" box_a = _create_box("box_a", size=0.2) box_b = _create_box("box_b", size=0.2) - relation = NoCollision(box_b, relation_loss_weight=1.0, clearance_m=0.0) strategy = NoCollisionLossStrategy(slope=10.0) child_pos = torch.tensor([0.1, 0.1, 0.1]) @@ -169,18 +143,20 @@ def test_no_collision_loss_volume_formula(): # Overlap [0.15, 0.3]^3, volume 0.15^3. Expected loss = 10 * 0.15^3. expected_loss = 10.0 * (0.15**3) - loss = strategy.compute_loss(relation, child_pos, box_a.bounding_box, parent_world_bbox) + loss = strategy.compute_loss( + clearance_m=0.0, child_pos=child_pos, child_bbox=box_a.bounding_box, parent_world_bbox=parent_world_bbox + ) assert torch.isclose(loss, torch.tensor(expected_loss), rtol=1e-4) # ============================================================================= -# RelationSolver with NoCollision tests +# RelationSolver built-in no-overlap tests (no explicit NoCollision relations) # ============================================================================= -def test_relation_solver_no_collision_produces_separated_positions(): - """Test that RelationSolver with NoCollision and On(table) places objects so they do not overlap.""" - table, box_a, box_b = _create_no_collision_scene() +def test_solver_separates_overlapping_objects_without_explicit_no_collision(): + """Solver should separate overlapping boxes using built-in no-overlap loss (no NoCollision relations).""" + table, box_a, box_b = _create_scene_no_explicit_collision_relations() objects = [table, box_a, box_b] initial_positions = { table: (0.0, 0.0, 0.0), @@ -188,7 +164,7 @@ def test_relation_solver_no_collision_produces_separated_positions(): box_b: (0.25, 0.25, 0.11), } - solver_params = RelationSolverParams(max_iters=200, convergence_threshold=1e-3) + solver_params = RelationSolverParams(max_iters=400, convergence_threshold=1e-4, verbose=False) solver = RelationSolver(params=solver_params) result = solver.solve(objects=objects, initial_positions=initial_positions) @@ -200,17 +176,89 @@ def test_relation_solver_no_collision_produces_separated_positions(): assert not bbox_a.overlaps(bbox_b), f"Solver should separate boxes; box_a at {pos_a}, box_b at {pos_b}" -def test_relation_solver_no_collision_same_inputs_reproducible(): - """Test that RelationSolver with same initial positions and NoCollision yields identical positions.""" - table1, box_a1, box_b1 = _create_no_collision_scene() +def test_solver_respects_clearance_m(): + """With clearance_m=0.05, solved boxes should be at least 5 cm apart.""" + table, box_a, box_b = _create_scene_no_explicit_collision_relations() + objects = [table, box_a, box_b] + initial_positions = { + table: (0.0, 0.0, 0.0), + box_a: (0.2, 0.2, 0.11), + box_b: (0.25, 0.25, 0.11), + } + + solver_params = RelationSolverParams(max_iters=800, convergence_threshold=1e-6, clearance_m=0.05, verbose=False) + solver = RelationSolver(params=solver_params) + result = solver.solve(objects=objects, initial_positions=initial_positions) + + pos_a = result[box_a] + pos_b = result[box_b] + bbox_a = box_a.get_bounding_box().translated(pos_a) + bbox_b = box_b.get_bounding_box().translated(pos_b) + + assert not bbox_a.overlaps( + bbox_b, margin=0.05 + ), f"Boxes should be at least 5 cm apart; box_a at {pos_a}, box_b at {pos_b}" + + +def test_negative_clearance_m_raises(): + """Negative clearance_m should be rejected.""" + import pytest + + with pytest.raises(AssertionError): + RelationSolverParams(clearance_m=-0.01) + + +def test_validation_accepts_on_parent_overlap(): + """Non-anchor sitting On(anchor) should pass validation even though bboxes overlap.""" + from isaaclab_arena.relations.object_placer import ObjectPlacer + from isaaclab_arena.relations.object_placer_params import ObjectPlacerParams + + table = _create_table() # bbox (0,0,0)-(1,1,0.1) + table.set_initial_pose(Pose(position_xyz=(0.0, 0.0, 0.0), rotation_xyzw=(0.0, 0.0, 0.0, 1.0))) + table.add_relation(IsAnchor()) + box = _create_box("box") # bbox (0,0,0)-(0.2,0.2,0.2) + box.add_relation(On(table, clearance_m=0.01)) + + # Box at z=0.11 (table top + clearance). Its bbox goes from 0.11 to 0.31. + # Table bbox goes to 0.1. With clearance_m=0.01, expanded table goes to 0.11. + # These just touch — the On-parent pair should be skipped. + positions = {table: (0.0, 0.0, 0.0), box: (0.4, 0.4, 0.11)} + + placer = ObjectPlacer(ObjectPlacerParams()) + assert placer._validate_no_overlap(positions) + + +def test_validation_rejects_non_anchor_overlap(): + """Two overlapping non-anchor boxes should fail validation.""" + from isaaclab_arena.relations.object_placer import ObjectPlacer + from isaaclab_arena.relations.object_placer_params import ObjectPlacerParams + + table = _create_table() + table.set_initial_pose(Pose(position_xyz=(0.0, 0.0, 0.0), rotation_xyzw=(0.0, 0.0, 0.0, 1.0))) + table.add_relation(IsAnchor()) + box_a = _create_box("box_a") + box_b = _create_box("box_b") + box_a.add_relation(On(table, clearance_m=0.01)) + box_b.add_relation(On(table, clearance_m=0.01)) + + # Both boxes at nearly the same position — they overlap + positions = {table: (0.0, 0.0, 0.0), box_a: (0.3, 0.3, 0.11), box_b: (0.35, 0.35, 0.11)} + + placer = ObjectPlacer(ObjectPlacerParams()) + assert not placer._validate_no_overlap(positions) + + +def test_solver_no_overlap_reproducible(): + """Same inputs should produce identical outputs (deterministic solver).""" + table1, box_a1, box_b1 = _create_scene_no_explicit_collision_relations() initial = (0.0, 0.0, 0.0), (0.3, 0.3, 0.11), (0.6, 0.6, 0.11) initial_positions1 = {table1: initial[0], box_a1: initial[1], box_b1: initial[2]} - solver_params = RelationSolverParams(max_iters=50) + solver_params = RelationSolverParams(max_iters=50, verbose=False) solver1 = RelationSolver(params=solver_params) result1 = solver1.solve(objects=[table1, box_a1, box_b1], initial_positions=initial_positions1) - table2, box_a2, box_b2 = _create_no_collision_scene() + table2, box_a2, box_b2 = _create_scene_no_explicit_collision_relations() initial_positions2 = {table2: initial[0], box_a2: initial[1], box_b2: initial[2]} solver2 = RelationSolver(params=solver_params) result2 = solver2.solve(objects=[table2, box_a2, box_b2], initial_positions=initial_positions2) diff --git a/isaaclab_arena_environments/gr1_table_multi_object_no_collision_environment.py b/isaaclab_arena_environments/gr1_table_multi_object_no_collision_environment.py index 3829c6006..92d76321d 100644 --- a/isaaclab_arena_environments/gr1_table_multi_object_no_collision_environment.py +++ b/isaaclab_arena_environments/gr1_table_multi_object_no_collision_environment.py @@ -4,8 +4,8 @@ # SPDX-License-Identifier: Apache-2.0 """ -Table + multi-object NoCollision environment. Office table with objects placed via -On(table) and pairwise NoCollision (relation solver). Includes a robot (e.g. GR1). +Table + multi-object no-overlap environment. Office table with objects placed via +On(table) with automatic no-overlap handling (relation solver). Includes a robot (e.g. GR1). No task — suitable for policy_runner with zero_action or any policy. Example: @@ -25,12 +25,12 @@ "mug", "brown_box", "dex_cube", -] # Default objects on table (On + pairwise NoCollision) +] # Default objects on table (On + automatic no-overlap) class GR1TableMultiObjectNoCollisionEnvironment(ExampleEnvironmentBase): """ - Table-based scene with multiple objects (On(table) + NoCollision) and a robot. + Table-based scene with multiple objects (On(table) + automatic no-overlap) and a robot. Layout is solved by ArenaEnvBuilder default relation solving; reset uses asset events. """ @@ -79,7 +79,7 @@ def get_env(self, args_cli: argparse.Namespace): # -> IsaacLabArenaEnvironment: obj = self.asset_registry.get_asset_by_name(name)() obj.add_relation(On(tabletop_reference)) placeable_assets.append(obj) - # NoCollision between all pairs is added automatically by ArenaEnvBuilder before solving. + # No-overlap between all pairs is handled automatically by the solver. if args_cli.teleop_device is not None: teleop_device = self.device_registry.get_device_by_name(args_cli.teleop_device)() @@ -111,7 +111,7 @@ def add_cli_args(parser: argparse.ArgumentParser) -> None: nargs="*", type=str, default=None, - help=f"Object names to spawn on the table (On + NoCollision). Default: {' '.join(DEFAULT_TABLE_OBJECTS)}", + help=f"Object names to spawn on the table (On + no-overlap). Default: {' '.join(DEFAULT_TABLE_OBJECTS)}", ) parser.add_argument("--embodiment", type=str, default="gr1_joint", help="Robot embodiment to use") parser.add_argument("--teleop_device", type=str, default=None, help="Teleoperation device to use") diff --git a/isaaclab_arena_examples/relations/dummy_object_placer_notebook.py b/isaaclab_arena_examples/relations/dummy_object_placer_notebook.py index b4bec9bc9..6eee41a18 100644 --- a/isaaclab_arena_examples/relations/dummy_object_placer_notebook.py +++ b/isaaclab_arena_examples/relations/dummy_object_placer_notebook.py @@ -15,7 +15,7 @@ from isaaclab_arena.relations.object_placer_params import ObjectPlacerParams from isaaclab_arena.relations.relation_solver import RelationSolver from isaaclab_arena.relations.relation_solver_params import RelationSolverParams -from isaaclab_arena.relations.relations import IsAnchor, NextTo, NoCollision, On, Side, get_anchor_objects +from isaaclab_arena.relations.relations import IsAnchor, NextTo, On, Side, get_anchor_objects from isaaclab_arena.utils.bounding_box import AxisAlignedBoundingBox from isaaclab_arena.utils.pose import Pose from isaaclab_arena_examples.relations.relation_solver_visualizer import RelationSolverVisualizer @@ -187,12 +187,9 @@ def run_dummy_no_collision_demo(): bounding_box=AxisAlignedBoundingBox(min_point=(0.0, 0.0, 0.0), max_point=(0.18, 0.1, 0.06)), ) - # Boxes on table with pairwise NoCollision (one-sided per pair is enough) + # Boxes on table; the solver handles no-overlap internally for box in (box_a, box_b, box_c): box.add_relation(On(table, clearance_m=0.01)) - box_a.add_relation(NoCollision(box_b)) - box_a.add_relation(NoCollision(box_c)) - box_b.add_relation(NoCollision(box_c)) all_objects = [table, box_a, box_b, box_c] @@ -238,7 +235,7 @@ def run_dummy_no_collision_demo(): # # Run multi-anchor demo run_dummy_multi_anchor_demo() - # Run NoCollision demo + # Run no-overlap demo run_dummy_no_collision_demo() # %% diff --git a/isaaclab_arena_examples/relations/isaac_sim_no_collision_notebook.py b/isaaclab_arena_examples/relations/isaac_sim_no_collision_notebook.py index e8799a812..46276a7a6 100644 --- a/isaaclab_arena_examples/relations/isaac_sim_no_collision_notebook.py +++ b/isaaclab_arena_examples/relations/isaac_sim_no_collision_notebook.py @@ -8,7 +8,7 @@ # pyright: reportArgumentType=false, reportCallIssue=false, reportAttributeAccessIssue=false -"""Example notebook demonstrating NoCollision with real Isaac Sim objects.""" +"""Example notebook demonstrating no-overlap placement with real Isaac Sim objects.""" # NOTE: When running as a notebook, first run this cell to launch the simulation app: import pinocchio # noqa: F401 @@ -30,10 +30,10 @@ def run_isaac_sim_no_collision_demo( reset_every_n_steps: int = 100, hold_overlapping_steps: int = 150, ): - """Run the NoCollision demo with Isaac Sim objects. + """Run the no-overlap placement demo with Isaac Sim objects. Three objects start overlapping on the table; the relation solver places them - so they satisfy On(table) and pairwise NoCollision. After each reset, overlapping + so they satisfy On(table) with automatic no-overlap handling. After each reset, overlapping pose is shown for hold_overlapping_steps frames (render only), then the solver runs and results are displayed. @@ -51,7 +51,7 @@ def run_isaac_sim_no_collision_demo( from isaaclab_arena.environments.arena_env_builder import ArenaEnvBuilder from isaaclab_arena.environments.isaaclab_arena_environment import IsaacLabArenaEnvironment from isaaclab_arena.relations.object_placer import ObjectPlacer - from isaaclab_arena.relations.relations import IsAnchor, NoCollision, On + from isaaclab_arena.relations.relations import IsAnchor, On from isaaclab_arena.scene.scene import Scene from isaaclab_arena.utils.pose import Pose @@ -82,10 +82,6 @@ def run_isaac_sim_no_collision_demo( tomato_soup_can.add_relation(On(tabletop_reference, clearance_m=0.02)) tomato_soup_can.set_initial_pose(same_pose) - cracker_box.add_relation(NoCollision(mug)) - cracker_box.add_relation(NoCollision(tomato_soup_can)) - mug.add_relation(NoCollision(tomato_soup_can)) - scene = Scene(assets=[ground_plane, table_background, tabletop_reference, cracker_box, mug, tomato_soup_can, light]) isaaclab_arena_environment = IsaacLabArenaEnvironment( name="isaac_sim_no_collision_demo", @@ -170,7 +166,7 @@ def apply_overlapping_pose_then_solve_and_display(): def smoke_test_isaac_sim_no_collision(simulation_app: SimulationApp) -> bool: - """Smoke test: run NoCollision demo with Isaac Sim objects (minimal steps).""" + """Smoke test: run no-overlap placement demo with Isaac Sim objects (minimal steps).""" run_isaac_sim_no_collision_demo(num_steps=2) return True diff --git a/isaaclab_arena_examples/tests/test_relation_solver_examples.py b/isaaclab_arena_examples/tests/test_relation_solver_examples.py index e04bdbd9a..35ff95e74 100644 --- a/isaaclab_arena_examples/tests/test_relation_solver_examples.py +++ b/isaaclab_arena_examples/tests/test_relation_solver_examples.py @@ -38,9 +38,9 @@ def test_isaac_sim_object_placer_smoke(): def test_isaac_sim_no_collision_smoke(): - """Smoke test: verify the Isaac Sim NoCollision notebook runs without errors.""" + """Smoke test: verify the Isaac Sim no-overlap notebook runs without errors.""" from isaaclab_arena.tests.utils.subprocess import run_simulation_app_function from isaaclab_arena_examples.relations.isaac_sim_no_collision_notebook import smoke_test_isaac_sim_no_collision result = run_simulation_app_function(smoke_test_isaac_sim_no_collision) - assert result, "Isaac Sim NoCollision smoke test failed" + assert result, "Isaac Sim no-overlap smoke test failed"