Adds body tracking option to ViewerCfg (#1620)

# Description

Resolves Issue #1534 by adding non-root body tracking as an option in
the ViewerCfg. See the issue for justification/discussion. API as
follows:

- Adds origin_type "asset_body"
- Adds field "body_name"

When origin_type = "asset_body" and "body_name" matches a body in the
asset specified in "asset_name", the viewport is configured to track the
link frame of the body in world coordinates. By using the existing eye
and lookat fields in ViewerCfg the additional features are enabled:

- Tracking the end effector of an arm during manipulation (see below)
- Tracking a fixed offset from the end effector of an arm (e.g.
mimicking the view of a wrist-mounted camera)
- Tracking a mobile base using the currently-supported workaround for
lack of wheel physics by defining a virtual arm to mimic the motion of
the robot

## Type of change

- New feature (non-breaking change which adds functionality)

## Training Video from "Isaac-Lift-Cube-Franka-v0" after 1000
iterations.

Viewer defined as follows:

viewer = ViewerCfg(eye=(0.5, 0.5, 0.0), origin_type="asset_body",
env_index=0, asset_name="robot", body_name="panda_hand")

No other changes were made from the default environment definition.


https://github.com/user-attachments/assets/fff24b50-c9d0-48dd-b4c2-3641a11f0d4d

## Checklist

- [X] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./isaaclab.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

---------

Signed-off-by: Kelly Guo <[email protected]>
Signed-off-by: Kelly Guo <[email protected]>
Co-authored-by: Kelly Guo <[email protected]>
Co-authored-by: Kelly Guo <[email protected]>

Author: KyleM73

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

@@ -1,7 +1,7 @@
 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.30.2"
+version = "0.30.3"
 
 # Description
 title = "Isaac Lab framework for Robot Learning"

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

@@ -1,6 +1,15 @@
 Changelog
 ---------
 
+0.30.3 (2025-01-02)
+~~~~~~~~~~~~~~~~~~~
+
+Added
+^^^^^
+
+* Added body tracking as an origin type to :class:`omni.isaac.lab.envs.ViewerCfg` and :class:`omni.isaac.lab.envs.ui.ViewportCameraController`.
+
+
 0.30.2 (2024-12-22)
 ~~~~~~~~~~~~~~~~~~~
 

source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object_data.py

@@ -202,7 +202,7 @@ def body_state_w(self):
     @property
     def body_link_state_w(self):
         """State of all bodies `[pos, quat, lin_vel, ang_vel]` in simulation world frame.
-        Shape is (num_instances,1, 13).
+        Shape is (num_instances, 1, 13).
 
         The position, quaternion, and linear/angular velocity are of the body's link frame relative to the world.
         """

source/extensions/omni.isaac.lab/omni/isaac/lab/envs/common.py

@@ -36,14 +36,15 @@ class ViewerCfg:
     Default is (1280, 720).
     """
 
-    origin_type: Literal["world", "env", "asset_root"] = "world"
+    origin_type: Literal["world", "env", "asset_root", "asset_body"] = "world"
     """The frame in which the camera position (eye) and target (lookat) are defined in. Default is "world".
 
     Available options are:
 
     * ``"world"``: The origin of the world.
     * ``"env"``: The origin of the environment defined by :attr:`env_index`.
     * ``"asset_root"``: The center of the asset defined by :attr:`asset_name` in environment :attr:`env_index`.
+    * ``"asset_body"``: The center of the body defined by :attr:`body_name` in asset defined by :attr:`asset_name` in environment :attr:`env_index`.
     """
 
     env_index: int = 0
@@ -58,6 +59,12 @@ class ViewerCfg:
     This quantity is only effective if :attr:`origin` is set to "asset_root".
     """
 
+    body_name: str | None = None
+    """The name of the body in :attr:`asset_name` in the interactive scene for the frame origin. Default is None.
+
+    This quantity is only effective if :attr:`origin` is set to "asset_body".
+    """
+
 
 ##
 # Types.

source/extensions/omni.isaac.lab/omni/isaac/lab/envs/ui/viewport_camera_controller.py

@@ -15,6 +15,8 @@
 import omni.kit.app
 import omni.timeline
 
+from omni.isaac.lab.assets.articulation.articulation import Articulation
+
 if TYPE_CHECKING:
     from omni.isaac.lab.envs import DirectRLEnv, ManagerBasedEnv, ViewerCfg
 
@@ -59,12 +61,15 @@ def __init__(self, env: ManagerBasedEnv | DirectRLEnv, cfg: ViewerCfg):
             self.set_view_env_index(self.cfg.env_index)
             # set the camera origin to the center of the environment
             self.update_view_to_env()
-        elif self.cfg.origin_type == "asset_root":
+        elif self.cfg.origin_type == "asset_root" or self.cfg.origin_type == "asset_body":
             # note: we do not yet update camera for tracking an asset origin, as the asset may not yet be
             # in the scene when this is called. Instead, we subscribe to the post update event to update the camera
             # at each rendering step.
             if self.cfg.asset_name is None:
                 raise ValueError(f"No asset name provided for viewer with origin type: '{self.cfg.origin_type}'.")
+            if self.cfg.origin_type == "asset_body":
+                if self.cfg.body_name is None:
+                    raise ValueError(f"No body name provided for viewer with origin type: '{self.cfg.origin_type}'.")
         else:
             # set the camera origin to the center of the world
             self.update_view_to_world()
@@ -160,6 +165,41 @@ def update_view_to_asset_root(self, asset_name: str):
         # update the camera view
         self.update_view_location()
 
+    def update_view_to_asset_body(self, asset_name: str, body_name: str):
+        """Updates the viewer's origin based upon the body of an asset in the scene.
+
+        Args:
+            asset_name: The name of the asset in the scene. The name should match the name of the
+                asset in the scene.
+            body_name: The name of the body in the asset.
+
+        Raises:
+            ValueError: If the asset is not in the scene or the body is not valid.
+        """
+        # check if the asset is in the scene
+        if self.cfg.asset_name != asset_name:
+            asset_entities = [*self._env.scene.rigid_objects.keys(), *self._env.scene.articulations.keys()]
+            if asset_name not in asset_entities:
+                raise ValueError(f"Asset '{asset_name}' is not in the scene. Available entities: {asset_entities}.")
+        # check if the body is in the asset
+        asset: Articulation = self._env.scene[asset_name]
+        if body_name not in asset.body_names:
+            raise ValueError(
+                f"'{body_name}' is not a body of Asset '{asset_name}'. Available bodies: {asset.body_names}."
+            )
+        # get the body index
+        body_id, _ = asset.find_bodies(body_name)
+        # update the asset name
+        self.cfg.asset_name = asset_name
+        # set origin type to asset_body
+        self.cfg.origin_type = "asset_body"
+        # update the camera origins
+        self.viewer_origin = (
+            self._env.scene[self.cfg.asset_name].data.body_link_pos_w[self.cfg.env_index, body_id].view(3)
+        )
+        # update the camera view
+        self.update_view_location()
+
     def update_view_location(self, eye: Sequence[float] | None = None, lookat: Sequence[float] | None = None):
         """Updates the camera view pose based on the current viewer origin and the eye and lookat positions.
 
@@ -190,3 +230,5 @@ def _update_tracking_callback(self, event):
         # in other cases, the camera view is static and does not need to be updated continuously
         if self.cfg.origin_type == "asset_root" and self.cfg.asset_name is not None:
             self.update_view_to_asset_root(self.cfg.asset_name)
+        if self.cfg.origin_type == "asset_body" and self.cfg.asset_name is not None and self.cfg.body_name is not None:
+            self.update_view_to_asset_body(self.cfg.asset_name, self.cfg.body_name)