Relation solver places embodiments

Author: qianl-nv

isaaclab_arena/assets/dummy_embodiment.py

@@ -0,0 +1,68 @@
+# Copyright (c) 2026, 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 __future__ import annotations
+
+import trimesh
+
+from isaaclab_arena.relations.relations import IsAnchor, Relation, RelationBase, UnaryRelation
+from isaaclab_arena.utils.bounding_box import AxisAlignedBoundingBox, quaternion_to_90_deg_z_quarters
+from isaaclab_arena.utils.pose import Pose
+
+
+class DummyEmbodiment:
+    """Embodiment stand-in for relation-solver tests without Isaac Sim dependencies."""
+
+    def __init__(
+        self,
+        name: str,
+        bounding_box: AxisAlignedBoundingBox,
+        initial_pose: Pose | None = None,
+        relations: list[RelationBase] | None = None,
+        collision_mesh: trimesh.Trimesh | None = None,
+        placement_scene_entity_name: str = "robot",
+    ):
+        self.name = name
+        self.initial_pose = initial_pose
+        self.bounding_box = bounding_box
+        self.relations = list(relations or [])
+        self._collision_mesh = collision_mesh
+        self.placement_scene_entity_name = placement_scene_entity_name
+
+    @property
+    def placement_kind(self) -> str:
+        return "embodiment"
+
+    def add_relation(self, relation: RelationBase) -> None:
+        assert not isinstance(relation, IsAnchor), "Embodiment cannot be marked as an anchor"
+        self.relations.append(relation)
+
+    def get_relations(self) -> list[RelationBase]:
+        return self.relations
+
+    def get_spatial_relations(self) -> list[RelationBase]:
+        return [relation for relation in self.relations if isinstance(relation, (Relation, UnaryRelation))]
+
+    def get_bounding_box(self) -> AxisAlignedBoundingBox:
+        return self.bounding_box
+
+    def get_world_bounding_box(self) -> AxisAlignedBoundingBox:
+        if self.initial_pose is None:
+            return self.bounding_box
+        quarters = quaternion_to_90_deg_z_quarters(self.initial_pose.rotation_xyzw)
+        return self.bounding_box.rotated_90_around_z(quarters).translated(self.initial_pose.position_xyz)
+
+    def get_collision_mesh(self) -> trimesh.Trimesh | None:
+        return self._collision_mesh
+
+    def get_initial_pose(self) -> Pose | None:
+        return self.initial_pose
+
+    def set_initial_pose(self, pose: Pose) -> None:
+        self.initial_pose = pose
+
+    @property
+    def is_anchor(self) -> bool:
+        return False

isaaclab_arena/assets/dummy_object.py

@@ -75,6 +75,14 @@ def is_initial_pose_set(self) -> bool:
     def is_anchor(self) -> bool:
         return any(isinstance(r, IsAnchor) for r in self.relations)
 
+    @property
+    def placement_kind(self) -> str:
+        return "object"
+
+    @property
+    def placement_scene_entity_name(self) -> str:
+        return self.name
+
     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.py

