Adds `IMU` sensor (#619)

# Description

Add `IMU` sensor with cfg class `IMUCfg` and data class `IMUData`.
Compared to the Isaac Sim implementation of the IMU Sensor, this sensor
directly accesses the PhysX view buffers for speed acceleration.

This PR also moves and renames a utility used for cameras to a general
utility location.

Fixes #440 

## Type of change

- New feature (non-breaking change which adds functionality)
- Breaking change (
## Checklist

- [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

docs/source/api/lab/omni.isaac.lab.sensors.rst

@@ -31,6 +31,8 @@
     RayCasterCfg
     RayCasterCamera
     RayCasterCameraCfg
+    Imu
+    ImuCfg
 
 Sensor Base
 -----------
@@ -150,3 +152,17 @@ Ray-Cast Camera
     :inherited-members:
     :show-inheritance:
     :exclude-members: __init__, class_type
+
+Inertia Measurement Unit
+------------------------
+
+.. autoclass:: Imu
+    :members:
+    :inherited-members:
+    :show-inheritance:
+
+.. autoclass:: ImuCfg
+    :members:
+    :inherited-members:
+    :show-inheritance:
+    :exclude-members: __init__, class_type

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

 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.25.2"
+version = "0.26.0"
 
 # Description
 title = "Isaac Lab framework for Robot Learning"

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

 Changelog
 ---------
 
+0.26.0 (2024-10-16)
+~~~~~~~~~~~~~~~~~~~
+
+Added
+^^^^^
+
+* Added Imu sensor implementation that directly accesses the physx view :class:`omni.isaac.lab.sensors.Imu`. The
+  sensor comes with a configuration class :class:`omni.isaac.lab.sensors.ImuCfg` and data class
+  :class:`omni.isaac.lab.sensors.ImuData`.
+* Moved and renamed :meth:`omni.isaac.lab.sensors.camera.utils.convert_orientation_convention` to :meth:`omni.isaac.lab.utils.math.convert_camera_frame_orientation_convention`
+* Moved :meth:`omni.isaac.lab.sensors.camera.utils.create_rotation_matrix_from_view` to :meth:`omni.isaac.lab.utils.math.create_rotation_matrix_from_view`
+
+
 0.25.2 (2024-10-16)
-~~~~~~~~~~~~~~~~~~~~
+~~~~~~~~~~~~~~~~~~~
 
 Added
 ^^^^^

source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/observations.py

@@ -17,7 +17,7 @@ from typing import TYPE_CHECKING
 import omni.isaac.lab.utils.math as math_utils
 from omni.isaac.lab.assets import Articulation, RigidObject
 from omni.isaac.lab.managers import SceneEntityCfg
-from omni.isaac.lab.sensors import Camera, RayCaster, RayCasterCamera, TiledCamera
+from omni.isaac.lab.sensors import Camera, Imu, RayCaster, RayCasterCamera, TiledCamera
 
 if TYPE_CHECKING:
     from omni.isaac.lab.envs import ManagerBasedEnv, ManagerBasedRLEnv
@@ -182,6 +182,48 @@ def body_incoming_wrench(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg) -> tor
     return link_incoming_forces.view(env.num_envs, -1)
 
 
+def imu_orientation(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("imu")) -> torch.Tensor:
+    """Imu sensor orientation w.r.t the env.scene.origin.
+
+    Args:
+        env: The environment.
+        asset_cfg: The SceneEntity associated with an Imu sensor.
+
+    Returns:
+        Orientation quaternion (wxyz), shape of torch.tensor is (num_env,4).
+    """
+    asset: Imu = env.scene[asset_cfg.name]
+    return asset.data.quat_w
+
+
+def imu_ang_vel(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("imu")) -> torch.Tensor:
+    """Imu sensor angular velocity w.r.t. env.scene.origin expressed in the sensor frame.
+
+    Args:
+        env: The environment.
+        asset_cfg: The SceneEntity associated with an Imu sensor.
+
+    Returns:
+        Angular velocity (rad/s), shape of torch.tensor is (num_env,3).
+    """
+    asset: Imu = env.scene[asset_cfg.name]
+    return asset.data.ang_vel_b
+
+
+def imu_lin_acc(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("imu")) -> torch.Tensor:
+    """Imu sensor linear acceleration w.r.t. env.scene.origin expressed in sensor frame.
+
+    Args:
+        env: The environment.
+        asset_cfg: The SceneEntity associated with an Imu sensor.
+
+    Returns:
+        linear acceleration (m/s^2), shape of torch.tensor is (num_env,3).
+    """
+    asset: Imu = env.scene[asset_cfg.name]
+    return asset.data.lin_acc_b
+
+
 def image(
     env: ManagerBasedEnv,
     sensor_cfg: SceneEntityCfg = SceneEntityCfg("tiled_camera"),

source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/__init__.py

@@ -30,12 +30,15 @@ interpretation of the prim paths for different sensor types:
 +---------------------+---------------------------+---------------------------------------------------------------+
 | Frame Transformer   | /World/robot/base         | Leaf exists and is a physics body (Articulation / Rigid Body) |
 +---------------------+---------------------------+---------------------------------------------------------------+
+| Imu                 | /World/robot/base         | Leaf exists and is a physics body (Rigid Body)                |
++---------------------+---------------------------+---------------------------------------------------------------+
 
 """
 
 from .camera import *  # noqa: F401, F403
 from .contact_sensor import *  # noqa: F401, F403
 from .frame_transformer import *  # noqa: F401
+from .imu import *  # noqa: F401, F403
 from .ray_caster import *  # noqa: F401, F403
 from .sensor_base import SensorBase  # noqa: F401
 from .sensor_base_cfg import SensorBaseCfg  # noqa: F401

source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py

@@ -13,6 +13,7 @@ from tensordict import TensorDict
 from typing import TYPE_CHECKING, Any, Literal
 
 import carb
+import omni.isaac.core.utils.stage as stage_utils
 import omni.kit.commands
 import omni.usd
 from omni.isaac.core.prims import XFormPrimView
@@ -21,11 +22,14 @@ from pxr import UsdGeom
 import omni.isaac.lab.sim as sim_utils
 from omni.isaac.lab.utils import to_camel_case
 from omni.isaac.lab.utils.array import convert_to_torch
-from omni.isaac.lab.utils.math import quat_from_matrix
+from omni.isaac.lab.utils.math import (
+    convert_camera_frame_orientation_convention,
+    create_rotation_matrix_from_view,
+    quat_from_matrix,
+)
 
 from ..sensor_base import SensorBase
 from .camera_data import CameraData
-from .utils import convert_orientation_convention, create_rotation_matrix_from_view
 
 if TYPE_CHECKING:
     from .camera_cfg import CameraCfg
@@ -116,7 +120,9 @@ class Camera(SensorBase):
         if self.cfg.spawn is not None:
             # compute the rotation offset
             rot = torch.tensor(self.cfg.offset.rot, dtype=torch.float32).unsqueeze(0)
-            rot_offset = convert_orientation_convention(rot, origin=self.cfg.offset.convention, target="opengl")
+            rot_offset = convert_camera_frame_orientation_convention(
+                rot, origin=self.cfg.offset.convention, target="opengl"
+            )
             rot_offset = rot_offset.squeeze(0).numpy()
             # ensure vertical aperture is set, otherwise replace with default for squared pixels
             if self.cfg.spawn.vertical_aperture is None:
@@ -289,7 +295,7 @@ class Camera(SensorBase):
         - :obj:`"ros"`    - forward axis: +Z - up axis -Y - Offset is applied in the ROS convention
         - :obj:`"world"`  - forward axis: +X - up axis +Z - Offset is applied in the World Frame convention
 
-        See :meth:`omni.isaac.lab.sensors.camera.utils.convert_orientation_convention` for more details
+        See :meth:`omni.isaac.lab.sensors.camera.utils.convert_camera_frame_orientation_convention` for more details
         on the conventions.
 
         Args:
@@ -318,7 +324,7 @@ class Camera(SensorBase):
                 orientations = torch.from_numpy(orientations).to(device=self._device)
             elif not isinstance(orientations, torch.Tensor):
                 orientations = torch.tensor(orientations, device=self._device)
-            orientations = convert_orientation_convention(orientations, origin=convention, target="opengl")
+            orientations = convert_camera_frame_orientation_convention(orientations, origin=convention, target="opengl")
         # set the pose
         self._view.set_world_poses(positions, orientations, env_ids)
 
@@ -339,8 +345,10 @@ class Camera(SensorBase):
         # resolve env_ids
         if env_ids is None:
             env_ids = self._ALL_INDICES
+        # get up axis of current stage
+        up_axis = stage_utils.get_stage_up_axis()
         # set camera poses using the view
-        orientations = quat_from_matrix(create_rotation_matrix_from_view(eyes, targets, device=self._device))
+        orientations = quat_from_matrix(create_rotation_matrix_from_view(eyes, targets, up_axis, device=self._device))
         self._view.set_world_poses(eyes, orientations, env_ids)
 
     """
@@ -596,7 +604,9 @@ class Camera(SensorBase):
         # get the poses from the view
         poses, quat = self._view.get_world_poses(env_ids)
         self._data.pos_w[env_ids] = poses
-        self._data.quat_w_world[env_ids] = convert_orientation_convention(quat, origin="opengl", target="world")
+        self._data.quat_w_world[env_ids] = convert_camera_frame_orientation_convention(
+            quat, origin="opengl", target="world"
+        )
 
     def _create_annotator_data(self):
         """Create the buffers to store the annotator data.

source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera_data.py

@@ -8,7 +8,7 @@ from dataclasses import dataclass
 from tensordict import TensorDict
 from typing import Any
 
-from .utils import convert_orientation_convention
+from omni.isaac.lab.utils.math import convert_camera_frame_orientation_convention
 
 
 @dataclass
@@ -77,7 +77,7 @@ class CameraData:
 
         Shape is (N, 4) where N is the number of sensors.
         """
-        return convert_orientation_convention(self.quat_w_world, origin="world", target="ros")
+        return convert_camera_frame_orientation_convention(self.quat_w_world, origin="world", target="ros")
 
     @property
     def quat_w_opengl(self) -> torch.Tensor:
@@ -89,4 +89,4 @@ class CameraData:
 
         Shape is (N, 4) where N is the number of sensors.
         """
-        return convert_orientation_convention(self.quat_w_world, origin="world", target="opengl")
+        return convert_camera_frame_orientation_convention(self.quat_w_world, origin="world", target="opengl")

source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/utils.py

@@ -8,16 +8,11 @@
 # needed to import for allowing type-hinting: torch.device | str | None
 from __future__ import annotations
 
-import math
 import numpy as np
 import torch
-import torch.nn.functional as F
 from collections.abc import Sequence
-from typing import Literal
 
-import omni.isaac.core.utils.stage as stage_utils
 import warp as wp
-from pxr import UsdGeom
 
 import omni.isaac.lab.utils.math as math_utils
 from omni.isaac.lab.utils.array import TensorData, convert_to_torch
@@ -262,143 +257,6 @@ def create_pointcloud_from_rgbd(
         return points_xyz, points_rgb
 
 
-def convert_orientation_convention(
-    orientation: torch.Tensor,
-    origin: Literal["opengl", "ros", "world"] = "opengl",
-    target: Literal["opengl", "ros", "world"] = "ros",
-) -> torch.Tensor:
-    r"""Converts a quaternion representing a rotation from one convention to another.
-
-    In USD, the camera follows the ``"opengl"`` convention. Thus, it is always in **Y up** convention.
-    This means that the camera is looking down the -Z axis with the +Y axis pointing up , and +X axis pointing right.
-    However, in ROS, the camera is looking down the +Z axis with the +Y axis pointing down, and +X axis pointing right.
-    Thus, the camera needs to be rotated by :math:`180^{\circ}` around the X axis to follow the ROS convention.
-
-    .. math::
-
-        T_{ROS} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & -1 & 0 & 0 \\ 0 & 0 & -1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} T_{USD}
-
-    On the other hand, the typical world coordinate system is with +X pointing forward, +Y pointing left,
-    and +Z pointing up. The camera can also be set in this convention by rotating the camera by :math:`90^{\circ}`
-    around the X axis and :math:`-90^{\circ}` around the Y axis.
-
-    .. math::
-
-        T_{WORLD} = \begin{bmatrix} 0 & 0 & -1 & 0 \\ -1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} T_{USD}
-
-    Thus, based on their application, cameras follow different conventions for their orientation. This function
-    converts a quaternion from one convention to another.
-
-    Possible conventions are:
-
-    - :obj:`"opengl"` - forward axis: -Z - up axis +Y - Offset is applied in the OpenGL (Usd.Camera) convention
-    - :obj:`"ros"`    - forward axis: +Z - up axis -Y - Offset is applied in the ROS convention
-    - :obj:`"world"`  - forward axis: +X - up axis +Z - Offset is applied in the World Frame convention
-
-    Args:
-        orientation: Quaternion of form `(w, x, y, z)` with shape (..., 4) in source convention
-        origin: Convention to convert to. Defaults to "ros".
-        target: Convention to convert from. Defaults to "opengl".
-
-    Returns:
-        Quaternion of form `(w, x, y, z)` with shape (..., 4) in target convention
-    """
-    if target == origin:
-        return orientation.clone()
-
-    # -- unify input type
-    if origin == "ros":
-        # convert from ros to opengl convention
-        rotm = math_utils.matrix_from_quat(orientation)
-        rotm[:, :, 2] = -rotm[:, :, 2]
-        rotm[:, :, 1] = -rotm[:, :, 1]
-        # convert to opengl convention
-        quat_gl = math_utils.quat_from_matrix(rotm)
-    elif origin == "world":
-        # convert from world (x forward and z up) to opengl convention
-        rotm = math_utils.matrix_from_quat(orientation)
-        rotm = torch.matmul(
-            rotm,
-            math_utils.matrix_from_euler(
-                torch.tensor([math.pi / 2, -math.pi / 2, 0], device=orientation.device), "XYZ"
-            ),
-        )
-        # convert to isaac-sim convention
-        quat_gl = math_utils.quat_from_matrix(rotm)
-    else:
-        quat_gl = orientation
-
-    # -- convert to target convention
-    if target == "ros":
-        # convert from opengl to ros convention
-        rotm = math_utils.matrix_from_quat(quat_gl)
-        rotm[:, :, 2] = -rotm[:, :, 2]
-        rotm[:, :, 1] = -rotm[:, :, 1]
-        return math_utils.quat_from_matrix(rotm)
-    elif target == "world":
-        # convert from opengl to world (x forward and z up) convention
-        rotm = math_utils.matrix_from_quat(quat_gl)
-        rotm = torch.matmul(
-            rotm,
-            math_utils.matrix_from_euler(
-                torch.tensor([math.pi / 2, -math.pi / 2, 0], device=orientation.device), "XYZ"
-            ).T,
-        )
-        return math_utils.quat_from_matrix(rotm)
-    else:
-        return quat_gl.clone()
-
-
-# @torch.jit.script
-def create_rotation_matrix_from_view(
-    eyes: torch.Tensor,
-    targets: torch.Tensor,
-    device: str = "cpu",
-) -> torch.Tensor:
-    """
-    This function takes a vector ''eyes'' which specifies the location
-    of the camera in world coordinates and the vector ''targets'' which
-    indicate the position of the object.
-    The output is a rotation matrix representing the transformation
-    from world coordinates -> view coordinates.
-
-        The inputs camera_position and targets can each be a
-        - 3 element tuple/list
-        - torch tensor of shape (1, 3)
-        - torch tensor of shape (N, 3)
-
-    Args:
-        eyes: position of the camera in world coordinates
-        targets: position of the object in world coordinates
-
-    The vectors are broadcast against each other so they all have shape (N, 3).
-
-    Returns:
-        R: (N, 3, 3) batched rotation matrices
-
-    Reference:
-    Based on PyTorch3D (https://github.com/facebookresearch/pytorch3d/blob/eaf0709d6af0025fe94d1ee7cec454bc3054826a/pytorch3d/renderer/cameras.py#L1635-L1685)
-    """
-    up_axis_token = stage_utils.get_stage_up_axis()
-    if up_axis_token == UsdGeom.Tokens.y:
-        up_axis = torch.tensor((0, 1, 0), device=device, dtype=torch.float32).repeat(eyes.shape[0], 1)
-    elif up_axis_token == UsdGeom.Tokens.z:
-        up_axis = torch.tensor((0, 0, 1), device=device, dtype=torch.float32).repeat(eyes.shape[0], 1)
-    else:
-        raise ValueError(f"Invalid up axis: {up_axis_token}")
-
-    # get rotation matrix in opengl format (-Z forward, +Y up)
-    z_axis = -F.normalize(targets - eyes, eps=1e-5)
-    x_axis = F.normalize(torch.cross(up_axis, z_axis, dim=1), eps=1e-5)
-    y_axis = F.normalize(torch.cross(z_axis, x_axis, dim=1), eps=1e-5)
-    is_close = torch.isclose(x_axis, torch.tensor(0.0), atol=5e-3).all(dim=1, keepdim=True)
-    if is_close.any():
-        replacement = F.normalize(torch.cross(y_axis, z_axis, dim=1), eps=1e-5)
-        x_axis = torch.where(is_close, replacement, x_axis)
-    R = torch.cat((x_axis[:, None, :], y_axis[:, None, :], z_axis[:, None, :]), dim=1)
-    return R.transpose(1, 2)
-
-
 def save_images_to_file(images: torch.Tensor, file_path: str):
     """Save images to file.
 

source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/imu/__init__.py

+# Copyright (c) 2022-2024, The Isaac Lab Project Developers.
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+"""
+Imu Sensor
+"""
+
+from .imu import Imu
+from .imu_cfg import ImuCfg
+from .imu_data import ImuData

source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/imu/imu.py

+# Copyright (c) 2022-2024, The Isaac Lab Project Developers.
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+from __future__ import annotations
+
+import torch
+from collections.abc import Sequence
+from typing import TYPE_CHECKING
+
+import omni.isaac.core.utils.stage as stage_utils
+import omni.physics.tensors.impl.api as physx
+from pxr import UsdPhysics
+
+import omni.isaac.lab.sim as sim_utils
+import omni.isaac.lab.utils.math as math_utils
+from omni.isaac.lab.markers import VisualizationMarkers
+
+from ..sensor_base import SensorBase
+from .imu_data import ImuData
+
+if TYPE_CHECKING:
+    from .imu_cfg import ImuCfg
+
+
+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.
+
+    .. note::
+
+        We are computing the accelerations using numerical differentiation from the velocities. Consequently, the
+        IMU sensor accuracy depends on the chosen phsyx timestep. For a sufficient accuracy, we recommend to keep the
+        timestep at least as 200Hz.
+
+    .. 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.
+
+    """
+
+    cfg: ImuCfg
+    """The configuration parameters."""
+
+    def __init__(self, cfg: ImuCfg):
+        """Initializes the Imu sensor.
+
+        Args:
+            cfg: The configuration parameters.
+        """
+        # initialize base class
+        super().__init__(cfg)
+        # Create empty variables for storing output data
+        self._data = ImuData()
+
+    def __str__(self) -> str:
+        """Returns: A string containing information about the instance."""
+        return (
+            f"Imu sensor @ '{self.cfg.prim_path}': \n"
+            f"\tview type         : {self._view.__class__}\n"
+            f"\tupdate period (s) : {self.cfg.update_period}\n"
+            f"\tnumber of sensors : {self._view.count}\n"
+        )
+
+    """
+    Properties
+    """
+
+    @property
+    def data(self) -> ImuData:
+        # update sensors if needed
+        self._update_outdated_buffers()
+        # return the data
+        return self._data
+
+    @property
+    def num_instances(self) -> int:
+        return self._view.count
+
+    """
+    Operations
+    """
+
+    def reset(self, env_ids: Sequence[int] | None = None):
+        # reset the timestamps
+        super().reset(env_ids)
+        # resolve None
+        if env_ids is None:
+            env_ids = slice(None)
+        # reset accumulative data buffers
+        self._data.quat_w[env_ids] = 0.0
+        self._data.lin_vel_b[env_ids] = 0.0
+        self._data.ang_vel_b[env_ids] = 0.0
+        self._data.lin_acc_b[env_ids] = 0.0
+        self._data.ang_acc_b[env_ids] = 0.0
+
+    def update(self, dt: float, force_recompute: bool = False):
+        # save timestamp
+        self._dt = dt
+        # execute updating
+        super().update(dt, force_recompute)
+
+    """
+    Implementation.
+    """
+
+    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
+        """
+        # Initialize parent class
+        super()._initialize_impl()
+        # create simulation view
+        self._physics_sim_view = physx.create_simulation_view(self._backend)
+        self._physics_sim_view.set_subspace_roots("/")
+        # check if the prim at path is a rigid prim
+        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}")
+
+        # Create internal buffers
+        self._initialize_buffers_impl()
+
+    def _update_buffers_impl(self, env_ids: Sequence[int]):
+        """Fills the buffers of the sensor data."""
+        # check if self._dt is set (this is set in the update function)
+        if not hasattr(self, "_dt"):
+            raise RuntimeError(
+                "The update function must be called before the data buffers are accessed the first time."
+            )
+        # default to all sensors
+        if len(env_ids) == self._num_envs:
+            env_ids = slice(None)
+        # obtain the poses of the sensors
+        pos_w, quat_w = self._view.get_transforms()[env_ids].split([3, 4], dim=-1)
+        quat_w = math_utils.convert_quat(quat_w, to="wxyz")
+
+        # store the poses
+        self._data.pos_w[env_ids] = pos_w + math_utils.quat_rotate(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_pos_b = self._view.get_coms().to(self.device).split([3, 4], dim=-1)[0]
+
+        # obtain the velocities of the link 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
+        lin_vel_w += torch.linalg.cross(
+            ang_vel_w, math_utils.quat_rotate(quat_w, self._offset_pos_b[env_ids] - com_pos_b[env_ids]), dim=-1
+        )
+
+        # numerical derivative
+        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
+        # store the velocities
+        self._data.lin_vel_b[env_ids] = math_utils.quat_rotate_inverse(self._data.quat_w[env_ids], lin_vel_w)
+        self._data.ang_vel_b[env_ids] = math_utils.quat_rotate_inverse(self._data.quat_w[env_ids], ang_vel_w)
+        # store the accelerations
+        self._data.lin_acc_b[env_ids] = math_utils.quat_rotate_inverse(self._data.quat_w[env_ids], lin_acc_w)
+        self._data.ang_acc_b[env_ids] = math_utils.quat_rotate_inverse(self._data.quat_w[env_ids], ang_acc_w)
+
+        self._prev_lin_vel_w[env_ids] = lin_vel_w
+        self._prev_ang_vel_w[env_ids] = ang_vel_w
+
+    def _initialize_buffers_impl(self):
+        """Create buffers for storing data."""
+        # data buffers
+        self._data.pos_w = torch.zeros(self._view.count, 3, device=self._device)
+        self._data.quat_w = torch.zeros(self._view.count, 4, device=self._device)
+        self._data.quat_w[:, 0] = 1.0
+        self._data.lin_vel_b = torch.zeros_like(self._data.pos_w)
+        self._data.ang_vel_b = torch.zeros_like(self._data.pos_w)
+        self._data.lin_acc_b = torch.zeros_like(self._data.pos_w)
+        self._data.ang_acc_b = torch.zeros_like(self._data.pos_w)
+        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
+        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
+        self._gravity_bias_w = torch.tensor(list(self.cfg.gravity_bias), device=self._device).repeat(
+            self._view.count, 1
+        )
+
+    def _set_debug_vis_impl(self, debug_vis: bool):
+        # set visibility of markers
+        # note: parent only deals with callbacks. not their visibility
+        if debug_vis:
+            # create markers if necessary for the first tome
+            if not hasattr(self, "acceleration_visualizer"):
+                self.acceleration_visualizer = VisualizationMarkers(self.cfg.visualizer_cfg)
+            # set their visibility to true
+            self.acceleration_visualizer.set_visibility(True)
+        else:
+            if hasattr(self, "acceleration_visualizer"):
+                self.acceleration_visualizer.set_visibility(False)
+
+    def _debug_vis_callback(self, event):
+        # safely return if view becomes invalid
+        # note: this invalidity happens because of isaac sim view callbacks
+        if self._view is None:
+            return
+        # get marker location
+        # -- base state
+        base_pos_w = self._data.pos_w.clone()
+        base_pos_w[:, 2] += 0.5
+        # -- resolve the scales
+        default_scale = self.acceleration_visualizer.cfg.markers["arrow"].scale
+        arrow_scale = torch.tensor(default_scale, device=self.device).repeat(self._data.lin_acc_b.shape[0], 1)
+        # get up axis of current stage
+        up_axis = stage_utils.get_stage_up_axis()
+        # arrow-direction
+        quat_opengl = math_utils.quat_from_matrix(
+            math_utils.create_rotation_matrix_from_view(
+                self._data.pos_w,
+                self._data.pos_w + math_utils.quat_rotate(self._data.quat_w, self._data.lin_acc_b),
+                up_axis=up_axis,
+                device=self._device,
+            )
+        )
+        quat_w = math_utils.convert_camera_frame_orientation_convention(quat_opengl, "opengl", "world")
+        # display markers
+        self.acceleration_visualizer.visualize(base_pos_w, quat_w, arrow_scale)

source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/imu/imu_cfg.py

+# Copyright (c) 2022-2024, The Isaac Lab Project Developers.
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+from __future__ import annotations
+
+from omni.isaac.lab.markers import VisualizationMarkersCfg
+from omni.isaac.lab.markers.config import RED_ARROW_X_MARKER_CFG
+from omni.isaac.lab.utils import configclass
+
+from ..sensor_base_cfg import SensorBaseCfg
+from .imu import Imu
+
+
+@configclass
+class ImuCfg(SensorBaseCfg):
+    """Configuration for an Inertial Measurement Unit (IMU) sensor."""
+
+    class_type: type = Imu
+
+    @configclass
+    class OffsetCfg:
+        """The offset pose of the sensor's frame from the sensor's parent frame."""
+
+        pos: tuple[float, float, float] = (0.0, 0.0, 0.0)
+        """Translation w.r.t. the parent frame. Defaults to (0.0, 0.0, 0.0)."""
+
+        rot: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0)
+        """Quaternion rotation (w, x, y, z) w.r.t. the parent frame. Defaults to (1.0, 0.0, 0.0, 0.0)."""
+
+    offset: OffsetCfg = OffsetCfg()
+    """The offset pose of the sensor's frame from the sensor's parent frame. Defaults to identity."""
+
+    visualizer_cfg: VisualizationMarkersCfg = RED_ARROW_X_MARKER_CFG.replace(prim_path="/Visuals/Command/velocity_goal")
+    """The configuration object for the visualization markers. Defaults to RED_ARROW_X_MARKER_CFG.
+
+    This attribute is only used when debug visualization is enabled.
+    """
+    gravity_bias: tuple[float, float, float] = (0.0, 0.0, 9.81)
+    """The linear acceleration bias applied to the linear acceleration in the world frame (x,y,z).
+
+    Imu sensors typically output a positive gravity acceleration in opposition to the direction of gravity. This
+    config parameter allows users to subtract that bias if set to (0.,0.,0.). By default this is set to (0.0,0.0,9.81)
+    which results in a positive acceleration reading in the world Z.
+    """

source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/imu/imu_data.py

+# Copyright (c) 2022-2024, The Isaac Lab Project Developers.
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+from __future__ import annotations
+
+import torch
+from dataclasses import dataclass
+
+
+@dataclass
+class ImuData:
+    """Data container for the Imu sensor."""
+
+    pos_w: torch.Tensor = None
+    """Position of the sensor origin in world frame.
+
+    Shape is (N, 3), where ``N`` is the number of environments.
+    """
+
+    quat_w: torch.Tensor = None
+    """Orientation of the sensor origin in quaternion ``(w, x, y, z)`` in world frame.
+
+    Shape is (N, 4), where ``N`` is the number of environments.
+    """
+
+    lin_vel_b: torch.Tensor = None
+    """IMU frame angular velocity relative to the world expressed in IMU frame.
+
+    Shape is (N, 3), where ``N`` is the number of environments.
+    """
+
+    ang_vel_b: torch.Tensor = None
+    """IMU frame angular velocity relative to the world expressed in IMU frame.
+
+    Shape is (N, 3), where ``N`` is the number of environments.
+    """
+
+    lin_acc_b: torch.Tensor = None
+    """IMU frame linear acceleration relative to the world expressed in IMU frame.
+
+    Shape is (N, 3), where ``N`` is the number of environments.
+    """
+
+    ang_acc_b: torch.Tensor = None
+    """IMU frame angular acceleration relative to the world expressed in IMU frame.
+
+    Shape is (N, 3), where ``N`` is the number of environments.
+    """

source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/ray_caster/ray_caster_camera.py

@@ -10,12 +10,12 @@ from collections.abc import Sequence
 from tensordict import TensorDict
 from typing import TYPE_CHECKING, ClassVar, Literal
 
+import omni.isaac.core.utils.stage as stage_utils
 import omni.physics.tensors.impl.api as physx
 from omni.isaac.core.prims import XFormPrimView
 
 import omni.isaac.lab.utils.math as math_utils
 from omni.isaac.lab.sensors.camera import CameraData
-from omni.isaac.lab.sensors.camera.utils import convert_orientation_convention, create_rotation_matrix_from_view
 from omni.isaac.lab.utils.warp import raycast_mesh
 
 from .ray_caster import RayCaster
@@ -170,7 +170,7 @@ class RayCasterCamera(RayCaster):
         - :obj:`"ros"`    - forward axis: +Z - up axis -Y - Offset is applied in the ROS convention
         - :obj:`"world"`  - forward axis: +X - up axis +Z - Offset is applied in the World Frame convention
 
-        See :meth:`omni.isaac.lab.sensors.camera.utils.convert_orientation_convention` for more details
+        See :meth:`omni.isaac.lab.utils.maths.convert_camera_frame_orientation_convention` for more details
         on the conventions.
 
         Args:
@@ -196,7 +196,9 @@ class RayCasterCamera(RayCaster):
             self._offset_pos[env_ids] = math_utils.quat_apply(math_utils.quat_inv(quat_w), pos_offset_world_frame)
         if orientations is not None:
             # convert rotation matrix from input convention to world
-            quat_w_set = convert_orientation_convention(orientations, origin=convention, target="world")
+            quat_w_set = math_utils.convert_camera_frame_orientation_convention(
+                orientations, origin=convention, target="world"
+            )
             self._offset_quat[env_ids] = math_utils.quat_mul(math_utils.quat_inv(quat_w), quat_w_set)
 
         # update the data
@@ -218,8 +220,12 @@ class RayCasterCamera(RayCaster):
             RuntimeError: If the camera prim is not set. Need to call :meth:`initialize` method first.
             NotImplementedError: If the stage up-axis is not "Y" or "Z".
         """
+        # get up axis of current stage
+        up_axis = stage_utils.get_stage_up_axis()
         # camera position and rotation in opengl convention
-        orientations = math_utils.quat_from_matrix(create_rotation_matrix_from_view(eyes, targets, device=self._device))
+        orientations = math_utils.quat_from_matrix(
+            math_utils.create_rotation_matrix_from_view(eyes, targets, up_axis=up_axis, device=self._device)
+        )
         self.set_world_poses(eyes, orientations, env_ids, convention="opengl")
 
     """
@@ -243,7 +249,7 @@ class RayCasterCamera(RayCaster):
         # create buffer to store ray hits
         self.ray_hits_w = torch.zeros(self._view.count, self.num_rays, 3, device=self._device)
         # set offsets
-        quat_w = convert_orientation_convention(
+        quat_w = math_utils.convert_camera_frame_orientation_convention(
             torch.tensor([self.cfg.offset.rot], device=self._device), origin=self.cfg.offset.convention, target="world"
         )
         self._offset_quat = quat_w.repeat(self._view.count, 1)

source/extensions/omni.isaac.lab/omni/isaac/lab/utils/math.py

@@ -8,6 +8,7 @@
 # needed to import for allowing type-hinting: torch.Tensor | np.ndarray
 from __future__ import annotations
 
+import math
 import numpy as np
 import torch
 import torch.nn.functional
@@ -1418,3 +1419,143 @@ def sample_cylinder(
     xyz[..., 2].uniform_(h_min, h_max)
     # return positions
     return xyz
+
+
+"""
+Orientation Conversions
+"""
+
+
+def convert_camera_frame_orientation_convention(
+    orientation: torch.Tensor,
+    origin: Literal["opengl", "ros", "world"] = "opengl",
+    target: Literal["opengl", "ros", "world"] = "ros",
+) -> torch.Tensor:
+    r"""Converts a quaternion representing a rotation from one convention to another.
+
+    In USD, the camera follows the ``"opengl"`` convention. Thus, it is always in **Y up** convention.
+    This means that the camera is looking down the -Z axis with the +Y axis pointing up , and +X axis pointing right.
+    However, in ROS, the camera is looking down the +Z axis with the +Y axis pointing down, and +X axis pointing right.
+    Thus, the camera needs to be rotated by :math:`180^{\circ}` around the X axis to follow the ROS convention.
+
+    .. math::
+
+        T_{ROS} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & -1 & 0 & 0 \\ 0 & 0 & -1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} T_{USD}
+
+    On the other hand, the typical world coordinate system is with +X pointing forward, +Y pointing left,
+    and +Z pointing up. The camera can also be set in this convention by rotating the camera by :math:`90^{\circ}`
+    around the X axis and :math:`-90^{\circ}` around the Y axis.
+
+    .. math::
+
+        T_{WORLD} = \begin{bmatrix} 0 & 0 & -1 & 0 \\ -1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} T_{USD}
+
+    Thus, based on their application, cameras follow different conventions for their orientation. This function
+    converts a quaternion from one convention to another.
+
+    Possible conventions are:
+
+    - :obj:`"opengl"` - forward axis: -Z - up axis +Y - Offset is applied in the OpenGL (Usd.Camera) convention
+    - :obj:`"ros"`    - forward axis: +Z - up axis -Y - Offset is applied in the ROS convention
+    - :obj:`"world"`  - forward axis: +X - up axis +Z - Offset is applied in the World Frame convention
+
+    Args:
+        orientation: Quaternion of form `(w, x, y, z)` with shape (..., 4) in source convention.
+        origin: Convention to convert from. Defaults to "opengl".
+        target: Convention to convert to. Defaults to "ros".
+
+    Returns:
+        Quaternion of form `(w, x, y, z)` with shape (..., 4) in target convention
+    """
+    if target == origin:
+        return orientation.clone()
+
+    # -- unify input type
+    if origin == "ros":
+        # convert from ros to opengl convention
+        rotm = matrix_from_quat(orientation)
+        rotm[:, :, 2] = -rotm[:, :, 2]
+        rotm[:, :, 1] = -rotm[:, :, 1]
+        # convert to opengl convention
+        quat_gl = quat_from_matrix(rotm)
+    elif origin == "world":
+        # convert from world (x forward and z up) to opengl convention
+        rotm = matrix_from_quat(orientation)
+        rotm = torch.matmul(
+            rotm,
+            matrix_from_euler(torch.tensor([math.pi / 2, -math.pi / 2, 0], device=orientation.device), "XYZ"),
+        )
+        # convert to isaac-sim convention
+        quat_gl = quat_from_matrix(rotm)
+    else:
+        quat_gl = orientation
+
+    # -- convert to target convention
+    if target == "ros":
+        # convert from opengl to ros convention
+        rotm = matrix_from_quat(quat_gl)
+        rotm[:, :, 2] = -rotm[:, :, 2]
+        rotm[:, :, 1] = -rotm[:, :, 1]
+        return quat_from_matrix(rotm)
+    elif target == "world":
+        # convert from opengl to world (x forward and z up) convention
+        rotm = matrix_from_quat(quat_gl)
+        rotm = torch.matmul(
+            rotm,
+            matrix_from_euler(torch.tensor([math.pi / 2, -math.pi / 2, 0], device=orientation.device), "XYZ").T,
+        )
+        return quat_from_matrix(rotm)
+    else:
+        return quat_gl.clone()
+
+
+def create_rotation_matrix_from_view(
+    eyes: torch.Tensor,
+    targets: torch.Tensor,
+    up_axis: Literal["Y", "Z"] = "Z",
+    device: str = "cpu",
+) -> torch.Tensor:
+    """Compute the rotation matrix from world to view coordinates.
+
+    This function takes a vector ''eyes'' which specifies the location
+    of the camera in world coordinates and the vector ''targets'' which
+    indicate the position of the object.
+    The output is a rotation matrix representing the transformation
+    from world coordinates -> view coordinates.
+
+        The inputs eyes and targets can each be a
+        - 3 element tuple/list
+        - torch tensor of shape (1, 3)
+        - torch tensor of shape (N, 3)
+
+    Args:
+        eyes: Position of the camera in world coordinates.
+        targets: Position of the object in world coordinates.
+        up_axis: The up axis of the camera. Defaults to "Z".
+        device: The device to create torch tensors on. Defaults to "cpu".
+
+    The vectors are broadcast against each other so they all have shape (N, 3).
+
+    Returns:
+        R: (N, 3, 3) batched rotation matrices
+
+    Reference:
+    Based on PyTorch3D (https://github.com/facebookresearch/pytorch3d/blob/eaf0709d6af0025fe94d1ee7cec454bc3054826a/pytorch3d/renderer/cameras.py#L1635-L1685)
+    """
+    if up_axis == "Y":
+        up_axis_vec = torch.tensor((0, 1, 0), device=device, dtype=torch.float32).repeat(eyes.shape[0], 1)
+    elif up_axis == "Z":
+        up_axis_vec = torch.tensor((0, 0, 1), device=device, dtype=torch.float32).repeat(eyes.shape[0], 1)
+    else:
+        raise ValueError(f"Invalid up axis: {up_axis}. Valid options are 'Y' and 'Z'.")
+
+    # get rotation matrix in opengl format (-Z forward, +Y up)
+    z_axis = -torch.nn.functional.normalize(targets - eyes, eps=1e-5)
+    x_axis = torch.nn.functional.normalize(torch.cross(up_axis_vec, z_axis, dim=1), eps=1e-5)
+    y_axis = torch.nn.functional.normalize(torch.cross(z_axis, x_axis, dim=1), eps=1e-5)
+    is_close = torch.isclose(x_axis, torch.tensor(0.0), atol=5e-3).all(dim=1, keepdim=True)
+    if is_close.any():
+        replacement = torch.nn.functional.normalize(torch.cross(y_axis, z_axis, dim=1), eps=1e-5)
+        x_axis = torch.where(is_close, replacement, x_axis)
+    R = torch.cat((x_axis[:, None, :], y_axis[:, None, :], z_axis[:, None, :]), dim=1)
+    return R.transpose(1, 2)

source/extensions/omni.isaac.lab/test/sensors/check_imu_sensor.py

+# Copyright (c) 2022-2024, The Isaac Lab Project Developers.
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+"""
+Visual test script for the imu sensor from the Orbit framework.
+"""
+
+from __future__ import annotations
+
+"""Launch Isaac Sim Simulator first."""
+
+import argparse
+
+from omni.isaac.kit import SimulationApp
+
+# add argparse arguments
+parser = argparse.ArgumentParser(description="Imu Test Script")
+parser.add_argument("--headless", action="store_true", default=False, help="Force display off at all times.")
+parser.add_argument("--num_envs", type=int, default=128, help="Number of environments to clone.")
+parser.add_argument(
+    "--terrain_type",
+    type=str,
+    default="generator",
+    choices=["generator", "usd", "plane"],
+    help="Type of terrain to import. Can be 'generator' or 'usd' or 'plane'.",
+)
+args_cli = parser.parse_args()
+
+# launch omniverse app
+config = {"headless": args_cli.headless}
+simulation_app = SimulationApp(config)
+
+
+"""Rest everything follows."""
+
+import torch
+import traceback
+
+import carb
+import omni
+from omni.isaac.cloner import GridCloner
+from omni.isaac.core.simulation_context import SimulationContext
+from omni.isaac.core.utils.viewports import set_camera_view
+from pxr import PhysxSchema
+
+import omni.isaac.lab.sim as sim_utils
+import omni.isaac.lab.terrains as terrain_gen
+from omni.isaac.lab.assets import RigidObject, RigidObjectCfg
+from omni.isaac.lab.sensors.imu import Imu, ImuCfg
+from omni.isaac.lab.terrains.config.rough import ROUGH_TERRAINS_CFG
+from omni.isaac.lab.terrains.terrain_importer import TerrainImporter
+from omni.isaac.lab.utils.assets import ISAAC_NUCLEUS_DIR
+from omni.isaac.lab.utils.timer import Timer
+
+
+def design_scene(sim: SimulationContext, num_envs: int = 2048) -> RigidObject:
+    """Design the scene."""
+    # Handler for terrains importing
+    terrain_importer_cfg = terrain_gen.TerrainImporterCfg(
+        prim_path="/World/ground",
+        terrain_type="generator",
+        terrain_generator=ROUGH_TERRAINS_CFG,
+        usd_path=f"{ISAAC_NUCLEUS_DIR}/Environments/Terrains/rough_plane.usd",
+        max_init_terrain_level=None,
+        num_envs=1,
+    )
+    _ = TerrainImporter(terrain_importer_cfg)
+    # obtain the current stage
+    stage = omni.usd.get_context().get_stage()
+    # Create interface to clone the scene
+    cloner = GridCloner(spacing=2.0)
+    cloner.define_base_env("/World/envs")
+    envs_prim_paths = cloner.generate_paths("/World/envs/env", num_paths=num_envs)
+    # create source prim
+    stage.DefinePrim(envs_prim_paths[0], "Xform")
+    # clone the env xform
+    cloner.clone(source_prim_path="/World/envs/env_0", prim_paths=envs_prim_paths, replicate_physics=True)
+    # Define the scene
+    # -- Light
+    cfg = sim_utils.DistantLightCfg(intensity=2000)
+    cfg.func("/World/light", cfg)
+    # -- Balls
+    cfg = RigidObjectCfg(
+        spawn=sim_utils.SphereCfg(
+            radius=0.25,
+            rigid_props=sim_utils.RigidBodyPropertiesCfg(),
+            mass_props=sim_utils.MassPropertiesCfg(mass=0.5),
+            collision_props=sim_utils.CollisionPropertiesCfg(),
+            visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0)),
+        ),
+        prim_path="/World/envs/env_.*/ball",
+        init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 5.0)),
+    )
+    balls = RigidObject(cfg)
+    # Clone the scene
+    # obtain the current physics scene
+    physics_scene_prim_path = None
+    for prim in stage.Traverse():
+        if prim.HasAPI(PhysxSchema.PhysxSceneAPI):
+            physics_scene_prim_path = prim.GetPrimPath()
+            carb.log_info(f"Physics scene prim path: {physics_scene_prim_path}")
+            break
+    # filter collisions within each environment instance
+    cloner.filter_collisions(
+        physics_scene_prim_path,
+        "/World/collisions",
+        envs_prim_paths,
+    )
+    return balls
+
+
+def main():
+    """Main function."""
+
+    # Load kit helper
+    sim_params = {
+        "use_gpu": True,
+        "use_gpu_pipeline": True,
+        "use_flatcache": True,  # deprecated from Isaac Sim 2023.1 onwards
+        "use_fabric": True,  # used from Isaac Sim 2023.1 onwards
+        "enable_scene_query_support": True,
+    }
+    sim = SimulationContext(
+        physics_dt=1.0 / 60.0, rendering_dt=1.0 / 60.0, sim_params=sim_params, backend="torch", device="cuda:0"
+    )
+    # Set main camera
+    set_camera_view([0.0, 30.0, 25.0], [0.0, 0.0, -2.5])
+
+    # Parameters
+    num_envs = args_cli.num_envs
+    # Design the scene
+    balls = design_scene(sim=sim, num_envs=num_envs)
+
+    # Create a ray-caster sensor
+    imu_cfg = ImuCfg(
+        prim_path="/World/envs/env_.*/ball",
+        debug_vis=not args_cli.headless,
+    )
+    # increase scale of the arrows for better visualization
+    imu_cfg.visualizer_cfg.markers["arrow"].scale = (1.0, 0.2, 0.2)
+    imu = Imu(cfg=imu_cfg)
+
+    # Play simulator and init the Imu
+    sim.reset()
+
+    # Print the sensor information
+    print(imu)
+
+    # Get the ball initial positions
+    sim.step(render=not args_cli.headless)
+    balls.update(sim.get_physics_dt())
+    ball_initial_positions = balls.data.root_pos_w.clone()
+    ball_initial_orientations = balls.data.root_quat_w.clone()
+
+    # Create a counter for resetting the scene
+    step_count = 0
+    # Simulate physics
+    while simulation_app.is_running():
+        # If simulation is stopped, then exit.
+        if sim.is_stopped():
+            break
+        # If simulation is paused, then skip.
+        if not sim.is_playing():
+            sim.step(render=not args_cli.headless)
+            continue
+        # Reset the scene
+        if step_count % 500 == 0:
+            # reset ball positions
+            balls.write_root_pose_to_sim(torch.cat([ball_initial_positions, ball_initial_orientations], dim=-1))
+            balls.reset()
+            # reset the sensor
+            imu.reset()
+            # reset the counter
+            step_count = 0
+        # Step simulation
+        sim.step()
+        # Update the imu sensor
+        with Timer(f"Imu sensor update with {num_envs}"):
+            imu.update(dt=sim.get_physics_dt(), force_recompute=True)
+        # Update counter
+        step_count += 1
+
+
+if __name__ == "__main__":
+    try:
+        # Run the main function
+        main()
+    except Exception as err:
+        carb.log_error(err)
+        carb.log_error(traceback.format_exc())
+        raise
+    finally:
+        # close sim app
+        simulation_app.close()

