Adds automatic transform discovery for imu sensors to a viable parent body.

CONTRIBUTORS.md

@@ -51,6 +51,7 @@ Guidelines for modifications:
 * Bingjie Tang
 * Brayden Zhang
 * Brian Bingham
+* Brian McCann
 * Cameron Upright
 * Calvin Yu
 * Cathy Y. Li

source/isaaclab/config/extension.toml

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

source/isaaclab/docs/CHANGELOG.rst

@@ -1,6 +1,14 @@
 Changelog
 ---------
 
+0.48.2 (2025-10-22)
+~~~~~~~~~~~~~~~~~~~
+
+Changed
+^^^^^^^
+
+* Implement ability to attach an imu sensor to xform primitives in a usd file. This PR is based on work by @GiulioRomualdi here: #3094 Addressing issue #3088.
+
 0.47.2 (2025-10-22)
 ~~~~~~~~~~~~~~~~~~~
 

source/isaaclab/isaaclab/sensors/imu/imu.py

@@ -11,11 +11,11 @@
 
 import isaacsim.core.utils.stage as stage_utils
 from isaacsim.core.simulation_manager import SimulationManager
-from pxr import UsdPhysics
 
 import isaaclab.sim as sim_utils
 import isaaclab.utils.math as math_utils
 from isaaclab.markers import VisualizationMarkers
+from isaaclab.sim.utils import resolve_pose_relative_to_physx_parent
 
 from ..sensor_base import SensorBase
 from .imu_data import ImuData
@@ -27,10 +27,12 @@
 class Imu(SensorBase):
     """The Inertia Measurement Unit (IMU) sensor.
 
-    The sensor can be attached to any :class:`RigidObject` or :class:`Articulation` in the scene. The sensor provides complete state information.
-    The sensor is primarily used to provide the linear acceleration and angular velocity of the object in the body frame. The sensor also provides
-    the position and orientation of the object in the world frame and the angular acceleration and linear velocity in the body frame. The extra
-    data outputs are useful for simulating with or comparing against "perfect" state estimation.
+    The sensor can be attached to any prim path and produces body-frame linear acceleration and angular velocity,
+    along with world-frame pose and body-frame linear and angular accelerations/velocities.
+
+    If the provided path is not a rigid body, the closest rigid-body ancestor is used for simulation queries.
+    The fixed transform from that ancestor to the target prim is computed once during initialization and
+    composed with the configured sensor offset.
 
     .. note::
 
@@ -40,10 +42,13 @@ class Imu(SensorBase):
 
     .. note::
 
-        It is suggested to use the OffsetCfg to define an IMU frame relative to a rigid body prim defined at the root of
-        a :class:`RigidObject` or  a prim that is defined by a non-fixed joint in an :class:`Articulation` (except for the
-        root of a fixed based articulation). The use frames with fixed joints and small mass/inertia to emulate a transform
-        relative to a body frame can result in lower performance and accuracy.
+        The user can configure the sensor offset in the configuration file. The offset is applied relative to the rigid source prim.
+        If the target prim is not a rigid body, the offset is composed with the fixed transform
+        from the rigid ancestor to the target prim. The offset is applied in the body frame of the rigid source prim.
+        The offset is defined as a position vector and a quaternion rotation, which
+        are applied in the order: position, then rotation. The position is applied as a translation
+        in the body frame of the rigid source prim, and the rotation is applied as a rotation
+        in the body frame of the rigid source prim.
 
     """
 
@@ -61,6 +66,9 @@ def __init__(self, cfg: ImuCfg):
         # Create empty variables for storing output data
         self._data = ImuData()
 
