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_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/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