From ea711bcf8064430b6f867973dab928ae0f2971cb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Anton=20Bj=C3=B8rndahl=20Mortensen?=
 <56405924+abmoRobotics@users.noreply.github.com>
Date: Tue, 9 Jan 2024 14:11:11 +0100
Subject: [PATCH] Fixes AttributeError in terrain-based position command (#191)

Fixes AttributeError in position_command.py and adds a dummy function to TerrainImporter for sampling target locations.

Fixes #189

- 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`
- [ ] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [ ] I have added tests that prove my fix is effective or that my
feature works
- [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
---
 CONTRIBUTORS.md                               |  1 +
 .../omni.isaac.orbit/config/extension.toml    |  2 +-
 .../omni.isaac.orbit/docs/CHANGELOG.rst       | 14 ++++++++++++
 .../envs/mdp/commands/position_command.py     |  4 ++--
 .../isaac/orbit/terrains/terrain_importer.py  | 22 ++++++++++++++++++-
 5 files changed, 39 insertions(+), 4 deletions(-)

diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md
index 1fb51d7165..113f5863c0 100644
--- a/CONTRIBUTORS.md
+++ b/CONTRIBUTORS.md
@@ -29,6 +29,7 @@ Guidelines for modifications:
 
 ## Contributors
 
+* Anton Bjørndahl Mortensen
 * Alice Zhou
 * Andrej Orsula
 * Antonio Serrano-Muñoz
diff --git a/source/extensions/omni.isaac.orbit/config/extension.toml b/source/extensions/omni.isaac.orbit/config/extension.toml
index 5fbf3e8ee1..17ce4be821 100644
--- a/source/extensions/omni.isaac.orbit/config/extension.toml
+++ b/source/extensions/omni.isaac.orbit/config/extension.toml
@@ -1,7 +1,7 @@
 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.10.10"
+version = "0.10.11"
 
 # Description
 title = "ORBIT framework for Robot Learning"
diff --git a/source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst b/source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst
index dcffd5c64e..2afb582d6c 100644
--- a/source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst
+++ b/source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst
@@ -1,6 +1,19 @@
 Changelog
 ---------
 
+0.10.11 (2024-01-08)
+~~~~~~~~~~~~~~~~~~~~
+
+Fixed
+^^^^^
+
+* Fixed attribute error raised when calling the :class:`omni.isaac.orbit.envs.mdp.TerrainBasedPositionCommand`
+  command term.
+* Added a dummy function in :class:`omni.isaac.orbit.terrain.TerrainImporter` that returns environment
+  origins as terrain-aware sampled targets. This function should be implemented by child classes based on
+  the terrain type.
+
+
 0.10.10 (2023-12-21)
 ~~~~~~~~~~~~~~~~~~~~
 
@@ -11,6 +24,7 @@ Fixed
   by ensuring that the extension ``omni.kit.viewport.window`` is enabled in :class:`omni.isaac.orbit.app.AppLauncher` when
   livestreaming is enabled
 
+
 0.10.9 (2023-12-21)
 ~~~~~~~~~~~~~~~~~~~
 
diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/envs/mdp/commands/position_command.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/envs/mdp/commands/position_command.py
index a9d9d81daa..1d12f5c9ae 100644
--- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/envs/mdp/commands/position_command.py
+++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/envs/mdp/commands/position_command.py
@@ -106,12 +106,12 @@ def _update_command(self):
         """Re-target the position command to the current root position and heading."""
         target_vec = self.pos_command_w - self.robot.data.root_pos_w[:, :3]
         self.pos_command_b[:] = quat_rotate_inverse(yaw_quat(self.robot.data.root_quat_w), target_vec)
-        self.heading_command_b[:] = wrap_to_pi(self.heading_command_w - self.robot.heading_w)
+        self.heading_command_b[:] = wrap_to_pi(self.heading_command_w - self.robot.data.heading_w)
 
     def _update_metrics(self):
         # logs data
         self.metrics["error_pos"] = torch.norm(self.pos_command_w - self.robot.data.root_pos_w[:, :3], dim=1)
-        self.metrics["error_heading"] = torch.abs(wrap_to_pi(self.heading_command_w - self.robot.heading_w))
+        self.metrics["error_heading"] = torch.abs(wrap_to_pi(self.heading_command_w - self.robot.data.heading_w))
 
     def _set_debug_vis_impl(self, debug_vis: bool):
         # create markers if necessary for the first tome
diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/terrains/terrain_importer.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/terrains/terrain_importer.py
index e573059e8b..06eab00a38 100644
--- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/terrains/terrain_importer.py
+++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/terrains/terrain_importer.py
@@ -8,7 +8,7 @@
 import numpy as np
 import torch
 import trimesh
-from typing import TYPE_CHECKING
+from typing import TYPE_CHECKING, Sequence
 
 import warp
 from pxr import UsdGeom
@@ -304,6 +304,26 @@ def update_env_origins(self, env_ids: torch.Tensor, move_up: torch.Tensor, move_
         # update the env origins
         self.env_origins[env_ids] = self.terrain_origins[self.terrain_levels[env_ids], self.terrain_types[env_ids]]
 
+    """
+    Operations - Sampling
+    """
+
+    def sample_new_targets(self, env_ids: Sequence[int]) -> torch.Tensor:
+        """Samples terrain-aware locations of flat patches to set spawn or target locations.
+
+        Note:
+            This is a dummy function that returns the environment origins as target locations.
+            Please inherit the class and reimplement the function for specific terrain types
+
+        Args:
+            env_ids: The environment indices to sample targets locations for.
+
+        Returns:
+            The sampled target locations as (x, y, z). Shape is (N, 3).
+        """
+
+        return self.env_origins[env_ids]
+
     """
     Internal helpers.
     """