Add IMU projected_gravity_b and IMU computation speed optimizations

scripts/tutorials/05_controllers/run_osc.py

@@ -49,8 +49,8 @@
 from isaaclab.utils.math import (
     combine_frame_transforms,
     matrix_from_quat,
+    quat_apply_inverse,
     quat_inv,
-    quat_rotate_inverse,
     subtract_frame_transforms,
 )
 
@@ -336,8 +336,8 @@ def update_states(
     ee_vel_w = robot.data.body_vel_w[:, ee_frame_idx, :]  # Extract end-effector velocity in the world frame
     root_vel_w = robot.data.root_vel_w  # Extract root velocity in the world frame
     relative_vel_w = ee_vel_w - root_vel_w  # Compute the relative velocity in the world frame
-    ee_lin_vel_b = quat_rotate_inverse(robot.data.root_quat_w, relative_vel_w[:, 0:3])  # From world to root frame
-    ee_ang_vel_b = quat_rotate_inverse(robot.data.root_quat_w, relative_vel_w[:, 3:6])
+    ee_lin_vel_b = quat_apply_inverse(robot.data.root_quat_w, relative_vel_w[:, 0:3])  # From world to root frame
+    ee_ang_vel_b = quat_apply_inverse(robot.data.root_quat_w, relative_vel_w[:, 3:6])
     ee_vel_b = torch.cat([ee_lin_vel_b, ee_ang_vel_b], dim=-1)
 
     # Calculate the contact force

source/isaaclab/config/extension.toml

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

source/isaaclab/docs/CHANGELOG.rst

@@ -1,6 +1,22 @@
 Changelog
 ---------
 
+0.40.0 (2025-05-16)
+~~~~~~~~~~~~~~~~~~~
+
+Changed
+^^^^^^^
+
+* Changed all calls to :meth:`~isaaclab.utils.math.quat_rotate` and :meth:`~isaaclab.utils.math.quat_rotate_inverse` to
+  :meth:`~isaaclab.utils.math.quat_apply` and :meth:`~isaaclab.utils.math.quat_apply_inverse` for speed.
+
+Added
+^^^^^
+
+* Added deprecation warning for :meth:`~isaaclab.utils.math.quat_rotate` and
+  :meth:`~isaaclab.utils.math.quat_rotate_inverse`
+
+
 0.39.2 (2025-05-15)
 ~~~~~~~~~~~~~~~~~~~
 

source/isaaclab/isaaclab/assets/articulation/articulation.py

@@ -391,7 +391,7 @@ def write_root_com_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[
         root_link_pos, root_link_quat = math_utils.combine_frame_transforms(
             root_pose[..., :3],
             root_pose[..., 3:7],
-            math_utils.quat_rotate(math_utils.quat_inv(com_quat), -com_pos),
+            math_utils.quat_apply(math_utils.quat_inv(com_quat), -com_pos),
             math_utils.quat_inv(com_quat),
         )
 
@@ -465,7 +465,7 @@ def write_root_link_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids:
         com_pos_b = self.data.com_pos_b[env_ids, 0, :]
         # transform given velocity to center of mass
         root_com_velocity[:, :3] += torch.linalg.cross(
-            root_com_velocity[:, 3:], math_utils.quat_rotate(quat, com_pos_b), dim=-1
+            root_com_velocity[:, 3:], math_utils.quat_apply(quat, com_pos_b), dim=-1
         )
         # write center of mass velocity to sim
         self.write_root_com_velocity_to_sim(root_velocity=root_com_velocity, env_ids=physx_env_ids)

source/isaaclab/isaaclab/assets/articulation/articulation_data.py

@@ -395,7 +395,7 @@ def root_link_state_w(self):
 
             # adjust linear velocity to link from center of mass
             velocity[:, :3] += torch.linalg.cross(
-                velocity[:, 3:], math_utils.quat_rotate(pose[:, 3:7], -self.com_pos_b[:, 0, :]), dim=-1
+                velocity[:, 3:], math_utils.quat_apply(pose[:, 3:7], -self.com_pos_b[:, 0, :]), dim=-1
             )
             # set the buffer data and timestamp
             self._root_link_state_w.data = torch.cat((pose, velocity), dim=-1)
@@ -463,7 +463,7 @@ def body_link_state_w(self):
 
             # adjust linear velocity to link from center of mass
             velocity[..., :3] += torch.linalg.cross(
-                velocity[..., 3:], math_utils.quat_rotate(pose[..., 3:7], -self.com_pos_b), dim=-1
+                velocity[..., 3:], math_utils.quat_apply(pose[..., 3:7], -self.com_pos_b), dim=-1
             )
             # set the buffer data and timestamp
             self._body_link_state_w.data = torch.cat((pose, velocity), dim=-1)
@@ -529,7 +529,7 @@ def body_incoming_joint_wrench_b(self) -> torch.Tensor:
     @property
     def projected_gravity_b(self):
         """Projection of the gravity direction on base frame. Shape is (num_instances, 3)."""
-        return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.GRAVITY_VEC_W)
+        return math_utils.quat_apply_inverse(self.root_link_quat_w, self.GRAVITY_VEC_W)
 
     @property
     def heading_w(self):
@@ -624,7 +624,7 @@ def root_lin_vel_b(self) -> torch.Tensor:
         This quantity is the linear velocity of the articulation root's center of mass frame relative to the world
         with respect to the articulation root's actor frame.
         """
-        return math_utils.quat_rotate_inverse(self.root_quat_w, self.root_lin_vel_w)
+        return math_utils.quat_apply_inverse(self.root_quat_w, self.root_lin_vel_w)
 
     @property
     def root_ang_vel_b(self) -> torch.Tensor:
@@ -633,7 +633,7 @@ def root_ang_vel_b(self) -> torch.Tensor:
         This quantity is the angular velocity of the articulation root's center of mass frame relative to the world with
         respect to the articulation root's actor frame.
         """
-        return math_utils.quat_rotate_inverse(self.root_quat_w, self.root_ang_vel_w)
+        return math_utils.quat_apply_inverse(self.root_quat_w, self.root_ang_vel_w)
 
     ##
     # Derived Root Link Frame Properties
@@ -696,7 +696,7 @@ def root_link_lin_vel_b(self) -> torch.Tensor:
         This quantity is the linear velocity of the actor frame of the root rigid body frame with respect to the
         rigid body's actor frame.
         """
-        return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.root_link_lin_vel_w)
+        return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_link_lin_vel_w)
 
     @property
     def root_link_ang_vel_b(self) -> torch.Tensor:
@@ -705,7 +705,7 @@ def root_link_ang_vel_b(self) -> torch.Tensor:
         This quantity is the angular velocity of the actor frame of the root rigid body frame with respect to the
         rigid body's actor frame.
         """
-        return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.root_link_ang_vel_w)
+        return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_link_ang_vel_w)
 
     ##
     # Root Center of Mass state properties
