Add math tests for transforms, rotations, and conversions (#103)

source/isaaclab/config/extension.toml

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

source/isaaclab/docs/CHANGELOG.rst

@@ -1,6 +1,39 @@
 Changelog
 ---------
 
+0.40.12 (2025-06-30)
+~~~~~~~~~~~~~~~~~~~~
+
+Added
+^^^^^
+
+* Added unit tests for multiple math functions:
+  :func:`~isaaclab.utils.math.scale_transform`.
+  :func:`~isaaclab.utils.math.unscale_transform`.
+  :func:`~isaaclab.utils.math.saturate`.
+  :func:`~isaaclab.utils.math.normalize`.
+  :func:`~isaaclab.utils.math.copysign`.
+  :func:`~isaaclab.utils.math.convert_quat`.
+  :func:`~isaaclab.utils.math.quat_conjugate`.
+  :func:`~isaaclab.utils.math.quat_from_euler_xyz`.
+  :func:`~isaaclab.utils.math.quat_from_matrix`.
+  :func:`~isaaclab.utils.math.euler_xyz_from_quat`.
+  :func:`~isaaclab.utils.math.matrix_from_euler`.
+  :func:`~isaaclab.utils.math.quat_from_angle_axis`.
+  :func:`~isaaclab.utils.math.axis_angle_from_quat`.
+  :func:`~isaaclab.utils.math.skew_symmetric_matrix`.
+  :func:`~isaaclab.utils.math.combine_transform`.
+  :func:`~isaaclab.utils.math.subtract_transform`.
+  :func:`~isaaclab.utils.math.compute_pose_error`.
+
+Changed
+^^^^^^^
+
+* Changed the implementation of :func:`~isaaclab.utils.math.copysign` to better reflect the documented functionality.
+* Changed the implementation of :func:`~isaaclab.utils.math.compute_pose_error` to better reflect the inverse of
+  :func:`~isaaclab.utils.math.quat_box_minus`
+
+
 0.40.11 (2025-06-27)
 ~~~~~~~~~~~~~~~~~~~~
 

source/isaaclab/isaaclab/utils/math.py

@@ -133,8 +133,8 @@ def copysign(mag: float, other: torch.Tensor) -> torch.Tensor:
     Returns:
         The output tensor.
     """
-    mag_torch = torch.tensor(mag, device=other.device, dtype=torch.float).repeat(other.shape[0])
-    return torch.abs(mag_torch) * torch.sign(other)
+    mag_torch = abs(mag) * torch.ones_like(other)
+    return torch.copysign(mag_torch, other)
 
 
 """
@@ -250,7 +250,7 @@ def quat_conjugate(q: torch.Tensor) -> torch.Tensor:
     """
     shape = q.shape
     q = q.reshape(-1, 4)
-    return torch.cat((q[:, 0:1], -q[:, 1:]), dim=-1).view(shape)
+    return torch.cat((q[..., 0:1], -q[..., 1:]), dim=-1).view(shape)
 
 
 @torch.jit.script
@@ -401,7 +401,7 @@ def _axis_angle_rotation(axis: Literal["X", "Y", "Z"], angle: torch.Tensor) -> t
 
 def matrix_from_euler(euler_angles: torch.Tensor, convention: str) -> torch.Tensor:
     """
-    Convert rotations given as Euler angles in radians to rotation matrices.
+    Convert rotations given as Euler angles (intrinsic) in radians to rotation matrices.
 
     Args:
         euler_angles: Euler angles in radians. Shape is (..., 3).
@@ -436,7 +436,7 @@ def euler_xyz_from_quat(
     """Convert rotations given as quaternions to Euler angles in radians.
 
     Note:
-        The euler angles are assumed in XYZ convention.
+        The euler angles are assumed in XYZ extrinsic convention.
 
     Args:
         quat: The quaternion orientation in (w, x, y, z). Shape is (N, 4).
@@ -928,14 +928,8 @@ def compute_pose_error(
     Raises:
         ValueError: Invalid rotation error type.
     """
-    # Compute quaternion error (i.e., difference quaternion)
-    # Reference: https://personal.utdallas.edu/~sxb027100/dock/quaternion.html
-    # q_current_norm = q_current * q_current_conj
-    source_quat_norm = quat_mul(q01, quat_conjugate(q01))[:, 0]
-    # q_current_inv = q_current_conj / q_current_norm
-    source_quat_inv = quat_conjugate(q01) / source_quat_norm.unsqueeze(-1)
-    # q_error = q_target * q_current_inv
-    quat_error = quat_mul(q02, source_quat_inv)
+    # Compute quaternion error (i.e., quat_box_minus)
+    quat_error = quat_mul(q01, quat_conjugate(q02))
 
     # Compute position error
     pos_error = t02 - t01