Fix tests

* Fix x1 anchor pose test in isaaclab_arena/tests/test_xr_anchor_pose.py
  to adapt to the new XrCfg
* Fix G1DecoupledWBCPinkAction to no crash for zero norm quaternion
  inputs from the
isaaclab_arena/tests/test_device_and_retargeter_registry.py test

Author: qianl-nv

isaaclab_arena/tests/test_device_and_retargeter_registry.py

@@ -64,10 +64,6 @@ def _test_all_devices_and_retargeters_in_registry(simulation_app):
             for _ in tqdm.tqdm(range(NUM_STEPS)):
                 with torch.inference_mode():
                     actions = torch.zeros(env.action_space.shape, device=env.unwrapped.device)
-                    if embodiment_name == "g1_wbc_pink":
-                        # G1 WBC pink action: wrist quats at 5:9 and 12:16 (wxyz); identity = (1,0,0,0)
-                        actions[..., 5] = 1.0
-                        actions[..., 12] = 1.0
                     env.step(actions)
 
             # Close the environment using safe teardown

isaaclab_arena/tests/test_xr_anchor_pose.py

@@ -89,42 +89,56 @@ def _test_gr1t2_xr_anchor_pose(simulation_app) -> bool:
 
 
 def _test_g1_xr_anchor_pose(simulation_app) -> bool:
-    """Test G1 XR anchor pose at origin and with robot transformation."""
+    """Test G1 XR anchor config: fixed prim-relative anchor (pelvis), independent of initial pose."""
+    from isaaclab.devices.openxr.xr_cfg import XrAnchorRotationMode
+
     from isaaclab_arena.assets.asset_registry import AssetRegistry
     from isaaclab_arena.utils.pose import Pose
 
-    # Test 1: XR anchor at origin (no initial pose)
     asset_registry = AssetRegistry()
     embodiment = asset_registry.get_asset_by_name("g1_wbc_pink")()
     xr_cfg = embodiment.get_xr_cfg()
 
-    expected_pos = embodiment._xr_offset.position_xyz
-    expected_rot = embodiment._xr_offset.rotation_wxyz
+    # G1 uses a fixed prim-relative XrCfg: anchor offset and rotation are constant
+    expected_pos = (0.0, 0.0, -1.0)
+    expected_rot = (0.70711, 0.0, 0.0, -0.70711)
+    expected_anchor_prim = "/World/envs/env_0/Robot/pelvis"
 
+    np.testing.assert_allclose(
+        xr_cfg.anchor_pos,
+        expected_pos,
+        rtol=1e-5,
+        err_msg=f"G1 XR anchor_pos should be fixed offset: expected {expected_pos}, got {xr_cfg.anchor_pos}",
+    )
+    np.testing.assert_allclose(
+        xr_cfg.anchor_rot,
+        expected_rot,
+        rtol=1e-5,
+        err_msg=f"G1 XR anchor_rot should be fixed: expected {expected_rot}, got {xr_cfg.anchor_rot}",
+    )
     assert (
-        xr_cfg.anchor_pos == expected_pos
-    ), f"XR anchor position should match offset at origin: expected {expected_pos}, got {xr_cfg.anchor_pos}"
+        xr_cfg.anchor_prim_path == expected_anchor_prim
+    ), f"G1 XR anchor_prim_path should be pelvis: expected {expected_anchor_prim}, got {xr_cfg.anchor_prim_path}"
+    assert xr_cfg.fixed_anchor_height is True, "G1 XR fixed_anchor_height should be True"
     assert (
-        xr_cfg.anchor_rot == expected_rot
-    ), f"XR anchor rotation should match offset at origin: expected {expected_rot}, got {xr_cfg.anchor_rot}"
+        xr_cfg.anchor_rotation_mode == XrAnchorRotationMode.FOLLOW_PRIM_SMOOTHED
+    ), "G1 XR anchor_rotation_mode should be FOLLOW_PRIM_SMOOTHED"
 
-    # Test 2: XR anchor with robot position
-    robot_pose = Pose(position_xyz=(0.5, 1.0, 0.0), rotation_wxyz=(1.0, 0.0, 0.0, 0.0))  # No rotation
+    # With initial pose set, config is unchanged (anchor is relative to pelvis prim, not world)
+    robot_pose = Pose(position_xyz=(0.5, 1.0, 0.0), rotation_wxyz=(1.0, 0.0, 0.0, 0.0))
     embodiment.set_initial_pose(robot_pose)