source/extensions/omni.isaac.lab/test/sensors/test_imu.py

+# Copyright (c) 2022-2024, The Isaac Lab Project Developers.
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+"""Launch Isaac Sim Simulator first."""
+
+from omni.isaac.lab.app import AppLauncher, run_tests
+
+# launch omniverse app
+app_launcher = AppLauncher(headless=True, enable_cameras=True)
+simulation_app = app_launcher.app
+
+"""Rest everything follows."""
+
+import pathlib
+import torch
+import unittest
+
+import omni.isaac.core.utils.stage as stage_utils
+
+import omni.isaac.lab.sim as sim_utils
+import omni.isaac.lab.utils.math as math_utils
+from omni.isaac.lab.actuators import ImplicitActuatorCfg
+from omni.isaac.lab.assets import ArticulationCfg, RigidObjectCfg
+from omni.isaac.lab.markers.config import GREEN_ARROW_X_MARKER_CFG, RED_ARROW_X_MARKER_CFG
+from omni.isaac.lab.scene import InteractiveScene, InteractiveSceneCfg
+from omni.isaac.lab.sensors.imu import ImuCfg
+from omni.isaac.lab.terrains import TerrainImporterCfg
+from omni.isaac.lab.utils import configclass
+
+##
+# Pre-defined configs
+##
+from omni.isaac.lab_assets.anymal import ANYMAL_C_CFG  # isort: skip
+from omni.isaac.lab.utils.assets import NUCLEUS_ASSET_ROOT_DIR  # isort: skip
+
+# offset of imu_link from base_link on anymal_c
+POS_OFFSET = (0.2488, 0.00835, 0.04628)
+ROT_OFFSET = (0.7071068, 0, 0, 0.7071068)
+
+# offset of imu_link from link_1 on simple_2_link
+PEND_POS_OFFSET = (0.4, 0.0, 0.1)
+PEND_ROT_OFFSET = (0.5, 0.5, 0.5, 0.5)
+
+
+@configclass
+class MySceneCfg(InteractiveSceneCfg):
+    """Example scene configuration."""
+
+    # terrain - flat terrain plane
+    terrain = TerrainImporterCfg(
+        prim_path="/World/ground",
+        terrain_type="plane",
+        max_init_terrain_level=None,
+    )
+
+    # rigid objects - balls
+    balls = RigidObjectCfg(
+        prim_path="{ENV_REGEX_NS}/ball",
+        init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 0.5)),
+        spawn=sim_utils.SphereCfg(
+            radius=0.25,
+            rigid_props=sim_utils.RigidBodyPropertiesCfg(),
+            mass_props=sim_utils.MassPropertiesCfg(mass=0.5),
+            collision_props=sim_utils.CollisionPropertiesCfg(),
+            visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0)),
+        ),
+    )
+
+    cube = RigidObjectCfg(
+        prim_path="{ENV_REGEX_NS}/cube",
+        init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, -2.0, 0.5)),
+        spawn=sim_utils.CuboidCfg(
+            size=(0.25, 0.25, 0.25),
+            rigid_props=sim_utils.RigidBodyPropertiesCfg(),
+            mass_props=sim_utils.MassPropertiesCfg(mass=0.5),
+            collision_props=sim_utils.CollisionPropertiesCfg(),
+            visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 0.0, 1.0)),
+        ),
+    )
+
+    # articulations - robot
+    robot = ANYMAL_C_CFG.replace(prim_path="{ENV_REGEX_NS}/robot")
+    pendulum = ArticulationCfg(
+        prim_path="{ENV_REGEX_NS}/pendulum",
+        spawn=sim_utils.UrdfFileCfg(
+            fix_base=True,
+            merge_fixed_joints=False,
+            make_instanceable=False,
+            asset_path=f"{pathlib.Path(__file__).parent.resolve()}/urdfs/simple_2_link.urdf",
+            articulation_props=sim_utils.ArticulationRootPropertiesCfg(
+                enabled_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=0
+            ),
+        ),
+        init_state=ArticulationCfg.InitialStateCfg(),
+        actuators={
+            "joint_1_act": ImplicitActuatorCfg(joint_names_expr=["joint_.*"], stiffness=0.0, damping=0.3),
+        },
+    )
+    # sensors - imu (filled inside unit test)
+    imu_ball: ImuCfg = ImuCfg(
+        prim_path="{ENV_REGEX_NS}/ball",
+        gravity_bias=(0.0, 0.0, 0.0),
+    )
+    imu_cube: ImuCfg = ImuCfg(
+        prim_path="{ENV_REGEX_NS}/cube",
+        gravity_bias=(0.0, 0.0, 0.0),
+    )
+    imu_robot_imu_link: ImuCfg = ImuCfg(
+        prim_path="{ENV_REGEX_NS}/robot/imu_link",
+        gravity_bias=(0.0, 0.0, 0.0),
+    )
+    imu_robot_base: ImuCfg = ImuCfg(
+        prim_path="{ENV_REGEX_NS}/robot/base",
+        offset=ImuCfg.OffsetCfg(
+            pos=POS_OFFSET,
+            rot=ROT_OFFSET,
+        ),
+        gravity_bias=(0.0, 0.0, 0.0),
+    )
+
+    imu_pendulum_imu_link: ImuCfg = ImuCfg(
+        prim_path="{ENV_REGEX_NS}/pendulum/imu_link",
+        debug_vis=not app_launcher._headless,
+        visualizer_cfg=RED_ARROW_X_MARKER_CFG.replace(prim_path="/Visuals/Acceleration/imu_link"),
+        gravity_bias=(0.0, 0.0, 9.81),
+    )
+    imu_pendulum_base: ImuCfg = ImuCfg(
+        prim_path="{ENV_REGEX_NS}/pendulum/link_1",
+        offset=ImuCfg.OffsetCfg(
+            pos=PEND_POS_OFFSET,
+            rot=PEND_ROT_OFFSET,
+        ),
+        debug_vis=not app_launcher._headless,
+        visualizer_cfg=GREEN_ARROW_X_MARKER_CFG.replace(prim_path="/Visuals/Acceleration/base"),
+        gravity_bias=(0.0, 0.0, 9.81),
+    )
+
+    def __post_init__(self):
+        """Post initialization."""
+        # change position of the robot
+        self.robot.init_state.pos = (0.0, 2.0, 1.0)
+        self.pendulum.init_state.pos = (-1.0, 1.0, 0.5)
+
+        # change asset
+        self.robot.spawn.usd_path = f"{NUCLEUS_ASSET_ROOT_DIR}/Isaac/Robots/ANYbotics/anymal_c.usd"
+        # change iterations
+        self.robot.spawn.articulation_props.solver_position_iteration_count = 32
+        self.robot.spawn.articulation_props.solver_velocity_iteration_count = 32
+
+
+class TestImu(unittest.TestCase):
+    """Test for Imu sensor."""
+
+    def setUp(self):
+        """Create a blank new stage for each test."""
+        # Create a new stage
+        stage_utils.create_new_stage()
+        # Load simulation context
+        sim_cfg = sim_utils.SimulationCfg(dt=0.001)
+        sim_cfg.physx.solver_type = 0  # 0: PGS, 1: TGS --> use PGS for more accurate results
+        self.sim = sim_utils.SimulationContext(sim_cfg)
+        # construct scene
+        scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False)
+        self.scene = InteractiveScene(scene_cfg)
+        # Play the simulator
+        self.sim.reset()
+
+    def tearDown(self):
+        """Stops simulator after each test."""
+        # clear the stage
+        self.sim.clear_all_callbacks()
+        self.sim.clear_instance()
+
+    """
+    Tests
+    """
+
+    def test_constant_velocity(self):
+        """Test the Imu sensor with a constant velocity.
+
+        Expected behavior is that the linear and angular are approx the same at every time step as in each step we set
+        the same velocity and therefore reset the physx buffers."""
+        prev_lin_acc_ball = torch.zeros((self.scene.num_envs, 3), dtype=torch.float32, device=self.scene.device)
+        prev_ang_acc_ball = torch.zeros((self.scene.num_envs, 3), dtype=torch.float32, device=self.scene.device)
+        prev_lin_acc_cube = torch.zeros((self.scene.num_envs, 3), dtype=torch.float32, device=self.scene.device)
+        prev_ang_acc_cube = torch.zeros((self.scene.num_envs, 3), dtype=torch.float32, device=self.scene.device)
+
+        for idx in range(200):
+            # set velocity
+            self.scene.rigid_objects["balls"].write_root_velocity_to_sim(
+                torch.tensor([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=self.scene.device).repeat(
+                    self.scene.num_envs, 1
+                )
+            )
+            self.scene.rigid_objects["cube"].write_root_velocity_to_sim(
+                torch.tensor([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=self.scene.device).repeat(
+                    self.scene.num_envs, 1
+                )
+            )
+            # write data to sim
+            self.scene.write_data_to_sim()
+
+            # perform step
+            self.sim.step()
+            # read data from sim
+            self.scene.update(self.sim.get_physics_dt())
+
+            if idx > 1:
+                # check the imu accelerations
+                torch.testing.assert_close(
+                    self.scene.sensors["imu_ball"].data.lin_acc_b,
+                    prev_lin_acc_ball,
+                    rtol=1e-3,
+                    atol=1e-3,
+                )
+                torch.testing.assert_close(
+                    self.scene.sensors["imu_ball"].data.ang_acc_b,
+                    prev_ang_acc_ball,
+                    rtol=1e-3,
+                    atol=1e-3,
+                )
+
+                torch.testing.assert_close(
+                    self.scene.sensors["imu_cube"].data.lin_acc_b,
+                    prev_lin_acc_cube,
+                    rtol=1e-3,
+                    atol=1e-3,
+                )
+                torch.testing.assert_close(
+                    self.scene.sensors["imu_cube"].data.ang_acc_b,
+                    prev_ang_acc_cube,
+                    rtol=1e-3,
+                    atol=1e-3,
+                )
+
+                # check the imu velocities
+                # NOTE: the expected lin_vel_b is the same as the set velocity, as write_root_velocity_to_sim is
+                #       setting v_0 (initial velocity) and then a calculation step of v_i = v_0 + a*dt. Consequently,
+                #       the data.lin_vel_b is returning approx. v_i.
+                torch.testing.assert_close(
+                    self.scene.sensors["imu_ball"].data.lin_vel_b,
+                    torch.tensor(
+                        [[1.0, 0.0, -self.scene.physics_dt * 9.81]], dtype=torch.float32, device=self.scene.device
+                    ).repeat(self.scene.num_envs, 1),
+                    rtol=1e-4,
+                    atol=1e-4,
+                )
+                torch.testing.assert_close(
+                    self.scene.sensors["imu_cube"].data.lin_vel_b,
+                    torch.tensor(
+                        [[1.0, 0.0, -self.scene.physics_dt * 9.81]], dtype=torch.float32, device=self.scene.device
+                    ).repeat(self.scene.num_envs, 1),
+                    rtol=1e-4,
+                    atol=1e-4,
+                )
+
+            # update previous values
+            prev_lin_acc_ball = self.scene.sensors["imu_ball"].data.lin_acc_b.clone()
+            prev_ang_acc_ball = self.scene.sensors["imu_ball"].data.ang_acc_b.clone()
+            prev_lin_acc_cube = self.scene.sensors["imu_cube"].data.lin_acc_b.clone()
+            prev_ang_acc_cube = self.scene.sensors["imu_cube"].data.ang_acc_b.clone()
+
+    def test_constant_acceleration(self):
+        """Test the Imu sensor with a constant acceleration."""
+        for idx in range(100):
+            # set acceleration
+            self.scene.rigid_objects["balls"].write_root_velocity_to_sim(
+                torch.tensor([[0.1, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=self.scene.device).repeat(
+                    self.scene.num_envs, 1
+                )
+                * (idx + 1)
+            )
+            # write data to sim
+            self.scene.write_data_to_sim()
+            # perform step
+            self.sim.step()
+            # read data from sim
+            self.scene.update(self.sim.get_physics_dt())
+
+            # skip first step where initial velocity is zero
+            if idx < 1:
+                continue
+
+            # check the imu data
+            torch.testing.assert_close(
+                self.scene.sensors["imu_ball"].data.lin_acc_b,
+                math_utils.quat_rotate_inverse(
+                    self.scene.rigid_objects["balls"].data.root_quat_w,
+                    torch.tensor([[0.1, 0.0, 0.0]], dtype=torch.float32, device=self.scene.device).repeat(
+                        self.scene.num_envs, 1
+                    )
+                    / self.sim.get_physics_dt(),
+                ),
+                rtol=1e-4,
+                atol=1e-4,
+            )
+
+            # check the angular velocity
+            torch.testing.assert_close(
+                self.scene.sensors["imu_ball"].data.ang_vel_b,
+                self.scene.rigid_objects["balls"].data.root_ang_vel_b,
+                rtol=1e-4,
+                atol=1e-4,
+            )
+
+    def test_single_dof_pendulum(self):
+        """Test imu against analytical pendulum problem."""
+
+        # pendulum length
+        pend_length = PEND_POS_OFFSET[0]
+
+        # should achieve same results between the two imu sensors on the robot
+        for idx in range(500):
+
+            # write data to sim
+            self.scene.write_data_to_sim()
+            # perform step
+            self.sim.step()
+            # read data from sim
+            self.scene.update(self.sim.get_physics_dt())
+
+            # get pendulum joint state
+            joint_pos = self.scene.articulations["pendulum"].data.joint_pos
+            joint_vel = self.scene.articulations["pendulum"].data.joint_vel
+            joint_acc = self.scene.articulations["pendulum"].data.joint_acc
+
+            # IMU and base data
+            imu_data = self.scene.sensors["imu_pendulum_imu_link"].data
+            base_data = self.scene.sensors["imu_pendulum_base"].data
+
+            # extract imu_link imu_sensor dynamics
+            lin_vel_w_imu_link = math_utils.quat_rotate(imu_data.quat_w, imu_data.lin_vel_b)
+            lin_acc_w_imu_link = math_utils.quat_rotate(imu_data.quat_w, imu_data.lin_acc_b)
+
+            # calculate the joint dynamics from the imu_sensor (y axis of imu_link is parallel to joint axis of pendulum)
+            joint_vel_imu = math_utils.quat_rotate(imu_data.quat_w, imu_data.ang_vel_b)[..., 1].unsqueeze(-1)
+            joint_acc_imu = math_utils.quat_rotate(imu_data.quat_w, imu_data.ang_acc_b)[..., 1].unsqueeze(-1)
+
+            # calculate analytical solution
+            vx = -joint_vel * pend_length * torch.sin(joint_pos)
+            vy = torch.zeros(2, 1, device=self.scene.device)
+            vz = -joint_vel * pend_length * torch.cos(joint_pos)
+            gt_linear_vel_w = torch.cat([vx, vy, vz], dim=-1)
+
+            ax = -joint_acc * pend_length * torch.sin(joint_pos) - joint_vel**2 * pend_length * torch.cos(joint_pos)
+            ay = torch.zeros(2, 1, device=self.scene.device)
+            az = (
+                -joint_acc * pend_length * torch.cos(joint_pos)
+                + joint_vel**2 * pend_length * torch.sin(joint_pos)
+                + 9.81
+            )
+            gt_linear_acc_w = torch.cat([ax, ay, az], dim=-1)
+
+            # skip first step where initial velocity is zero
+            if idx < 2:
+                continue
+
+            # compare imu angular velocity with joint velocity
+            torch.testing.assert_close(
+                joint_vel,
+                joint_vel_imu,
+                rtol=1e-1,
+                atol=1e-3,
+            )
+            # compare imu angular acceleration with joint acceleration
+            torch.testing.assert_close(
+                joint_acc,
+                joint_acc_imu,
+                rtol=1e-1,
+                atol=1e-3,
+            )
+            # compare imu linear velocituy with simple pendulum calculation
+            torch.testing.assert_close(
+                gt_linear_vel_w,
+                lin_vel_w_imu_link,
+                rtol=1e-1,
+                atol=1e-3,
+            )
+            # compare imu linear acceleration with simple pendulum calculation
+            torch.testing.assert_close(
+                gt_linear_acc_w,
+                lin_acc_w_imu_link,
+                rtol=1e-1,
+                atol=1e0,
+            )
+
+            # check the position between offset and imu definition
+            torch.testing.assert_close(
+                base_data.pos_w,
+                imu_data.pos_w,
+                rtol=1e-5,
+                atol=1e-5,
+            )
+
+            # check the orientation between offset and imu definition
+            torch.testing.assert_close(
+                base_data.quat_w,
+                imu_data.quat_w,
+                rtol=1e-4,
+                atol=1e-4,
+            )
+
+            # check the angular velocities of the imus between offset and imu definition
+            torch.testing.assert_close(
+                base_data.ang_vel_b,
+                imu_data.ang_vel_b,
+                rtol=1e-4,
+                atol=1e-4,
+            )
+            # check the angular acceleration of the imus between offset and imu definition
+            torch.testing.assert_close(
+                base_data.ang_acc_b,
+                imu_data.ang_acc_b,
+                rtol=1e-4,
+                atol=1e-4,
+            )
+
+            # check the linear velocity of the imus between offset and imu definition
+            torch.testing.assert_close(
+                base_data.lin_vel_b,
+                imu_data.lin_vel_b,
+                rtol=1e-2,
+                atol=5e-3,
+            )
+
+            # check the linear acceleration of the imus between offset and imu definition
+            torch.testing.assert_close(
+                base_data.lin_acc_b,
+                imu_data.lin_acc_b,
+                rtol=1e-1,
+                atol=1e-1,
+            )
+
+    def test_offset_calculation(self):
+        """Test offset configuration argument."""
+        # should achieve same results between the two imu sensors on the robot
+        for idx in range(500):
+            # set acceleration
+            self.scene.articulations["robot"].write_root_velocity_to_sim(
+                torch.tensor([[0.05, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=self.scene.device).repeat(
+                    self.scene.num_envs, 1
+                )
+                * (idx + 1)
+            )
+            # write data to sim
+            self.scene.write_data_to_sim()
+            # perform step
+            self.sim.step()
+            # read data from sim
+            self.scene.update(self.sim.get_physics_dt())
+
+            # skip first step where initial velocity is zero
+            if idx < 1:
+                continue
+
+            # check the accelerations
+            torch.testing.assert_close(
+                self.scene.sensors["imu_robot_base"].data.lin_acc_b,
+                self.scene.sensors["imu_robot_imu_link"].data.lin_acc_b,
+                rtol=1e-4,
+                atol=1e-4,
+            )
+            torch.testing.assert_close(
+                self.scene.sensors["imu_robot_base"].data.ang_acc_b,
+                self.scene.sensors["imu_robot_imu_link"].data.ang_acc_b,
+                rtol=1e-4,
+                atol=1e-4,
+            )
+
+            # check the velocities
+            torch.testing.assert_close(
+                self.scene.sensors["imu_robot_base"].data.ang_vel_b,
+                self.scene.sensors["imu_robot_imu_link"].data.ang_vel_b,
+                rtol=1e-4,
+                atol=1e-4,
+            )
+            torch.testing.assert_close(
+                self.scene.sensors["imu_robot_base"].data.lin_vel_b,
+                self.scene.sensors["imu_robot_imu_link"].data.lin_vel_b,
+                rtol=1e-4,
+                atol=1e-4,
+            )
+
+            # check the orientation
+            torch.testing.assert_close(
+                self.scene.sensors["imu_robot_base"].data.quat_w,
+                self.scene.sensors["imu_robot_imu_link"].data.quat_w,
+                rtol=1e-4,
+                atol=1e-4,
+            )
+            # check the position
+            torch.testing.assert_close(
+                self.scene.sensors["imu_robot_base"].data.pos_w,
+                self.scene.sensors["imu_robot_imu_link"].data.pos_w,
+                rtol=1e-4,
+                atol=1e-4,
+            )
+
+
+def test_env_ids_propogation(self):
+    """Test that env_ids argument propagates through update and reset methods"""
+    self.scene.reset()
+
+    for idx in range(10):
+        # set acceleration
+        self.scene.articulations["robot"].write_root_velocity_to_sim(
+            torch.tensor([[0.5, 0.0, 0.0, 0.0, 0.0, 0.0]], dtype=torch.float32, device=self.scene.device).repeat(
+                self.scene.num_envs, 1
+            )
+            * (idx + 1)
+        )
+        # write data to sim
+        self.scene.write_data_to_sim()
+        # perform step
+        self.sim.step()
+        # read data from sim
+        self.scene.update(self.sim.get_physics_dt())
+
+    # reset scene for env 1
+    self.scene.reset(env_ids=[1])
+    # read data from sim
+    self.scene.update(self.sim.get_physics_dt())
+    # perform step
+    self.sim.step()
+    # read data from sim
+    self.scene.update(self.sim.get_physics_dt())
+
+
+if __name__ == "__main__":
+    run_tests()

source/extensions/omni.isaac.lab/test/sensors/urdfs/simple_2_link.urdf

+<?xml version="1.0"?>
+<robot name="simple_2_link">
+    <link name="world">
+        <inertial>
+            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+            <mass value="0.001"/>
+            <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001"/>
+        </inertial>
+    </link>
+    <link name="base">
+        <inertial>
+            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+            <mass value="10"/>
+            <inertia ixx="0.004" ixy="0.0" ixz="0.0" iyy="0.004" iyz="0.000" izz="0.002"/>
+         </inertial>
+        <visual>
+            <origin xyz="0.0 0.0 0.3" rpy="0.0 0.0 0.0"/>
+            <geometry>
+                <cylinder length="0.6" radius="0.015"/>
+            </geometry>
+            <material name="blue">
+                <color rgba="0.0 0.0 0.8 1.0"/>
+            </material>
+        </visual>
+        <!-- <collision>
+            <origin xyz="0.0 0.0 0.3" rpy="0.0 0.0 0.0"/>
+            <geometry>
+                <cylinder length="0.6" radius="0.015"/>
+            </geometry>
+         </collision> -->
+    </link>
+    <link name="link_1">
+        <inertial>
+            <origin xyz="0.2 0.0 0.1" rpy="0.0 0.0 0.0"/>
+            <mass value="10"/>
+            <inertia ixx="0.004" ixy="0.0" ixz="0.0" iyy="0.004" iyz="0.0" izz="0.002"/>
+         </inertial>
+        <visual>
+            <origin xyz="0.2 0.0 0.1" rpy="0.0 1.5708 0.0"/>
+            <geometry>
+                <cylinder length="0.4" radius="0.01"/>
+            </geometry>
+            <material name="red">
+                <color rgba="0.8 0.0 0.0 1.0"/>
+            </material>
+        </visual>
+        <!-- <collision>
+            <origin xyz="0.2 0.0 0.2" rpy="0.0 1.5708 0.0"/>
+            <geometry>
+                <cylinder length="0.4" radius="0.1"/>
+            </geometry>
+         </collision> -->
+    </link>
+
+    <link name="imu_link">
+        <inertial>
+            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+            <mass value="0.01"/>
+            <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
+        </inertial>
+    </link>
+
+    <joint name="world" type="fixed">
+        <parent link="world"/>
+        <child link="base"/>
+        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+    </joint>
+
+    <joint name="joint_1" type="revolute">
+        <parent link="base"/>
+        <child link="link_1"/>
+        <origin xyz="0.0 0.0 0.6" rpy="-1.57079632679 0 0"/>
+        <axis xyz="0.0 0.0 1.0"/>
+        <limit effort="1000.0" velocity="10.0" lower="-10.0" upper="10.0"/>
+    </joint>
+
+    <joint name="imu_joint" type="fixed">
+        <parent link="link_1"/>
+        <child link="imu_link"/>
+        <origin xyz="0.4 0.0 0.1" rpy="1.57079632679 0 1.57087963"/>
+    </joint>
+</robot>

source/extensions/omni.isaac.lab/test/utils/test_math.py

@@ -195,6 +195,43 @@ class TestMathUtilities(unittest.TestCase):
         torch.testing.assert_close(error_3, error_4)
         torch.testing.assert_close(error_4, error_1)
 
+    def test_convention_converter(self):
+        """Test convert_camera_frame_orientation_convention to and from ros, opengl, and world conventions."""
+        quat_ros = torch.tensor([[-0.17591989, 0.33985114, 0.82047325, -0.42470819]])
+        quat_opengl = torch.tensor([[0.33985113, 0.17591988, 0.42470818, 0.82047324]])
+        quat_world = torch.tensor([[-0.3647052, -0.27984815, -0.1159169, 0.88047623]])
+
+        # from ROS
+        torch.testing.assert_close(
+            math_utils.convert_camera_frame_orientation_convention(quat_ros, "ros", "opengl"), quat_opengl
+        )
+        torch.testing.assert_close(
+            math_utils.convert_camera_frame_orientation_convention(quat_ros, "ros", "world"), quat_world
+        )
+        torch.testing.assert_close(
+            math_utils.convert_camera_frame_orientation_convention(quat_ros, "ros", "ros"), quat_ros
+        )
+        # from OpenGL
+        torch.testing.assert_close(
+            math_utils.convert_camera_frame_orientation_convention(quat_opengl, "opengl", "ros"), quat_ros
+        )
+        torch.testing.assert_close(
+            math_utils.convert_camera_frame_orientation_convention(quat_opengl, "opengl", "world"), quat_world
+        )
+        torch.testing.assert_close(
+            math_utils.convert_camera_frame_orientation_convention(quat_opengl, "opengl", "opengl"), quat_opengl
+        )
+        # from World
+        torch.testing.assert_close(
+            math_utils.convert_camera_frame_orientation_convention(quat_world, "world", "ros"), quat_ros
+        )
+        torch.testing.assert_close(
+            math_utils.convert_camera_frame_orientation_convention(quat_world, "world", "opengl"), quat_opengl
+        )
+        torch.testing.assert_close(
+            math_utils.convert_camera_frame_orientation_convention(quat_world, "world", "world"), quat_world
+        )
+
     def test_wrap_to_pi(self):
         """Test wrap_to_pi method."""
         # Define test cases