Adds locomanipulation data generation via. disjoint navigation #3259

jaybdub · 2025-08-22T22:47:06Z

Description

This PR adds locomanipulation data generation via disjoint navigation. It allows the user to replay static manipulation data recorded with the G1 through an end to end pick-up -> navigate -> drop-off pipeline.

The code for this PR is contained in the folder scripts/isaaclab/imitation_learning/disjoint_navigation, and does not modify existing IsaacLab extensions.

The code is as follows:

replay.py - Main entry script for loading the static-manipulation dataset and generating locomanipulation dataset
common.py - Common classes and abstract base classes for the data generation pipeline. This code may be re-used by different scenarios.
occupancy_map.py - Helper class and methods for working with occupancy maps.
path_utils.py - Helper class used by path controller for following a path
display_dataset.py -- Utility script for visualizing generated trajectories with matplotlib
g1_29dof.py - The example scenario using G1 + WBC with random forklifts and boxes. This may be copied modified by end users to support their own scenarios.
visualization.py - Code to visualize occupancy map in USD stage

Type of change

New feature (non-breaking change which adds functionality)

Screenshots

Please attach before and after screenshots of the change if applicable.

Checklist

I have run the pre-commit checks with ./isaaclab.sh --format
I have made corresponding changes to the documentation
My changes generate no new warnings
I have added tests that prove my fix is effective or that my feature works
I have updated the changelog and the corresponding version in the extension's config/extension.toml file
I have added my name to the CONTRIBUTORS.md or my name already exists there

michaellin6

Made a first pass. Will go more in depth again later.

scripts/imitation_learning/disjoint_navigation/common.py

scripts/imitation_learning/disjoint_navigation/display_dataset.py

scripts/imitation_learning/disjoint_navigation/g1_29dof.py

huihuaNvidia2023

This PR needs to run the lint check as it is missing necessary docstrings

scripts/imitation_learning/disjoint_navigation/common.py

docs/source/overview/locomanipulation.rst

huihuaNvidia2023 · 2025-08-30T00:12:26Z

docs/source/overview/locomanipulation.rst

+1. Record a static manipulation trajectory of "picking up" and "dropping off" an object.
+   In this phase, the robot base is stationary.  This is done by human teleoperation.
+
+2. Augment the static manipulation trajectory using mimic data generation pipeline.  This will


Suggested change

2. Augment the static manipulation trajectory using mimic data generation pipeline. This will

2. Augment the in-place manipulation trajectory using mimic data generation pipeline. This will

@michaellin6 can you please confirm? Is the data collected using a floating base config or fixed base config?

huihuaNvidia2023 · 2025-08-30T00:16:51Z

docs/source/overview/locomanipulation.rst

+        --output_file_name=dataset_generated_disjoint_nav.hdf5
+
+
+If you are using a different trajectory, you will need to change some parameters.  Notably, you will need to set


Suggested change

If you are using a different trajectory, you will need to change some parameters. Notably, you will need to set

If you are using a different trajectory, you will need to change some parameters. Notably, you will need to set

huihuaNvidia2023 · 2025-08-30T00:18:16Z

docs/source/overview/locomanipulation.rst

+
+If you are using a different trajectory, you will need to change some parameters.  Notably, you will need to set
+
+- --lift_step - The step index where the robot has finished grasping the object and is ready to lift it


In mimic, they use the object height change for this purpose. Can we do the same here? This is quite manual.

Good idea. Since we haven't tested this yet, I would prefer to add this post-merge. We can expose the ability to do this manually or automatically.

huihuaNvidia2023 · 2025-08-30T00:18:42Z

scripts/imitation_learning/disjoint_navigation/README.md

@@ -0,0 +1,25 @@
+# Disjoint Navigation
+
+This folder contains code for running the disjoint navigation data generation script.  This assumes that you have collected a static manipulation dataset.


Suggested change

This folder contains code for running the disjoint navigation data generation script. This assumes that you have collected a static manipulation dataset.

This folder contains code for running the disjoint navigation data generation script. This assumes that you have collected a static manipulation dataset.

scripts/imitation_learning/disjoint_navigation/common.py

scripts/imitation_learning/disjoint_navigation/mdp/__init__.py

huihuaNvidia2023 · 2025-08-30T00:20:38Z

scripts/imitation_learning/disjoint_navigation/mdp/actions.py