-    xr_cfg = embodiment.get_xr_cfg()
-
-    # G1 offset is (0.0, 0.0, -1.0)
-    expected_pos = (
-        robot_pose.position_xyz[0] + embodiment._xr_offset.position_xyz[0],  # 0.5 + 0.0 = 0.5
-        robot_pose.position_xyz[1] + embodiment._xr_offset.position_xyz[1],  # 1.0 + 0.0 = 1.0
-        robot_pose.position_xyz[2] + embodiment._xr_offset.position_xyz[2],  # 0.0 + (-1.0) = -1.0
-    )
-
+    xr_cfg_after = embodiment.get_xr_cfg()
     np.testing.assert_allclose(
-        xr_cfg.anchor_pos,
+        xr_cfg_after.anchor_pos,
         expected_pos,
         rtol=1e-5,
-        err_msg=f"XR anchor position incorrect with robot pose: expected {expected_pos}, got {xr_cfg.anchor_pos}",
+        err_msg="G1 XR anchor_pos should remain fixed after set_initial_pose",
+    )
+    np.testing.assert_allclose(
+        xr_cfg_after.anchor_rot,
+        expected_rot,
+        rtol=1e-5,
+        err_msg="G1 XR anchor_rot should remain fixed after set_initial_pose",
     )
 
     return True
@@ -176,13 +190,13 @@ def test_gr1t2_xr_anchor_pose():
     assert result, "GR1T2 XR anchor pose test failed"
 
 
-# def test_g1_xr_anchor_pose():
-#     """Test G1 XR anchor pose behavior."""
-#     result = run_simulation_app_function(
-#         _test_g1_xr_anchor_pose,
-#         headless=HEADLESS,
-#     )
-#     assert result, "G1 XR anchor pose test failed"
+def test_g1_xr_anchor_pose():
+    """Test G1 XR anchor pose behavior."""
+    result = run_simulation_app_function(
+        _test_g1_xr_anchor_pose,
+        headless=HEADLESS,
+    )
+    assert result, "G1 XR anchor pose test failed"
 
 
 def test_xr_anchor_multiple_positions():
@@ -196,5 +210,5 @@ def test_xr_anchor_multiple_positions():
 
 if __name__ == "__main__":
     test_gr1t2_xr_anchor_pose()
-    # test_g1_xr_anchor_pose()
+    test_g1_xr_anchor_pose()
     test_xr_anchor_multiple_positions()

isaaclab_arena_g1/g1_env/mdp/actions/g1_decoupled_wbc_pink_action.py

@@ -215,16 +215,22 @@ def process_actions(self, actions: torch.Tensor):
         """
         # Extract upper body left/right arm pos/quat from actions
         left_arm_pos = actions_clone[:, LEFT_WRIST_POS_START_IDX:LEFT_WRIST_POS_END_IDX].squeeze(0).cpu()
-        left_arm_quat = actions_clone[:, LEFT_WRIST_QUAT_START_IDX:LEFT_WRIST_QUAT_END_IDX].squeeze(0).cpu()
+        left_arm_quat = actions_clone[:, LEFT_WRIST_QUAT_START_IDX:LEFT_WRIST_QUAT_END_IDX].squeeze(0).cpu().numpy()
         right_arm_pos = actions_clone[:, RIGHT_WRIST_POS_START_IDX:RIGHT_WRIST_POS_END_IDX].squeeze(0).cpu()
-        right_arm_quat = actions_clone[:, RIGHT_WRIST_QUAT_START_IDX:RIGHT_WRIST_QUAT_END_IDX].squeeze(0).cpu()
+        right_arm_quat = actions_clone[:, RIGHT_WRIST_QUAT_START_IDX:RIGHT_WRIST_QUAT_END_IDX].squeeze(0).cpu().numpy()
+
+        # Replace zero-norm quaternions with identity (e.g. zero actions from env.step(zeros))
+        _IDENTITY_QUAT_WXYZ = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64)
+        for q in (left_arm_quat, right_arm_quat):
+            if np.linalg.norm(q) < 1e-8:
+                q[:] = _IDENTITY_QUAT_WXYZ
 
         # Convert from pos/quat to 4x4 transform matrix
         # Scipy requires quat xyzw, IsaacLab uses wxyz so a conversion is needed
-        left_arm_quat = np.roll(left_arm_quat, -1)
-        right_arm_quat = np.roll(right_arm_quat, -1)
-        left_rotmat = R.from_quat(left_arm_quat).as_matrix()
-        right_rotmat = R.from_quat(right_arm_quat).as_matrix()
+        left_arm_quat_xyzw = np.roll(left_arm_quat, -1)
+        right_arm_quat_xyzw = np.roll(right_arm_quat, -1)
+        left_rotmat = R.from_quat(left_arm_quat_xyzw).as_matrix()
+        right_rotmat = R.from_quat(right_arm_quat_xyzw).as_matrix()
 
         left_arm_pose = np.eye(4)
         left_arm_pose[:3, :3] = left_rotmat