Changes the quat_box_minus implementation and adds quat_box_plus and rigid_body_twist_transform

source/isaaclab/config/extension.toml

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

source/isaaclab/docs/CHANGELOG.rst

@@ -1,6 +1,21 @@
 Changelog
 ---------
 
+0.37.0 (2025-04-01)
+~~~~~~~~~~~~~~~~~~~
+
+Changed
+^^^^^^^
+
+* Changed the implementation of :meth:`~isaaclab.utils.math.quat_box_minus`
+
+Added
+^^^^^
+
+* Added :meth:`~isaaclab.utils.math.quat_box_minus`
+* Added :meth:`~isaaclab.utils.math.rigid_body_twist_transform`
+
+
 0.36.4 (2025-03-24)
 ~~~~~~~~~~~~~~~~~~~
 

source/isaaclab/isaaclab/utils/math.py

@@ -499,25 +499,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 +523,46 @@ 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.
@@ -685,8 +706,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 +802,37 @@ 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

@@ -241,6 +241,94 @@ def test_convention_converter(self):
             math_utils.convert_camera_frame_orientation_convention(quat_world, "world", "world"), quat_world
         )
 
+    def test_quat_box_minus(self):
+        """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])
+        angle_a = math.pi - 0.1
+        angle_b = -math.pi + 0.1
+        quat_a = math_utils.quat_from_angle_axis(torch.tensor([angle_a]), axis_angles)
+        quat_b = math_utils.quat_from_angle_axis(torch.tensor([angle_b]), 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))
+        torch.testing.assert_close(expected_diff, axis_diff, atol=1e-06, rtol=1e-06)
+
+    def test_quat_box_minus_and_quat_box_plus(self):
+        """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 device in ("cpu", "cuda:0"):
+            with self.subTest(device=device):
+                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)
+
+    def test_rigid_body_twist_transform(self):
+        """Test rigid_body_twist_transform method.
+
+        Verifies correct transformation of twists (linear and angular velocity) between coordinate frames.
+        """
+        num_bodies = 100
+        for device in ("cpu", "cuda:0"):
+            with self.subTest(device=device):
+                # 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)
+
     def test_wrap_to_pi(self):
         """Test wrap_to_pi method."""
         # Define test cases