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

…ed testing - Add NullSpacePostureTask for natural shoulder/waist joint postures during manipulation - Refactor IK with damping, null-space control, and joint optimization - Improve testing framework with JSON-based configurations and stricter tolerances (1mm/1° vs 30mm/10°) - Apply IK improvements across all environments (PickPlace, NutPour, ExhaustPipe) - Add waist-enabled manipulation environments and target_eef_link_names mapping - Disable IK joint limits safety checks so that it always produces a solution

the locomanipulation environment for G1. Moved policy inference to action term. Adding test files for controllers in each environment.

Adds G1 fixed base teleop support, G1 retargeter with hand rotation fixes, comprehensive unit tests with pytest, and mock device testing.

- Migrate from command manager to using retargeter. - Enhance pink IK with kinemaitcs model accounting for non-pink-controlled joints, and introducing pink task LocalFrameTask that can specify ee goal poses relative to any robot link reference frame. - Generalize observation functions for manipulation and locomanipulation tasks.

- Added locomotion and locomanipulation policies with agile observation - Enhanced IK controller with gravity compensation and tuned gains - Expanded test coverage for ik - Added unit tests for new pink kinematics config class, and new custom pink frame task

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

…cLab into disjoint-navigation

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.

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.

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.

michaellin6 and others added 12 commits August 7, 2025 16:50

Fix retargeting with the G1

3d740df

Implemented fixed-base upper body IK environment for G1 and GR1, also

713682b

the locomanipulation environment for G1. Moved policy inference to action term. Adding test files for controllers in each environment.

Implement G1 teleop with retargeter and testing

f0ace83

Adds G1 fixed base teleop support, G1 retargeter with hand rotation fixes, comprehensive unit tests with pytest, and mock device testing.

initial commit of disjoint navigation

eb54b56

parameterize transition steps

f55c6c8

add lower body obs recorder

dc4e90d

rename to obs_lower_body

962d514

add documentation

637e1df

add support for replay state recording

52217b2

michaellin6 reviewed Aug 28, 2025

View reviewed changes

jaybdub added 4 commits August 28, 2025 12:32

add locomanipulation doc

a672a24

import observation cfg from existing locomanip env

ac3216b

make obstacle spawning less verbose

95f55cd

rename disjoint nav env

fac92e7

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

jaybdub added 2 commits August 29, 2025 09:26

record obstacle poses

ab6171d

Merge branch 'disjoint-navigation' of https://github.com/jaybdub/Isaa…

4a63c4c

…cLab into disjoint-navigation

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

jaybdub added 3 commits September 2, 2025 14:34

add docstrings and doc fixes

6c1a6b8

run format

f8ac461

address code formatting changes

6cb8fb6

added additional docstrings to occupancy map

48ac9f7

michaellin6 reviewed Sep 4, 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):

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!

Uh oh!

jaybdub commented Aug 22, 2025 •

edited

Loading