Fixes failing environment and IK tests (#2372)

# Description

Fixes failing tests in test_environments and test_pink_ik.

## 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)


## 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
- [ ] I have added tests that prove my fix is effective or that my
feature works
- [ ] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file
- [ ] 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
-->

---------
Co-authored-by: Ashwin Varghese Kuruttukulam <ashwinvk@nvidia.com>

source/isaaclab/isaaclab/devices/openxr/test_oxr_device.py

@@ -73,7 +73,7 @@ class TestOpenXRDevice(unittest.TestCase):
         # Create environment.
         env = ManagerBasedEnv(cfg=env_cfg)
 
-        device = OpenXRDevice(env_cfg.xr, OpenXRDevice.Hand.RIGHT)
+        device = OpenXRDevice(env_cfg.xr)
 
         # Check that the xr anchor prim is created with the correct pose.
         xr_anchor_prim = XFormPrim("/XRAnchor")
@@ -97,7 +97,7 @@ class TestOpenXRDevice(unittest.TestCase):
         # Create environment.
         env = ManagerBasedEnv(cfg=env_cfg)
 
-        device = OpenXRDevice(None, OpenXRDevice.Hand.RIGHT)
+        device = OpenXRDevice(None)
 
         # Check that the xr anchor prim is created with the correct default pose.
         xr_anchor_prim = XFormPrim("/XRAnchor")
@@ -121,8 +121,8 @@ class TestOpenXRDevice(unittest.TestCase):
         # Create environment.
         env = ManagerBasedEnv(cfg=env_cfg)
 
-        device_1 = OpenXRDevice(None, OpenXRDevice.Hand.LEFT)
-        device_2 = OpenXRDevice(None, OpenXRDevice.Hand.RIGHT)
+        device_1 = OpenXRDevice(None)
+        device_2 = OpenXRDevice(None)
 
         # Check that the xr anchor prim is created with the correct default pose.
         xr_anchor_prim = XFormPrim("/XRAnchor")

source/isaaclab/test/controllers/test_pink_ik.py

@@ -4,10 +4,12 @@
 # SPDX-License-Identifier: BSD-3-Clause
 
 """Launch Isaac Sim Simulator first."""
+import sys
 
 # Import pinocchio in the main script to force the use of the dependencies installed by IsaacLab and not the one installed by Isaac Sim
 # pinocchio is required by the Pink IK controller
-import pinocchio  # noqa: F401
+if sys.platform != "win32":
+    import pinocchio  # noqa: F401
 
 from isaaclab.app import AppLauncher, run_tests
 
@@ -21,7 +23,10 @@ import gymnasium as gym
 import torch
 import unittest
 
+from isaaclab.utils.math import axis_angle_from_quat, matrix_from_quat, quat_from_matrix, quat_inv
+
 import isaaclab_tasks  # noqa: F401
+import isaaclab_tasks.manager_based.manipulation.pick_place  # noqa: F401
 from isaaclab_tasks.utils.parse_cfg import parse_env_cfg
 
 
@@ -42,8 +47,10 @@ class TestPinkIKController(unittest.TestCase):
 
     def setUp(self):
 
-        # End effector position mean square error tolerance
-        self.pose_tolerance = 1e-2
+        # End effector position mean square error tolerance in meters
+        self.pos_tolerance = 0.02  # 2 cm
+        # End effector orientation mean square error tolerance in radians
+        self.rot_tolerance = 0.17  # 10 degrees
 
         # Number of environments
         self.num_envs = 1
@@ -55,7 +62,7 @@ class TestPinkIKController(unittest.TestCase):
         self.num_steps_controller_convergence = 25
 
         self.num_times_to_move_hands_up = 3
-        self.num_times_to_move_hands_down = 6
+        self.num_times_to_move_hands_down = 3
 
         # Create starting setpoints with respect to the env origin frame
         # These are the setpoints for the forward kinematics result of the
@@ -63,7 +70,7 @@ class TestPinkIKController(unittest.TestCase):
         y_axis_z_axis_90_rot_quaternion = [0.5, 0.5, -0.5, 0.5]
         left_hand_roll_link_pos = [-0.23, 0.28, 1.1]
         self.left_hand_roll_link_pose = left_hand_roll_link_pos + y_axis_z_axis_90_rot_quaternion
-        right_hand_roll_link_pos = [0.16, 0.26, 1.13]
+        right_hand_roll_link_pos = [0.23, 0.28, 1.1]
         self.right_hand_roll_link_pose = right_hand_roll_link_pos + y_axis_z_axis_90_rot_quaternion
 
     """
@@ -113,44 +120,81 @@ class TestPinkIKController(unittest.TestCase):
                 left_hand_roll_link_setpoint = (
                     torch.tensor(self.left_hand_roll_link_pose, device=device).unsqueeze(0).repeat(env.num_envs, 1)
                 )
-                left_hand_roll_link_error = left_hand_roll_link_setpoint - left_hand_roll_link_feedback
+                left_hand_roll_link_pos_error = (
+                    left_hand_roll_link_setpoint[:, :3] - left_hand_roll_link_feedback[:, :3]
+                )
+                left_hand_roll_link_rot_error = axis_angle_from_quat(
+                    quat_from_matrix(
+                        matrix_from_quat(left_hand_roll_link_setpoint[:, 3:])
+                        * matrix_from_quat(quat_inv(left_hand_roll_link_feedback[:, 3:]))
+                    )
+                )
+
                 right_hand_roll_link_feedback = right_hand_roll_link_pose_obs
                 right_hand_roll_link_setpoint = (
                     torch.tensor(self.right_hand_roll_link_pose, device=device).unsqueeze(0).repeat(env.num_envs, 1)
                 )
-                right_hand_roll_link_error = right_hand_roll_link_setpoint - right_hand_roll_link_feedback
+                right_hand_roll_link_pos_error = (
+                    right_hand_roll_link_setpoint[:, :3] - right_hand_roll_link_feedback[:, :3]
+                )
+                right_hand_roll_link_rot_error = axis_angle_from_quat(
+                    quat_from_matrix(
+                        matrix_from_quat(right_hand_roll_link_setpoint[:, 3:])
+                        * matrix_from_quat(quat_inv(right_hand_roll_link_feedback[:, 3:]))
+                    )
+                )
 
                 if num_runs % self.num_steps_controller_convergence == 0:
+                    # Check if the left hand roll link is at the target position
+                    torch.testing.assert_close(
+                        torch.mean(torch.abs(left_hand_roll_link_pos_error), dim=1),
+                        torch.zeros(env.num_envs, device="cuda:0"),
+                        rtol=0.0,
+                        atol=self.pos_tolerance,
+                    )
+
+                    # Check if the right hand roll link is at the target position
+                    torch.testing.assert_close(
+                        torch.mean(torch.abs(right_hand_roll_link_pos_error), dim=1),
+                        torch.zeros(env.num_envs, device="cuda:0"),
+                        rtol=0.0,
+                        atol=self.pos_tolerance,
+                    )
+
+                    # Check if the left hand roll link is at the target orientation
                     torch.testing.assert_close(
-                        torch.mean(torch.abs(left_hand_roll_link_error), dim=1),
+                        torch.mean(torch.abs(left_hand_roll_link_rot_error), dim=1),
                         torch.zeros(env.num_envs, device="cuda:0"),
                         rtol=0.0,
-                        atol=self.pose_tolerance,
+                        atol=self.rot_tolerance,
                     )
+
+                    # Check if the right hand roll link is at the target orientation
                     torch.testing.assert_close(
-                        torch.mean(torch.abs(right_hand_roll_link_error), dim=1),
+                        torch.mean(torch.abs(right_hand_roll_link_rot_error), dim=1),
                         torch.zeros(env.num_envs, device="cuda:0"),
                         rtol=0.0,
-                        atol=self.pose_tolerance,
+                        atol=self.rot_tolerance,
                     )
 
                     # Change the setpoints to move the hands up and down as per the counter
                     test_counter += 1
-                    if test_counter == self.num_times_to_move_hands_up:
+                    if move_hands_up and test_counter > self.num_times_to_move_hands_up:
                         move_hands_up = False
-                    elif test_counter == self.num_times_to_move_hands_down:
-                        move_hands_up = True
-                        # Test is done after moving the hands up and then down
+                    elif not move_hands_up and test_counter > (
+                        self.num_times_to_move_hands_down + self.num_times_to_move_hands_up
+                    ):
+                        # Test is done after moving the hands up and down
                         break
                     if move_hands_up:
-                        self.left_hand_roll_link_pose[0] += 0.05
+                        self.left_hand_roll_link_pose[1] += 0.05
                         self.left_hand_roll_link_pose[2] += 0.05
-                        self.right_hand_roll_link_pose[0] += 0.05
+                        self.right_hand_roll_link_pose[1] += 0.05
                         self.right_hand_roll_link_pose[2] += 0.05
                     else:
-                        self.left_hand_roll_link_pose[0] -= 0.05
+                        self.left_hand_roll_link_pose[1] -= 0.05
                         self.left_hand_roll_link_pose[2] -= 0.05
-                        self.right_hand_roll_link_pose[0] -= 0.05
+                        self.right_hand_roll_link_pose[1] -= 0.05
                         self.right_hand_roll_link_pose[2] -= 0.05
 
         env.close()

source/isaaclab_tasks/test/test_environments.py

@@ -5,12 +5,15 @@
 
 """Launch Isaac Sim Simulator first."""
 
+import sys
+
 # Omniverse logger
 import omni.log
 
 # Import pinocchio in the main script to force the use of the dependencies installed by IsaacLab and not the one installed by Isaac Sim
 # pinocchio is required by the Pink IK controller
-import pinocchio  # noqa: F401
+if sys.platform != "win32":
+    import pinocchio  # noqa: F401
 
 from isaaclab.app import AppLauncher, run_tests
 
@@ -53,6 +56,7 @@ class TestEnvironments(unittest.TestCase):
             "Isaac-Stack-Cube-Franka-IK-Rel-Blueprint-v0",
             "Isaac-Stack-Cube-Instance-Randomize-Franka-IK-Rel-v0",
             "Isaac-Stack-Cube-Instance-Randomize-Franka-v0",
+            "Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-v0",
         ]
 
         # this flag is necessary to prevent a bug where the simulation gets stuck randomly when running the