diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml index b47bd022ef4..b4711d2624d 100644 --- a/source/isaaclab/config/extension.toml +++ b/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" diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 7ab5e86c40f..371c53033ce 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/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) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab/isaaclab/utils/math.py b/source/isaaclab/isaaclab/utils/math.py index 73ee4cae35d..b6fac19614f 100644 --- a/source/isaaclab/isaaclab/utils/math.py +++ b/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 diff --git a/source/isaaclab/test/utils/test_math.py b/source/isaaclab/test/utils/test_math.py index d08feb1cb68..ecf9e679710 100644 --- a/source/isaaclab/test/utils/test_math.py +++ b/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