diff --git a/docs/source/overview/imitation-learning/teleop_imitation.rst b/docs/source/overview/imitation-learning/teleop_imitation.rst index bbe69082012..141b2969e2c 100644 --- a/docs/source/overview/imitation-learning/teleop_imitation.rst +++ b/docs/source/overview/imitation-learning/teleop_imitation.rst @@ -636,32 +636,33 @@ To create a comprehensive locomanipulation dataset that combines both manipulati G1 humanoid robot performing locomanipulation with navigation capabilities. -The navigation dataset generation process takes the previously generated manipulation dataset and creates scenarios where the robot must navigate from one location to another while performing manipulation tasks. This creates a more complex dataset that includes both locomotion and manipulation behaviors. +The locomanipulation dataset generation process takes the previously generated manipulation dataset and creates scenarios where the robot must navigate from one location to another while performing manipulation tasks. This creates a more complex dataset that includes both locomotion and manipulation behaviors. -To generate the navigation dataset, use the following command: +To generate the locomanipulation dataset, use the following command: .. code:: bash ./isaaclab.sh -p \ - scripts/imitation_learning/disjoint_navigation/generate_navigation.py \ + scripts/imitation_learning/locomanipulation_sdg/generate_data.py \ --device cpu \ --kit_args="--enable isaacsim.replicator.mobility_gen" \ - --task="Isaac-G1-Disjoint-Navigation" \ + --task="Isaac-G1-SteeringWheel-Locomanipulation" \ --dataset ./datasets/generated_dataset_g1_locomanip.hdf5 \ --num_runs 1 \ --lift_step 70 \ --navigate_step 120 \ --enable_pinocchio \ - --output_file ./datasets/generated_dataset_g1_navigation.hdf5 + --output_file ./datasets/generated_dataset_g1_locomanipulation_sdg.hdf5 \ + --enable_cameras .. note:: The input dataset (``--dataset``) should be the manipulation dataset generated in the previous step. You can specify any output filename using the ``--output_file_name`` parameter. -The key parameters for navigation dataset generation are: +The key parameters for locomanipulation dataset generation are: -* ``--lift_step 70``: Number of steps for the lifting phase of the manipulation task -* ``--navigate_step 120``: Number of steps for the navigation phase between locations +* ``--lift_step 70``: Number of steps for the lifting phase of the manipulation task. This should mark the point immediately after the robot has grasped the object. +* ``--navigate_step 120``: Number of steps for the navigation phase between locations. This should make the point where the robot has lifted the object and is ready to walk. * ``--output_file``: Name of the output dataset file This process creates a dataset where the robot performs the manipulation task at different locations, requiring it to navigate between points while maintaining the learned manipulation behaviors. The resulting dataset can be used to train policies that combine both locomotion and manipulation capabilities. @@ -672,7 +673,22 @@ This process creates a dataset where the robot performs the manipulation task at .. code:: bash - ./isaaclab.sh -p scripts/imitation_learning/disjoint_navigation/plot_navigation_trajectory.py --input_file datasets/generated_dataset_g1_navigation.hdf5 --output_dir /PATH/TO/DESIRED_OUTPUT_DIR + ./isaaclab.sh -p scripts/imitation_learning/locomanipulation_sdg/plot_navigation_trajectory.py --input_file datasets/generated_dataset_g1_locomanipulation_sdg.hdf5 --output_dir /PATH/TO/DESIRED_OUTPUT_DIR + +The data generated from this locomanipulation pipeline can also be used to finetune an imitation learning policy using GR00T N1.5. To do this, +you may convert the generated dataset to LeRobot format as expected by GR00T N1.5, and then run the finetuning script provided +in the GR00T N1.5 repository. An example closed-loop policy rollout is shown in the video below: + +.. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/locomanipulation_sdg_disjoint_nav_groot_policy_4x.gif + :width: 100% + :align: center + :alt: Simulation rollout of GR00T N1.5 policy finetuned for locomanipulation + :figclass: align-center + + Simulation rollout of GR00T N1.5 policy finetuned for locomanipulation. + +The policy shown above uses the camera image, hand poses, hand joint positions, object pose, and base goal pose as inputs. +The output of the model is the target base velocity, hand poses, and hand joint positions for the next several timesteps. Demo 3: Visuomotor Policy for a Humanoid Robot diff --git a/scripts/imitation_learning/disjoint_navigation/README.md b/scripts/imitation_learning/disjoint_navigation/README.md deleted file mode 100644 index e9c91c58955..00000000000 --- a/scripts/imitation_learning/disjoint_navigation/README.md +++ /dev/null @@ -1,36 +0,0 @@ -# Disjoint Navigation - -This folder contains code for running the disjoint navigation data generation script. This assumes that you have collected a static manipulation dataset. - - -## Usage - -To run the disjoint navigation replay script execute the following command. - - -```bash -./isaaclab.sh -p \ - scripts/imitation_learning/disjoint_navigation/generate_navigation.py \ - --device cpu \ - --kit_args="--enable isaacsim.replicator.mobility_gen" \ - --task="Isaac-G1-Disjoint-Navigation" \ - --dataset="datasets/dataset_generated_g1_locomanipulation_teacher_release.hdf5" \ - --num_runs=1 \ - --lift_step=70 \ - --navigate_step=120 \ - --enable_pinocchio \ - --output_file=datasets/dataset_generated_disjoint_nav.hdf5 -``` - - -Please check ``replay.py`` for details on the arguments. - -To view the generated trajectories - - -```bash -./isaaclab.sh -p \ - scripts/imitation_learning/disjoint_navigation/display_dataset.py \ - datasets/dataset_generated_disjoint_nav.hdf5 \ - datasets/ -``` diff --git a/scripts/imitation_learning/disjoint_navigation/generate_navigation.py b/scripts/imitation_learning/locomanipulation_sdg/generate_data.py similarity index 86% rename from scripts/imitation_learning/disjoint_navigation/generate_navigation.py rename to scripts/imitation_learning/locomanipulation_sdg/generate_data.py index 78d5c5c7477..46f80d97d70 100644 --- a/scripts/imitation_learning/disjoint_navigation/generate_navigation.py +++ b/scripts/imitation_learning/locomanipulation_sdg/generate_data.py @@ -14,8 +14,8 @@ from isaaclab.app import AppLauncher # Launch Isaac Lab -parser = argparse.ArgumentParser(description="Disjoint navigation") -parser.add_argument("--task", type=str, help="The Isaac Lab disjoint navigation task to load for data generation.") +parser = argparse.ArgumentParser(description="Locomanipulation SDG") +parser.add_argument("--task", type=str, help="The Isaac Lab locomanipulation SDG task to load for data generation.") parser.add_argument("--dataset", type=str, help="The static manipulation dataset recorded via teleoperation.") parser.add_argument("--output_file", type=str, help="The file name for the generated output dataset.") parser.add_argument( @@ -118,17 +118,17 @@ from isaaclab.utils import configclass from isaaclab.utils.datasets import EpisodeData, HDF5DatasetFileHandler -import isaaclab_mimic.disjoint_nav_envs # noqa: F401 -from isaaclab_mimic.disjoint_nav_envs.disjoint_nav_env import DisjointNavEnv -from isaaclab_mimic.navigation_path_planner.disjoint_nav.disjoint_nav_data import DisjointNavOutputData -from isaaclab_mimic.navigation_path_planner.disjoint_nav.disjoint_nav_path_utils import ParameterizedPath, plan_path -from isaaclab_mimic.navigation_path_planner.disjoint_nav.disjoint_nav_scene_utils import RelativePose, place_randomly -from isaaclab_mimic.navigation_path_planner.disjoint_nav.disjoint_nav_transform_utils import ( +import isaaclab_mimic.locomanipulation_sdg.envs # noqa: F401 +from isaaclab_mimic.locomanipulation_sdg.envs.locomanipulation_sdg_env import LocomanipulationSDGEnv +from isaaclab_mimic.locomanipulation_sdg.data_classes import LocomanipulationSDGOutputData +from isaaclab_mimic.locomanipulation_sdg.path_utils import ParameterizedPath, plan_path +from isaaclab_mimic.locomanipulation_sdg.scene_utils import RelativePose, place_randomly +from isaaclab_mimic.locomanipulation_sdg.transform_utils import ( transform_inv, transform_mul, transform_relative_pose, ) -from isaaclab_mimic.navigation_path_planner.disjoint_nav.occupancy_map_utils import ( +from isaaclab_mimic.locomanipulation_sdg.occupancy_map_utils import ( OccupancyMap, merge_occupancy_maps, occupancy_map_add_to_stage, @@ -137,8 +137,8 @@ from isaaclab_tasks.utils import parse_env_cfg -class DisjointNavDataGenerationState(enum.IntEnum): - """States for the disjoint navigation data generation state machine.""" +class LocomanipulationSDGDataGenerationState(enum.IntEnum): + """States for the locomanipulation SDG data generation state machine.""" GRASP_OBJECT = 0 """Robot grasps object at start position""" @@ -160,7 +160,7 @@ class DisjointNavDataGenerationState(enum.IntEnum): @configclass -class DisjointNavControlConfig: +class LocomanipulationSDGControlConfig: """Configuration for navigation control parameters.""" angular_gain: float = 2.0 @@ -186,7 +186,7 @@ class DisjointNavControlConfig: def compute_navigation_velocity( - current_pose: torch.Tensor, target_xy: torch.Tensor, config: DisjointNavControlConfig + current_pose: torch.Tensor, target_xy: torch.Tensor, config: LocomanipulationSDGControlConfig ) -> tuple[torch.Tensor, torch.Tensor]: """Compute linear and angular velocities for navigation control. @@ -220,7 +220,7 @@ def compute_navigation_velocity( def load_and_transform_recording_data( - env: DisjointNavEnv, + env: LocomanipulationSDGEnv, input_episode_data: EpisodeData, recording_step: int, reference_pose: torch.Tensor, @@ -229,7 +229,7 @@ def load_and_transform_recording_data( """Load recording data and transform hand targets to current reference frame. Args: - env: The disjoint navigation environment + env: The locomanipulation SDG environment input_episode_data: Input episode data from static manipulation recording_step: Current step in the recording reference_pose: Original reference pose for the hand targets @@ -249,12 +249,12 @@ def load_and_transform_recording_data( def setup_navigation_scene( - env: DisjointNavEnv, input_episode_data: EpisodeData, approach_distance: float, randomize_placement: bool = True + env: LocomanipulationSDGEnv, input_episode_data: EpisodeData, approach_distance: float, randomize_placement: bool = True ) -> tuple[OccupancyMap, ParameterizedPath, RelativePose, RelativePose]: """Set up the navigation scene with occupancy map and path planning. Args: - env: The disjoint navigation environment + env: The locomanipulation SDG environment input_episode_data: Input episode data approach_distance: Buffer distance from final goal randomize_placement: Whether to randomize fixture placement @@ -295,12 +295,12 @@ def setup_navigation_scene( def handle_grasp_state( - env: DisjointNavEnv, + env: LocomanipulationSDGEnv, input_episode_data: EpisodeData, recording_step: int, lift_step: int, - output_data: DisjointNavOutputData, -) -> tuple[int, DisjointNavDataGenerationState]: + output_data: LocomanipulationSDGOutputData, +) -> tuple[int, LocomanipulationSDGDataGenerationState]: """Handle the GRASP_OBJECT state logic. Args: @@ -316,7 +316,7 @@ def handle_grasp_state( recording_item = env.load_input_data(input_episode_data, recording_step) # Set control targets - robot stays stationary during grasping - output_data.data_generation_state = int(DisjointNavDataGenerationState.GRASP_OBJECT) + output_data.data_generation_state = int(LocomanipulationSDGDataGenerationState.GRASP_OBJECT) output_data.recording_step = recording_step output_data.base_velocity_target = torch.tensor([0.0, 0.0, 0.0]) @@ -333,21 +333,21 @@ def handle_grasp_state( # Update state next_recording_step = recording_step + 1 next_state = ( - DisjointNavDataGenerationState.LIFT_OBJECT + LocomanipulationSDGDataGenerationState.LIFT_OBJECT if next_recording_step > lift_step - else DisjointNavDataGenerationState.GRASP_OBJECT + else LocomanipulationSDGDataGenerationState.GRASP_OBJECT ) return next_recording_step, next_state def handle_lift_state( - env: DisjointNavEnv, + env: LocomanipulationSDGEnv, input_episode_data: EpisodeData, recording_step: int, navigate_step: int, - output_data: DisjointNavOutputData, -) -> tuple[int, DisjointNavDataGenerationState]: + output_data: LocomanipulationSDGOutputData, +) -> tuple[int, LocomanipulationSDGDataGenerationState]: """Handle the LIFT_OBJECT state logic. Args: @@ -363,7 +363,7 @@ def handle_lift_state( recording_item = env.load_input_data(input_episode_data, recording_step) # Set control targets - robot stays stationary during lifting - output_data.data_generation_state = int(DisjointNavDataGenerationState.LIFT_OBJECT) + output_data.data_generation_state = int(LocomanipulationSDGDataGenerationState.LIFT_OBJECT) output_data.recording_step = recording_step output_data.base_velocity_target = torch.tensor([0.0, 0.0, 0.0]) @@ -380,23 +380,23 @@ def handle_lift_state( # Update state next_recording_step = recording_step + 1 next_state = ( - DisjointNavDataGenerationState.NAVIGATE + LocomanipulationSDGDataGenerationState.NAVIGATE if next_recording_step > navigate_step - else DisjointNavDataGenerationState.LIFT_OBJECT + else LocomanipulationSDGDataGenerationState.LIFT_OBJECT ) return next_recording_step, next_state def handle_navigate_state( - env: DisjointNavEnv, + env: LocomanipulationSDGEnv, input_episode_data: EpisodeData, recording_step: int, base_path_helper: ParameterizedPath, base_goal_approach: RelativePose, - config: DisjointNavControlConfig, - output_data: DisjointNavOutputData, -) -> DisjointNavDataGenerationState: + config: LocomanipulationSDGControlConfig, + output_data: LocomanipulationSDGOutputData, +) -> LocomanipulationSDGDataGenerationState: """Handle the NAVIGATE state logic. Args: @@ -422,7 +422,7 @@ def handle_navigate_state( linear_velocity, angular_velocity = compute_navigation_velocity(current_pose, target_xy, config) # Set control targets - output_data.data_generation_state = int(DisjointNavDataGenerationState.NAVIGATE) + output_data.data_generation_state = int(LocomanipulationSDGDataGenerationState.NAVIGATE) output_data.recording_step = recording_step output_data.base_velocity_target = torch.tensor([linear_velocity, 0.0, angular_velocity]) @@ -441,20 +441,20 @@ def handle_navigate_state( distance_to_goal = torch.sqrt(torch.sum((current_pose[:2] - goal_xy) ** 2)) return ( - DisjointNavDataGenerationState.APPROACH + LocomanipulationSDGDataGenerationState.APPROACH if distance_to_goal < config.distance_threshold - else DisjointNavDataGenerationState.NAVIGATE + else LocomanipulationSDGDataGenerationState.NAVIGATE ) def handle_approach_state( - env: DisjointNavEnv, + env: LocomanipulationSDGEnv, input_episode_data: EpisodeData, recording_step: int, base_goal: RelativePose, - config: DisjointNavControlConfig, - output_data: DisjointNavOutputData, -) -> DisjointNavDataGenerationState: + config: LocomanipulationSDGControlConfig, + output_data: LocomanipulationSDGOutputData, +) -> LocomanipulationSDGDataGenerationState: """Handle the APPROACH state logic. Args: @@ -476,7 +476,7 @@ def handle_approach_state( linear_velocity, angular_velocity = compute_navigation_velocity(current_pose, goal_xy, config) # Set control targets - output_data.data_generation_state = int(DisjointNavDataGenerationState.APPROACH) + output_data.data_generation_state = int(LocomanipulationSDGDataGenerationState.APPROACH) output_data.recording_step = recording_step output_data.base_velocity_target = torch.tensor([linear_velocity, 0.0, angular_velocity]) @@ -494,20 +494,20 @@ def handle_approach_state( distance_to_goal = torch.sqrt(torch.sum((current_pose[:2] - goal_xy) ** 2)) return ( - DisjointNavDataGenerationState.DROP_OFF_OBJECT + LocomanipulationSDGDataGenerationState.DROP_OFF_OBJECT if distance_to_goal < config.distance_threshold - else DisjointNavDataGenerationState.APPROACH + else LocomanipulationSDGDataGenerationState.APPROACH ) def handle_drop_off_state( - env: DisjointNavEnv, + env: LocomanipulationSDGEnv, input_episode_data: EpisodeData, recording_step: int, base_goal: RelativePose, - config: DisjointNavControlConfig, - output_data: DisjointNavOutputData, -) -> tuple[int, DisjointNavDataGenerationState | None]: + config: LocomanipulationSDGControlConfig, + output_data: LocomanipulationSDGOutputData, +) -> tuple[int, LocomanipulationSDGDataGenerationState | None]: """Handle the DROP_OFF_OBJECT state logic. Args: @@ -537,7 +537,7 @@ def handle_drop_off_state( linear_velocity = 0.0 # Stay in place while orienting # Set control targets - output_data.data_generation_state = int(DisjointNavDataGenerationState.DROP_OFF_OBJECT) + output_data.data_generation_state = int(LocomanipulationSDGDataGenerationState.DROP_OFF_OBJECT) output_data.recording_step = recording_step output_data.base_velocity_target = torch.tensor([linear_velocity, 0.0, angular_velocity]) @@ -558,12 +558,12 @@ def handle_drop_off_state( # Continue playback if orientation is within threshold next_recording_step = recording_step + 1 if abs(delta_yaw) < config.angle_threshold else recording_step - return next_recording_step, DisjointNavDataGenerationState.DROP_OFF_OBJECT + return next_recording_step, LocomanipulationSDGDataGenerationState.DROP_OFF_OBJECT def populate_output_data( - env: DisjointNavEnv, - output_data: DisjointNavOutputData, + env: LocomanipulationSDGEnv, + output_data: LocomanipulationSDGOutputData, base_goal: RelativePose, base_goal_approach: RelativePose, base_path: torch.Tensor, @@ -596,7 +596,7 @@ def populate_output_data( def replay( - env: DisjointNavEnv, + env: LocomanipulationSDGEnv, input_episode_data: EpisodeData, lift_step: int, navigate_step: int, @@ -610,9 +610,9 @@ def replay( approach_distance: float = 1.0, randomize_placement: bool = True, ) -> None: - """Replay a disjoint navigation episode with state machine control. + """Replay a locomanipulation SDG episode with state machine control. - This function implements a state machine for disjoint navigation, where the robot: + This function implements a state machine for locomanipulation SDG, where the robot: 1. Grasps an object at the start position 2. Lifts the object while stationary 3. Navigates with the object to an approach position @@ -620,7 +620,7 @@ def replay( 5. Places the object at the end position Args: - env: The disjoint navigation environment + env: The locomanipulation SDG environment input_episode_data: Static manipulation episode data to replay lift_step: Recording step where lifting phase begins navigate_step: Recording step where navigation phase begins @@ -639,7 +639,7 @@ def replay( env.reset_to(state=input_episode_data.get_initial_state(), env_ids=torch.tensor([0]), is_relative=True) # Create navigation control configuration - config = DisjointNavControlConfig( + config = LocomanipulationSDGControlConfig( angular_gain=angular_gain, linear_gain=linear_gain, linear_max=linear_max, @@ -665,8 +665,8 @@ def replay( ) # Initialize state machine - output_data = DisjointNavOutputData() - current_state = DisjointNavDataGenerationState.GRASP_OBJECT + output_data = LocomanipulationSDGOutputData() + current_state = LocomanipulationSDGDataGenerationState.GRASP_OBJECT recording_step = 0 # Main simulation loop with state machine @@ -675,27 +675,27 @@ def replay( print(f"Current state: {current_state.name}, Recording step: {recording_step}") # Execute state-specific logic using helper functions - if current_state == DisjointNavDataGenerationState.GRASP_OBJECT: + if current_state == LocomanipulationSDGDataGenerationState.GRASP_OBJECT: recording_step, current_state = handle_grasp_state( env, input_episode_data, recording_step, lift_step, output_data ) - elif current_state == DisjointNavDataGenerationState.LIFT_OBJECT: + elif current_state == LocomanipulationSDGDataGenerationState.LIFT_OBJECT: recording_step, current_state = handle_lift_state( env, input_episode_data, recording_step, navigate_step, output_data ) - elif current_state == DisjointNavDataGenerationState.NAVIGATE: + elif current_state == LocomanipulationSDGDataGenerationState.NAVIGATE: current_state = handle_navigate_state( env, input_episode_data, recording_step, base_path_helper, base_goal_approach, config, output_data ) - elif current_state == DisjointNavDataGenerationState.APPROACH: + elif current_state == LocomanipulationSDGDataGenerationState.APPROACH: current_state = handle_approach_state( env, input_episode_data, recording_step, base_goal, config, output_data ) - elif current_state == DisjointNavDataGenerationState.DROP_OFF_OBJECT: + elif current_state == LocomanipulationSDGDataGenerationState.DROP_OFF_OBJECT: recording_step, next_state = handle_drop_off_state( env, input_episode_data, recording_step, base_goal, config, output_data ) @@ -707,7 +707,7 @@ def replay( populate_output_data(env, output_data, base_goal, base_goal_approach, base_path_helper.points) # Attach output data to environment for recording - env._disjoint_nav_output_data = output_data + env._locomanipulation_sdg_output_data = output_data # Build and execute action action = env.build_action_vector( diff --git a/scripts/imitation_learning/disjoint_navigation/plot_navigation_trajectory.py b/scripts/imitation_learning/locomanipulation_sdg/plot_navigation_trajectory.py similarity index 95% rename from scripts/imitation_learning/disjoint_navigation/plot_navigation_trajectory.py rename to scripts/imitation_learning/locomanipulation_sdg/plot_navigation_trajectory.py index b824bbd1a5c..6981ff803d1 100644 --- a/scripts/imitation_learning/disjoint_navigation/plot_navigation_trajectory.py +++ b/scripts/imitation_learning/locomanipulation_sdg/plot_navigation_trajectory.py @@ -24,7 +24,7 @@ def main(): """Main function to process dataset and generate visualizations.""" # add argparse arguments parser = argparse.ArgumentParser( - description="Visualize navigation dataset from disjoint navigation demonstrations." + description="Visualize navigation dataset from locomanipulation sdg demonstrations." ) parser.add_argument( "--input_file", type=str, help="Path to the HDF5 dataset file containing recorded demonstrations." @@ -72,7 +72,7 @@ def main(): for i, demo in enumerate(demos): print(f"Processing demo {i + 1}/{len(demos)}: {demo}") - replay_data = dataset["data"][demo]["disjoint_nav_output_data"] + replay_data = dataset["data"][demo]["locomanipulation_sdg_output_data"] path = replay_data["base_path"] base_pose = replay_data["base_pose"] object_pose = replay_data["object_pose"] diff --git a/source/isaaclab_mimic/isaaclab_mimic/disjoint_nav_envs/__init__.py b/source/isaaclab_mimic/isaaclab_mimic/disjoint_nav_envs/__init__.py deleted file mode 100644 index 2a097db516d..00000000000 --- a/source/isaaclab_mimic/isaaclab_mimic/disjoint_nav_envs/__init__.py +++ /dev/null @@ -1,19 +0,0 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: Apache-2.0 - -"""Sub-package with environment wrappers for Disjoint Navigation.""" - -import gymnasium as gym - -from .g1_disjoint_nav_env import G1DisjointNavEnvCfg - -gym.register( - id="Isaac-G1-Disjoint-Navigation", - entry_point="isaaclab_mimic.disjoint_nav_envs.g1_disjoint_nav_env:G1DisjointNavEnv", - kwargs={ - "env_cfg_entry_point": G1DisjointNavEnvCfg, - }, - disable_env_checker=True, -) diff --git a/source/isaaclab_mimic/isaaclab_mimic/navigation_path_planner/disjoint_nav/__init__.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/__init__.py similarity index 77% rename from source/isaaclab_mimic/isaaclab_mimic/navigation_path_planner/disjoint_nav/__init__.py rename to source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/__init__.py index 41f6e70d149..63333b6811e 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/navigation_path_planner/disjoint_nav/__init__.py +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/__init__.py @@ -3,4 +3,4 @@ # # SPDX-License-Identifier: Apache-2.0 -"""Sub-package with disjoint navigation utilities.""" +"""Sub-package with locomanipulation SDG utilities.""" diff --git a/source/isaaclab_mimic/isaaclab_mimic/navigation_path_planner/disjoint_nav/disjoint_nav_data.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/data_classes.py similarity index 94% rename from source/isaaclab_mimic/isaaclab_mimic/navigation_path_planner/disjoint_nav/disjoint_nav_data.py rename to source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/data_classes.py index 0e2189029d1..2d2e656e288 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/navigation_path_planner/disjoint_nav/disjoint_nav_data.py +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/data_classes.py @@ -8,7 +8,7 @@ @dataclass -class DisjointNavInputData: +class LocomanipulationSDGInputData: """Data container for in-place manipulation recording state. Used during locomanipulation replay.""" left_hand_pose_target: torch.Tensor @@ -34,7 +34,7 @@ class DisjointNavInputData: @dataclass -class DisjointNavOutputData: +class LocomanipulationSDGOutputData: """A container for data that is recorded during locomanipulation replay. This is the final output of the pipeline""" left_hand_pose_target: torch.Tensor | None = None @@ -65,7 +65,7 @@ class DisjointNavOutputData: """The pose of the robot base.""" data_generation_state: int | None = None - """The state of the the disjoint navigation replay script's state machine.""" + """The state of the the locomanipulation SDG replay script's state machine.""" base_goal_pose: torch.Tensor | None = None """The goal pose of the robot base (ie: the final destination before dropping off the object)""" diff --git a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/__init__.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/__init__.py new file mode 100644 index 00000000000..d73d89b0b06 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/__init__.py @@ -0,0 +1,17 @@ +# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Sub-package with environment wrappers for Locomanipulation SDG.""" + +import gymnasium as gym + +gym.register( + id="Isaac-G1-SteeringWheel-Locomanipulation", + entry_point=f"{__name__}.g1_locomanipulation_sdg_env:G1LocomanipulationSDGEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.g1_locomanipulation_sdg_env:G1LocomanipulationSDGEnvCfg", + }, + disable_env_checker=True, +) diff --git a/source/isaaclab_mimic/isaaclab_mimic/disjoint_nav_envs/g1_disjoint_nav_env.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/g1_locomanipulation_sdg_env.py similarity index 87% rename from source/isaaclab_mimic/isaaclab_mimic/disjoint_nav_envs/g1_disjoint_nav_env.py rename to source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/g1_locomanipulation_sdg_env.py index 09fb93c1cfb..4650e578147 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/disjoint_nav_envs/g1_disjoint_nav_env.py +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/g1_locomanipulation_sdg_env.py @@ -12,9 +12,10 @@ from isaaclab.utils import configclass from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR, retrieve_file_path from isaaclab.utils.datasets import EpisodeData +from isaaclab.envs.common import ViewerCfg -from isaaclab_mimic.navigation_path_planner.disjoint_nav.disjoint_nav_data import DisjointNavInputData -from isaaclab_mimic.navigation_path_planner.disjoint_nav.disjoint_nav_scene_utils import ( +from isaaclab_mimic.locomanipulation_sdg.data_classes import LocomanipulationSDGInputData +from isaaclab_mimic.locomanipulation_sdg.scene_utils import ( HasPose, SceneBody, SceneFixture, @@ -25,15 +26,15 @@ LocomanipulationG1SceneCfg, ) -from .disjoint_nav_env import DisjointNavEnv -from .disjoint_nav_env_cfg import DisjointNavEnvCfg, DisjointNavRecorderManagerCfg +from .locomanipulation_sdg_env import LocomanipulationSDGEnv +from .locomanipulation_sdg_env_cfg import LocomanipulationSDGEnvCfg, LocomanipulationSDGRecorderManagerCfg NUM_FORKLIFTS = 6 NUM_BOXES = 12 @configclass -class G1DisjointNavSceneCfg(LocomanipulationG1SceneCfg): +class G1LocomanipulationSDGSceneCfg(LocomanipulationG1SceneCfg): packing_table_2 = AssetBaseCfg( prim_path="/World/envs/env_.*/PackingTable2", @@ -52,7 +53,7 @@ class G1DisjointNavSceneCfg(LocomanipulationG1SceneCfg): # Add forklifts for i in range(NUM_FORKLIFTS): setattr( - G1DisjointNavSceneCfg, + G1LocomanipulationSDGSceneCfg, f"forklift_{i}", AssetBaseCfg( prim_path=f"/World/envs/env_.*/Forklift{i}", @@ -67,7 +68,7 @@ class G1DisjointNavSceneCfg(LocomanipulationG1SceneCfg): # Add boxes for i in range(NUM_BOXES): setattr( - G1DisjointNavSceneCfg, + G1LocomanipulationSDGSceneCfg, f"box_{i}", AssetBaseCfg( prim_path=f"/World/envs/env_.*/Box{i}", @@ -81,12 +82,19 @@ class G1DisjointNavSceneCfg(LocomanipulationG1SceneCfg): @configclass -class G1DisjointNavEnvCfg(LocomanipulationG1EnvCfg, DisjointNavEnvCfg): +class G1LocomanipulationSDGEnvCfg(LocomanipulationG1EnvCfg, LocomanipulationSDGEnvCfg): """Configuration for the G1 29DoF environment.""" - + viewer: ViewerCfg = ViewerCfg( + eye=(0.0, 3.0, 1.25), + lookat=(0.0, 0.0, 0.5), + origin_type="asset_body", + asset_name="robot", + body_name="pelvis" + ) + # Scene settings - scene: G1DisjointNavSceneCfg = G1DisjointNavSceneCfg(num_envs=1, env_spacing=2.5, replicate_physics=True) - recorders: DisjointNavRecorderManagerCfg = DisjointNavRecorderManagerCfg() + scene: G1LocomanipulationSDGSceneCfg = G1LocomanipulationSDGSceneCfg(num_envs=1, env_spacing=2.5, replicate_physics=True) + recorders: LocomanipulationSDGRecorderManagerCfg = LocomanipulationSDGRecorderManagerCfg() def __post_init__(self): """Post initialization.""" @@ -104,9 +112,9 @@ def __post_init__(self): self.actions.upper_body_ik.controller.urdf_path = retrieve_file_path(urdf_omniverse_path) -class G1DisjointNavEnv(DisjointNavEnv): +class G1LocomanipulationSDGEnv(LocomanipulationSDGEnv): - def __init__(self, cfg: G1DisjointNavEnvCfg, **kwargs): + def __init__(self, cfg: G1LocomanipulationSDGEnvCfg, **kwargs): super().__init__(cfg) self.sim.set_camera_view([10.5, 10.5, 10.5], [0.0, 0.0, 0.5]) self._upper_body_dim = self.action_manager.get_term("upper_body_ik").action_dim @@ -115,7 +123,7 @@ def __init__(self, cfg: G1DisjointNavEnvCfg, **kwargs): self._frame_pose_dim = 7 self._number_of_finger_joints = 7 - def load_input_data(self, episode_data: EpisodeData, step: int) -> DisjointNavInputData | None: + def load_input_data(self, episode_data: EpisodeData, step: int) -> LocomanipulationSDGInputData | None: dataset_action = episode_data.get_action(step) dataset_state = episode_data.get_state(step) @@ -127,7 +135,7 @@ def load_input_data(self, episode_data: EpisodeData, step: int) -> DisjointNavIn object_pose = dataset_state["rigid_object"]["object"]["root_pose"] - data = DisjointNavInputData( + data = LocomanipulationSDGInputData( left_hand_pose_target=dataset_action[0:7], right_hand_pose_target=dataset_action[7:14], left_hand_joint_positions_target=dataset_action[14:21], diff --git a/source/isaaclab_mimic/isaaclab_mimic/disjoint_nav_envs/disjoint_nav_env.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/locomanipulation_sdg_env.py similarity index 83% rename from source/isaaclab_mimic/isaaclab_mimic/disjoint_nav_envs/disjoint_nav_env.py rename to source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/locomanipulation_sdg_env.py index d031c29c810..b19ee80390b 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/disjoint_nav_envs/disjoint_nav_env.py +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/locomanipulation_sdg_env.py @@ -9,17 +9,17 @@ from isaaclab.managers.recorder_manager import RecorderTerm from isaaclab.utils.datasets import EpisodeData -from isaaclab_mimic.navigation_path_planner.disjoint_nav.disjoint_nav_data import ( - DisjointNavInputData, - DisjointNavOutputData, +from isaaclab_mimic.locomanipulation_sdg.data_classes import ( + LocomanipulationSDGInputData, + LocomanipulationSDGOutputData, ) -from isaaclab_mimic.navigation_path_planner.disjoint_nav.disjoint_nav_scene_utils import HasPose, SceneFixture +from isaaclab_mimic.locomanipulation_sdg.scene_utils import HasPose, SceneFixture -class DisjointNavOutputDataRecorder(RecorderTerm): +class LocomanipulationSDGOutputDataRecorder(RecorderTerm): def record_pre_step(self): - output_data: DisjointNavOutputData = self._env._disjoint_nav_output_data + output_data: LocomanipulationSDGOutputData = self._env._locomanipulation_sdg_output_data output_data_dict = { "left_hand_pose_target": output_data.left_hand_pose_target[None, :], @@ -39,19 +39,19 @@ def record_pre_step(self): "obstacle_fixture_poses": output_data.obstacle_fixture_poses, } - return "disjoint_nav_output_data", output_data_dict + return "locomanipulation_sdg_output_data", output_data_dict -class DisjointNavEnv(ManagerBasedRLEnv): +class LocomanipulationSDGEnv(ManagerBasedRLEnv): """An abstract base class that wraps the underlying environment, exposing methods needed for integration with locomanipulation replay. - This class defines the core methods needed to integrate an environment with the disjoint navigation pipeline for + This class defines the core methods needed to integrate an environment with the locomanipulation SDG pipeline for locomanipulation replay. By implementing these methods for a new environment, the environment can be used with - the disjoint navigation replay function. + the locomanipulation SDG replay function. """ - def load_input_data(self, episode_data: EpisodeData, step: int) -> DisjointNavInputData: + def load_input_data(self, episode_data: EpisodeData, step: int) -> LocomanipulationSDGInputData: raise NotImplementedError def build_action_vector( diff --git a/source/isaaclab_mimic/isaaclab_mimic/disjoint_nav_envs/disjoint_nav_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/locomanipulation_sdg_env_cfg.py similarity index 55% rename from source/isaaclab_mimic/isaaclab_mimic/disjoint_nav_envs/disjoint_nav_env_cfg.py rename to source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/locomanipulation_sdg_env_cfg.py index f806b71a8d8..77b82710026 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/disjoint_nav_envs/disjoint_nav_env_cfg.py +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/locomanipulation_sdg_env_cfg.py @@ -11,37 +11,37 @@ from isaaclab.managers.recorder_manager import RecorderTerm, RecorderTermCfg from isaaclab.utils import configclass -from .disjoint_nav_env import DisjointNavOutputDataRecorder +from .locomanipulation_sdg_env import LocomanipulationSDGOutputDataRecorder @configclass -class DisjointNavOutputDataRecorderCfg(RecorderTermCfg): +class LocomanipulationSDGOutputDataRecorderCfg(RecorderTermCfg): """Configuration for the step policy observation recorder term.""" - class_type: type[RecorderTerm] = DisjointNavOutputDataRecorder + class_type: type[RecorderTerm] = LocomanipulationSDGOutputDataRecorder @configclass -class DisjointNavRecorderManagerCfg(ActionStateRecorderManagerCfg): - record_pre_step_disjoint_nav_output_data = DisjointNavOutputDataRecorderCfg() +class LocomanipulationSDGRecorderManagerCfg(ActionStateRecorderManagerCfg): + record_pre_step_locomanipulation_sdg_output_data = LocomanipulationSDGOutputDataRecorderCfg() @configclass -class DisjointNavTerminationsCfg: +class LocomanipulationSDGTerminationsCfg: """Termination terms for the MDP.""" time_out = DoneTerm(func=base_mdp.time_out, time_out=True) @configclass -class DisjointNavEventCfg: +class LocomanipulationSDGEventCfg: """Configuration for events.""" reset_all = EventTerm(func=base_mdp.reset_scene_to_default, mode="reset") @configclass -class DisjointNavEnvCfg(ManagerBasedRLEnvCfg): - recorders: DisjointNavRecorderManagerCfg = DisjointNavRecorderManagerCfg() - terminations: DisjointNavTerminationsCfg = DisjointNavTerminationsCfg() - events: DisjointNavEventCfg = DisjointNavEventCfg() +class LocomanipulationSDGEnvCfg(ManagerBasedRLEnvCfg): + recorders: LocomanipulationSDGRecorderManagerCfg = LocomanipulationSDGRecorderManagerCfg() + terminations: LocomanipulationSDGTerminationsCfg = LocomanipulationSDGTerminationsCfg() + events: LocomanipulationSDGEventCfg = LocomanipulationSDGEventCfg() diff --git a/source/isaaclab_mimic/isaaclab_mimic/navigation_path_planner/disjoint_nav/occupancy_map_utils.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/occupancy_map_utils.py similarity index 100% rename from source/isaaclab_mimic/isaaclab_mimic/navigation_path_planner/disjoint_nav/occupancy_map_utils.py rename to source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/occupancy_map_utils.py diff --git a/source/isaaclab_mimic/isaaclab_mimic/navigation_path_planner/disjoint_nav/disjoint_nav_path_utils.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/path_utils.py similarity index 99% rename from source/isaaclab_mimic/isaaclab_mimic/navigation_path_planner/disjoint_nav/disjoint_nav_path_utils.py rename to source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/path_utils.py index fc573a40768..79f8afbbf82 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/navigation_path_planner/disjoint_nav/disjoint_nav_path_utils.py +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/path_utils.py @@ -8,7 +8,7 @@ from isaacsim.replicator.mobility_gen.impl.path_planner import compress_path, generate_paths -from .disjoint_nav_scene_utils import HasPose2d +from .scene_utils import HasPose2d from .occupancy_map_utils import OccupancyMap diff --git a/source/isaaclab_mimic/isaaclab_mimic/navigation_path_planner/disjoint_nav/disjoint_nav_scene_utils.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/scene_utils.py similarity index 99% rename from source/isaaclab_mimic/isaaclab_mimic/navigation_path_planner/disjoint_nav/disjoint_nav_scene_utils.py rename to source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/scene_utils.py index 70b4bccdf2c..1844bdb4c86 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/navigation_path_planner/disjoint_nav/disjoint_nav_scene_utils.py +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/scene_utils.py @@ -9,7 +9,7 @@ import isaaclab.utils.math as math_utils -from .disjoint_nav_transform_utils import transform_mul +from .transform_utils import transform_mul from .occupancy_map_utils import OccupancyMap, intersect_occupancy_maps diff --git a/source/isaaclab_mimic/isaaclab_mimic/navigation_path_planner/disjoint_nav/disjoint_nav_transform_utils.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/transform_utils.py similarity index 100% rename from source/isaaclab_mimic/isaaclab_mimic/navigation_path_planner/disjoint_nav/disjoint_nav_transform_utils.py rename to source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/transform_utils.py diff --git a/source/isaaclab_mimic/isaaclab_mimic/navigation_path_planner/__init__.py b/source/isaaclab_mimic/isaaclab_mimic/navigation_path_planner/__init__.py deleted file mode 100644 index aa8000c00dc..00000000000 --- a/source/isaaclab_mimic/isaaclab_mimic/navigation_path_planner/__init__.py +++ /dev/null @@ -1,6 +0,0 @@ -# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: Apache-2.0 - -"""Sub-package with navigation path planner utilities.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_g1_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_g1_env_cfg.py index 2de10b13ca0..63c938c8237 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_g1_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_g1_env_cfg.py @@ -21,6 +21,7 @@ from isaaclab.managers import SceneEntityCfg from isaaclab.managers import TerminationTermCfg as DoneTerm from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import CameraCfg from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg from isaaclab.utils import configclass from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR, retrieve_file_path @@ -84,6 +85,16 @@ class LocomanipulationG1SceneCfg(InteractiveSceneCfg): spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), ) + robot_pov_cam = CameraCfg( + prim_path="/World/envs/env_.*/Robot/torso_link/d435_link/tiled_camera", + update_period=0.0, + height=160, + width=256, + data_types=["rgb"], + spawn=sim_utils.PinholeCameraCfg(focal_length=8.0, clipping_range=(0.1, 20.0)), + offset=CameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(0.9848078, 0.0, -0.1736482, 0.0), convention="world"), + ) + @configclass class ActionsCfg: @@ -137,6 +148,11 @@ class PolicyCfg(ObsGroup): params={"left_eef_link_name": "left_wrist_yaw_link", "right_eef_link_name": "right_wrist_yaw_link"}, ) + robot_pov_cam = ObsTerm( + func=manip_mdp.image, + params={"sensor_cfg": SceneEntityCfg("robot_pov_cam"), "data_type": "rgb", "normalize": False}, + ) + def __post_init__(self): self.enable_corruption = False self.concatenate_terms = False