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

This reverts commit 3e77641.

Author: jtigue-bdai

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 = abs(mag) * torch.ones_like(other)
-    return torch.copysign(mag_torch, other)
+    mag_torch = torch.tensor(mag, device=other.device, dtype=torch.float).repeat(other.shape[0])
+    return torch.abs(mag_torch) * torch.sign(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 (intrinsic) in radians to rotation matrices.
+    Convert rotations given as Euler angles 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 extrinsic convention.
+        The euler angles are assumed in XYZ convention.
 
     Args:
         quat: The quaternion orientation in (w, x, y, z). Shape is (N, 4).
@@ -928,8 +928,14 @@ def compute_pose_error(
     Raises:
         ValueError: Invalid rotation error type.
     """
-    # Compute quaternion error (i.e., quat_box_minus)
-    quat_error = quat_mul(q01, quat_conjugate(q02))
+    # 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 position error
     pos_error = t02 - t01