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/isaaclab/utils/math.py

@@ -140,6 +140,22 @@ Rotation
 """
 
 
+@torch.jit.script
+def quat_unique(q: torch.Tensor) -> torch.Tensor:
+    """Convert a unit quaternion to a standard form where the real part is non-negative.
+
+    Quaternion representations have a singularity since ``q`` and ``-q`` represent the same
+    rotation. This function ensures the real part of the quaternion is non-negative.
+
+    Args:
+        q: The quaternion orientation in (w, x, y, z). Shape is (..., 4).
+
+    Returns:
+        Standardized quaternions. Shape is (..., 4).
+    """
+    return torch.where(q[..., 0:1] < 0, -q, q)
+
+
 @torch.jit.script
 def matrix_from_quat(quaternions: torch.Tensor) -> torch.Tensor:
     """Convert rotations given as quaternions to rotation matrices.
@@ -445,19 +461,52 @@ def euler_xyz_from_quat(quat: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor,
 
 
 @torch.jit.script
-def quat_unique(q: torch.Tensor) -> torch.Tensor:
-    """Convert a unit quaternion to a standard form where the real part is non-negative.
+def axis_angle_from_quat(quat: torch.Tensor, eps: float = 1.0e-6) -> torch.Tensor:
+    """Convert rotations given as quaternions to axis/angle.
 
-    Quaternion representations have a singularity since ``q`` and ``-q`` represent the same
-    rotation. This function ensures the real part of the quaternion is non-negative.
+    Args:
+        quat: The quaternion orientation in (w, x, y, z). Shape is (..., 4).
+        eps: The tolerance for Taylor approximation. Defaults to 1.0e-6.
+
+    Returns:
+        Rotations given as a vector in axis angle form. Shape is (..., 3).
+        The vector's magnitude is the angle turned anti-clockwise in radians around the vector's direction.
+
+    Reference:
+        https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L526-L554
+    """
+    # Modified to take in quat as [q_w, q_x, q_y, q_z]
+    # Quaternion is [q_w, q_x, q_y, q_z] = [cos(theta/2), n_x * sin(theta/2), n_y * sin(theta/2), n_z * sin(theta/2)]
+    # Axis-angle is [a_x, a_y, a_z] = [theta * n_x, theta * n_y, theta * n_z]
+    # Thus, axis-angle is [q_x, q_y, q_z] / (sin(theta/2) / theta)
+    # When theta = 0, (sin(theta/2) / theta) is undefined
+    # However, as theta --> 0, we can use the Taylor approximation 1/2 - theta^2 / 48
+    quat = quat * (1.0 - 2.0 * (quat[..., 0:1] < 0.0))
+    mag = torch.linalg.norm(quat[..., 1:], dim=-1)
+    half_angle = torch.atan2(mag, quat[..., 0])
+    angle = 2.0 * half_angle
+    # check whether to apply Taylor approximation
+    sin_half_angles_over_angles = torch.where(
+        angle.abs() > eps, torch.sin(half_angle) / angle, 0.5 - angle * angle / 48
+    )
+    return quat[..., 1:4] / sin_half_angles_over_angles.unsqueeze(-1)
+
+
+@torch.jit.script
+def quat_from_angle_axis(angle: torch.Tensor, axis: torch.Tensor) -> torch.Tensor:
+    """Convert rotations given as angle-axis to quaternions.
 
     Args:
-        q: The quaternion orientation in (w, x, y, z). Shape is (..., 4).
+        angle: The angle turned anti-clockwise in radians around the vector's direction. Shape is (N,).
+        axis: The axis of rotation. Shape is (N, 3).
 
     Returns:
-        Standardized quaternions. Shape is (..., 4).
+        The quaternion in (w, x, y, z). Shape is (N, 4).
     """
-    return torch.where(q[..., 0:1] < 0, -q, q)
+    theta = (angle / 2).unsqueeze(-1)
+    xyz = normalize(axis) * theta.sin()
+    w = theta.cos()
+    return normalize(torch.cat([w, xyz], dim=-1))
 
 
 @torch.jit.script
@@ -499,25 +548,6 @@ def quat_mul(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor:
     return torch.stack([w, x, y, z], dim=-1).view(shape)
 
 
-@torch.jit.script
-def quat_box_minus(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor:
-    """The box-minus operator (quaternion difference) between two quaternions.
-
-    Args:
-        q1: The first quaternion in (w, x, y, z). Shape is (N, 4).
-        q2: The second quaternion in (w, x, y, z). Shape is (N, 4).
-
-    Returns:
-        The difference between the two quaternions. Shape is (N, 3).
-    """
-    quat_diff = quat_mul(q1, quat_conjugate(q2))  # q1 * q2^-1
-    re = quat_diff[:, 0]  # real part, q = [w, x, y, z] = [re, im]
-    im = quat_diff[:, 1:]  # imaginary part
-    norm_im = torch.norm(im, dim=1)
-    scale = 2.0 * torch.where(norm_im > 1.0e-7, torch.atan2(norm_im, re) / norm_im, torch.sign(re))
-    return scale.unsqueeze(-1) * im
-
-
 @torch.jit.script
 def yaw_quat(quat: torch.Tensor) -> torch.Tensor:
     """Extract the yaw component of a quaternion.
@@ -542,6 +572,45 @@ def yaw_quat(quat: torch.Tensor) -> torch.Tensor:
     return quat_yaw.view(shape)
 
 
+@torch.jit.script
+def quat_box_minus(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor:
+    """The box-minus operator (quaternion difference) between two quaternions.
+
+    Args:
+        q1: The first quaternion in (w, x, y, z). Shape is (N, 4).
+        q2: The second quaternion in (w, x, y, z). Shape is (N, 4).
+
+    Returns:
+        The difference between the two quaternions. Shape is (N, 3).
+
+    Reference:
+        https://github.com/ANYbotics/kindr/blob/master/doc/cheatsheet/cheatsheet_latest.pdf
+    """
+    quat_diff = quat_mul(q1, quat_conjugate(q2))  # q1 * q2^-1
+    return axis_angle_from_quat(quat_diff)  # log(qd)
+
+
+@torch.jit.script
+def quat_box_plus(q: torch.Tensor, delta: torch.Tensor, eps: float = 1.0e-6) -> torch.Tensor:
+    """The box-plus operator (quaternion update) to apply an increment to a quaternion.
+
+    Args:
+        q: The initial quaternion in (w, x, y, z). Shape is (N, 4).
+        delta: The axis-angle perturbation. Shape is (N, 3).
+            eps: A small value to avoid division by zero. Defaults to 1e-6.
+
+    Returns:
+        The updated quaternion after applying the perturbation. Shape is (N, 4).
+
+    Reference:
+        https://github.com/ANYbotics/kindr/blob/master/doc/cheatsheet/cheatsheet_latest.pdf
+    """
+    delta_norm = torch.clamp_min(torch.linalg.norm(delta, dim=-1, keepdim=True), min=eps)
+    delta_quat = quat_from_angle_axis(delta_norm.squeeze(-1), delta / delta_norm)  # exp(dq)
+    new_quat = quat_mul(delta_quat, q)  # Apply perturbation
+    return quat_unique(new_quat)
+
+
 @torch.jit.script
 def quat_apply(quat: torch.Tensor, vec: torch.Tensor) -> torch.Tensor:
     """Apply a quaternion rotation to a vector.
@@ -625,55 +694,6 @@ def quat_rotate_inverse(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor:
     return a - b + c
 
 
-@torch.jit.script
-def quat_from_angle_axis(angle: torch.Tensor, axis: torch.Tensor) -> torch.Tensor:
-    """Convert rotations given as angle-axis to quaternions.
-
-    Args:
-        angle: The angle turned anti-clockwise in radians around the vector's direction. Shape is (N,).
-        axis: The axis of rotation. Shape is (N, 3).
-
-    Returns:
-        The quaternion in (w, x, y, z). Shape is (N, 4).
-    """
-    theta = (angle / 2).unsqueeze(-1)
-    xyz = normalize(axis) * theta.sin()
-    w = theta.cos()
-    return normalize(torch.cat([w, xyz], dim=-1))
-
-
-@torch.jit.script
-def axis_angle_from_quat(quat: torch.Tensor, eps: float = 1.0e-6) -> torch.Tensor:
-    """Convert rotations given as quaternions to axis/angle.
-
-    Args:
-        quat: The quaternion orientation in (w, x, y, z). Shape is (..., 4).
-        eps: The tolerance for Taylor approximation. Defaults to 1.0e-6.
-
-    Returns:
-        Rotations given as a vector in axis angle form. Shape is (..., 3).
-        The vector's magnitude is the angle turned anti-clockwise in radians around the vector's direction.
-
-    Reference:
-        https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L526-L554
-    """
-    # Modified to take in quat as [q_w, q_x, q_y, q_z]
-    # Quaternion is [q_w, q_x, q_y, q_z] = [cos(theta/2), n_x * sin(theta/2), n_y * sin(theta/2), n_z * sin(theta/2)]
-    # Axis-angle is [a_x, a_y, a_z] = [theta * n_x, theta * n_y, theta * n_z]
-    # Thus, axis-angle is [q_x, q_y, q_z] / (sin(theta/2) / theta)
-    # When theta = 0, (sin(theta/2) / theta) is undefined
-    # However, as theta --> 0, we can use the Taylor approximation 1/2 - theta^2 / 48
-    quat = quat * (1.0 - 2.0 * (quat[..., 0:1] < 0.0))
-    mag = torch.linalg.norm(quat[..., 1:], dim=-1)
-    half_angle = torch.atan2(mag, quat[..., 0])
-    angle = 2.0 * half_angle
-    # check whether to apply Taylor approximation
-    sin_half_angles_over_angles = torch.where(
-        angle.abs() > eps, torch.sin(half_angle) / angle, 0.5 - angle * angle / 48
-    )
-    return quat[..., 1:4] / sin_half_angles_over_angles.unsqueeze(-1)
-
-
 @torch.jit.script
 def quat_error_magnitude(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor:
     """Computes the rotation difference between two quaternions.
@@ -685,8 +705,8 @@ def quat_error_magnitude(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor:
     Returns:
         Angular error between input quaternions in radians.
     """
-    quat_diff = quat_mul(q1, quat_conjugate(q2))
-    return torch.norm(axis_angle_from_quat(quat_diff), dim=-1)
+    axis_angle_error = quat_box_minus(q1, q2)
+    return torch.norm(axis_angle_error, dim=-1)
 
 
 @torch.jit.script
@@ -781,6 +801,43 @@ def combine_frame_transforms(
     return t02, q02
 
 
+def rigid_body_twist_transform(
+    v0: torch.Tensor, w0: torch.Tensor, t01: torch.Tensor, q01: torch.Tensor
+) -> tuple[torch.Tensor, torch.Tensor]:
+    r"""Transform the linear and angular velocity of a rigid body between reference frames.
+
+    Given the twist of 0 relative to frame 0, this function computes the twist of 1 relative to frame 1
+    from the position and orientation of frame 1 relative to frame 0. The transformation follows the
+    equations:
+
+    .. math::
+
+        w_11 = R_{10} w_00 = R_{01}^{-1} w_00
+        v_11 = R_{10} v_00 + R_{10} (w_00 \times t_01) = R_{01}^{-1} (v_00 + (w_00 \times t_01))
+
+    where
+
+        - :math:`R_{01}` is the rotation matrix from frame 0 to frame 1 derived from quaternion :math:`q_{01}`.
+        - :math:`t_{01}` is the position of frame 1 relative to frame 0 expressed in frame 0
+        - :math:`w_0` is the angular velocity of 0 in frame 0
+        - :math:`v_0` is the linear velocity of 0 in frame 0
+
+    Args:
+        v0: Linear velocity of 0 in frame 0. Shape is (N, 3).
+        w0: Angular velocity of 0 in frame 0. Shape is (N, 3).
+        t01: Position of frame 1 w.r.t. frame 0. Shape is (N, 3).
+        q01: Quaternion orientation of frame 1 w.r.t. frame 0 in (w, x, y, z). Shape is (N, 4).
+
+    Returns:
+        A tuple containing:
+        - The transformed linear velocity in frame 1. Shape is (N, 3).
+        - The transformed angular velocity in frame 1. Shape is (N, 3).
+    """
+    w1 = quat_rotate_inverse(q01, w0)
+    v1 = quat_rotate_inverse(q01, v0 + torch.cross(w0, t01, dim=-1))
+    return v1, w1
+
+
 # @torch.jit.script
 def subtract_frame_transforms(
     t01: torch.Tensor, q01: torch.Tensor, t02: torch.Tensor | None = None, q02: torch.Tensor | None = None

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):
     """