@@ -771,7 +771,7 @@ def root_com_lin_vel_b(self) -> torch.Tensor:
         This quantity is the linear velocity of the root rigid body's center of mass frame with respect to the
         rigid body's actor frame.
         """
-        return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.root_com_lin_vel_w)
+        return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_com_lin_vel_w)
 
     @property
     def root_com_ang_vel_b(self) -> torch.Tensor:
@@ -780,7 +780,7 @@ def root_com_ang_vel_b(self) -> torch.Tensor:
         This quantity is the angular velocity of the root rigid body's center of mass frame with respect to the
         rigid body's actor frame.
         """
-        return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.root_com_ang_vel_w)
+        return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_com_ang_vel_w)
 
     @property
     def body_pos_w(self) -> torch.Tensor:

source/isaaclab/isaaclab/assets/rigid_object/rigid_object.py

@@ -259,7 +259,7 @@ def write_root_com_pose_to_sim(self, root_pose: torch.Tensor, env_ids: Sequence[
         root_link_pos, root_link_quat = math_utils.combine_frame_transforms(
             root_pose[..., :3],
             root_pose[..., 3:7],
-            math_utils.quat_rotate(math_utils.quat_inv(com_quat), -com_pos),
+            math_utils.quat_apply(math_utils.quat_inv(com_quat), -com_pos),
             math_utils.quat_inv(com_quat),
         )
 
@@ -333,7 +333,7 @@ def write_root_link_velocity_to_sim(self, root_velocity: torch.Tensor, env_ids:
         com_pos_b = self.data.com_pos_b[local_env_ids, 0, :]
         # transform given velocity to center of mass
         root_com_velocity[:, :3] += torch.linalg.cross(
-            root_com_velocity[:, 3:], math_utils.quat_rotate(quat, com_pos_b), dim=-1
+            root_com_velocity[:, 3:], math_utils.quat_apply(quat, com_pos_b), dim=-1
         )
         # write center of mass velocity to sim
         self.write_root_com_velocity_to_sim(root_velocity=root_com_velocity, env_ids=env_ids)

source/isaaclab/isaaclab/assets/rigid_object/rigid_object_data.py

@@ -142,7 +142,7 @@ def root_link_state_w(self):
 
             # adjust linear velocity to link from center of mass
             velocity[:, :3] += torch.linalg.cross(
-                velocity[:, 3:], math_utils.quat_rotate(pose[:, 3:7], -self.com_pos_b[:, 0, :]), dim=-1
+                velocity[:, 3:], math_utils.quat_apply(pose[:, 3:7], -self.com_pos_b[:, 0, :]), dim=-1
             )
             # set the buffer data and timestamp
             self._root_link_state_w.data = torch.cat((pose, velocity), dim=-1)
@@ -218,7 +218,7 @@ def body_acc_w(self):
     @property
     def projected_gravity_b(self):
         """Projection of the gravity direction on base frame. Shape is (num_instances, 3)."""
-        return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.GRAVITY_VEC_W)
+        return math_utils.quat_apply_inverse(self.root_link_quat_w, self.GRAVITY_VEC_W)
 
     @property
     def heading_w(self):
@@ -282,7 +282,7 @@ def root_lin_vel_b(self) -> torch.Tensor:
         This quantity is the linear velocity of the root rigid body's center of mass frame with respect to the
         rigid body's actor frame.
         """
-        return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.root_lin_vel_w)
+        return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_lin_vel_w)
 
     @property
     def root_ang_vel_b(self) -> torch.Tensor:
@@ -291,7 +291,7 @@ def root_ang_vel_b(self) -> torch.Tensor:
         This quantity is the angular velocity of the root rigid body's center of mass frame with respect to the
         rigid body's actor frame.
         """
-        return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.root_ang_vel_w)
+        return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_ang_vel_w)
 
     @property
     def root_link_pos_w(self) -> torch.Tensor:
