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

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

@@ -107,6 +107,7 @@ Table of Contents
   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

kellyguo11 · 2025-09-04T23:59:55Z

scripts/imitation_learning/disjoint_navigation/common.py

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


please also make sure to follow the contribution guidelines - https://isaac-sim.github.io/IsaacLab/main/source/refs/contributing.html#type-hinting. function signature should have type hints and the docstring should include a description for each argument

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

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

scripts/imitation_learning/disjoint_navigation/mdp/actions.py

+Upper Body IK Action
+"""
+
+G1_UPPER_BODY_IK_CONTROLLER_CFG = PinkIKControllerCfg(


these should not belong in scripts

removed and refactored. This code is now using the isaaclab_tasks directly as it shoud've been.

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

kellyguo11 · 2025-09-05T00:16:26Z

scripts/imitation_learning/disjoint_navigation/occupancy_map.py

+
+
+def _omap_world_to_px(points, origin, width_meters, height_meters, width_pixels, height_pixels):
+


needs docstrings and type hints

Added hints

kellyguo11 · 2025-09-05T00:17:16Z

scripts/imitation_learning/disjoint_navigation/occupancy_map.py

+OCCUPANCY_MAP_DEFAULT_CELL_SIZE = 0.05
+
+
+class OccupancyMapDataValue(enum.IntEnum):


this file also doesn't look like a script we can run directly

this has been refactored and moved into isaaclab_mimic/navigation_path_planner/disjoint_nav/occupancy_map_utils.py

kellyguo11 · 2025-09-05T00:20:20Z

scripts/imitation_learning/disjoint_navigation/path_utils.py

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


also need to pick license here

picked Apache V2

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

… support - Implement fixed-base upper body IK and locomanipulation environments - Add G1 teleop with retargeter, hand rotation fixes, and comprehensive tests - Refactor locomotion to use retargeter instead of command manager - Enhance pink IK with kinematics model and LocalFrameTask for relative poses - Add locomanipulation policies with gravity compensation and tuned gains - Implement lower body standing retargeter with zero root velocity Co-authored-by: Michael Lin <michalin@nvidia.com> Co-authored-by: Huihua Zhao <huihuaz@nvidia.com> Co-authored-by: Rafael Wiltz <rwiltz@nvidia.com> Co-authored-by: Sergey Grizan <sgrizan@nvidia.com>

- 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>

kellyguo11 · 2025-09-06T04:39:08Z

docs/source/overview/locomanipulation.rst

@@ -0,0 +1,82 @@
+.. _locomanipulation:


should we move this page under imitation learning? not sure if it makes sense for it to be on the root level in overview

kellyguo11 · 2025-09-06T04:40:30Z

scripts/imitation_learning/disjoint_navigation/generate_navigation.py

+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+# Copyright (c) 2024-2025, The Isaac Lab Project Developers.


we can keep just the first block and remove this part of the license header

kellyguo11 · 2025-09-06T04:41:57Z

source/isaaclab_mimic/isaaclab_mimic/disjoint_nav_envs/__init__.py

+
+"""Sub-package with environment wrappers for Disjoint Navigation."""
+
+import gymnasium as gym


is this import required here?

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 jtigue-bdai, kellyguo11, Mayankm96, pascal-roth and ooctipus 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 2 times, most recently from 7024319 to 42cc2a0 Compare September 6, 2025 00:43

michaellin6 and others added 3 commits September 5, 2025 17:47

documention update

ebbea14

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

cleaning up transform utils to reduce shim functions

0d0d313

kellyguo11 reviewed Sep 6, 2025

View reviewed changes

	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).



		def _omap_world_to_px(points, origin, width_meters, height_meters, width_pixels, height_pixels):

		OCCUPANCY_MAP_DEFAULT_CELL_SIZE = 0.05


		class OccupancyMapDataValue(enum.IntEnum):

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


		"""Sub-package with environment wrappers for Disjoint Navigation."""

		import gymnasium as gym

Adds locomanipulation data generation via. disjoint navigation #3259

Are you sure you want to change the base?

Adds locomanipulation data generation via. disjoint navigation #3259

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!

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

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