From d456e106f03cda25eb161a6e9374ead7180f9211 Mon Sep 17 00:00:00 2001 From: Xinjie Yao Date: Tue, 7 Apr 2026 14:37:29 -0700 Subject: [PATCH] example --- isaaclab_arena/affordances/movable.py | 75 ++++++++++++++++ isaaclab_arena/assets/object_library.py | 43 ++++++++++ isaaclab_arena/tasks/move_object_task.py | 85 +++++++++++++++++++ isaaclab_arena/tasks/terminations.py | 25 ++++++ isaaclab_arena/utils/usd/spawners.py | 48 +++++++++++ isaaclab_arena_environments/cli.py | 2 + .../kitchen_move_object_environment.py | 70 +++++++++++++++ 7 files changed, 348 insertions(+) create mode 100644 isaaclab_arena/affordances/movable.py create mode 100644 isaaclab_arena/tasks/move_object_task.py create mode 100644 isaaclab_arena/utils/usd/spawners.py create mode 100644 isaaclab_arena_environments/kitchen_move_object_environment.py diff --git a/isaaclab_arena/affordances/movable.py b/isaaclab_arena/affordances/movable.py new file mode 100644 index 000000000..c449677b5 --- /dev/null +++ b/isaaclab_arena/affordances/movable.py @@ -0,0 +1,75 @@ +# Copyright (c) 2025-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 + +import torch + +import warp as wp +from isaaclab.envs.manager_based_env import ManagerBasedEnv +from isaaclab.managers import SceneEntityCfg + +from isaaclab_arena.affordances.affordance_base import AffordanceBase + + +class Movable(AffordanceBase): + """Interface for objects that can be pushed or moved across the floor. + + Movable objects are characterized by the ability to be displaced from their + initial position (e.g. carts on wheels, trolleys, wheeled furniture). + Displacement is measured in the XY-plane (horizontal movement only). + """ + + def __init__(self, displacement_threshold: float = 0.1, **kwargs): + """Initialize a movable object. + + Args: + displacement_threshold: Default threshold (meters) for determining + whether the object has been moved. Measured as Euclidean + distance in the XY-plane. + **kwargs: Additional arguments passed to AffordanceBase. + """ + super().__init__(**kwargs) + self.displacement_threshold = displacement_threshold + + def get_displacement(self, env: ManagerBasedEnv, asset_cfg: SceneEntityCfg | None = None) -> torch.Tensor: + """Get horizontal (XY) displacement from the initial position. + + Args: + env: The environment instance. + asset_cfg: Asset configuration. If None, uses the object's name. + + Returns: + Euclidean distance in the XY-plane from the initial position. + Shape: [num_envs]. + """ + if asset_cfg is None: + asset_cfg = SceneEntityCfg(self.name) + object_entity = env.scene[asset_cfg.name] + current_pos = wp.to_torch(object_entity.data.root_pos_w)[:, :2] + initial_pos = wp.to_torch(object_entity.data.default_root_state)[:, :2] + env_origins_xy = env.scene.env_origins[:, :2] + return torch.norm(current_pos - (initial_pos + env_origins_xy), dim=-1) + + def is_moved( + self, + env: ManagerBasedEnv, + asset_cfg: SceneEntityCfg | None = None, + displacement_threshold: float | None = None, + ) -> torch.Tensor: + """Check whether the object has been displaced beyond a threshold. + + Args: + env: The environment instance. + asset_cfg: Asset configuration. If None, uses the object's name. + displacement_threshold: Threshold in meters. If None, uses the + object's default displacement_threshold. + + Returns: + Boolean tensor indicating whether the object has been moved. + Shape: [num_envs]. + """ + if displacement_threshold is None: + displacement_threshold = self.displacement_threshold + displacement = self.get_displacement(env, asset_cfg) + return displacement > displacement_threshold diff --git a/isaaclab_arena/assets/object_library.py b/isaaclab_arena/assets/object_library.py index 72c6b6b4f..a2a0b9171 100644 --- a/isaaclab_arena/assets/object_library.py +++ b/isaaclab_arena/assets/object_library.py @@ -14,6 +14,7 @@ from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR +from isaaclab_arena.affordances.movable import Movable from isaaclab_arena.affordances.openable import Openable from isaaclab_arena.affordances.placeable import Placeable from isaaclab_arena.affordances.pressable import Pressable @@ -1968,3 +1969,45 @@ def _generate_rigid_cfg(self) -> RigidObjectCfg: **self.asset_cfg_addon, ) return self._add_initial_pose_to_cfg(cfg) + + +@register_asset +class MobileShelvingCart(LibraryObject, Movable): + """A mobile shelving cart on caster wheels. + + The USD has joints connecting rigid body links (body, swivels, wheels) + but no ArticulationRootAPI baked in, so we apply it at spawn time via + a custom spawner that calls ``define_articulation_root_properties``. + """ + + name = "mobile_shelving_cart" + tags = ["object", "movable"] + usd_path = "/datasets/assets/Collected_sm_mobileshelvingcart_a01/sm_mobileshelvingcart_a01.usd" + object_type = ObjectType.ARTICULATION + default_prim_path = "{ENV_REGEX_NS}/mobile_shelving_cart" + scale = (0.7, 0.7, 0.7) + displacement_threshold = 0.15 + spawn_cfg_addon = { + "func": "isaaclab_arena.utils.usd.spawners:spawn_from_usd_and_add_articulation_root", + "articulation_props": sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + ), + } + asset_cfg_addon = { + "init_state": EMPTY_ARTICULATION_INIT_STATE_CFG, + } + + def __init__( + self, + instance_name: str | None = None, + prim_path: str | None = None, + initial_pose: Pose | None = None, + scale: tuple[float, float, float] | None = None, + ): + super().__init__( + instance_name=instance_name, + prim_path=prim_path, + initial_pose=initial_pose, + scale=scale, + displacement_threshold=self.displacement_threshold, + ) diff --git a/isaaclab_arena/tasks/move_object_task.py b/isaaclab_arena/tasks/move_object_task.py new file mode 100644 index 000000000..e75012ae6 --- /dev/null +++ b/isaaclab_arena/tasks/move_object_task.py @@ -0,0 +1,85 @@ +# Copyright (c) 2025-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 + +import numpy as np +from dataclasses import MISSING + +import isaaclab.envs.mdp as mdp_isaac_lab +from isaaclab.envs.common import ViewerCfg +from isaaclab.managers import SceneEntityCfg, TerminationTermCfg +from isaaclab.utils import configclass + +from isaaclab_arena.assets.asset import Asset +from isaaclab_arena.embodiments.common.arm_mode import ArmMode +from isaaclab_arena.metrics.metric_base import MetricBase +from isaaclab_arena.metrics.success_rate import SuccessRateMetric +from isaaclab_arena.tasks.task_base import TaskBase +from isaaclab_arena.tasks.terminations import object_displaced +from isaaclab_arena.utils.cameras import get_viewer_cfg_look_at_object + + +class MoveObjectTask(TaskBase): + """Task where the robot must push/move a movable articulated object (e.g. a cart).""" + + def __init__( + self, + movable_object: Asset, + background_scene: Asset, + displacement_threshold: float = 0.5, + episode_length_s: float = 10.0, + task_description: str | None = None, + ): + super().__init__(episode_length_s=episode_length_s) + self.movable_object = movable_object + self.background_scene = background_scene + self.displacement_threshold = displacement_threshold + + self.scene_config = None + self.events_cfg = None + self.termination_cfg = self._make_termination_cfg() + self.task_description = ( + f"Push the {movable_object.name} at least {displacement_threshold:.1f}m from its start" + if task_description is None + else task_description + ) + + def get_scene_cfg(self): + return self.scene_config + + def get_termination_cfg(self): + return self.termination_cfg + + def get_events_cfg(self): + return self.events_cfg + + def get_mimic_env_cfg(self, arm_mode: ArmMode): + raise NotImplementedError("Mimic is not yet supported for MoveObjectTask.") + + def get_metrics(self) -> list[MetricBase]: + return [SuccessRateMetric()] + + def get_viewer_cfg(self) -> ViewerCfg: + return get_viewer_cfg_look_at_object( + lookat_object=self.movable_object, + offset=np.array([-2.0, -2.0, 2.0]), + ) + + def _make_termination_cfg(self): + success = TerminationTermCfg( + func=object_displaced, + params={ + "object_cfg": SceneEntityCfg(self.movable_object.name), + "displacement_threshold": self.displacement_threshold, + }, + ) + return MoveObjectTerminationsCfg(success=success) + + +@configclass +class MoveObjectTerminationsCfg: + """Termination terms for the move-object task.""" + + time_out: TerminationTermCfg = TerminationTermCfg(func=mdp_isaac_lab.time_out) + success: TerminationTermCfg = MISSING diff --git a/isaaclab_arena/tasks/terminations.py b/isaaclab_arena/tasks/terminations.py index d8f18d851..b68ade828 100644 --- a/isaaclab_arena/tasks/terminations.py +++ b/isaaclab_arena/tasks/terminations.py @@ -240,6 +240,31 @@ def goal_pose_task_termination( return success +def object_displaced( + env: ManagerBasedRLEnv, + object_cfg: SceneEntityCfg = SceneEntityCfg("movable_object"), + displacement_threshold: float = 0.5, +) -> torch.Tensor: + """Terminate (success) when the object has been displaced beyond a threshold. + + Displacement is measured as XY-plane Euclidean distance from the initial position. + + Args: + env: The RL environment instance. + object_cfg: The configuration of the movable object. + displacement_threshold: Minimum XY displacement in meters to count as success. + + Returns: + A boolean tensor of shape (num_envs,) indicating success. + """ + object_entity = env.scene[object_cfg.name] + current_pos = wp.to_torch(object_entity.data.root_pos_w)[:, :2] + initial_pos = wp.to_torch(object_entity.data.default_root_state)[:, :2] + env_origins_xy = env.scene.env_origins[:, :2] + displacement = torch.norm(current_pos - (initial_pos + env_origins_xy), dim=-1) + return displacement > displacement_threshold + + def root_height_below_minimum_multi_objects( env: ManagerBasedRLEnv, minimum_height: float, asset_cfg_list: list[SceneEntityCfg] = [SceneEntityCfg("robot")] ) -> torch.Tensor: diff --git a/isaaclab_arena/utils/usd/spawners.py b/isaaclab_arena/utils/usd/spawners.py new file mode 100644 index 000000000..910dd2100 --- /dev/null +++ b/isaaclab_arena/utils/usd/spawners.py @@ -0,0 +1,48 @@ +# Copyright (c) 2025-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 + +from pxr import Usd + +from isaaclab.sim import schemas +from isaaclab.sim.spawners.from_files.from_files import _spawn_from_usd_file +from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg +from isaaclab.sim.utils import clone + + +@clone +def spawn_from_usd_and_add_articulation_root( + prim_path: str, + cfg: UsdFileCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, + **kwargs, +) -> Usd.Prim: + """Spawn a USD file and apply ArticulationRootAPI to the root prim. + + Use this for USD assets that have joints connecting rigid body links but + were authored without an ArticulationRootAPI. The API is applied to the + root prim after loading, so Isaac Lab can initialise the asset as an + articulation. + + The function delegates to the standard USD spawning pipeline + (``_spawn_from_usd_file``) and then calls + ``schemas.define_articulation_root_properties`` which creates the + ArticulationRootAPI if it is not already present. + + Args: + prim_path: The prim path or pattern to spawn the asset at. + cfg: The UsdFileCfg configuration instance. + translation: Translation w.r.t. parent prim. Defaults to None. + orientation: Orientation (x, y, z, w) w.r.t. parent prim. Defaults to None. + + Returns: + The prim of the spawned asset. + """ + prim = _spawn_from_usd_file(prim_path, cfg.usd_path, cfg, translation, orientation) + if cfg.articulation_props is not None: + schemas.define_articulation_root_properties(prim_path, cfg.articulation_props) + return prim diff --git a/isaaclab_arena_environments/cli.py b/isaaclab_arena_environments/cli.py index d6bfdaaba..ba98e69a1 100644 --- a/isaaclab_arena_environments/cli.py +++ b/isaaclab_arena_environments/cli.py @@ -21,6 +21,7 @@ GR1TableMultiObjectNoCollisionEnvironment, ) from isaaclab_arena_environments.gr1_turn_stand_mixer_knob_environment import Gr1TurnStandMixerKnobEnvironment +from isaaclab_arena_environments.kitchen_move_object_environment import KitchenMoveObjectEnvironment from isaaclab_arena_environments.kitchen_pick_and_place_environment import KitchenPickAndPlaceEnvironment from isaaclab_arena_environments.lift_object_environment import LiftObjectEnvironment from isaaclab_arena_environments.pick_and_place_maple_table_environment import PickAndPlaceMapleTableEnvironment @@ -39,6 +40,7 @@ FrankaPutAndCloseDoorEnvironment.name: FrankaPutAndCloseDoorEnvironment, Gr1OpenMicrowaveEnvironment.name: Gr1OpenMicrowaveEnvironment, GR1PutAndCloseDoorEnvironment.name: GR1PutAndCloseDoorEnvironment, + KitchenMoveObjectEnvironment.name: KitchenMoveObjectEnvironment, KitchenPickAndPlaceEnvironment.name: KitchenPickAndPlaceEnvironment, GalileoPickAndPlaceEnvironment.name: GalileoPickAndPlaceEnvironment, PickAndPlaceMapleTableEnvironment.name: PickAndPlaceMapleTableEnvironment, diff --git a/isaaclab_arena_environments/kitchen_move_object_environment.py b/isaaclab_arena_environments/kitchen_move_object_environment.py new file mode 100644 index 000000000..90f94d878 --- /dev/null +++ b/isaaclab_arena_environments/kitchen_move_object_environment.py @@ -0,0 +1,70 @@ +# Copyright (c) 2025-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 + +import argparse + +from isaaclab_arena_environments.example_environment_base import ExampleEnvironmentBase + + +class KitchenMoveObjectEnvironment(ExampleEnvironmentBase): + + name: str = "kitchen_move_object" + + def get_env(self, args_cli: argparse.Namespace): # -> IsaacLabArenaEnvironment: + from isaaclab_arena.environments.isaaclab_arena_environment import IsaacLabArenaEnvironment + from isaaclab_arena.scene.scene import Scene + from isaaclab_arena.tasks.move_object_task import MoveObjectTask + from isaaclab_arena.utils.pose import Pose + + background = self.asset_registry.get_asset_by_name("kitchen")() + + embodiment = self.asset_registry.get_asset_by_name(args_cli.embodiment)(enable_cameras=args_cli.enable_cameras) + + movable_object = self.asset_registry.get_asset_by_name(args_cli.object)() + movable_object.set_initial_pose(Pose(position_xyz=(-2, -0.1, 0), rotation_xyzw=(0.0, 0.0, 0.0, 1.0))) + + if args_cli.teleop_device is not None: + teleop_device = self.device_registry.get_device_by_name(args_cli.teleop_device)() + else: + teleop_device = None + + scene = Scene(assets=[background, movable_object]) + move_object_task = MoveObjectTask( + movable_object=movable_object, + background_scene=background, + displacement_threshold=args_cli.displacement_threshold, + episode_length_s=args_cli.episode_length, + ) + isaaclab_arena_environment = IsaacLabArenaEnvironment( + name=self.name, + embodiment=embodiment, + scene=scene, + task=move_object_task, + teleop_device=teleop_device, + ) + return isaaclab_arena_environment + + @staticmethod + def add_cli_args(parser: argparse.ArgumentParser) -> None: + parser.add_argument( + "--object", + type=str, + default="mobile_shelving_cart", + help="Movable articulated object to push.", + ) + parser.add_argument("--embodiment", type=str, default="gr1_joint") + parser.add_argument( + "--displacement_threshold", + type=float, + default=0.5, + help="XY displacement (meters) to count as success.", + ) + parser.add_argument( + "--episode_length", + type=float, + default=10.0, + help="Episode length in seconds.", + ) + parser.add_argument("--teleop_device", type=str, default=None)