extend anymal-c navigation envs to 1) have obstacle and camera sensor. 2) have new pose command and reset event.

Author: KingsleyLiu-NV

source/isaaclab/isaaclab/envs/mdp/commands/__init__.py

@@ -8,12 +8,13 @@
 from .commands_cfg import (
     NormalVelocityCommandCfg,
     NullCommandCfg,
+    ObstaclePose2dCommandCfg,
     TerrainBasedPose2dCommandCfg,
     UniformPose2dCommandCfg,
     UniformPoseCommandCfg,
     UniformVelocityCommandCfg,
 )
 from .null_command import NullCommand
-from .pose_2d_command import TerrainBasedPose2dCommand, UniformPose2dCommand
+from .pose_2d_command import ObstaclePose2dCommand, TerrainBasedPose2dCommand, UniformPose2dCommand
 from .pose_command import UniformPoseCommand
 from .velocity_command import NormalVelocityCommand, UniformVelocityCommand

source/isaaclab/isaaclab/envs/mdp/commands/commands_cfg.py

@@ -8,11 +8,15 @@
 
 from isaaclab.managers import CommandTermCfg
 from isaaclab.markers import VisualizationMarkersCfg
-from isaaclab.markers.config import BLUE_ARROW_X_MARKER_CFG, FRAME_MARKER_CFG, GREEN_ARROW_X_MARKER_CFG
+from isaaclab.markers.config import (
+    BLUE_ARROW_X_MARKER_CFG,
+    FRAME_MARKER_CFG,
+    GREEN_ARROW_X_MARKER_CFG,
+)
 from isaaclab.utils import configclass
 
 from .null_command import NullCommand
-from .pose_2d_command import TerrainBasedPose2dCommand, UniformPose2dCommand
+from .pose_2d_command import ObstaclePose2dCommand, TerrainBasedPose2dCommand, UniformPose2dCommand
 from .pose_command import UniformPoseCommand
 from .velocity_command import NormalVelocityCommand, UniformVelocityCommand
 
@@ -172,7 +176,9 @@ class Ranges:
     ranges: Ranges = MISSING
     """Ranges for the commands."""
 
-    goal_pose_visualizer_cfg: VisualizationMarkersCfg = FRAME_MARKER_CFG.replace(prim_path="/Visuals/Command/goal_pose")
+    goal_pose_visualizer_cfg: VisualizationMarkersCfg = FRAME_MARKER_CFG.replace(
+        prim_path="/Visuals/Command/goal_pose"
+    )
     """The configuration for the goal pose visualization marker. Defaults to FRAME_MARKER_CFG."""
 
     current_pose_visualizer_cfg: VisualizationMarkersCfg = FRAME_MARKER_CFG.replace(
@@ -228,6 +234,29 @@ class Ranges:
     goal_pose_visualizer_cfg.markers["arrow"].scale = (0.2, 0.2, 0.8)
 
 
+@configclass
+class ObstaclePose2dCommandCfg(UniformPose2dCommandCfg):
+    """Configuration for the obstacle-based 2D-pose command generator."""
+
+    class_type: type = ObstaclePose2dCommand
+
+    object_name: str = MISSING
+    """Name of the obstacle object in the environment."""
+
+    @configclass
+    class Ranges:
+        """Uniform distribution ranges for the position commands."""
+
+        heading: tuple[float, float] = MISSING
+        """Heading range for the position commands (in rad).
+
+        Used only if :attr:`simple_heading` is False.
+        """
+
+    ranges: Ranges = MISSING
+    """Distribution ranges for the sampled commands."""
+
+
 @configclass
 class TerrainBasedPose2dCommandCfg(UniformPose2dCommandCfg):
     """Configuration for the terrain-based position command generator."""

source/isaaclab/isaaclab/envs/mdp/commands/pose_2d_command.py

@@ -81,8 +81,12 @@ def command(self) -> torch.Tensor:
 
     def _update_metrics(self):
         # logs data
-        self.metrics["error_pos_2d"] = torch.norm(self.pos_command_w[:, :2] - self.robot.data.root_pos_w[:, :2], dim=1)
-        self.metrics["error_heading"] = torch.abs(wrap_to_pi(self.heading_command_w - self.robot.data.heading_w))
+        self.metrics["error_pos_2d"] = torch.norm(
+            self.pos_command_w[:, :2] - self.robot.data.root_pos_w[:, :2], dim=1
+        )
+        self.metrics["error_heading"] = torch.abs(
+            wrap_to_pi(self.heading_command_w - self.robot.data.heading_w)
+        )
 
     def _resample_command(self, env_ids: Sequence[int]):
         # obtain env origins for the environments
@@ -102,7 +106,9 @@ def _resample_command(self, env_ids: Sequence[int]):
             # compute errors to find the closest direction to the current heading
             # this is done to avoid the discontinuity at the -pi/pi boundary
             curr_to_target = wrap_to_pi(target_direction - self.robot.data.heading_w[env_ids]).abs()
-            curr_to_flipped_target = wrap_to_pi(flipped_target_direction - self.robot.data.heading_w[env_ids]).abs()
+            curr_to_flipped_target = wrap_to_pi(
+                flipped_target_direction - self.robot.data.heading_w[env_ids]
+            ).abs()
 
             # set the heading command to the closest direction
             self.heading_command_w[env_ids] = torch.where(
@@ -117,7 +123,9 @@ def _resample_command(self, env_ids: Sequence[int]):
     def _update_command(self):
         """Re-target the position command to the current root state."""
         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.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.data.heading_w)
 
     def _set_debug_vis_impl(self, debug_vis: bool):
@@ -143,6 +151,68 @@ def _debug_vis_callback(self, event):
         )
 
 