+        # Internal: expression used to build the rigid body view (may be different from cfg.prim_path)
+        self._rigid_parent_expr: str | None = None
+
     def __str__(self) -> str:
         """Returns: A string containing information about the instance."""
         return (
@@ -119,11 +127,9 @@ def update(self, dt: float, force_recompute: bool = False):
     def _initialize_impl(self):
         """Initializes the sensor handles and internal buffers.
 
-        This function creates handles and registers the provided data types with the replicator registry to
-        be able to access the data from the sensor. It also initializes the internal buffers to store the data.
-
-        Raises:
-            RuntimeError: If the imu prim is not a RigidBodyPrim
+        - If the target prim path is a rigid body, build the view directly on it.
+        - Otherwise find the closest rigid-body ancestor, cache the fixed transform from that ancestor
+          to the target prim, and build the view on the ancestor expression.
         """
         # Initialize parent class
         super()._initialize_impl()
@@ -133,11 +139,12 @@ def _initialize_impl(self):
         prim = sim_utils.find_first_matching_prim(self.cfg.prim_path)
         if prim is None:
             raise RuntimeError(f"Failed to find a prim at path expression: {self.cfg.prim_path}")
-        # check if it is a RigidBody Prim
-        if prim.HasAPI(UsdPhysics.RigidBodyAPI):
-            self._view = self._physics_sim_view.create_rigid_body_view(self.cfg.prim_path.replace(".*", "*"))
-        else:
-            raise RuntimeError(f"Failed to find a RigidBodyAPI for the prim paths: {self.cfg.prim_path}")
+
+        # Determine rigid source prim and (if needed) the fixed transform from that rigid prim to target prim
+        self._rigid_parent_expr, fixed_pos_b, fixed_quat_b = resolve_pose_relative_to_physx_parent(self.cfg.prim_path)
+
+        # Create the rigid body view on the ancestor
+        self._view = self._physics_sim_view.create_rigid_body_view(self._rigid_parent_expr)
 
         # Get world gravity
         gravity = self._physics_sim_view.get_gravity()
@@ -148,35 +155,53 @@ def _initialize_impl(self):
         # Create internal buffers
         self._initialize_buffers_impl()
 
+        # Compose the configured offset with the fixed ancestor->target transform (done once)
+        # new_offset = fixed * cfg.offset
+        # where composition is: p = p_fixed + R_fixed * p_cfg, q = q_fixed * q_cfg
+        if fixed_pos_b is not None and fixed_quat_b is not None:
+            # Broadcast fixed transform across instances
+            fixed_p = torch.tensor(fixed_pos_b, device=self._device).repeat(self._view.count, 1)
+            fixed_q = torch.tensor(fixed_quat_b, device=self._device).repeat(self._view.count, 1)
+
+            cfg_p = self._offset_pos_b.clone()
+            cfg_q = self._offset_quat_b.clone()
+
+            composed_p = fixed_p + math_utils.quat_apply(fixed_q, cfg_p)
+            composed_q = math_utils.quat_mul(fixed_q, cfg_q)
+
+            self._offset_pos_b = composed_p
+            self._offset_quat_b = composed_q
+
     def _update_buffers_impl(self, env_ids: Sequence[int]):
         """Fills the buffers of the sensor data."""
 
         # default to all sensors
         if len(env_ids) == self._num_envs:
             env_ids = slice(None)
-        # obtain the poses of the sensors
+        # world pose of the rigid source (ancestor) from the PhysX view
         pos_w, quat_w = self._view.get_transforms()[env_ids].split([3, 4], dim=-1)
         quat_w = quat_w.roll(1, dims=-1)
 
-        # store the poses
+        # sensor pose in world: apply composed offset
         self._data.pos_w[env_ids] = pos_w + math_utils.quat_apply(quat_w, self._offset_pos_b[env_ids])
         self._data.quat_w[env_ids] = math_utils.quat_mul(quat_w, self._offset_quat_b[env_ids])
 
-        # get the offset from COM to link origin
+        # COM of rigid source (body frame)
         com_pos_b = self._view.get_coms().to(self.device).split([3, 4], dim=-1)[0]
 
-        # obtain the velocities of the link COM
+        # Velocities at rigid source COM
         lin_vel_w, ang_vel_w = self._view.get_velocities()[env_ids].split([3, 3], dim=-1)
-        # if an offset is present or the COM does not agree with the link origin, the linear velocity has to be
-        # transformed taking the angular velocity into account
+
+        # If sensor offset or COM != link origin, account for angular velocity contribution
         lin_vel_w += torch.linalg.cross(
             ang_vel_w, math_utils.quat_apply(quat_w, self._offset_pos_b[env_ids] - com_pos_b[env_ids]), dim=-1
         )
 
-        # numerical derivative
+        # numerical derivative (world frame)
         lin_acc_w = (lin_vel_w - self._prev_lin_vel_w[env_ids]) / self._dt + self._gravity_bias_w[env_ids]
         ang_acc_w = (ang_vel_w - self._prev_ang_vel_w[env_ids]) / self._dt
-        # stack data in world frame and batch rotate
+
+        # batch rotate world->body using current sensor orientation
         dynamics_data = torch.stack((lin_vel_w, ang_vel_w, lin_acc_w, ang_acc_w, self.GRAVITY_VEC_W[env_ids]), dim=0)
         dynamics_data_rot = math_utils.quat_apply_inverse(self._data.quat_w[env_ids].repeat(5, 1), dynamics_data).chunk(
             5, dim=0
@@ -207,7 +232,7 @@ def _initialize_buffers_impl(self):
         self._prev_lin_vel_w = torch.zeros_like(self._data.pos_w)
         self._prev_ang_vel_w = torch.zeros_like(self._data.pos_w)
 
-        # store sensor offset transformation
+        # store sensor offset (applied relative to rigid source). This may be composed later with a fixed ancestor->target transform.
         self._offset_pos_b = torch.tensor(list(self.cfg.offset.pos), device=self._device).repeat(self._view.count, 1)
         self._offset_quat_b = torch.tensor(list(self.cfg.offset.rot), device=self._device).repeat(self._view.count, 1)
         # set gravity bias

source/isaaclab/isaaclab/sim/utils.py

@@ -895,6 +895,90 @@ def find_matching_prim_paths(prim_path_regex: str, stage: Usd.Stage | None = Non
     return output_prim_paths
 
 
+def check_prim_implements_apis(
+    prim: Usd.Prim, apis: list[Usd.APISchemaBase] | Usd.APISchemaBase = UsdPhysics.RigidBodyAPI
+) -> bool:
+    """Return true if prim implements all apis, False otherwise."""
+
+    if type(apis) is not list:
+        return prim.HasAPI(apis)
+    else:
+        return all(prim.HasAPI(api) for api in apis)
+
+
+def resolve_pose_relative_to_physx_parent(
+    prim_path_regex: str,
+    implements_apis: list[Usd.APISchemaBase] | Usd.APISchemaBase = UsdPhysics.RigidBodyAPI,
+    *,
+    stage: Usd.Stage | None = None,
+) -> tuple[str, tuple[float, float, float], tuple[float, float, float, float]]:
+    """For some applications, it can be important to identify the closest parent primitive which implements certain APIs
+    in order to retrieve data from PhysX (for example, force information requires more than an XFormPrim). When an object is
+    nested beneath a reference frame which is not represented by a PhysX tensor, it can be useful to extract the relative pose
+    between the primitive and the closest parent implementing the necessary API in the PhysX representation. This function
+    identifies the closest appropriate parent. The fixed transform is computed as ancestor->target (in ancestor
+    /body frame). If the first primitive in the prim_path already implements the necessary APIs, return identity.
+
+        Args:
+            prim_path_regex (str): A str refelcting a primitive path pattern (e.g. from a cfg)
+            implements_apis (list[ Usd.APISchemaBase] | Usd.APISchemaBase): APIs ancestor must implement.
+        Returns:
+            ancestor_path (str): Prim Path Expression including wildcards for the closest PhysX Parent
+            fixed_pos_b (tuple[float, float, float]): positional offset
+            fixed_quat_b (tuple[float, float, float, float]): rotational offset
+        ).
+    """
+    target_prim = find_first_matching_prim(prim_path_regex, stage)
+
+    if target_prim is None:
+        raise RuntimeError(f"Path: {prim_path_regex} does not match any existing primitives.")
+    # If target prim itself implements all required APIs, we can use it directly.
+    if check_prim_implements_apis(target_prim, implements_apis):
+        return prim_path_regex.replace(".*", "*"), None, None
+    # Walk up to find closest ancestor which implements all required APIs
+    ancestor = target_prim.GetParent()
+    while ancestor and ancestor.IsValid():
+        if check_prim_implements_apis(ancestor, implements_apis):
+            break
+        ancestor = ancestor.GetParent()
+    if not ancestor or not ancestor.IsValid():
+        raise RuntimeError(f"Path '{target_prim.GetPath()}' has no ancestor which implements all required APIs.")
+    # Compute fixed transform ancestor->target at default time
+    xcache = UsdGeom.XformCache(Usd.TimeCode.Default())
+
+    # Compute relative transform
+    X_ancestor_to_target, __ = xcache.ComputeRelativeTransform(target_prim, ancestor)
+
+    # Extract pos, quat from matrix (right-handed, column major)
+    # Gf decomposes as translation and rotation quaternion
+    t = X_ancestor_to_target.ExtractTranslation()
+    r = X_ancestor_to_target.ExtractRotationQuat()
+
+    fixed_pos_b = (t[0], t[1], t[2])
+    # Convert Gf.Quatf (w, x, y, z) to tensor order [w, x, y, z]
+    fixed_quat_b = (float(r.GetReal()), r.GetImaginary()[0], r.GetImaginary()[1], r.GetImaginary()[2])
+
+    # This restores regex patterns from the original PathPattern in the path to return.
+    # ( Omnikit 18+ may provide a cleaner approach without relying on strings )
+    child_path = target_prim.GetPrimPath()
+    ancestor_path = ancestor.GetPrimPath()
+    rel = child_path.MakeRelativePath(ancestor_path).pathString
+
+    if rel and prim_path_regex.endswith(rel):
+        # Remove "/<rel>" or "<rel>" at end
+        cut_len = len(rel)
+        trimmed = prim_path_regex
+        if trimmed.endswith("/" + rel):
+            trimmed = trimmed[: -(cut_len + 1)]
+        else:
+            trimmed = trimmed[:-cut_len]
+        ancestor_path = trimmed
+
+    ancestor_path = ancestor_path.replace(".*", "*")
+
+    return ancestor_path, fixed_pos_b, fixed_quat_b
+
+
 def find_global_fixed_joint_prim(
     prim_path: str | Sdf.Path, check_enabled_only: bool = False, stage: Usd.Stage | None = None
 ) -> UsdPhysics.Joint | None: