Changes the quat_box_minus implementation and adds quat_box_plus and...

Changes the quat_box_minus implementation and adds quat_box_plus and rigid_body_twist_transform (#2217)

# Description

<!--
Thank you for your interest in sending a pull request. Please make sure
to check the contribution guidelines.

Link:
https://isaac-sim.github.io/IsaacLab/main/source/refs/contributing.html
-->
- Changes the quat_box_minus implementation see changes for reference
links
- Adds quat_box_plus and rigid_body_twist_transform methods

# Reason for Change

While using `quat_box_minus` to get angular velocities through finite
differences we encountered that the angular velocity contained some
unreasonably large values at given points along the trajectory. After
some investigation we observed:

- The problem comes from `quat_box_minus` yielding unreasonably large
axis-angle differences at some points (”outliers”) of the trajectory.
- Those “outliers” appear when the real part of the difference
quaternion (`quat_diff = quat_mul(q1, quat_conjugate(q2))`) becomes
negative ($w < 0$).
- This happens even when the inputs are standardized with $w≥0$.

## Type of change

<!-- As you go through the list, delete the ones that are not
applicable. -->

- Bug fix (non-breaking change which fixes an issue)
- New feature (non-breaking change which adds functionality)

<!--
Example:

| Before | After |
| ------ | ----- |
| _gif/png before_ | _gif/png after_ |

To upload images to a PR -- simply drag and drop an image while in edit
mode and it should upload the image directly. You can then paste that
source into the above before/after sections.
-->

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

<!--
As you go through the checklist above, you can mark something as done by
putting an x character in it

For example,
- [x] I have done this task
- [ ] I have not done this task
-->

credit to @alopez-bdai

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

source/isaaclab/docs/CHANGELOG.rst

 Changelog
 ---------
 
+0.39.3 (2025-05-16)
+~~~~~~~~~~~~~~~~~~~
+
+Changed
+^^^^^^^
+
+* Changed the implementation of :meth:`~isaaclab.utils.math.quat_box_minus`
+
+Added
+^^^^^
+
+* Added :meth:`~isaaclab.utils.math.quat_box_plus`
+* Added :meth:`~isaaclab.utils.math.rigid_body_twist_transform`
+
+
 0.39.2 (2025-05-15)
 ~~~~~~~~~~~~~~~~~~~
 

source/isaaclab/test/utils/test_math.py

@@ -529,6 +529,94 @@ def test_interpolate_poses(device):
         np.testing.assert_array_almost_equal(result_pos, expected_pos, decimal=DECIMAL_PRECISION)
 
 
+@pytest.mark.parametrize("device", ["cpu", "cuda:0"])
+def test_quat_box_minus(device):
+    """Test quat_box_minus method.
+
+    Ensures that quat_box_minus correctly computes the axis-angle difference
+    between two quaternions representing rotations around the same axis.
+    """
+    axis_angles = torch.tensor([0.0, 0.0, 1.0], device=device)
+    angle_a = math.pi - 0.1
+    angle_b = -math.pi + 0.1
+    quat_a = math_utils.quat_from_angle_axis(torch.tensor([angle_a], device=device), axis_angles)
+    quat_b = math_utils.quat_from_angle_axis(torch.tensor([angle_b], device=device), axis_angles)
+
+    axis_diff = math_utils.quat_box_minus(quat_a, quat_b).squeeze(0)
+    expected_diff = axis_angles * math_utils.wrap_to_pi(torch.tensor(angle_a - angle_b, device=device))
+    torch.testing.assert_close(expected_diff, axis_diff, atol=1e-06, rtol=1e-06)
+
+
+@pytest.mark.parametrize("device", ["cpu", "cuda:0"])
+def test_quat_box_minus_and_quat_box_plus(device):
+    """Test consistency of quat_box_plus and quat_box_minus.
+
+    Checks that applying quat_box_plus to accumulate rotations and then using
+    quat_box_minus to retrieve differences results in expected values.
+    """
+
+    # Perform closed-loop integration using quat_box_plus to accumulate rotations,
+    # and then use quat_box_minus to compute the incremental differences between quaternions.
+    # NOTE: Accuracy may decrease for very small angle increments due to numerical precision limits.
+    for n in (2, 10, 100, 1000):
+        # Define small incremental rotations around principal axes
+        delta_angle = torch.tensor(
+            [
+                [0, 0, -math.pi / n],
+                [0, -math.pi / n, 0],
+                [-math.pi / n, 0, 0],
+                [0, 0, math.pi / n],
+                [0, math.pi / n, 0],
+                [math.pi / n, 0, 0],
+            ],
+            device=device,
+        )
+
+        # Initialize quaternion trajectory starting from identity quaternion
+        quat_trajectory = torch.zeros((len(delta_angle), 2 * n + 1, 4), device=device)
+        quat_trajectory[:, 0, :] = torch.tensor([[1.0, 0.0, 0.0, 0.0]], device=device).repeat(len(delta_angle), 1)
+
+        # Integrate incremental rotations forward to form a closed loop trajectory
+        for i in range(1, 2 * n + 1):
+            quat_trajectory[:, i] = math_utils.quat_box_plus(quat_trajectory[:, i - 1], delta_angle)
+
+        # Validate the loop closure: start and end quaternions should be approximately equal
+        torch.testing.assert_close(quat_trajectory[:, 0], quat_trajectory[:, -1], atol=1e-04, rtol=1e-04)
+
+        # Validate that the differences between consecutive quaternions match the original increments
+        for i in range(2 * n):
+            delta_result = math_utils.quat_box_minus(quat_trajectory[:, i + 1], quat_trajectory[:, i])
+            torch.testing.assert_close(delta_result, delta_angle, atol=1e-04, rtol=1e-04)
+
+
+@pytest.mark.parametrize("device", ["cpu", "cuda:0"])
+def test_rigid_body_twist_transform(device):
+    """Test rigid_body_twist_transform method.
+
+    Verifies correct transformation of twists (linear and angular velocity) between coordinate frames.
+    """
+    num_bodies = 100
+    # Frame A to B
+    t_AB = torch.randn((num_bodies, 3), device=device)
+    q_AB = math_utils.random_orientation(num=num_bodies, device=device)
+
+    # Twists in A in frame A
+    v_AA = torch.randn((num_bodies, 3), device=device)
+    w_AA = torch.randn((num_bodies, 3), device=device)
+
+    # Get twists in B in frame B
+    v_BB, w_BB = math_utils.rigid_body_twist_transform(v_AA, w_AA, t_AB, q_AB)
+
+    # Get back twists in A in frame A
+    t_BA = -math_utils.quat_rotate_inverse(q_AB, t_AB)
+    q_BA = math_utils.quat_conjugate(q_AB)
+    v_AA_, w_AA_ = math_utils.rigid_body_twist_transform(v_BB, w_BB, t_BA, q_BA)
+
+    # Check
+    torch.testing.assert_close(v_AA_, v_AA)
+    torch.testing.assert_close(w_AA_, w_AA)
+
+
 @pytest.mark.parametrize("device", ["cpu", "cuda:0"])
 def test_yaw_quat(device):
     """