MVP single object placement api aabb

isaaclab_arena/assets/object.py

@@ -3,6 +3,8 @@
 #
 # SPDX-License-Identifier: Apache-2.0
 
+from typing import TYPE_CHECKING, Union
+
 from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg
 from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg
 
@@ -11,10 +13,13 @@
 from isaaclab_arena.utils.pose import Pose
 from isaaclab_arena.utils.usd_helpers import has_light, open_stage
 
+if TYPE_CHECKING:
+    from isaaclab_arena.assets.relations import Relation
+
 
 class Object(ObjectBase):
     """
-    Encapsulates the pick-up object config for a pick-and-place environment.
+    A general-purpose object wrapper that encapsulates asset configurations for simulation environments.
     """
 
     def __init__(
@@ -24,7 +29,7 @@ def __init__(
         object_type: ObjectType | None = None,
         usd_path: str | None = None,
         scale: tuple[float, float, float] = (1.0, 1.0, 1.0),
-        initial_pose: Pose | None = None,
+        initial_pose: Union[Pose, "Relation", None] = None,
         **kwargs,
     ):
         if object_type is not ObjectType.SPAWNER:
@@ -38,9 +43,12 @@ def __init__(
         self.initial_pose = initial_pose
         self.object_cfg = self._init_object_cfg()
 
-    def set_initial_pose(self, pose: Pose) -> None:
+    def set_initial_pose(self, pose: Union[Pose, "Relation"]) -> None:
         self.initial_pose = pose
-        self.object_cfg = self._add_initial_pose_to_cfg(self.object_cfg)
+
+        # TODO(cvolk): How to do it properly?
+        # TODO(cvolk): Does the object_cfg need the initial pose here already?
+        # self.object_cfg = self._add_initial_pose_to_cfg(self.object_cfg)
 
     def get_initial_pose(self) -> Pose | None:
         return self.initial_pose

isaaclab_arena/assets/object_base.py

@@ -6,14 +6,17 @@
 import torch
 from abc import ABC, abstractmethod
 from enum import Enum
-from typing import Any
+from typing import TYPE_CHECKING, Any
 
 from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg
 from isaaclab.envs import ManagerBasedEnv
 from isaaclab.sensors.contact_sensor.contact_sensor_cfg import ContactSensorCfg
 
 from isaaclab_arena.assets.asset import Asset
 
+if TYPE_CHECKING:
+    from isaaclab_arena.utils.pose import Pose
+
 
 class ObjectType(Enum):
     BASE = "base"
@@ -110,3 +113,148 @@ def _generate_base_cfg(self) -> AssetBaseCfg:
     def _generate_spawner_cfg(self) -> AssetBaseCfg:
         # Object Subclasses must implement this method
         pass
+
+    # Spatial Relationship Methods
+    def on_top_of(
+        self,
+        target: "ObjectBase",
+        clearance: float = 0.0,
+        x_offset: float = 0.0,
+        y_offset: float = 0.0,
+    ) -> "ObjectBase":
+        """
+        Place this object on top of a target object.
+
+        This method automatically computes the appropriate pose to place this object
+        on top of the target object, accounting for both objects' geometries.
+
+        Args:
+            target: The target object to place this object on top of.
+            clearance: Additional vertical clearance between objects (default: 0.0).
+            x_offset: Horizontal offset in x direction from center (default: 0.0).
+            y_offset: Horizontal offset in y direction from center (default: 0.0).
+
+        Returns:
+            Self, to allow method chaining.
+
+        Example:
+            ```python
+            table = asset_registry.get_asset_by_name("table")()
+            box = asset_registry.get_asset_by_name("cracker_box")()
+            box.on_top_of(table)
+            scene = Scene(assets=[table, box])
+            ```
+        """
+        from isaaclab_arena.utils.spatial_relationships import compute_bounding_box_from_usd, compute_on_top_of_pose
+
+        # Get bounding boxes for both objects
+        # We need to access the usd_path and scale from the concrete Object class
+        # For now, we'll use getattr to handle this polymorphically
+        object_usd_path = getattr(self, "usd_path", None)
+        object_scale = getattr(self, "scale", (1.0, 1.0, 1.0))
+        target_usd_path = getattr(target, "usd_path", None)
+        target_scale = getattr(target, "scale", (1.0, 1.0, 1.0))
+
+        if object_usd_path is None:
+            raise ValueError(f"Object {self.name} does not have a usd_path attribute")
+        if target_usd_path is None:
+            raise ValueError(f"Target object {target.name} does not have a usd_path attribute")
+
+        # Get the target's current pose (if set)
+        target_pose = getattr(target, "initial_pose", None)
+
+        # Compute bounding boxes
+        object_bbox = compute_bounding_box_from_usd(object_usd_path, scale=object_scale)
+        target_bbox = compute_bounding_box_from_usd(target_usd_path, scale=target_scale, pose=target_pose)
+
+        # Compute the placement pose
+        placement_pose = compute_on_top_of_pose(
+            object_bbox=object_bbox,
+            target_bbox=target_bbox,
+            clearance=clearance,
+            x_offset=x_offset,
+            y_offset=y_offset,
+        )
+
+        # Set the initial pose on this object
+        self.set_initial_pose(placement_pose)
+
+    def next_to(
+        self,
+        target: "ObjectBase",
+        side: str = "right",
+        clearance: float = 0.01,
+        align_bottom: bool = True,
+    ) -> "ObjectBase":
+        """
+        Place this object next to a target object.
+
+        This method automatically computes the appropriate pose to place this object
+        beside the target object, accounting for both objects' geometries.
+
+        **Important**: Directions are in the world coordinate frame, not relative to
+        the target object's orientation:
+        - "right" = -Y world direction
+        - "left" = +Y world direction
+        - "front" = -X world direction
+        - "back" = +X world direction
+
+        Args:
+            target: The target object to place this object next to.
+            side: Which side to place the object ("left", "right", "front", "back").
+                  These directions are in world frame, not relative to target's orientation.
+            clearance: Horizontal clearance between objects (default: 0.01).
+            align_bottom: If True, align bottoms; if False, center vertically (default: True).
+
+        Returns:
+            Self, to allow method chaining.
+
+        Example:
+            ```python
+            laptop = asset_registry.get_asset_by_name("laptop")()
+            mug = asset_registry.get_asset_by_name("mug")()
+            # Places mug in -Y direction from laptop (world frame)
+            mug.next_to(laptop, side="right")
+            scene = Scene(assets=[laptop, mug])
+            ```
+
+        Note:
+            This is a limitation of the MVP. Future versions may support
+            placement relative to the target object's local coordinate frame.
+        """
+        from isaaclab_arena.utils.spatial_relationships import compute_bounding_box_from_usd, compute_next_to_pose
+
+        # Get bounding boxes for both objects
+        object_usd_path = getattr(self, "usd_path", None)
+        object_scale = getattr(self, "scale", (1.0, 1.0, 1.0))
+        target_usd_path = getattr(target, "usd_path", None)
+        target_scale = getattr(target, "scale", (1.0, 1.0, 1.0))
+
+        if object_usd_path is None:
+            raise ValueError(f"Object {self.name} does not have a usd_path attribute")
+        if target_usd_path is None:
+            raise ValueError(f"Target object {target.name} does not have a usd_path attribute")
+
+        # Get the target's current pose (if set)
+        target_pose = getattr(target, "initial_pose", None)
+
+        # Compute bounding boxes
+        object_bbox = compute_bounding_box_from_usd(object_usd_path, scale=object_scale)
+        target_bbox = compute_bounding_box_from_usd(target_usd_path, scale=target_scale, pose=target_pose)
+
+        # Compute the placement pose
+        placement_pose = compute_next_to_pose(
+            object_bbox=object_bbox,
+            target_bbox=target_bbox,
+            side=side,
+            clearance=clearance,
+            align_bottom=align_bottom,
+        )
+
+        # Set the initial pose on this object
+        self.set_initial_pose(placement_pose)
+
+    @abstractmethod
+    def set_initial_pose(self, pose: "Pose") -> None:
+        """Set the initial pose of the object. Must be implemented by subclasses."""
+        pass

isaaclab_arena/assets/relations.py

@@ -0,0 +1,51 @@
+# Copyright (c) 2025, The Isaac Lab Arena Project Developers (https://github.com/isaac-sim/IsaacLab-Arena/blob/main/CONTRIBUTORS.md).
+# All rights reserved.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+from abc import abstractmethod
+from typing import TYPE_CHECKING
+
+from isaaclab_arena.assets.asset import Asset
+from isaaclab_arena.utils.pose import Pose
+
+# Use TYPE_CHECKING to avoid circular import at runtime
+if TYPE_CHECKING:
+    from isaaclab_arena.scene.scene import Scene
+
+
+class Relation:
+    def __init__(self, parent_asset: Asset, child_asset: Asset):
+        self.parent_asset: Asset = parent_asset
+        self.child_asset: Asset = child_asset
+
+    @abstractmethod
+    def resolve(self):
+        pass
+
+
+class OnRelation(Relation):
+    def resolve(self):
+        # Get the pose of the parent
+        # Resolve the pose of the child relative to the parent
+        # Add to world frame
+        # return the pose in world frame
+        print(f"Resolving on relation between {self.parent_asset.name} and {self.child_asset.name}")
+        return Pose(position_xyz=(0.0, 0.0, 0.0), rotation_wxyz=(1.0, 0.0, 0.0, 0.0))
+
+
+class RelationResolver:
+    def __init__(self, scene: "Scene"):
+        self.scene = scene
+
+    # This would actually set the initial_poses
+    def resolve_relations(self):
+        # Get assets
+        for asset in self.scene.assets.values():
+            if isinstance(asset.initial_pose, Relation):
+                print(f"Asset {asset.name} has initial pose of type {type(asset.initial_pose)}")
+                pose = asset.initial_pose.resolve()
+                print(f"Now setting initial pose of asset {asset.name} to {pose}")
+                asset.set_initial_pose(pose)
+            else:
+                print(f"Asset {asset.name} has no initial pose")

isaaclab_arena/examples/compile_env_notebook.py

@@ -15,6 +15,7 @@
 simulation_app = AppLauncher()
 
 from isaaclab_arena.assets.asset_registry import AssetRegistry
+from isaaclab_arena.assets.relations import OnRelation, RelationResolver
 from isaaclab_arena.cli.isaaclab_arena_cli import get_isaaclab_arena_cli_parser
 from isaaclab_arena.environments.arena_env_builder import ArenaEnvBuilder
 from isaaclab_arena.environments.isaaclab_arena_environment import IsaacLabArenaEnvironment
@@ -27,10 +28,15 @@
 background = asset_registry.get_asset_by_name("kitchen")()
 embodiment = asset_registry.get_asset_by_name("franka")()
 cracker_box = asset_registry.get_asset_by_name("cracker_box")()
-
+tomato_soup_can = asset_registry.get_asset_by_name("tomato_soup_can")()
 cracker_box.set_initial_pose(Pose(position_xyz=(0.4, 0.0, 0.1), rotation_wxyz=(1.0, 0.0, 0.0, 0.0)))
 
-scene = Scene(assets=[background, cracker_box])
+print("Setting initial pose of tomato soup can to on relation with cracker box")
+tomato_soup_can.set_initial_pose(OnRelation(tomato_soup_can, cracker_box))
+
+scene = Scene(assets=[background, cracker_box, tomato_soup_can])
+
+
 isaaclab_arena_environment = IsaacLabArenaEnvironment(
     name="reference_object_test",
     embodiment=embodiment,
@@ -54,3 +60,6 @@
         env.step(actions)
 
 # %%
+
+# TODO(cvolk)
+# We still need an anchor point

isaaclab_arena/examples/compile_env_notebook_v2.py

@@ -0,0 +1,66 @@
+# Copyright (c) 2025, The Isaac Lab Arena Project Developers (https://github.com/isaac-sim/IsaacLab-Arena/blob/main/CONTRIBUTORS.md).
+# All rights reserved.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+# %%
+
+import torch
+import tqdm
+
+import pinocchio  # noqa: F401
+from isaaclab.app import AppLauncher
+
+print("Launching simulation app once in notebook")
+simulation_app = AppLauncher()
+
+from isaaclab_arena.assets.asset_registry import AssetRegistry
+from isaaclab_arena.cli.isaaclab_arena_cli import get_isaaclab_arena_cli_parser
+from isaaclab_arena.environments.arena_env_builder import ArenaEnvBuilder
+from isaaclab_arena.environments.isaaclab_arena_environment import IsaacLabArenaEnvironment
+from isaaclab_arena.scene.scene import Scene
+from isaaclab_arena.tasks.dummy_task import DummyTask
+from isaaclab_arena.utils.pose import Pose
+
+asset_registry = AssetRegistry()
+
+background = asset_registry.get_asset_by_name("kitchen")()
+embodiment = asset_registry.get_asset_by_name("franka")()
+cracker_box = asset_registry.get_asset_by_name("cracker_box")()
+cracker_box.set_initial_pose(Pose(position_xyz=(0.4, 0.0, 0.1), rotation_wxyz=(1.0, 0.0, 0.0, 0.0)))
+tomato_soup_can = asset_registry.get_asset_by_name("tomato_soup_can")()
+
+microwave = asset_registry.get_asset_by_name("microwave")()
+
+tomato_soup_can.next_to(cracker_box, side="right", clearance=0.05)
+microwave.next_to(tomato_soup_can, side="right", clearance=0.05)
+mustard_bottle.on_top_of(microwave)
+
+scene = Scene(assets=[background, cracker_box, tomato_soup_can, microwave, mustard_bottle])
+
+isaaclab_arena_environment = IsaacLabArenaEnvironment(
+    name="reference_object_test",
+    embodiment=embodiment,
+    scene=scene,
+    task=DummyTask(),
+    teleop_device=None,
+)
+
+args_cli = get_isaaclab_arena_cli_parser().parse_args([])
+env_builder = ArenaEnvBuilder(isaaclab_arena_environment, args_cli)
+env = env_builder.make_registered()
+env.reset()
+
+# %%
+
+# Run some zero actions.
+NUM_STEPS = 1000
+for _ in tqdm.tqdm(range(NUM_STEPS)):
+    with torch.inference_mode():
+        actions = torch.zeros(env.action_space.shape, device=env.unwrapped.device)
+        env.step(actions)
+
+# %%
+
+# TODO(cvolk)
+# We still need an anchor point