@@ -350,7 +350,7 @@ def root_link_lin_vel_b(self) -> torch.Tensor:
         This quantity is the linear velocity of the actor frame of the root rigid body frame with respect to the
         rigid body's actor frame.
         """
-        return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.root_link_lin_vel_w)
+        return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_link_lin_vel_w)
 
     @property
     def root_link_ang_vel_b(self) -> torch.Tensor:
@@ -359,7 +359,7 @@ def root_link_ang_vel_b(self) -> torch.Tensor:
         This quantity is the angular velocity of the actor frame of the root rigid body frame with respect to the
         rigid body's actor frame.
         """
-        return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.root_link_ang_vel_w)
+        return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_link_ang_vel_w)
 
     @property
     def root_com_pos_w(self) -> torch.Tensor:
@@ -420,7 +420,7 @@ def root_com_lin_vel_b(self) -> torch.Tensor:
         This quantity is the linear velocity of the root rigid body's center of mass frame with respect to the
         rigid body's actor frame.
         """
-        return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.root_com_lin_vel_w)
+        return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_com_lin_vel_w)
 
     @property
     def root_com_ang_vel_b(self) -> torch.Tensor:
@@ -429,7 +429,7 @@ def root_com_ang_vel_b(self) -> torch.Tensor:
         This quantity is the angular velocity of the root rigid body's center of mass frame with respect to the
         rigid body's actor frame.
         """
-        return math_utils.quat_rotate_inverse(self.root_link_quat_w, self.root_com_ang_vel_w)
+        return math_utils.quat_apply_inverse(self.root_link_quat_w, self.root_com_ang_vel_w)
 
     @property
     def body_pos_w(self) -> torch.Tensor:

source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection.py

