From a714181a1445fb9c57d32785588a7c0ae3a83006 Mon Sep 17 00:00:00 2001 From: John Welsh Date: Tue, 16 Sep 2025 09:57:20 -0700 Subject: [PATCH 01/11] add camera to g1 locomanipulation env --- .../pick_place/locomanipulation_g1_env_cfg.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) 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 e807f100c41..baec57385bb 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 @@ -24,6 +24,7 @@ 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 +from isaaclab.sensors import CameraCfg from isaaclab_tasks.manager_based.locomanipulation.pick_place import mdp as locomanip_mdp from isaaclab_tasks.manager_based.locomanipulation.pick_place.configs.action_cfg import AgileBasedLowerBodyActionCfg @@ -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.1736482, 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 From 72dcbd8d7d57c016f74cde5146691f458afb51d9 Mon Sep 17 00:00:00 2001 From: John Welsh Date: Wed, 24 Sep 2025 14:45:09 -0700 Subject: [PATCH 02/11] run format --- .../pick_place/locomanipulation_g1_env_cfg.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 baec57385bb..e71dc92f0e5 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,10 +21,10 @@ 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 -from isaaclab.sensors import CameraCfg from isaaclab_tasks.manager_based.locomanipulation.pick_place import mdp as locomanip_mdp from isaaclab_tasks.manager_based.locomanipulation.pick_place.configs.action_cfg import AgileBasedLowerBodyActionCfg @@ -92,7 +92,7 @@ class LocomanipulationG1SceneCfg(InteractiveSceneCfg): 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.1736482, 0.), convention="world"), + offset=CameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(0.9848078, 0.0, -0.1736482, 0.0), convention="world"), ) From f72371ddffe18d98114e7cf80e5debe4835ed122 Mon Sep 17 00:00:00 2001 From: John Welsh Date: Wed, 24 Sep 2025 14:47:45 -0700 Subject: [PATCH 03/11] update docs and readme --- docs/source/overview/imitation-learning/teleop_imitation.rst | 3 ++- scripts/imitation_learning/disjoint_navigation/README.md | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/docs/source/overview/imitation-learning/teleop_imitation.rst b/docs/source/overview/imitation-learning/teleop_imitation.rst index 00037beedb4..cafc9c99556 100644 --- a/docs/source/overview/imitation-learning/teleop_imitation.rst +++ b/docs/source/overview/imitation-learning/teleop_imitation.rst @@ -652,7 +652,8 @@ To generate the navigation dataset, use the following command: --lift_step 70 \ --navigate_step 120 \ --enable_pinocchio \ - --output_file ./datasets/generated_dataset_g1_navigation.hdf5 + --output_file ./datasets/generated_dataset_g1_navigation.hdf5 \ + --enable_cameras .. note:: diff --git a/scripts/imitation_learning/disjoint_navigation/README.md b/scripts/imitation_learning/disjoint_navigation/README.md index e9c91c58955..c2d21a7e3f2 100644 --- a/scripts/imitation_learning/disjoint_navigation/README.md +++ b/scripts/imitation_learning/disjoint_navigation/README.md @@ -19,7 +19,8 @@ To run the disjoint navigation replay script execute the following command. --lift_step=70 \ --navigate_step=120 \ --enable_pinocchio \ - --output_file=datasets/dataset_generated_disjoint_nav.hdf5 + --output_file=datasets/dataset_generated_disjoint_nav.hdf5 \ + --enable_cameras ``` From f9a99316c8d5d8c4a5fdc0f7cf4565dc6144c928 Mon Sep 17 00:00:00 2001 From: John Welsh Date: Thu, 25 Sep 2025 12:58:15 -0700 Subject: [PATCH 04/11] add gr00t N1.5 note with GIF placeholder --- .../imitation-learning/teleop_imitation.rst | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/docs/source/overview/imitation-learning/teleop_imitation.rst b/docs/source/overview/imitation-learning/teleop_imitation.rst index cafc9c99556..b58098896b3 100644 --- a/docs/source/overview/imitation-learning/teleop_imitation.rst +++ b/docs/source/overview/imitation-learning/teleop_imitation.rst @@ -675,6 +675,21 @@ This process creates a dataset where the robot performs the manipulation task at ./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 +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/disjoint_navigation.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 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ From 9541cdb7bf37b2171cb5591284a97742698b045c Mon Sep 17 00:00:00 2001 From: John Welsh Date: Thu, 2 Oct 2025 10:53:43 -0700 Subject: [PATCH 05/11] add gif of locomanipulation policy rollout --- docs/source/overview/imitation-learning/teleop_imitation.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/source/overview/imitation-learning/teleop_imitation.rst b/docs/source/overview/imitation-learning/teleop_imitation.rst index b58098896b3..f22a44f917c 100644 --- a/docs/source/overview/imitation-learning/teleop_imitation.rst +++ b/docs/source/overview/imitation-learning/teleop_imitation.rst @@ -679,7 +679,7 @@ The data generated from this locomanipulation pipeline can also be used to finet 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/disjoint_navigation.gif +.. 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 From 7bb37ebb39a1bab1d94a02af8817c7fe1542ae40 Mon Sep 17 00:00:00 2001 From: John Welsh Date: Thu, 2 Oct 2025 18:54:38 -0700 Subject: [PATCH 06/11] rename disjoint nav -> locomanipulation SDG --- .../imitation-learning/teleop_imitation.rst | 8 +- .../disjoint_navigation/README.md | 37 ----- .../locomanipulation_sdg/README.md | 30 ++++ .../generate_navigation.py | 128 +++++++++--------- .../plot_navigation_trajectory.py | 4 +- .../disjoint_nav_envs/__init__.py | 19 --- .../__init__.py | 2 +- .../data_classes.py} | 6 +- .../locomanipulation_sdg/envs/__init__.py | 19 +++ .../envs/g1_locomanipulation_sdg_env.py} | 38 ++++-- .../envs/locomanipulation_sdg_env.py} | 22 +-- .../envs/locomanipulation_sdg_env_cfg.py} | 22 +-- .../occupancy_map_utils.py | 0 .../path_utils.py} | 2 +- .../scene_utils.py} | 2 +- .../transform_utils.py} | 0 .../navigation_path_planner/__init__.py | 6 - 17 files changed, 170 insertions(+), 175 deletions(-) delete mode 100644 scripts/imitation_learning/disjoint_navigation/README.md create mode 100644 scripts/imitation_learning/locomanipulation_sdg/README.md rename scripts/imitation_learning/{disjoint_navigation => locomanipulation_sdg}/generate_navigation.py (86%) rename scripts/imitation_learning/{disjoint_navigation => locomanipulation_sdg}/plot_navigation_trajectory.py (95%) delete mode 100644 source/isaaclab_mimic/isaaclab_mimic/disjoint_nav_envs/__init__.py rename source/isaaclab_mimic/isaaclab_mimic/{navigation_path_planner/disjoint_nav => locomanipulation_sdg}/__init__.py (77%) rename source/isaaclab_mimic/isaaclab_mimic/{navigation_path_planner/disjoint_nav/disjoint_nav_data.py => locomanipulation_sdg/data_classes.py} (94%) create mode 100644 source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/__init__.py rename source/isaaclab_mimic/isaaclab_mimic/{disjoint_nav_envs/g1_disjoint_nav_env.py => locomanipulation_sdg/envs/g1_locomanipulation_sdg_env.py} (87%) rename source/isaaclab_mimic/isaaclab_mimic/{disjoint_nav_envs/disjoint_nav_env.py => locomanipulation_sdg/envs/locomanipulation_sdg_env.py} (83%) rename source/isaaclab_mimic/isaaclab_mimic/{disjoint_nav_envs/disjoint_nav_env_cfg.py => locomanipulation_sdg/envs/locomanipulation_sdg_env_cfg.py} (55%) rename source/isaaclab_mimic/isaaclab_mimic/{navigation_path_planner/disjoint_nav => locomanipulation_sdg}/occupancy_map_utils.py (100%) rename source/isaaclab_mimic/isaaclab_mimic/{navigation_path_planner/disjoint_nav/disjoint_nav_path_utils.py => locomanipulation_sdg/path_utils.py} (99%) rename source/isaaclab_mimic/isaaclab_mimic/{navigation_path_planner/disjoint_nav/disjoint_nav_scene_utils.py => locomanipulation_sdg/scene_utils.py} (99%) rename source/isaaclab_mimic/isaaclab_mimic/{navigation_path_planner/disjoint_nav/disjoint_nav_transform_utils.py => locomanipulation_sdg/transform_utils.py} (100%) delete mode 100644 source/isaaclab_mimic/isaaclab_mimic/navigation_path_planner/__init__.py diff --git a/docs/source/overview/imitation-learning/teleop_imitation.rst b/docs/source/overview/imitation-learning/teleop_imitation.rst index f22a44f917c..fb0d3556d26 100644 --- a/docs/source/overview/imitation-learning/teleop_imitation.rst +++ b/docs/source/overview/imitation-learning/teleop_imitation.rst @@ -643,16 +643,16 @@ To generate the navigation dataset, use the following command: .. code:: bash ./isaaclab.sh -p \ - scripts/imitation_learning/disjoint_navigation/generate_navigation.py \ + scripts/imitation_learning/locomanipulation_sdg/generate_navigation.py \ --device cpu \ --kit_args="--enable isaacsim.replicator.mobility_gen" \ - --task="Isaac-G1-Disjoint-Navigation" \ + --task="Isaac-G1-Locomanipulation-SDG" \ --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:: @@ -673,7 +673,7 @@ 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_navigation.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 diff --git a/scripts/imitation_learning/disjoint_navigation/README.md b/scripts/imitation_learning/disjoint_navigation/README.md deleted file mode 100644 index c2d21a7e3f2..00000000000 --- a/scripts/imitation_learning/disjoint_navigation/README.md +++ /dev/null @@ -1,37 +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 \ - --enable_cameras -``` - - -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/locomanipulation_sdg/README.md b/scripts/imitation_learning/locomanipulation_sdg/README.md new file mode 100644 index 00000000000..35f480f1aff --- /dev/null +++ b/scripts/imitation_learning/locomanipulation_sdg/README.md @@ -0,0 +1,30 @@ +# Locomanipulation SDG + +This folder contains code for running the locomanipulation SDG data generation script. +This assumes that you have collected a static manipulation dataset. + + +## Usage + +To run the locomanipulation SDG replay script execute the following command. + + +```bash +./isaaclab.sh -p \ + scripts/imitation_learning/locomanipulation_sdg/generate_navigation.py \ + --device cpu \ + --kit_args="--enable isaacsim.replicator.mobility_gen" \ + --task="Isaac-G1-Locomanipulation-SDG" \ + --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_locomanipulation_sdg.hdf5 \ + --enable_cameras +``` + + +Please check ``replay.py`` for details on the arguments. + +To view the generated trajectories diff --git a/scripts/imitation_learning/disjoint_navigation/generate_navigation.py b/scripts/imitation_learning/locomanipulation_sdg/generate_navigation.py similarity index 86% rename from scripts/imitation_learning/disjoint_navigation/generate_navigation.py rename to scripts/imitation_learning/locomanipulation_sdg/generate_navigation.py index 78d5c5c7477..46f80d97d70 100644 --- a/scripts/imitation_learning/disjoint_navigation/generate_navigation.py +++ b/scripts/imitation_learning/locomanipulation_sdg/generate_navigation.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..6e628711a8a --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/__init__.py @@ -0,0 +1,19 @@ +# 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 + +from .g1_locomanipulation_sdg_env import G1LocomanipulationSDGEnvCfg + +gym.register( + id="Isaac-G1-Locomanipulation-SDG", + entry_point="isaaclab_mimic.locomanipulation_sdg.envs.g1_locomanipulation_sdg_env:G1LocomanipulationSDGEnv", + kwargs={ + "env_cfg_entry_point": 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.""" From bcb835aa7da28394a463a057f0c4bceebd06a8de Mon Sep 17 00:00:00 2001 From: John Welsh Date: Fri, 3 Oct 2025 15:31:51 -0700 Subject: [PATCH 07/11] address PR feedback --- .../overview/imitation-learning/teleop_imitation.rst | 12 ++++++------ .../locomanipulation_sdg/README.md | 2 +- .../{generate_navigation.py => generate_data.py} | 0 3 files changed, 7 insertions(+), 7 deletions(-) rename scripts/imitation_learning/locomanipulation_sdg/{generate_navigation.py => generate_data.py} (100%) diff --git a/docs/source/overview/imitation-learning/teleop_imitation.rst b/docs/source/overview/imitation-learning/teleop_imitation.rst index fb0d3556d26..cf29adbf659 100644 --- a/docs/source/overview/imitation-learning/teleop_imitation.rst +++ b/docs/source/overview/imitation-learning/teleop_imitation.rst @@ -636,14 +636,14 @@ 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/locomanipulation_sdg/generate_navigation.py \ + scripts/imitation_learning/locomanipulation_sdg/generate_data.py \ --device cpu \ --kit_args="--enable isaacsim.replicator.mobility_gen" \ --task="Isaac-G1-Locomanipulation-SDG" \ @@ -659,10 +659,10 @@ To generate the navigation dataset, use the following command: 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. diff --git a/scripts/imitation_learning/locomanipulation_sdg/README.md b/scripts/imitation_learning/locomanipulation_sdg/README.md index 35f480f1aff..588e552747f 100644 --- a/scripts/imitation_learning/locomanipulation_sdg/README.md +++ b/scripts/imitation_learning/locomanipulation_sdg/README.md @@ -11,7 +11,7 @@ To run the locomanipulation SDG replay script execute the following command. ```bash ./isaaclab.sh -p \ - scripts/imitation_learning/locomanipulation_sdg/generate_navigation.py \ + scripts/imitation_learning/locomanipulation_sdg/generate_data.py \ --device cpu \ --kit_args="--enable isaacsim.replicator.mobility_gen" \ --task="Isaac-G1-Locomanipulation-SDG" \ diff --git a/scripts/imitation_learning/locomanipulation_sdg/generate_navigation.py b/scripts/imitation_learning/locomanipulation_sdg/generate_data.py similarity index 100% rename from scripts/imitation_learning/locomanipulation_sdg/generate_navigation.py rename to scripts/imitation_learning/locomanipulation_sdg/generate_data.py From aafe10c9e4d609ddff0214d5ded7c65d7d45a8fb Mon Sep 17 00:00:00 2001 From: John Welsh Date: Fri, 3 Oct 2025 15:34:35 -0700 Subject: [PATCH 08/11] fix hdf5 output path --- docs/source/overview/imitation-learning/teleop_imitation.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/source/overview/imitation-learning/teleop_imitation.rst b/docs/source/overview/imitation-learning/teleop_imitation.rst index cf29adbf659..4027570d2d8 100644 --- a/docs/source/overview/imitation-learning/teleop_imitation.rst +++ b/docs/source/overview/imitation-learning/teleop_imitation.rst @@ -673,7 +673,7 @@ This process creates a dataset where the robot performs the manipulation task at .. code:: bash - ./isaaclab.sh -p scripts/imitation_learning/locomanipulation_sdg/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 From 320119ac69c2afd72be90de7b397a9075dd55c9a Mon Sep 17 00:00:00 2001 From: John Welsh Date: Tue, 7 Oct 2025 10:49:55 -0700 Subject: [PATCH 09/11] Address PR comments --- .../imitation-learning/teleop_imitation.rst | 2 +- .../locomanipulation_sdg/README.md | 30 ------------------- .../locomanipulation_sdg/envs/__init__.py | 8 ++--- 3 files changed, 4 insertions(+), 36 deletions(-) delete mode 100644 scripts/imitation_learning/locomanipulation_sdg/README.md diff --git a/docs/source/overview/imitation-learning/teleop_imitation.rst b/docs/source/overview/imitation-learning/teleop_imitation.rst index 4027570d2d8..f98d1d9912e 100644 --- a/docs/source/overview/imitation-learning/teleop_imitation.rst +++ b/docs/source/overview/imitation-learning/teleop_imitation.rst @@ -646,7 +646,7 @@ To generate the locomanipulation dataset, use the following command: scripts/imitation_learning/locomanipulation_sdg/generate_data.py \ --device cpu \ --kit_args="--enable isaacsim.replicator.mobility_gen" \ - --task="Isaac-G1-Locomanipulation-SDG" \ + --task="Isaac-G1-SteeringWheel-Pickup-Dropoff" \ --dataset ./datasets/generated_dataset_g1_locomanip.hdf5 \ --num_runs 1 \ --lift_step 70 \ diff --git a/scripts/imitation_learning/locomanipulation_sdg/README.md b/scripts/imitation_learning/locomanipulation_sdg/README.md deleted file mode 100644 index 588e552747f..00000000000 --- a/scripts/imitation_learning/locomanipulation_sdg/README.md +++ /dev/null @@ -1,30 +0,0 @@ -# Locomanipulation SDG - -This folder contains code for running the locomanipulation SDG data generation script. -This assumes that you have collected a static manipulation dataset. - - -## Usage - -To run the locomanipulation SDG replay script execute the following command. - - -```bash -./isaaclab.sh -p \ - scripts/imitation_learning/locomanipulation_sdg/generate_data.py \ - --device cpu \ - --kit_args="--enable isaacsim.replicator.mobility_gen" \ - --task="Isaac-G1-Locomanipulation-SDG" \ - --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_locomanipulation_sdg.hdf5 \ - --enable_cameras -``` - - -Please check ``replay.py`` for details on the arguments. - -To view the generated trajectories diff --git a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/__init__.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/__init__.py index 6e628711a8a..446d3a7e9bd 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/__init__.py +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/__init__.py @@ -7,13 +7,11 @@ import gymnasium as gym -from .g1_locomanipulation_sdg_env import G1LocomanipulationSDGEnvCfg - gym.register( - id="Isaac-G1-Locomanipulation-SDG", - entry_point="isaaclab_mimic.locomanipulation_sdg.envs.g1_locomanipulation_sdg_env:G1LocomanipulationSDGEnv", + id="Isaac-G1-SteeringWheel-Pickup-Dropoff", + entry_point=f"{__name__}.g1_locomanipulation_sdg_env:G1LocomanipulationSDGEnv", kwargs={ - "env_cfg_entry_point": G1LocomanipulationSDGEnvCfg, + "env_cfg_entry_point": f"{__name__}.g1_locomanipulation_sdg_env:G1LocomanipulationSDGEnvCfg", }, disable_env_checker=True, ) From 7c07e972a4b400d6e8a8f30b64bd2a59b472a837 Mon Sep 17 00:00:00 2001 From: John Welsh Date: Tue, 7 Oct 2025 10:53:20 -0700 Subject: [PATCH 10/11] rename task --- docs/source/overview/imitation-learning/teleop_imitation.rst | 2 +- .../isaaclab_mimic/locomanipulation_sdg/envs/__init__.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/source/overview/imitation-learning/teleop_imitation.rst b/docs/source/overview/imitation-learning/teleop_imitation.rst index f98d1d9912e..bfc82310e1f 100644 --- a/docs/source/overview/imitation-learning/teleop_imitation.rst +++ b/docs/source/overview/imitation-learning/teleop_imitation.rst @@ -646,7 +646,7 @@ To generate the locomanipulation dataset, use the following command: scripts/imitation_learning/locomanipulation_sdg/generate_data.py \ --device cpu \ --kit_args="--enable isaacsim.replicator.mobility_gen" \ - --task="Isaac-G1-SteeringWheel-Pickup-Dropoff" \ + --task="Isaac-G1-SteeringWheel-Locomanipulation" \ --dataset ./datasets/generated_dataset_g1_locomanip.hdf5 \ --num_runs 1 \ --lift_step 70 \ diff --git a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/__init__.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/__init__.py index 446d3a7e9bd..d73d89b0b06 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/__init__.py +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/envs/__init__.py @@ -8,7 +8,7 @@ import gymnasium as gym gym.register( - id="Isaac-G1-SteeringWheel-Pickup-Dropoff", + 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", From c610593abac10a9029d17e018e2fe3269177e2a4 Mon Sep 17 00:00:00 2001 From: John Welsh Date: Tue, 14 Oct 2025 15:36:08 -0700 Subject: [PATCH 11/11] rename camera usd path --- .../locomanipulation/pick_place/locomanipulation_g1_env_cfg.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 63c938c8237..312e726f064 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 @@ -86,7 +86,7 @@ class LocomanipulationG1SceneCfg(InteractiveSceneCfg): ) robot_pov_cam = CameraCfg( - prim_path="/World/envs/env_.*/Robot/torso_link/d435_link/tiled_camera", + prim_path="/World/envs/env_.*/Robot/torso_link/d435_link/camera", update_period=0.0, height=160, width=256,