+    num_hand_joints=14,
+    show_ik_warnings=False,
+    variable_input_tasks=[
+        FrameTask(


I assume this needs to be updated with @michaellin6's latest update.

michaellin6 · 2025-09-04T00:02:16Z

docs/source/overview/locomanipulation.rst

+   end-to-end locomanipulation trajectories by combining the static manipulation sequences with
+   path planning.
+
+Step 1 - Static manipulation teleoperation


Here you can link the teleop_imitation.rst sections that have more detailed instructions on this

michaellin6 · 2025-09-04T00:03:34Z

docs/source/overview/locomanipulation.rst

+.. code:: bash
+
+    ./isaaclab.sh -p \
+        scripts/imitation_learning/disjoint_navigation/replay.py \


I think it may be confusing for a user that the script is called replay. Can we name it something else? Like generate_navigation?

michaellin6 · 2025-09-04T00:05:23Z

scripts/imitation_learning/disjoint_navigation/README.md

+
+```bash
+./isaaclab.sh -p \
+    scripts/imitation_learning/disjoint_navigation/replay.py \


Same as above. Replay seems misleading.

michaellin6 · 2025-09-04T00:06:42Z

scripts/imitation_learning/disjoint_navigation/common.py

+import isaaclab.utils.math as math_utils
+
+
+def transform_to_matrix(transform: torch.Tensor):


There is already tools in Isaac Lab for this. check out unmake_pose in source/isaaclab/isaaclab/utils/math.py

This function uses unmake_pose under the hood. It's a helper function to reduce repeated / copied code.

I want to vote against the shim wrapper that introduces another naming-convention in doing common math.

if it just saves few lines just use unmake_pose, don't make a wrapper

I agree, we should just use unmake_pose directly in the code to avoid another abstraction layer

We have reduced these functions as much as possible and use make_pose/unmake_pose where possible

michaellin6 · 2025-09-04T00:07:06Z

scripts/imitation_learning/disjoint_navigation/common.py

+    return pose_matrix
+
+
+def transform_from_matrix(matrix: torch.Tensor):


There is already tools in Isaac Lab for this. check out make_pose in source/isaaclab/isaaclab/utils/math.py

This function uses make_pose under the hood.

michaellin6 · 2025-09-04T00:07:26Z

scripts/imitation_learning/disjoint_navigation/common.py

+    return torch.cat([pos, quat], dim=-1)
+
+
+def transform_inv(transform: torch.Tensor):


There is already tools in Isaac Lab for this. check out pose_inv in source/isaaclab/isaaclab/utils/math.py

Hi @michaellin6 . This function actually uses pose_inv under the hood. I started by using the math functions directly. This quickly became cumbersome and there was a lot of repeated code. These transform_ functions were added organically to remove code redundancy.

michaellin6 · 2025-09-04T00:08:29Z

scripts/imitation_learning/disjoint_navigation/common.py

+    return transform_from_matrix(matrix)
+
+
+def transform_mul(transform_a, transform_b):


for all these math functions, please check that they are already available in IL

Similar to above. These methods use existing utilities under the hood. They are helper functions added to remove a bit of duplicate code inside the pipeline, and standardize on a [translation,quaternion] format for pose to reduce bookkeeping.

It maybe acceptable to have a one or two shim wrapper for cumbersome utility, but do we need these many? and most of them seem to only save 1 or 2 lines.

we have simplified the math utility functions. Still need some of them as the navigation code works with pose vectors [translation, quaternion], rather than homogeneous transforms.

michaellin6 · 2025-09-04T00:15:04Z

scripts/imitation_learning/disjoint_navigation/common.py

In general, this common file is kind of large and seems like a big mix of many components. Can we split it up a bit? Some seem to be utilities, and some seem to be abstract classes which are different.

ooctipus · 2025-09-04T20:54:48Z

scripts/imitation_learning/disjoint_navigation/g1_disjoint_nav_env.py

+
+
+class G1DisjointNavScenario(DisjointNavScenario):
+


why are we defining a new environment in script? shoudn't that be apart of isaaclab_task?

this has been moved to isaaclab_mimic, as the env is really just for data generation purpose, we found that to be more appropriate.

ooctipus · 2025-09-04T21:07:18Z

scripts/imitation_learning/disjoint_navigation/replay.py

+
+from isaaclab.app import AppLauncher
+
+# Launch Isaac Lab


why is there a replay.py and there is also a tools/replay_demos.py?

script has been renamed and refactored. Now it is called generate_navigation.py and we have improved the organization of the script.

ooctipus · 2025-09-04T21:08:15Z

scripts/imitation_learning/disjoint_navigation/policy/g1/agile_locomotion.pt

this probably should no go into github, maybe nucleus

this is in nucleus. Its outdated.

ooctipus

Thanks for the PR. It does looks like an interesting environment and a lot of cool utilities.

The main concern is how the files and utility are introduced diverge quite a lot from the rest of isaaclab components. It almost feel like a new repository build on top of isaac lab rather than something that sits nicely in isaaclab and extend features. I was not prepared to see so enviornment, mdps, and policy replay all defined under scripts. Other changes to the core seems to have some duplicates with other PR, and it is hard to judge what changes it make. If this PR depends on other PRs, we might need to merge other PR first then spent more time reviewing this.

Thanks for the PR again,
but for above reason it doesn't quite make sense to merge it in main at current stage.

I am not sure how much work this PR was done with in loop with other folks in Mimic Team. Please work with them closely to ensure this MR is an extension improves on existing functionity and feature, rather than whole new pipeline. If it is whole new pipeline we need better code quality and discussion to make sure it is done right.

kellyguo11

This PR will need a lot of work, please review the repo structure (https://isaac-sim.github.io/IsaacLab/main/source/overview/developer-guide/repo_structure.html) and contribution guidelines (https://isaac-sim.github.io/IsaacLab/main/source/refs/contributing.html#) to get an understanding of where files should belong and the coding style guides we follow.

Many of the scripts are lacking comments and docstrings, making it very difficult to understand. Would be great to do a pass through everything to make sure we have all the components outlined in the contribution guidelines covered.

kellyguo11 · 2025-09-04T23:49:13Z

docs/index.rst

   source/overview/augmented_imitation
   source/overview/showroom
   source/overview/simple_agents
+   source/overview/locomanipulation


can we move this under the imitation learning section - https://isaac-sim.github.io/IsaacLab/main/source/overview/imitation-learning/index.html? it seems more relevant to teleoperation and imitation learning

I think this is more of a placeholder. We will move all documentation in this PR into teleop_imitation.rst

moved docs to appropriate location

scripts/imitation_learning/disjoint_navigation/README.md

kellyguo11 · 2025-09-04T23:53:21Z

scripts/imitation_learning/disjoint_navigation/common.py

+import isaaclab.utils.math as math_utils
+
+
+def transform_to_matrix(transform: torch.Tensor):


I agree, we should just use unmake_pose directly in the code to avoid another abstraction layer

scripts/imitation_learning/disjoint_navigation/common.py

kellyguo11 · 2025-09-05T00:01:22Z

scripts/imitation_learning/disjoint_navigation/common.py

+"""Launch Isaac Sim Simulator first."""
+
+
+import enum


what is the purpose of this script? it doesn't seem like a script we can run directly. the Scripts folder should hold scripts we can run directly from python. this seems to belong more into the mimic extension or its own extension

this was refactored into multiple components and split up into different files for path planning, navigation base classes and and scene utilities

scripts/imitation_learning/disjoint_navigation/mdp/actions.py

kellyguo11 · 2025-09-05T00:15:45Z

scripts/imitation_learning/disjoint_navigation/occupancy_map.py

@@ -0,0 +1,665 @@
+# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).


we should pick one license for this, if it's for mimic, we can use Apach

Picked Apache V2

scripts/imitation_learning/disjoint_navigation/occupancy_map.py

scripts/imitation_learning/disjoint_navigation/path_utils.py

jaybdub · 2025-09-05T16:17:43Z

Hi @kellyguo11 @ooctipus,

Thank you for taking the time to review this PR.

I have force-pushed an update that now places these changes on top of #3150. I hope this makes the diff cleaner.

For context: the changes that this PR introduces are currently all contained in scripts/imitation_learning/disjoint_navigation/. We discussed with the mimic team that this data generation pipeline would be a separate script from the existing mimic pipeline, reusing the existing teleoperation and mimic data generation code to create input datasets for our pipeline.

I'm happy to address any restructuring needs to better align with Isaac Lab's format. Here are two organizational approaches for your consideration:

Current structure:

scripts/imitation_learning/disjoint_navigation/
├── replay.py
├── occupancy_map.py
├── visualization.py
├── common.py
├── path_utils.py
└── g1_disjoint_nav_env.py

Option 1: Integrated with Isaac Lab Tasks Structure

Proposed reorganization:

Environment definition

source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/
└── disjoint_navigation/
└── g1_disjoint_nav_env.py

Data generation script and utilities

scripts/imitation_learning/disjoint_nav/
├── run_data_generation.py # (renamed from replay.py)
├── occupancy_map_utils.py # (renamed from occupancy_map.py + visualization.py)
├── path_planning_utils.py # (path planning from common.py + path_utils.py)
├── scene_randomization_utils.py # (scene randomization from common.py)
├── base_classes.py # (base classes from common.py)
└── data_classes.py # (data classes from common.py)

Option 2: Integrated with Isaac Lab Mimic Structure

Proposed reorganization:

Data generation script

scripts/imitation_learning/isaaclab_mimic/disjoint_nav/
└── run_data_generation.py

Helper functions and utilities

source/isaaclab_mimic/isaaclab_mimic/disjoint_nav/
├── occupancy_map_utils.py
├── path_planning_utils.py
├── scene_randomization_utils.py
├── base_classes.py
└── data_classes.py

Environment definition

source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/
└── disjoint_navigation/
└── g1_disjoint_nav_env.py

Both options include plans to add comprehensive documentation, type hints, and adherence to Isaac Lab code style guidelines.

Looking forward to your thoughts.
Best,
John

docs/source/overview/locomanipulation.rst

scripts/imitation_learning/disjoint_navigation/generate_navigation.py

source/isaaclab_mimic/isaaclab_mimic/disjoint_nav_envs/__init__.py

docs/source/overview/locomanipulation.rst

scripts/imitation_learning/disjoint_navigation/generate_navigation.py

kellyguo11 · 2025-09-07T06:30:56Z

source/isaaclab_mimic/isaaclab_mimic/disjoint_nav_envs/__init__.py

@@ -0,0 +1,8 @@
+# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).


in the proposal, I believe the environment was placed under isaaclab_tasks. is there a reason why this is now under isaaclab_mimic?

We took this cue from the existing isaaclab_mimic codebase, where the "mimic-specific" environments are defined in isaaclab_mimic.

I see, should we group this together with isaaclab_mimic/envs or is it preferred to keep it separate?

source/isaaclab_mimic/isaaclab_mimic/navigation_path_planner/disjoint_nav/disjoint_nav_data.py

source/isaaclab_assets/isaaclab_assets/robots/unitree.py

kellyguo11 · 2025-09-07T06:41:52Z

can you please run ./isaaclab.sh -f for the linter checks when the changes are ready?

also seeing some test failures in CI currently:

| /workspace/isaaclab/source/isaaclab/test/controllers/test_pink_ik_components.py        | FAILED |     0.00 |   0/1   |
| /workspace/isaaclab/source/isaaclab/test/controllers/test_controller_utils.py          | FAILED |     0.06 |  26/32  |
| /workspace/isaaclab/source/isaaclab/test/controllers/test_pink_ik.py                   | FAILED |   244.45 |  11/24  |
| /workspace/isaaclab/source/isaaclab/test/controllers/test_local_frame_task.py          | FAILED |     0.00 |   0/1   |

michaellin6 · 2025-09-08T18:02:34Z

can you please run ./isaaclab.sh -f for the linter checks when the changes are ready?

also seeing some test failures in CI currently:

| /workspace/isaaclab/source/isaaclab/test/controllers/test_pink_ik_components.py        | FAILED |     0.00 |   0/1   |
| /workspace/isaaclab/source/isaaclab/test/controllers/test_controller_utils.py          | FAILED |     0.06 |  26/32  |
| /workspace/isaaclab/source/isaaclab/test/controllers/test_pink_ik.py                   | FAILED |   244.45 |  11/24  |
| /workspace/isaaclab/source/isaaclab/test/controllers/test_local_frame_task.py          | FAILED |     0.00 |   0/1   |

Fixed the unit tests in the upstream PR, and rebased this PR. Also, checked that formatting is passing.

michaellin6

Changes look good to me. From the refactoring I see that:

We are now using the task from locomanipulation_g1_env from isaaclab_tasks
We've moved the environments and config files used for navigation data generation into isaaclab_mimic which is more appropriate as that is where other mimic data generation envrionments and config files are.
Reduced the code in scripts/ to really only scripts
Improved the code documentation and readability.

LGTM. If other reviewers can also take a look, it would be great.

- Add data generation scripts to add navigation data to manipulation datasets - Implement DisjointNavEnv base class and G1-specific environment - Add path planning utilities with occupancy mapping and trajectory generation - Include visualization tools for trajectory analysis and debugging - Update documentation for imitation learning and locomanipulation workflows Co-authored-by: John Welsh <jwelsh@nvidia.com> Co-authored-by: Michael Lin <michalin@nvidia.com> Co-authored-by: Huihua Zhao <huihuaz@nvidia.com>

michaellin6 reviewed Aug 28, 2025

View reviewed changes

jaybdub changed the title ~~[WIP] Disjoint navigation~~ Adds locomanipulation data generation via. disjoint navigation Aug 28, 2025

jaybdub marked this pull request as ready for review August 29, 2025 18:37

jaybdub requested review from Mayankm96, jtigue-bdai, kellyguo11, ooctipus and pascal-roth as code owners August 29, 2025 18:37

huihuaNvidia2023 reviewed Aug 30, 2025

View reviewed changes

michaellin6 reviewed Sep 4, 2025

View reviewed changes

ooctipus reviewed Sep 4, 2025

View reviewed changes

ooctipus requested changes Sep 4, 2025

View reviewed changes

kellyguo11 requested changes Sep 5, 2025

View reviewed changes

jaybdub force-pushed the disjoint-navigation branch from 48ac9f7 to da6be51 Compare September 5, 2025 04:35

michaellin6 force-pushed the disjoint-navigation branch from da6be51 to 9d6b321 Compare September 5, 2025 06:18

michaellin6 force-pushed the disjoint-navigation branch 3 times, most recently from 42cc2a0 to 6d85bf3 Compare September 6, 2025 00:59

kellyguo11 reviewed Sep 6, 2025

View reviewed changes

docs/source/overview/locomanipulation.rst Outdated Show resolved Hide resolved

scripts/imitation_learning/disjoint_navigation/generate_navigation.py Outdated Show resolved Hide resolved

source/isaaclab_mimic/isaaclab_mimic/disjoint_nav_envs/__init__.py Show resolved Hide resolved

kellyguo11 reviewed Sep 7, 2025

View reviewed changes

source/isaaclab_assets/isaaclab_assets/robots/unitree.py Outdated Show resolved Hide resolved

michaellin6 force-pushed the disjoint-navigation branch from 7fd270a to 70b3e90 Compare September 8, 2025 17:57

michaellin6 force-pushed the disjoint-navigation branch from 70b3e90 to 0a7198c Compare September 8, 2025 18:16

michaellin6 approved these changes Sep 8, 2025

View reviewed changes

michaellin6 force-pushed the disjoint-navigation branch from 0a7198c to 0aa36fa Compare September 8, 2025 21:39

michaellin6 requested a review from hhansen-bdai as a code owner September 8, 2025 21:39

michaellin6 force-pushed the disjoint-navigation branch 3 times, most recently from 2920d88 to 81830fb Compare September 9, 2025 14:19

jaybdub changed the base branch from main to release/2.3.0 September 9, 2025 21:47

michaellin6 force-pushed the disjoint-navigation branch from fb59c25 to 1a5cca1 Compare September 10, 2025 01:39

Merge branch 'release/2.3.0' into disjoint-navigation

72016a2

kellyguo11 approved these changes Sep 10, 2025

View reviewed changes

kellyguo11 merged commit aabdb06 into isaac-sim:release/2.3.0 Sep 10, 2025
4 checks passed

	2. Augment the static manipulation trajectory using mimic data generation pipeline. This will
	2. Augment the in-place manipulation trajectory using mimic data generation pipeline. This will

		--output_file_name=dataset_generated_disjoint_nav.hdf5


		If you are using a different trajectory, you will need to change some parameters. Notably, you will need to set


		If you are using a different trajectory, you will need to change some parameters. Notably, you will need to set

		- --lift_step - The step index where the robot has finished grasping the object and is ready to lift it

		@@ -0,0 +1,25 @@
		# Disjoint Navigation

		This folder contains code for running the disjoint navigation data generation script. This assumes that you have collected a static manipulation dataset.

		import isaaclab.utils.math as math_utils


		def transform_to_matrix(transform: torch.Tensor):

		return pose_matrix


		def transform_from_matrix(matrix: torch.Tensor):

		return torch.cat([pos, quat], dim=-1)


		def transform_inv(transform: torch.Tensor):

		return transform_from_matrix(matrix)


		def transform_mul(transform_a, transform_b):

		@@ -0,0 +1,665 @@
		# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).

		@@ -0,0 +1,8 @@
		# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).

Adds locomanipulation data generation via. disjoint navigation #3259

Adds locomanipulation data generation via. disjoint navigation #3259

Uh oh!

Conversation

jaybdub commented Aug 22, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Type of change

Screenshots

Checklist

Uh oh!

michaellin6 left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

huihuaNvidia2023 left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

ooctipus Sep 4, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

jaybdub commented Aug 22, 2025 •

edited

Loading

ooctipus Sep 4, 2025 •

edited

Loading

ooctipus left a comment •

edited

Loading