Fix geometric pick-and-place success checks (#1132)

Provide a runtime lookup to provide a positional check
`GeometricObjectOnDestinationTerm` that holds the cached aabb's of the
objects and provide runtime position lookup.

Success now requires all three conditions:

- New `object_bounds_center_over_destination`: The center of the spawned
object bounds is over the destination footprint and above its bottom.
- New `contact_force_is_upward_support`: The filtered contact force on
the object points upward and exceeds the configured threshold.
- The object linear speed is below the configured threshold.

The old max_separation workaround is removed because the destination
footprint now provides the geometric placement check

---------

Signed-off-by: Clemens Volk <cvolk@nvidia.com>

Author: cvolkcvolk

isaaclab_arena/assets/object_reference.py

@@ -18,7 +18,7 @@
 from isaaclab_arena.utils.pose import Pose
 from isaaclab_arena.utils.usd_helpers import (
     NoCollisionMeshError,
-    compute_local_bounding_box_from_prim,
+    compute_world_aligned_bounding_box_relative_to_prim_origin,
     extract_trimesh_from_prim,
     open_stage,
 )
@@ -81,10 +81,10 @@ def add_relation(self, relation: RelationBase) -> None:
         self.relations.append(relation)
 
     def get_bounding_box(self) -> AxisAlignedBoundingBox:
-        """Get local bounding box of the referenced prim (relative to prim transform).
+        """Get world-axis-aligned bounds measured from the referenced prim's origin.
 
-        The bounding box is relative to the prim's transform origin, consistent with
-        how Object.get_bounding_box() returns bbox relative to USD origin.
+        The coordinates use the parent asset's USD axes, with the origin shifted to
+        the referenced prim's world position.
 
         The bounding box is computed lazily and cached for subsequent calls.
         """
@@ -93,7 +93,7 @@ def get_bounding_box(self) -> AxisAlignedBoundingBox:
                 prim_path_in_usd = self.isaaclab_prim_path_to_original_prim_path(
                     self.prim_path, self.parent_asset, parent_stage
                 )
-                raw_bbox = compute_local_bounding_box_from_prim(parent_stage, prim_path_in_usd)
+                raw_bbox = compute_world_aligned_bounding_box_relative_to_prim_origin(parent_stage, prim_path_in_usd)
                 # Apply parent's scale (no centering - solver is origin-agnostic)
                 self._bounding_box = raw_bbox.scaled(self._parent_scale)
         return self._bounding_box

isaaclab_arena/tasks/composite_task_base.py

@@ -174,26 +174,26 @@ def _remove_configclass_transform(fields: list[tuple], exclude_fields: set[str])
     @staticmethod
     def _evaluate_subtask_successes(
         env,
-        subtasks: list[TaskBase],
+        subtask_success_cfgs: list[TerminationTermCfg],
         subtask_indices,
     ) -> list[list[bool]]:
         """Evaluate the success function of selected subtasks across all envs.
 
         Args:
             env: The environment instance.
-            subtasks: Full list of subtasks for this composite task.
+            subtask_success_cfgs: Success configurations whose class-based functions have been
+                constructed by the manager.
             subtask_indices: Iterable of subtask indices to evaluate. Indices not in this
                 iterable are left as False in the returned matrix.
 
         Returns:
-            A (num_envs x len(subtasks)) list of bools, where entry [env_idx][subtask_idx]
-            is True if that subtask's success function returned True this step.
+            A (num_envs x len(subtask_success_cfgs)) list of bools, where entry
+            [env_idx][subtask_idx] is True if that subtask's success function returned True this step.
         """
-        subtask_currently_succeeding = [[False for _ in subtasks] for _ in range(env.num_envs)]
+        subtask_currently_succeeding = [[False for _ in subtask_success_cfgs] for _ in range(env.num_envs)]
         for subtask_idx in subtask_indices:
-            subtask_success_func = subtasks[subtask_idx].get_termination_cfg().success.func
-            subtask_success_params = subtasks[subtask_idx].get_termination_cfg().success.params
-            results = subtask_success_func(env, **subtask_success_params)
+            subtask_success_cfg = subtask_success_cfgs[subtask_idx]
+            results = subtask_success_cfg.func(env, **subtask_success_cfg.params)
             for env_idx in range(env.num_envs):
                 if results[env_idx]:
                     subtask_currently_succeeding[env_idx][subtask_idx] = True
@@ -202,30 +202,33 @@ def _evaluate_subtask_successes(
     @staticmethod
     def composite_task_success_func(
         env,
-        subtasks: list[TaskBase],
+        subtask_success_cfgs: list[TerminationTermCfg],
         desired_subtask_success_state: list[bool | None] | None,
     ) -> torch.Tensor:
         """Composite task composite success function.
 
         Args:
             env: The environment instance.
-            subtasks: List of subtasks that compose this composite task.
+            subtask_success_cfgs: Success configurations whose class-based functions have been
+                constructed by the manager.
             desired_subtask_success_state: (Optional) Precise success state for each subtask during the final time step.
                 Can be used to enforce a specific current state for each subtask at the end of the episode.
 
         Returns:
             A bool tensor of shape (num_envs,) indicating composite success per env.
         """
+        num_subtasks = len(subtask_success_cfgs)
+
         # Initialize each env's subtask success state to False if not already initialized
         if not hasattr(env, "_subtask_ever_succeeded"):
-            env._subtask_ever_succeeded = [[False for _ in subtasks] for _ in range(env.num_envs)]
+            env._subtask_ever_succeeded = [[False for _ in range(num_subtasks)] for _ in range(env.num_envs)]
 
         # Evaluate every subtask's success function (composite tasks have no ordering constraint).
         subtask_currently_succeeding = CompositeTaskBase._evaluate_subtask_successes(
-            env, subtasks, range(len(subtasks))
+            env, subtask_success_cfgs, range(num_subtasks)
         )
         for env_idx in range(env.num_envs):
-            for subtask_idx in range(len(subtasks)):
+            for subtask_idx in range(num_subtasks):
                 if subtask_currently_succeeding[env_idx][subtask_idx]:
                     env._subtask_ever_succeeded[env_idx][subtask_idx] = True
 
@@ -321,10 +324,13 @@ def get_events_cfg(self) -> Any:
 
     def _make_composite_task_termination_cfg(self) -> Any:
         "Make composite success check termination term."
+        subtask_success_cfgs = [subtask.get_termination_cfg().success for subtask in self.subtasks]
         success = TerminationTermCfg(
             func=self.composite_task_success_func,
             params={
-                "subtasks": self.subtasks,
+                # Child success configs must be direct term parameters so Isaac Lab can construct
+                # class-backed child terms; ManagerBase does not traverse TaskBase objects.
+                "subtask_success_cfgs": subtask_success_cfgs,
                 "desired_subtask_success_state": self.desired_subtask_success_state,
             },
         )

isaaclab_arena/tasks/pick_and_place_task.py

@@ -23,33 +23,37 @@
 from isaaclab_arena.metrics.success_rate import SuccessRateMetric
 from isaaclab_arena.progress_tracking.progress_objective import ProgressObjective
 from isaaclab_arena.tasks.common.mimic_default_params import MIMIC_DATAGEN_CONFIG_DEFAULTS
+from isaaclab_arena.tasks.predicates.object_on_destination_term import ObjectOnDestinationTerm
 from isaaclab_arena.tasks.predicates.object_settling import objects_settled
-from isaaclab_arena.tasks.predicates.spatial import object_is_above_height, object_on_destination, objects_in_proximity
+from isaaclab_arena.tasks.predicates.spatial import object_is_above_height, object_on_destination
 from isaaclab_arena.tasks.task_base import TaskBase
 from isaaclab_arena.tasks.task_transition import Relocate, TaskTransition
-from isaaclab_arena.tasks.terminations import SuccessMode, check_success
 from isaaclab_arena.utils.cameras import get_viewer_cfg_look_at_object
 from isaaclab_arena.utils.configclass import make_configclass
 
 
 @agent_ready
 @register_task
 class PickAndPlaceTask(TaskBase):
-    """Pick-and-place task. Success fires when the pick-up object contacts the destination
-    with low velocity and, when ``max_separation`` is set, is within axis-aligned proximity
-    of the destination. Failure (object_dropped) fires when the object falls below the
-    background's ``object_min_z``.
+    """Pick an object up and place it on or in a destination.
+
+    Success requires the object's bounds center over the destination footprint, upward support
+    force, and low linear speed. Failure occurs when the object falls below the background.
+
+    Args:
+        pick_up_object: Rigid object or rigid object set to pick up.
+        destination_location: Destination whose live pose and spawned geometry define placement.
+            It must be included in the environment scene.
+        background_scene: Background whose minimum object height defines the drop failure.
+        destination_object: Destination asset used by the default Mimic configuration.
+        episode_length_s: Maximum episode duration in seconds.
+        task_description: Natural-language task instruction. A description is generated when omitted.
+        force_threshold: Minimum filtered normal force exerted on the object by the destination, in newtons.
+        velocity_threshold: Object linear-speed threshold in meters per second. Speed must be below it.
+        mimic_env_cfg_factory: Optional factory for a custom Mimic environment configuration.
+        support_cone_half_angle_deg: Maximum angle in degrees between the filtered contact force and
+            world +Z. Smaller values require the support force to be more vertical.
 
-    The default Mimic cfg is ``PickPlaceMimicEnvCfg``. When a task needs a different cfg
-    shape (different arm subtask sequences, different per-subtask numerical knobs,
-    bespoke fields), pass ``mimic_env_cfg_factory`` to inject a custom ``MimicEnvCfg``::
-
-        def _factory(arm_mode):
-            return MyCustomMimicEnvCfg(arm_mode=arm_mode, ...)
-
-        PickAndPlaceTask(..., mimic_env_cfg_factory=_factory)
-
-    The factory receives ``arm_mode`` from the env builder and returns a constructed cfg.
     """
 
     def __init__(
@@ -62,8 +66,8 @@ def __init__(
         task_description: str | None = None,
         force_threshold: float = 0.1,
         velocity_threshold: float = 0.1,
-        max_separation: tuple[float, float, float] | None = None,
         mimic_env_cfg_factory: Callable[[ArmMode], MimicEnvCfg] | None = None,
+        support_cone_half_angle_deg: float = 45.0,
     ):
         super().__init__(episode_length_s=episode_length_s)
         self.pick_up_object = pick_up_object
@@ -73,10 +77,12 @@ def __init__(
         self.contact_sensor_name = f"contact_sensor_{pick_up_object.name}"
         self.scene_config = self.make_scene_cfg()
         self.force_threshold = force_threshold
+        assert velocity_threshold >= 0.0, f"velocity_threshold must be non-negative, got {velocity_threshold}"
         self.velocity_threshold = velocity_threshold
-        if max_separation is not None:
-            assert len(max_separation) == 3, f"max_separation must be (x, y, z), got {max_separation!r}"
-        self.max_separation = max_separation
+        assert (
+            0.0 <= support_cone_half_angle_deg < 90.0
+        ), f"support_cone_half_angle_deg must be in [0, 90), got {support_cone_half_angle_deg}"
+        self.support_cone_half_angle_deg = support_cone_half_angle_deg
         self.mimic_env_cfg_factory = mimic_env_cfg_factory
         self.events_cfg = None
         self.termination_cfg = self.make_termination_cfg()
@@ -107,39 +113,15 @@ def get_termination_cfg(self):
         return self.termination_cfg
 
     def make_termination_cfg(self):
-        predicates = [
-            TerminationTermCfg(
-                func=object_on_destination,
-                params={
-                    "object_cfg": SceneEntityCfg(self.pick_up_object.name),
-                    "contact_sensor_cfg": SceneEntityCfg(self.contact_sensor_name),
-                    "force_threshold": self.force_threshold,
-                    "velocity_threshold": self.velocity_threshold,
-                },
-            ),
-        ]
-        if self.max_separation is not None:
-            # TODO(qianl): replace objects_in_proximity with object_centroid_in_proximity
-            # for tighter container placement checks.
-            # TODO (qianl): current implementation doesn't support ObjectReference as target_object_cfg.
-            max_x_separation, max_y_separation, max_z_separation = self.max_separation
-            predicates.append(
-                TerminationTermCfg(
-                    func=objects_in_proximity,
-                    params={
-                        "object_cfg": SceneEntityCfg(self.pick_up_object.name),
-                        "target_object_cfg": SceneEntityCfg(self.destination_location.name),
-                        "max_x_separation": max_x_separation,
-                        "max_y_separation": max_y_separation,
-                        "max_z_separation": max_z_separation,
-                    },
-                )
-            )
         success = TerminationTermCfg(
-            func=check_success,
+            func=ObjectOnDestinationTerm,
             params={
-                "mode": SuccessMode.ALL,
-                "predicates": predicates,
+                "object_cfg": SceneEntityCfg(self.pick_up_object.name),
+                "destination_cfg": SceneEntityCfg(self.destination_location.name),
+                "contact_sensor_cfg": SceneEntityCfg(self.contact_sensor_name),
+                "force_threshold": self.force_threshold,
+                "velocity_threshold": self.velocity_threshold,
+                "support_cone_half_angle_deg": self.support_cone_half_angle_deg,
             },
         )
         object_dropped = TerminationTermCfg(
@@ -208,7 +190,7 @@ def get_viewer_cfg(self) -> ViewerCfg:
 
     @classmethod
     def success_state_transition(cls, pick_up_object: str, destination_location: str, **_) -> TaskTransition:
-        """Success (``object_on_destination``): the picked object ends up a relation with the destination."""
+        """Relate the picked object to the destination after geometric placement succeeds."""
         # Note: with the current AABB-based object solver, placing an object ``on`` an open container
         # and letting it fall is equivalent to it being ``in`` the container, so a single ``on``
         # relation covers both surfaces and containers.