@@ -366,7 +366,7 @@ def write_object_com_pose_to_sim(
         object_link_pos, object_link_quat = math_utils.combine_frame_transforms(
             object_pose[..., :3],
             object_pose[..., 3:7],
-            math_utils.quat_rotate(math_utils.quat_inv(com_quat), -com_pos),
+            math_utils.quat_apply(math_utils.quat_inv(com_quat), -com_pos),
             math_utils.quat_inv(com_quat),
         )
 
@@ -463,7 +463,7 @@ def write_object_link_velocity_to_sim(
         com_pos_b = self.data.com_pos_b[local_env_ids][:, local_object_ids, :]
         # transform given velocity to center of mass
         object_com_velocity[..., :3] += torch.linalg.cross(
-            object_com_velocity[..., 3:], math_utils.quat_rotate(quat, com_pos_b), dim=-1
+            object_com_velocity[..., 3:], math_utils.quat_apply(quat, com_pos_b), dim=-1
         )
         # write center of mass velocity to sim
         self.write_object_com_velocity_to_sim(

source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection_data.py

@@ -150,7 +150,7 @@ def object_link_state_w(self):
 
             # adjust linear velocity to link from center of mass
             velocity[..., :3] += torch.linalg.cross(
-                velocity[..., 3:], math_utils.quat_rotate(pose[..., 3:7], -self.com_pos_b[..., :]), dim=-1
+                velocity[..., 3:], math_utils.quat_apply(pose[..., 3:7], -self.com_pos_b[..., :]), dim=-1
             )
 
             # set the buffer data and timestamp
@@ -198,7 +198,7 @@ def object_acc_w(self):
     @property
     def projected_gravity_b(self):
         """Projection of the gravity direction on base frame. Shape is (num_instances, num_objects, 3)."""
-        return math_utils.quat_rotate_inverse(self.object_link_quat_w, self.GRAVITY_VEC_W)
+        return math_utils.quat_apply_inverse(self.object_link_quat_w, self.GRAVITY_VEC_W)
 
     @property
     def heading_w(self):
@@ -262,7 +262,7 @@ def object_lin_vel_b(self) -> torch.Tensor:
         This quantity is the linear velocity of the rigid bodies' center of mass frame with respect to the
         rigid body's actor frame.
         """
-        return math_utils.quat_rotate_inverse(self.object_quat_w, self.object_lin_vel_w)
+        return math_utils.quat_apply_inverse(self.object_quat_w, self.object_lin_vel_w)
 
     @property
     def object_ang_vel_b(self) -> torch.Tensor:
@@ -271,7 +271,7 @@ def object_ang_vel_b(self) -> torch.Tensor:
         This quantity is the angular velocity of the rigid bodies' center of mass frame with respect to the
         rigid body's actor frame.
         """
-        return math_utils.quat_rotate_inverse(self.object_quat_w, self.object_ang_vel_w)
+        return math_utils.quat_apply_inverse(self.object_quat_w, self.object_ang_vel_w)
 
     @property
     def object_lin_acc_w(self) -> torch.Tensor:
@@ -345,7 +345,7 @@ def object_link_lin_vel_b(self) -> torch.Tensor:
         This quantity is the linear velocity of the actor frame of the root rigid body frame with respect to the
         rigid body's actor frame.
         """
-        return math_utils.quat_rotate_inverse(self.object_link_quat_w, self.object_link_lin_vel_w)
+        return math_utils.quat_apply_inverse(self.object_link_quat_w, self.object_link_lin_vel_w)
 
     @property
     def object_link_ang_vel_b(self) -> torch.Tensor:
@@ -354,7 +354,7 @@ def object_link_ang_vel_b(self) -> torch.Tensor:
         This quantity is the angular velocity of the actor frame of the root rigid body frame with respect to the
         rigid body's actor frame.
         """
-        return math_utils.quat_rotate_inverse(self.object_link_quat_w, self.object_link_ang_vel_w)
+        return math_utils.quat_apply_inverse(self.object_link_quat_w, self.object_link_ang_vel_w)
 
     @property
     def object_com_pos_w(self) -> torch.Tensor:
@@ -415,7 +415,7 @@ def object_com_lin_vel_b(self) -> torch.Tensor:
         This quantity is the linear velocity of the center of mass frame of the root rigid body frame with respect to the
         rigid body's actor frame.
         """
-        return math_utils.quat_rotate_inverse(self.object_link_quat_w, self.object_com_lin_vel_w)
+        return math_utils.quat_apply_inverse(self.object_link_quat_w, self.object_com_lin_vel_w)
 
     @property
     def object_com_ang_vel_b(self) -> torch.Tensor:
@@ -424,7 +424,7 @@ def object_com_ang_vel_b(self) -> torch.Tensor:
         This quantity is the angular velocity of the center of mass frame of the root rigid body frame with respect to the
         rigid body's actor frame.
         """
-        return math_utils.quat_rotate_inverse(self.object_link_quat_w, self.object_com_ang_vel_w)
+        return math_utils.quat_apply_inverse(self.object_link_quat_w, self.object_com_ang_vel_w)
 
     @property
     def com_pos_b(self) -> torch.Tensor:

source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py

@@ -622,13 +622,13 @@ def _compute_ee_velocity(self):
         relative_vel_w = self._ee_vel_w - self._asset.data.root_vel_w
 
         # Convert ee velocities from world to root frame
-        self._ee_vel_b[:, 0:3] = math_utils.quat_rotate_inverse(self._asset.data.root_quat_w, relative_vel_w[:, 0:3])
-        self._ee_vel_b[:, 3:6] = math_utils.quat_rotate_inverse(self._asset.data.root_quat_w, relative_vel_w[:, 3:6])
+        self._ee_vel_b[:, 0:3] = math_utils.quat_apply_inverse(self._asset.data.root_quat_w, relative_vel_w[:, 0:3])
+        self._ee_vel_b[:, 3:6] = math_utils.quat_apply_inverse(self._asset.data.root_quat_w, relative_vel_w[:, 3:6])
 
         # Account for the offset
         if self.cfg.body_offset is not None:
             # Compute offset vector in root frame
-            r_offset_b = math_utils.quat_rotate(self._ee_pose_b_no_offset[:, 3:7], self._offset_pos)
+            r_offset_b = math_utils.quat_apply(self._ee_pose_b_no_offset[:, 3:7], self._offset_pos)
             # Adjust the linear velocity to account for the offset
             self._ee_vel_b[:, :3] += torch.cross(self._ee_vel_b[:, 3:], r_offset_b, dim=-1)
             # Angular velocity is not affected by the offset
@@ -640,7 +640,7 @@ def _compute_ee_force(self):
             self._contact_sensor.update(self._sim_dt)
             self._ee_force_w[:] = self._contact_sensor.data.net_forces_w[:, 0, :]  # type: ignore
             # Rotate forces and torques into root frame
-            self._ee_force_b[:] = math_utils.quat_rotate_inverse(self._asset.data.root_quat_w, self._ee_force_w)
+            self._ee_force_b[:] = math_utils.quat_apply_inverse(self._asset.data.root_quat_w, self._ee_force_w)
 
     def _compute_joint_states(self):
         """Computes the joint states for operational space control."""

source/isaaclab/isaaclab/envs/mdp/commands/pose_2d_command.py

@@ -15,7 +15,7 @@
 from isaaclab.managers import CommandTerm
 from isaaclab.markers import VisualizationMarkers
 from isaaclab.terrains import TerrainImporter
-from isaaclab.utils.math import quat_from_euler_xyz, quat_rotate_inverse, wrap_to_pi, yaw_quat
+from isaaclab.utils.math import quat_apply_inverse, quat_from_euler_xyz, wrap_to_pi, yaw_quat
 
 if TYPE_CHECKING:
     from isaaclab.envs import ManagerBasedEnv
@@ -117,7 +117,7 @@ def _resample_command(self, env_ids: Sequence[int]):
     def _update_command(self):
         """Re-target the position command to the current root state."""
         target_vec = self.pos_command_w - self.robot.data.root_pos_w[:, :3]
-        self.pos_command_b[:] = quat_rotate_inverse(yaw_quat(self.robot.data.root_quat_w), target_vec)
+        self.pos_command_b[:] = quat_apply_inverse(yaw_quat(self.robot.data.root_quat_w), target_vec)
         self.heading_command_b[:] = wrap_to_pi(self.heading_command_w - self.robot.data.heading_w)
 
     def _set_debug_vis_impl(self, debug_vis: bool):