wip

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

@@ -17,6 +17,7 @@
 import omni.kit.commands
 import omni.usd
 from omni.isaac.core.prims import XFormPrimView
+import omni.isaac.core.utils.stage as stage_utils
 from pxr import UsdGeom
 
 import omni.isaac.lab.sim as sim_utils
@@ -328,8 +329,10 @@ def set_world_poses_from_view(
         # 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)
 
     """

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

@@ -7,10 +7,6 @@
 IMU Sensor
 """
 
-from __future__ import annotations
-
 from .imu import IMU
 from .imu_cfg import IMUCfg
 from .imu_data import IMUData
-
-__all__ = ["IMU", "IMUCfg", "IMUData"]

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

@@ -9,6 +9,7 @@
 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
 
@@ -132,7 +133,6 @@ def _update_buffers_impl(self, env_ids: Sequence[int]):
         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
-        # note: we clone here because the obtained tensors are read-only
         self._data.pos_w[env_ids] = pos_w + math_utils.quat_rotate(quat_w, self._offset_pos)
         self._data.quat_w[env_ids] = math_utils.quat_mul(quat_w, self._offset_quat)
 
@@ -187,11 +187,14 @@ def _debug_vis_callback(self, event):
         # -- 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,
             )
         )

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

@@ -30,6 +30,7 @@ class OffsetCfg:
         """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.

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

@@ -12,6 +12,7 @@
 
 import omni.physics.tensors.impl.api as physx
 from omni.isaac.core.prims import XFormPrimView
+import omni.isaac.core.utils.stage as stage_utils
 
 import omni.isaac.lab.utils.math as math_utils
 from omni.isaac.lab.sensors.camera import CameraData
@@ -217,9 +218,11 @@ def set_world_poses_from_view(
             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(
-            math_utils.create_rotation_matrix_from_view(eyes, targets, device=self._device)
+            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")
 

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

@@ -14,9 +14,6 @@
 import torch.nn.functional
 from typing import Literal
 
-import omni.isaac.core.utils.stage as stage_utils
-from pxr import UsdGeom
-
 """
 General
 """
@@ -1390,9 +1387,11 @@ def convert_orientation_convention(
 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.
@@ -1407,6 +1406,7 @@ def create_rotation_matrix_from_view(
     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".
 
     The vectors are broadcast against each other so they all have shape (N, 3).
 
@@ -1416,17 +1416,16 @@ def create_rotation_matrix_from_view(
     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)
+    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_token}")
+        raise ValueError(f"Invalid up axis: {up_axis}")
 
     # 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, z_axis, dim=1), 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():