+class ObstaclePose2dCommand(UniformPose2dCommand):
+    """Command generator that generates pose commands based on the obstacle.
+
+    This command generator determines the command position based on the position of the obstacle.
+    The heading commands are either set to point towards the target or are sampled uniformly.
+    This can be configured through the :attr:`Pose2dCommandCfg.simple_heading` parameter in
+    the configuration.
+    """
+
+    cfg: ObstaclePose2dCommandCfg
+    """Configuration for the command generator."""
+
+    def __init__(self, cfg: ObstaclePose2dCommandCfg, env: ManagerBasedEnv):
+        """Initialize the command generator class.
+
+        Args:
+            cfg: The configuration parameters for the command generator.
+            env: The environment object.
+        """
+        # initialize the base class
+        super().__init__(cfg, env)
+
+        # obtain the obstacle object
+        self.object: RigidObject = env.scene[cfg.object_name]
+
+    def _resample_command(self, env_ids: Sequence[int]):
+        # obtain env origins for the environments
+        self.pos_command_w[env_ids] = self._env.scene.env_origins[env_ids]
+        r = torch.empty(len(env_ids), device=self.device)
+        # offset the position command by the current root position
+        self.pos_command_w[env_ids, 0] = (
+            2 * self.object.data.root_pos_w[env_ids, 0] - self.robot.data.root_pos_w[env_ids, 0]
+        )
+        self.pos_command_w[env_ids, 1] = (
+            2 * self.object.data.root_pos_w[env_ids, 1] - self.robot.data.root_pos_w[env_ids, 1]
+        )
+        self.pos_command_w[env_ids, 2] = self.robot.data.default_root_state[env_ids, 2]
+
+        if self.cfg.simple_heading:
+            # set heading command to point towards target
+            target_vec = self.pos_command_w[env_ids] - self.robot.data.root_pos_w[env_ids]
+            target_direction = torch.atan2(target_vec[:, 1], target_vec[:, 0])
+            flipped_target_direction = wrap_to_pi(target_direction + torch.pi)
+
+            # compute errors to find the closest direction to the current heading
+            # this is done to avoid the discontinuity at the -pi/pi boundary
+            curr_to_target = wrap_to_pi(target_direction - self.robot.data.heading_w[env_ids]).abs()
+            curr_to_flipped_target = wrap_to_pi(
+                flipped_target_direction - self.robot.data.heading_w[env_ids]
+            ).abs()
+
+            # set the heading command to the closest direction
+            self.heading_command_w[env_ids] = torch.where(
+                curr_to_target < curr_to_flipped_target,
+                target_direction,
+                flipped_target_direction,
+            )
+        else:
+            # random heading command
+            self.heading_command_w[env_ids] = r.uniform_(*self.cfg.ranges.heading)
+
+
 class TerrainBasedPose2dCommand(UniformPose2dCommand):
     """Command generator that generates pose commands based on the terrain.
 
@@ -173,7 +243,9 @@ def __init__(self, cfg: TerrainBasedPose2dCommandCfg, env: ManagerBasedEnv):
 
     def _resample_command(self, env_ids: Sequence[int]):
         # sample new position targets from the terrain
-        ids = torch.randint(0, self.valid_targets.shape[2], size=(len(env_ids),), device=self.device)
+        ids = torch.randint(
+            0, self.valid_targets.shape[2], size=(len(env_ids),), device=self.device
+        )
         self.pos_command_w[env_ids] = self.valid_targets[
             self.terrain.terrain_levels[env_ids], self.terrain.terrain_types[env_ids], ids
         ]
@@ -189,7 +261,9 @@ def _resample_command(self, env_ids: Sequence[int]):
             # compute errors to find the closest direction to the current heading
             # this is done to avoid the discontinuity at the -pi/pi boundary
             curr_to_target = wrap_to_pi(target_direction - self.robot.data.heading_w[env_ids]).abs()
-            curr_to_flipped_target = wrap_to_pi(flipped_target_direction - self.robot.data.heading_w[env_ids]).abs()
+            curr_to_flipped_target = wrap_to_pi(
+                flipped_target_direction - self.robot.data.heading_w[env_ids]
+            ).abs()
 
             # set the heading command to the closest direction
             self.heading_command_w[env_ids] = torch.where(