@@ -66,10 +66,6 @@ def __init__(
         self.object_cfg = self._init_object_cfg()
         self.event_cfg = self._init_event_cfg()
 
-    def add_relation(self, relation: RelationBase) -> None:
-        """Add a relation to this object."""
-        self.relations.append(relation)
-
     def get_bounding_box(self) -> AxisAlignedBoundingBox:
         """Get local bounding box (relative to object origin)."""
         assert self.usd_path is not None

isaaclab_arena/assets/object_base.py

@@ -182,6 +182,20 @@ def get_relations(self) -> list[RelationBase]:
         """Get all relations for this object."""
         return self.relations
 
+    def add_relation(self, relation: RelationBase) -> None:
+        """Add a relation to this object."""
+        self.relations.append(relation)
+
+    @property
+    def placement_kind(self) -> str:
+        """Scene objects use the object placement apply path."""
+        return "object"
+
+    @property
+    def placement_scene_entity_name(self) -> str:
+        """Isaac Lab scene entity name used when writing solved poses to sim."""
+        return self.name
+
     @property
     def is_anchor(self) -> bool:
         """True if this object has an IsAnchor relation."""

isaaclab_arena/embodiments/droid/droid.py

@@ -40,6 +40,7 @@
 from isaaclab_arena.embodiments.droid.observations import arm_joint_pos, ee_pos, ee_quat, gripper_pos
 from isaaclab_arena.embodiments.embodiment_base import EmbodimentBase
 from isaaclab_arena.embodiments.franka.franka import franka_stack_events
+from isaaclab_arena.utils.embodiment_placement import PlacementUsdSource
 from isaaclab_arena.utils.pose import Pose
 from isaaclab_arena.variations.camera_extrinsics_variation import CameraExtrinsicsVariation
 
@@ -85,6 +86,31 @@ def _update_scene_cfg_with_robot_initial_pose(self, scene_config: Any, pose: Pos
 
         return scene_config
 
+    def get_placement_usd_sources(self) -> list[PlacementUsdSource]:
+        """Union the robot and stand USD footprints for relation solving."""
+        sources = super().get_placement_usd_sources()
+        scene_config = self.scene_config
+        assert scene_config is not None and hasattr(scene_config, "stand")
+        stand = scene_config.stand
+        stand_spawn = stand.spawn
+        stand_scale = getattr(stand_spawn, "scale", None) or (1.0, 1.0, 1.0)
+        if not isinstance(stand_scale, tuple):
+            stand_scale = tuple(stand_scale)
+        stand_pos = stand.init_state.pos
+        stand_rot = stand.init_state.rot
+        if not isinstance(stand_pos, tuple):
+            stand_pos = tuple(stand_pos)
+        if not isinstance(stand_rot, tuple):
+            stand_rot = tuple(stand_rot)
+        sources.append(
+            PlacementUsdSource(
+                usd_path=stand_spawn.usd_path,
+                scale=stand_scale,
+                offset_pose=Pose(position_xyz=stand_pos, rotation_xyzw=stand_rot),
+            )
+        )
+        return sources
+
     def set_initial_joint_pose(self, initial_joint_pose: list[float]) -> None:
         self.event_config.init_franka_arm_pose.params["default_pose"] = initial_joint_pose
 
@@ -162,7 +188,7 @@ class DroidSceneCfg:
         prim_path="{ENV_REGEX_NS}/Robot",
         spawn=sim_utils.UsdFileCfg(
             usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Arena/assets/robot_library/droid/franka_robotiq_2f_85_flattened.usd",
-            activate_contact_sensors=True,
+            activate_contact_sensors=False,
             rigid_props=sim_utils.RigidBodyPropertiesCfg(
                 disable_gravity=True,
                 max_depenetration_velocity=5.0,

isaaclab_arena/embodiments/embodiment_base.py

@@ -3,18 +3,27 @@
 #
 # SPDX-License-Identifier: Apache-2.0
 
+from __future__ import annotations
+
 from collections.abc import Mapping
-from typing import Any
+from typing import TYPE_CHECKING, Any
 
 from isaaclab.envs import ManagerBasedRLMimicEnv
 from isaaclab.managers.recorder_manager import RecorderManagerBaseCfg
 
 from isaaclab_arena.assets.asset import Asset
 from isaaclab_arena.embodiments.common.arm_mode import ArmMode
+from isaaclab_arena.relations.collision_mode import CollisionMode
+from isaaclab_arena.relations.relations import IsAnchor, Relation, RelationBase, UnaryRelation
+from isaaclab_arena.utils.bounding_box import AxisAlignedBoundingBox, quaternion_to_90_deg_z_quarters
 from isaaclab_arena.utils.cameras import make_camera_observation_cfg
 from isaaclab_arena.utils.configclass import combine_configclass_instances
+from isaaclab_arena.utils.embodiment_placement import PlacementUsdSource, compute_embodiment_placement_bbox
 from isaaclab_arena.utils.pose import Pose
 
+if TYPE_CHECKING:
+    import trimesh
+
 
 class EmbodimentBase(Asset):
 
@@ -37,6 +46,12 @@ def __init__(
         self.initial_pose = initial_pose
         self.concatenate_observation_terms = concatenate_observation_terms
         self.arm_mode = arm_mode or self.default_arm_mode
+        self.relations: list[RelationBase] = []
+        self._placement_bounding_box: AxisAlignedBoundingBox | None = None
+        # None means use the solver's default collision mode for this embodiment.
+        self.collision_mode: CollisionMode | None = None
+        # If True, mesh collision replaces non-watertight meshes with their convex hull.
+        self.repair_collision_mesh_non_watertight = True
         # These should be filled by the subclass
         self.scene_config: Any | None = None
         self.camera_config: Any | None = None
@@ -50,6 +65,70 @@ def __init__(
         self.xr: Any | None = None
         self.termination_cfg: Any | None = None
 
+    @property
+    def placement_kind(self) -> str:
+        """Embodiments use the embodiment placement apply path."""
+        return "embodiment"
+
+    @property
+    def placement_scene_entity_name(self) -> str:
+        """Isaac Lab scene entity name used when writing solved poses to sim."""
+        return self.get_embodiment_name_in_scene()
+
+    def add_relation(self, relation: RelationBase) -> None:
+        """Attach a spatial relation to this embodiment."""
+        assert not isinstance(relation, IsAnchor), "Embodiment cannot be marked as an anchor"
+        self.relations.append(relation)
+
+    def get_relations(self) -> list[RelationBase]:
+        """Return all relations attached to this embodiment."""
+        return self.relations
+
+    def get_spatial_relations(self) -> list[RelationBase]:
+        """Return spatial constraints, excluding placement markers."""
+        return [relation for relation in self.relations if isinstance(relation, (Relation, UnaryRelation))]
+
+    def get_initial_pose(self) -> Pose | None:
+        """Return the embodiment's initial pose."""
+        return self.initial_pose
+
+    @property
+    def is_anchor(self) -> bool:
+        """Embodiments are never relation anchors."""
+        return False
+
+    def get_placement_usd_sources(self) -> list[PlacementUsdSource]:
+        """Return USD assets that define this embodiment's placement footprint."""
+        scene_config = self.scene_config
+        assert scene_config is not None and hasattr(
+            scene_config, "robot"
+        ), f"{self.__class__.__name__} must populate scene_config.robot before placement bbox lookup"
+        robot = scene_config.robot
+        spawn = robot.spawn
+        usd_path = spawn.usd_path
+        scale = getattr(spawn, "scale", None) or (1.0, 1.0, 1.0)
+        if not isinstance(scale, tuple):
+            scale = tuple(scale)
+        return [PlacementUsdSource(usd_path=usd_path, scale=scale)]
+
+    def get_bounding_box(self) -> AxisAlignedBoundingBox:
+        """Return the USD-derived local placement footprint relative to the robot origin."""
+        if self._placement_bounding_box is None:
+            self._placement_bounding_box = compute_embodiment_placement_bbox(self.get_placement_usd_sources())
+        return self._placement_bounding_box
+
+    def get_world_bounding_box(self) -> AxisAlignedBoundingBox:
+        """Return the placement footprint in world coordinates."""
+        local_bbox = self.get_bounding_box()
+        pose = self.get_initial_pose()
+        if pose is None:
+            return local_bbox
+        quarters = quaternion_to_90_deg_z_quarters(pose.rotation_xyzw)
+        return local_bbox.rotated_90_around_z(quarters).translated(pose.position_xyz)
+
+    def get_collision_mesh(self) -> trimesh.Trimesh | None:
+        """Embodiments fall back to AABB overlap checks in the relation solver."""
+
     def set_initial_pose(self, pose: Pose) -> None:
         self.initial_pose = pose
 

isaaclab_arena/environment_spec/arena_env_graph_conversion_utils.py

@@ -23,6 +23,10 @@
 if TYPE_CHECKING:
     from isaaclab_arena.environment_spec.arena_env_graph_spec import ArenaEnvGraphSpec
 
+_EMBODIMENT_SPATIAL_RELATION_KINDS = frozenset(
+    {"on", "next_to", "not_next_to", "at_position", "position_limits", "face_to"}
+)
+
 
 def build_arena_env_from_graph_spec(graph_spec: ArenaEnvGraphSpec, enable_cameras: bool = False) -> Any:
     """Build an IsaacLabArenaEnvironment from a validated ArenaEnvGraphSpec.
@@ -33,6 +37,7 @@ def build_arena_env_from_graph_spec(graph_spec: ArenaEnvGraphSpec, enable_camera
     """
     assets_by_node_id = _instantiate_assets_from_spec(graph_spec, AssetRegistry(), enable_cameras=enable_cameras)
     _ensure_scene_lighting(graph_spec, assets_by_node_id)
+    _validate_embodiment_relations(graph_spec, graph_spec.relations)
     _attach_spatial_relations_to_assets(graph_spec.relations, assets_by_node_id)
     scene_assets = [asset for node_id, asset in assets_by_node_id.items() if node_id != graph_spec.embodiment.id]
     return IsaacLabArenaEnvironment(
@@ -129,6 +134,18 @@ def _instantiate_assets_from_spec(
     return assets_by_node_id
 
 
+def _validate_embodiment_relations(graph_spec: ArenaEnvGraphSpec, relations: list[SpatialRelationSpec]) -> None:
+    """Reject unsupported or anchor-style relations on the embodiment node."""
+    embodiment_id = graph_spec.embodiment.id
+    for relation in relations:
+        if relation.subject != embodiment_id:
+            continue
+        assert relation.kind != "is_anchor", "Embodiment cannot be marked is_anchor"
+        assert (
+            relation.kind in _EMBODIMENT_SPATIAL_RELATION_KINDS
+        ), f"Embodiment relation kind {relation.kind!r} is not supported for placement solving"
+
+
 def _attach_spatial_relations_to_assets(
     relations: list[SpatialRelationSpec], assets_by_node_id: dict[str, type[Asset]]
 ) -> None:

isaaclab_arena/environments/arena_env_builder.py

@@ -67,14 +67,12 @@ def __init__(
         self._placement_event_cfg: EventTermCfg | None = None
 
     def _solve_relations(self) -> None:
-        """Solve spatial relations for objects in the scene.
+        """Solve spatial relations for scene objects and optional embodiment placement.
 
         This method:
-        1. Collects all objects from the scene that have relations
-        2. Builds an object-placement pool
-        3. Reuses the object-only relation placer
-        4. Applies solved positions either by writing fixed per-object initial poses
-           or by registering a pooled reset placement event
+        1. Collects scene objects and the embodiment when it carries spatial relations
+        2. Builds a placement pool and runs the relation solver jointly
+        3. Applies solved positions to scene objects and/or the embodiment spawn pose
 
         Behaviour on reset depends on ``ObjectPlacerParams.resolve_on_reset``.
         When the environment does not provide placer parameters, the builder creates
@@ -85,7 +83,10 @@ def _solve_relations(self) -> None:
         * **False** — applies one layout per environment so per-object reset
           events restore the same layout every time.
         """
-        objects_with_relations = self.arena_env.scene.get_objects_with_relations()
+        placement_entities = list(self.arena_env.scene.get_objects_with_relations())
+        embodiment = self.arena_env.embodiment
+        if embodiment is not None and embodiment.get_spatial_relations():
+            placement_entities.append(embodiment)
 
         placer_params = self.arena_env.placer_params
         if placer_params is None:
@@ -96,7 +97,7 @@ def _solve_relations(self) -> None:
             if self.cfg.resolve_on_reset is not None:
                 placer_params.resolve_on_reset = self.cfg.resolve_on_reset
         self._placement_event_cfg = solve_and_apply_relation_placement(
-            objects_with_relations,
+            placement_entities,
             num_envs=self.cfg.num_envs,
             placer_params=placer_params,
             scene_assets=self.arena_env.scene.assets.values(),