Changes to quat_apply and quat_apply_inverse for speed (#2129)

# Description

As per findings in #1711, `quat_apply` was found to be faster that
`quat_rotate`. This PR:
- adds `quat_apply_inverse`
- changes all instances of `quat_rotate` and `quat_rotate_inverse` to
their apply counterparts.

Fixes #1711

## Type of change

- Bug fix (non-breaking change which fixes an issue)

## Screenshots

| Per 1000 | cpu | cuda |
|:----------|:-------:|:---------:|
|**quat_apply:** |			**217.91 us** |	**47.07 us**|
|einsum_quat_rotate: |		295.95 us |	127.62 us|
|iter_quat_apply: |		679.10 us |	850.25 us|
|iter_bmm_quat_rotate: |		829.62 us |	1.28 ms|
|iter_einsum_quat_rotate: |	937.73 us |	1.46 ms|
|**quat_apply_inverse:** |		**212.20 us** |	**48.43 us**|
|einsum_quat_rotate_inverse: |	278.43 us |	114.25 us|
|iter_quat_apply_inverse: |	681.85 us |	774.82 us|
|iter_bmm_quat_rotate_inverse: |	863.27 us |	1.23 ms|
|iter_einsum_quat_rotate_inverse: |	1.04 ms |	1.45 ms|

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

---------

Signed-off-by: James Tigue <166445701+jtigue-bdai@users.noreply.github.com>
Signed-off-by: Mayank Mittal <12863862+Mayankm96@users.noreply.github.com>
Signed-off-by: Kelly Guo <kellyg@nvidia.com>
Co-authored-by: Mayank Mittal <12863862+Mayankm96@users.noreply.github.com>
Co-authored-by: Kelly Guo <kellyg@nvidia.com>

Author: 3 people

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,8 @@
 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.39.7"
+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)
+~~~~~~~~~~~~~~~~~~~
+
+Added
+^^^^^
+
+* Added deprecation warning for :meth:`~isaaclab.utils.math.quat_rotate` and
+  :meth:`~isaaclab.utils.math.quat_rotate_inverse`
+
+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.
+
+
 0.39.7 (2025-05-19)
 ~~~~~~~~~~~~~~~~~~~
 

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

@@ -368,7 +368,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),
         )
 
@@ -465,7 +465,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."""