Adds configurable target coordinate frame for teleop output poses (#5050)

# Description

Add a target_T_world parameter to IsaacTeleopDevice.advance() and
TeleopSessionLifecycle.step() that left-multiplies the anchor matrix to
rebase all output poses into an arbitrary target coordinate frame (e.g.
robot base link for IK).
Add a config-driven alternative via
IsaacTeleopCfg.target_frame_prim_path: when set to a USD prim path, the
device automatically reads the prim's world transform each frame via
USDRT/Fabric and uses its inverse as the rebase matrix, removing the
need for callers to compute it manually.
Add _to_numpy_4x4 helper that accepts np.ndarray, torch.Tensor,
wp.array, or any duck-typed .numpy() object and normalizes to float32
NumPy.
Add comprehensive unit tests for the conversion helper, matrix rebase
math, and config-vs-explicit auto-selection logic.

Fixes # (issue)

## Type of change

<!-- As you go through the list, delete the ones that are not
applicable. -->

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

## Checklist

- [x] I have read and understood the [contribution
guidelines](https://isaac-sim.github.io/IsaacLab/main/source/refs/contributing.html)
- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./isaaclab.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 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

Author: rwiltz

source/isaaclab_teleop/config/extension.toml

@@ -1,6 +1,6 @@
 [package]
 # Semantic Versioning is used: https://semver.org/
-version = "0.3.3"
+version = "0.3.4"
 
 # Description
 title =  "Isaac Lab Teleop"

source/isaaclab_teleop/docs/CHANGELOG.rst

@@ -1,6 +1,25 @@
 Changelog
 ---------
 
+0.3.4 (2026-03-17)
+~~~~~~~~~~~~~~~~~~~
+
+Added
+^^^^^
+
+* Added :attr:`~isaaclab_teleop.IsaacTeleopCfg.target_frame_prim_path` for
+  config-driven frame rebasing.  When set to a USD prim path, the device
+  automatically reads the prim's world transform each frame and uses its
+  inverse as the ``target_T_world`` rebase matrix, so all output poses are
+  expressed in the target frame (e.g. robot base link for IK).
+
+* Added ``target_T_world`` parameter to
+  :meth:`~isaaclab_teleop.IsaacTeleopDevice.advance` for rebasing all output
+  poses into an arbitrary target coordinate frame (e.g. robot base link for
+  IK).  Accepts :class:`numpy.ndarray`, :class:`torch.Tensor`, or
+  ``wp.array``.
+
+
 0.3.3 (2026-03-13)
 ~~~~~~~~~~~~~~~~~~~
 

source/isaaclab_teleop/isaaclab_teleop/isaac_teleop_cfg.py

@@ -108,5 +108,26 @@ def build_pipeline():
     If ``None``, the tuning UI will not be opened.
     """
 
+    target_frame_prim_path: str | None = None
+    """Optional USD prim path whose world frame becomes the target coordinate
+    frame for all output poses.
+
+    When set, the device automatically reads this prim's world transform each
+    frame and uses its inverse as the ``target_T_world`` rebase matrix in
+    :meth:`~isaaclab_teleop.IsaacTeleopDevice.advance`.  An explicit
+    ``target_T_world`` argument to :meth:`~isaaclab_teleop.IsaacTeleopDevice.advance`
+    takes precedence over this config.
+
+    Typical usage: set to the robot base link prim path so that an IK
+    controller receives end-effector poses in the robot's base frame.
+
+    Example::
+
+        IsaacTeleopCfg(
+            target_frame_prim_path="/World/envs/env_0/Robot/base_link",
+            ...
+        )
+    """
+
     app_name: str = "IsaacLabTeleop"
     """Application name for the IsaacTeleop session."""

source/isaaclab_teleop/isaaclab_teleop/isaac_teleop_device.py

@@ -9,14 +9,19 @@
 
 import logging
 from collections.abc import Callable
+from typing import TYPE_CHECKING
 
+import numpy as np
 import torch
 
 from .command_handler import CommandHandler
 from .isaac_teleop_cfg import IsaacTeleopCfg
 from .session_lifecycle import TeleopSessionLifecycle
 from .xr_anchor_manager import XrAnchorManager
 
+if TYPE_CHECKING:
+    from .session_lifecycle import SupportsDLPack
+
 logger = logging.getLogger(__name__)
 
 
@@ -42,6 +47,24 @@ class IsaacTeleopDevice:
     The device uses IsaacTeleop's TensorReorderer to flatten pipeline outputs
     into a single action tensor matching the environment's action space.
 
+    Frame rebasing:
+        By default, all output poses are expressed in the simulation world
+        frame.  When an application needs poses in a different frame (e.g.
+        robot base link for IK), there are two options:
+
+        * **Config-driven** (recommended): set
+          :attr:`~IsaacTeleopCfg.target_frame_prim_path` to the USD prim
+          whose frame the output should be expressed in.  The device reads
+          the prim's world transform each frame and applies the rebase
+          automatically.
+        * **Explicit**: pass a ``target_T_world`` matrix directly to
+          :meth:`advance`.
+
+        In both cases the device composes
+        ``target_T_world @ world_T_anchor`` before feeding the matrix into
+        the retargeting pipeline, so all resulting poses are expressed in the
+        target frame.
+
     Teleop commands:
         The device supports callbacks for START, STOP, and RESET commands
         that can be triggered via XR controller buttons or the message bus.
@@ -54,10 +77,25 @@ class IsaacTeleopDevice:
                 sim_device="cuda:0",
             )
 
+            # Poses in world frame (default)
+            with IsaacTeleopDevice(cfg) as device:
+                while running:
+                    action = device.advance()
+                    env.step(action.repeat(num_envs, 1))
+
+            # Config-driven rebase into robot base frame
+            cfg.target_frame_prim_path = "/World/Robot/base_link"
             with IsaacTeleopDevice(cfg) as device:
                 while running:
                     action = device.advance()
                     env.step(action.repeat(num_envs, 1))
+
+            # Explicit rebase into robot base frame
+            with IsaacTeleopDevice(cfg) as device:
+                while running:
+                    robot_T_world = get_robot_base_transform()
+                    action = device.advance(target_T_world=robot_T_world)
+                    env.step(action.repeat(num_envs, 1))
     """
 
     def __init__(self, cfg: IsaacTeleopCfg):
@@ -147,14 +185,35 @@ def add_callback(self, key: str, func: Callable) -> None:
         """
         self._command_handler.add_callback(key, func)
 
-    def advance(self) -> torch.Tensor | None:
+    def advance(self, target_T_world: np.ndarray | torch.Tensor | SupportsDLPack | None = None) -> torch.Tensor | None:
         """Process current device state and return control commands.
 
         If the IsaacTeleop session has not been started yet (because the OpenXR
         handles were not available at ``__enter__`` time), this method will
         attempt to start it on each call.  Once the user clicks "Start AR" and
         the handles become available, the session is created transparently.
 
+        Args:
+            target_T_world: Optional 4x4 transform matrix that rebases all
+                output poses into an arbitrary target coordinate frame.  When
+                provided, the matrix sent to the retargeting pipeline becomes
+                ``target_T_world @ world_T_anchor`` instead of just
+                ``world_T_anchor``, so all resulting poses are expressed in
+                the target frame rather than the simulation world frame.
+
+                Typical use case: pass ``robot_base_T_world`` so that an IK
+                controller receives end-effector poses in the robot's base
+                link frame.
+
+                Accepts any object supporting the DLPack buffer protocol
+                (``__dlpack__``), including :class:`numpy.ndarray`,
+                :class:`torch.Tensor`, and ``wp.array``.
+
+                When ``None`` and
+                :attr:`~IsaacTeleopCfg.target_frame_prim_path` is set, the
+                transform is computed automatically by reading the prim's
+                world matrix from Fabric and inverting it.
+
         Returns:
             A flattened action :class:`torch.Tensor` ready for the Isaac Lab
             environment, or ``None`` if the session has not started yet
@@ -163,9 +222,14 @@ def advance(self) -> torch.Tensor | None:
         Raises:
             RuntimeError: If called outside of a context manager.
         """
+        # Auto-compute target_T_world from config if not explicitly provided
+        if target_T_world is None and self._cfg.target_frame_prim_path is not None:
+            target_T_world = self._get_target_frame_T_world()
+
         # Step the session (handles lazy start and action extraction)
         action = self._session_lifecycle.step(
             anchor_world_matrix_fn=self._anchor_manager.get_world_matrix,
+            target_T_world=target_T_world,
         )
 
         if action is not None:
@@ -174,6 +238,75 @@ def advance(self) -> torch.Tensor | None:
 
         return action
 
+    # ------------------------------------------------------------------
+    # Target frame transform (config-driven rebase)
+    # ------------------------------------------------------------------
+
+    def _get_target_frame_T_world(self) -> np.ndarray | None:
+        """Read the target-frame prim's world matrix from Fabric and return its inverse.
+
+        Uses USDRT to read the prim's hierarchical world matrix, matching the
+        pattern used by :class:`XrAnchorSynchronizer` for anchor prim reads.
+
+        Returns:
+            A (4, 4) float32 :class:`numpy.ndarray` representing the inverse
+            of the prim's world transform (i.e. ``target_T_world``), or
+            ``None`` if the prim cannot be read.
+        """
+        try:
+            import omni.usd
+            import usdrt
+            from pxr import UsdUtils
+            from usdrt import Rt
+
+            stage = omni.usd.get_context().get_stage()
+            stage_cache = UsdUtils.StageCache.Get()
+            stage_id = stage_cache.GetId(stage).ToLongInt()
+            if stage_id < 0:
+                stage_id = stage_cache.Insert(stage).ToLongInt()
+            rt_stage = usdrt.Usd.Stage.Attach(stage_id)
+            if rt_stage is None:
+                return None
+
+            rt_prim = rt_stage.GetPrimAtPath(self._cfg.target_frame_prim_path)
+            if not rt_prim.IsValid():
+                return None
+
+            rt_xformable = Rt.Xformable(rt_prim)
+            if not rt_xformable.GetPrim().IsValid():
+                return None
+
+            world_matrix_attr = rt_xformable.GetFabricHierarchyWorldMatrixAttr()
+            if world_matrix_attr is None:
+                return None
+
+            rt_matrix = world_matrix_attr.Get()
+            if rt_matrix is None:
+                return None
+
+            pos = rt_matrix.ExtractTranslation()
+            rt_quat = rt_matrix.ExtractRotationQuat()
+
+            from scipy.spatial.transform import Rotation
+
+            quat_xyzw = [
+                float(rt_quat.GetImaginary()[0]),
+                float(rt_quat.GetImaginary()[1]),
+                float(rt_quat.GetImaginary()[2]),
+                float(rt_quat.GetReal()),
+            ]
+
+            R = Rotation.from_quat(quat_xyzw).as_matrix().astype(np.float32)
+            t = np.array([float(pos[0]), float(pos[1]), float(pos[2])], dtype=np.float32)
+
+            inv_mat = np.eye(4, dtype=np.float32)
+            inv_mat[:3, :3] = R.T
+            inv_mat[:3, 3] = -(R.T @ t)
+            return inv_mat
+        except Exception as e:
+            logger.warning(f"Failed to read target frame prim '{self._cfg.target_frame_prim_path}': {e}")
+            return None
+
     # ------------------------------------------------------------------
     # Controller button polling (glue between session and anchor manager)
     # ------------------------------------------------------------------