Fixes grid origins in TerrainImporter to match Isaac Sim cloner (#300)

The logic for grid cloning in Isaac Sim GridCloner (func:
`get_clone_transforms()`) and in TerrainImporter.py (func:
`_compute_env_origins_grid()`) are different. Consequently, they give
inconsistent values.

This PR fixes the TerrainImporter by updating the logic of
`_compute_env_origins_grid()` to make it consistent with IsaacSim.

Fixes #287

- Bug fix (non-breaking change which fixes an issue)

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./orbit.sh --format`
- [x] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [x] I have added tests that prove my fix is effective or that my
feature works
- [x] I have run all the tests with `./orbit.sh --test` and they pass
- [x] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file
- [x] I have added my name to the `CONTRIBUTORS.md` or my name already
exists there

---------
Signed-off-by: Shafeef Omar <shafeef901@gmail.com>
Co-authored-by: Mayank Mittal <12863862+Mayankm96@users.noreply.github.com>

CONTRIBUTORS.md

@@ -43,6 +43,7 @@ Guidelines for modifications:
 * René Zurbrügg
 * Ritvik Singh
 * Rosario Scalise
+* Shafeef Omar
 
 ## Acknowledgements
 

source/extensions/omni.isaac.orbit/config/extension.toml

 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.15.4"
+version = "0.15.5"
 
 # Description
 title = "ORBIT framework for Robot Learning"

source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst

 Changelog
 ---------
 
+0.15.5 (2024-03-23)
+~~~~~~~~~~~~~~~~~~~
+
+Fixed
+^^^^^
+
+* Fixed the env origins in :meth:`_compute_env_origins_grid` of :class:`omni.isaac.orbit.terrain.TerrainImporter`
+  to match that obtained from the Isaac Sim :class:`omni.isaac.cloner.GridCloner` class.
+
+Added
+^^^^^
+
+* Added unit test to ensure consistency between environment origins generated by IsaacSim's Grid Cloner and those
+  produced by the TerrainImporter.
+
+
 0.15.4 (2024-03-22)
 ~~~~~~~~~~~~~~~~~~~
 

source/extensions/omni.isaac.orbit/omni/isaac/orbit/terrains/terrain_importer.py

@@ -351,10 +351,12 @@ class TerrainImporter:
         # create tensor based on number of environments
         env_origins = torch.zeros(num_envs, 3, device=self.device)
         # create a grid of origins
-        num_cols = np.floor(np.sqrt(num_envs))
-        num_rows = np.ceil(num_envs / num_cols)
-        xx, yy = torch.meshgrid(torch.arange(num_rows), torch.arange(num_cols), indexing="xy")
-        env_origins[:, 0] = env_spacing * xx.flatten()[:num_envs] - env_spacing * (num_rows - 1) / 2
-        env_origins[:, 1] = env_spacing * yy.flatten()[:num_envs] - env_spacing * (num_cols - 1) / 2
+        num_rows = np.ceil(num_envs / int(np.sqrt(num_envs)))
+        num_cols = np.ceil(num_envs / num_rows)
+        ii, jj = torch.meshgrid(
+            torch.arange(num_rows, device=self.device), torch.arange(num_cols, device=self.device), indexing="ij"
+        )
+        env_origins[:, 0] = -(ii.flatten()[:num_envs] - (num_rows - 1) / 2) * env_spacing
+        env_origins[:, 1] = (jj.flatten()[:num_envs] - (num_cols - 1) / 2) * env_spacing
         env_origins[:, 2] = 0.0
         return env_origins

source/extensions/omni.isaac.orbit/test/terrains/test_terrain_importer.py

+# Copyright (c) 2022-2024, The ORBIT Project Developers.
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+from __future__ import annotations
+
+"""Launch Isaac Sim Simulator first."""
+
+from omni.isaac.orbit.app import AppLauncher
+
+# launch omniverse app
+app_launcher = AppLauncher(headless=True)
+simulation_app = app_launcher.app
+
+"""Rest everything follows."""
+
+import torch
+import unittest
+
+import omni.isaac.core.utils.prims as prim_utils
+from omni.isaac.cloner import GridCloner
+
+import omni.isaac.orbit.sim as sim_utils
+from omni.isaac.orbit.terrains import TerrainImporter, TerrainImporterCfg
+
+
+class TestTerrainImporter(unittest.TestCase):
+    """Test the terrain importer for different ground and procedural terrains."""
+
+    def test_grid_clone_env_origins(self):
+        """Tests that env origins are consistent when computed using the TerrainImporter and IsaacSim GridCloner."""
+        # iterate over different number of environments and environment spacing
+        for env_spacing in [1.0, 4.325, 8.0]:
+            for num_envs in [1, 4, 125, 379, 1024]:
+                with self.subTest(num_envs=num_envs, env_spacing=env_spacing):
+                    with sim_utils.build_simulation_context(auto_add_lighting=True) as sim:
+                        # create terrain importer
+                        terrain_importer_cfg = TerrainImporterCfg(
+                            num_envs=num_envs,
+                            env_spacing=env_spacing,
+                            prim_path="/World/ground",
+                            terrain_type="plane",  # for flat ground, origins are in grid
+                            terrain_generator=None,
+                        )
+                        terrain_importer = TerrainImporter(terrain_importer_cfg)
+                        # obtain env origins using terrain importer
+                        terrain_importer_origins = terrain_importer.env_origins
+
+                        # obtain env origins using grid cloner
+                        grid_cloner_origins = self.obtain_grid_cloner_env_origins(
+                            num_envs, env_spacing, device=sim.device
+                        )
+
+                        # check if the env origins are the same
+                        torch.testing.assert_close(terrain_importer_origins, grid_cloner_origins, rtol=1e-5, atol=1e-5)
+
+    """
+    Helper functions.
+    """
+
+    @staticmethod
+    def obtain_grid_cloner_env_origins(num_envs: int, env_spacing: float, device: str) -> torch.Tensor:
+        """Obtain the env origins generated by IsaacSim GridCloner (grid_cloner.py)."""
+        # create grid cloner
+        cloner = GridCloner(spacing=env_spacing)
+        cloner.define_base_env("/World/envs")
+        envs_prim_paths = cloner.generate_paths("/World/envs/env", num_paths=num_envs)
+        prim_utils.define_prim("/World/envs/env_0")
+        # clone envs using grid cloner
+        env_origins = cloner.clone(
+            source_prim_path="/World/envs/env_0", prim_paths=envs_prim_paths, replicate_physics=True
+        )
+        # return as tensor
+        return torch.tensor(env_origins, dtype=torch.float32, device=device)
+
+
+if __name__ == "__main__":
+    # run main
+    runner = unittest.main(verbosity=2, exit=False)
+    # close sim app
+    simulation_app.close()
+    # report success
+    exit(not runner.result.wasSuccessful())