From 608d318c25497c91758cb4caa6142033e05ea27b Mon Sep 17 00:00:00 2001 From: Qian Lin Date: Wed, 15 Apr 2026 23:58:18 +0800 Subject: [PATCH 1/6] Fix XrCfg initial view anchor --- isaaclab_arena/embodiments/gr1t2/gr1t2.py | 23 +++++++++-------------- 1 file changed, 9 insertions(+), 14 deletions(-) diff --git a/isaaclab_arena/embodiments/gr1t2/gr1t2.py b/isaaclab_arena/embodiments/gr1t2/gr1t2.py index 37f92ec0f..04e8c1a09 100644 --- a/isaaclab_arena/embodiments/gr1t2/gr1t2.py +++ b/isaaclab_arena/embodiments/gr1t2/gr1t2.py @@ -26,6 +26,7 @@ from isaaclab_assets.robots.fourier import GR1T2_CFG from isaaclab_tasks.manager_based.manipulation.pick_place.pickplace_gr1t2_env_cfg import ActionsCfg as GR1T2ActionsCfg from isaaclab_teleop import XrCfg +from isaaclab_teleop.xr_cfg import XrAnchorRotationMode from isaaclab_arena.assets.register import register_asset from isaaclab_arena.embodiments.common.arm_mode import ArmMode @@ -104,21 +105,15 @@ def __init__( self.action_config = MISSING self.camera_config = GR1T2CameraCfg() - # XR settings (relative to robot base) - # These offsets are defined relative to the robot's base frame - self._xr_offset = Pose( - position_xyz=(-0.5, 0.0, -1.0), - rotation_xyzw=(0.0, 0.0, -0.70711, 0.70711), + # XR settings + # Anchor to the robot's pelvis for first-person view that follows the robot + self.xr: XrCfg = XrCfg( + anchor_pos=(0.0, 0.0, -1.0), + anchor_rot=(0.0, 0.0, -0.70711, 0.70711), + anchor_prim_path="/World/envs/env_0/Robot/pelvis", + anchor_rotation_mode=XrAnchorRotationMode.FOLLOW_PRIM_SMOOTHED, + fixed_anchor_height=True, ) - self.xr: XrCfg | None = None - - def get_xr_cfg(self) -> XrCfg: - """Get XR configuration with anchor pose adjusted for robot's initial pose. - - Returns: - XR configuration with anchor position and rotation in global coordinates. - """ - return get_default_xr_cfg(self.initial_pose, self._xr_offset) @register_asset From 268eb98fa9d170af9614d4eb6b8426e1d18d5e23 Mon Sep 17 00:00:00 2001 From: Qian Lin Date: Thu, 16 Apr 2026 01:34:51 +0800 Subject: [PATCH 2/6] lint --- isaaclab_arena/embodiments/gr1t2/gr1t2.py | 1 - 1 file changed, 1 deletion(-) diff --git a/isaaclab_arena/embodiments/gr1t2/gr1t2.py b/isaaclab_arena/embodiments/gr1t2/gr1t2.py index 04e8c1a09..2843aec8a 100644 --- a/isaaclab_arena/embodiments/gr1t2/gr1t2.py +++ b/isaaclab_arena/embodiments/gr1t2/gr1t2.py @@ -30,7 +30,6 @@ from isaaclab_arena.assets.register import register_asset from isaaclab_arena.embodiments.common.arm_mode import ArmMode -from isaaclab_arena.embodiments.common.common import get_default_xr_cfg from isaaclab_arena.embodiments.common.mimic_utils import get_rigid_and_articulated_object_poses from isaaclab_arena.embodiments.embodiment_base import EmbodimentBase from isaaclab_arena.terms.events import reset_all_articulation_joints From 3412729f2a9a8141122bb0f206b837e4c0c2ec45 Mon Sep 17 00:00:00 2001 From: Qian Lin Date: Thu, 16 Apr 2026 10:19:36 +0800 Subject: [PATCH 3/6] Remove unused deadcode get_default_xr_cfg --- isaaclab_arena/embodiments/common/common.py | 39 --------------------- 1 file changed, 39 deletions(-) delete mode 100644 isaaclab_arena/embodiments/common/common.py diff --git a/isaaclab_arena/embodiments/common/common.py b/isaaclab_arena/embodiments/common/common.py deleted file mode 100644 index 20c325fd3..000000000 --- a/isaaclab_arena/embodiments/common/common.py +++ /dev/null @@ -1,39 +0,0 @@ -# Copyright (c) 2025-2026, The Isaac Lab Arena Project Developers (https://github.com/isaac-sim/IsaacLab-Arena/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: Apache-2.0 - -from isaaclab_teleop import XrCfg - -from isaaclab_arena.utils.pose import Pose - - -def get_default_xr_cfg(initial_pose: Pose | None = None, xr_offset: Pose | None = None) -> XrCfg: - """ - Get the default XR configuration for the robot. - Args: - initial_pose: The initial pose of the robot. - xr_offset: The offset of the XR device from the robot. - - Returns: - The default XR configuration for the robot. - """ - if initial_pose is None and xr_offset is None: - raise ValueError("initial_pose or xr_offset must be provided") - # If robot has an initial pose, compose it with the XR offset - if initial_pose is not None: - from isaaclab_arena.utils.pose import compose_poses - - # Compose robot pose with XR offset: T_world_xr = T_world_robot * T_robot_xr - xr_pose_global = compose_poses(initial_pose, xr_offset) - - return XrCfg( - anchor_pos=xr_pose_global.position_xyz, - anchor_rot=xr_pose_global.rotation_xyzw, - ) - else: - # If no initial pose set, use the offset as global coordinates (robot at origin) - return XrCfg( - anchor_pos=xr_offset.position_xyz, - anchor_rot=xr_offset.rotation_xyzw, - ) From eccc39c410c139283d8b3670026cbc8a3dd97224 Mon Sep 17 00:00:00 2001 From: Qian Lin Date: Thu, 16 Apr 2026 10:19:54 +0800 Subject: [PATCH 4/6] Fix xr_anchor_pose unit test --- isaaclab_arena/tests/test_xr_anchor_pose.py | 192 ++++---------------- 1 file changed, 38 insertions(+), 154 deletions(-) diff --git a/isaaclab_arena/tests/test_xr_anchor_pose.py b/isaaclab_arena/tests/test_xr_anchor_pose.py index e39005efc..02784d10b 100644 --- a/isaaclab_arena/tests/test_xr_anchor_pose.py +++ b/isaaclab_arena/tests/test_xr_anchor_pose.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: Apache-2.0 -"""Tests for XR anchor pose configuration in embodiments.""" +"""Tests for XR anchor configuration on pelvis-relative humanoid embodiments.""" import numpy as np @@ -11,204 +11,88 @@ HEADLESS = True +_EXPECTED_ANCHOR_POS = (0.0, 0.0, -1.0) +_EXPECTED_ANCHOR_ROT = (0.0, 0.0, -0.70711, 0.70711) +_EXPECTED_ANCHOR_PRIM = "/World/envs/env_0/Robot/pelvis" -def _test_gr1t2_xr_anchor_pose(simulation_app) -> bool: - """Test GR1T2 XR anchor pose at origin and with robot transformation.""" - 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("gr1_pink")() - xr_cfg = embodiment.get_xr_cfg() - - expected_pos = embodiment._xr_offset.position_xyz - expected_rot = embodiment._xr_offset.rotation_xyzw - - assert ( - xr_cfg.anchor_pos == expected_pos - ), f"XR anchor position should match offset at origin: expected {expected_pos}, got {xr_cfg.anchor_pos}" - assert ( - xr_cfg.anchor_rot == expected_rot - ), f"XR anchor rotation should match offset at origin: expected {expected_rot}, got {xr_cfg.anchor_rot}" - - print("✓ GR1T2 XR anchor at origin: PASSED") - - # Test 2: XR anchor with robot position and rotation - robot_pose = Pose(position_xyz=(1.0, 2.0, 0.0), rotation_xyzw=(0.0, 0.0, 0.0, 1.0)) # No rotation - embodiment.set_initial_pose(robot_pose) - xr_cfg = embodiment.get_xr_cfg() - - # Expected position: robot_pos + offset - expected_pos = ( - robot_pose.position_xyz[0] + embodiment._xr_offset.position_xyz[0], # 1.0 + (-0.5) = 0.5 - robot_pose.position_xyz[1] + embodiment._xr_offset.position_xyz[1], # 2.0 + 0.0 = 2.0 - robot_pose.position_xyz[2] + embodiment._xr_offset.position_xyz[2], # 0.0 + (-1.0) = -1.0 - ) - - np.testing.assert_allclose( - xr_cfg.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}", - ) - - # Test 3: XR anchor with robot rotation - robot_pose_rotated = Pose( - position_xyz=(0.0, 0.0, 0.0), rotation_xyzw=(0.0, 0.0, 0.70711, 0.70711) # 90° rotation around Z - ) - embodiment.set_initial_pose(robot_pose_rotated) - xr_cfg_rotated = embodiment.get_xr_cfg() - - # Rotation should be composed, not same as offset - assert ( - xr_cfg_rotated.anchor_rot != embodiment._xr_offset.rotation_xyzw - ), "XR anchor rotation should be composed with robot rotation" - - print("✓ GR1T2 XR anchor with robot rotation: PASSED") - - # Test 4: Dynamic recomputation - pose1 = Pose(position_xyz=(1.0, 0.0, 0.0), rotation_xyzw=(0.0, 0.0, 0.0, 1.0)) - embodiment.set_initial_pose(pose1) - xr_cfg1 = embodiment.get_xr_cfg() - pose2 = Pose(position_xyz=(2.0, 0.0, 0.0), rotation_xyzw=(0.0, 0.0, 0.0, 1.0)) - embodiment.set_initial_pose(pose2) - xr_cfg2 = embodiment.get_xr_cfg() - - assert xr_cfg1.anchor_pos != xr_cfg2.anchor_pos, "XR anchor should change when robot pose changes" - - pos_diff = tuple(xr_cfg2.anchor_pos[i] - xr_cfg1.anchor_pos[i] for i in range(3)) - expected_diff = (1.0, 0.0, 0.0) - - np.testing.assert_allclose( - pos_diff, expected_diff, rtol=1e-5, err_msg="XR anchor position difference should match robot movement" - ) - - return True - - -def _test_g1_xr_anchor_pose(simulation_app) -> bool: - """Test G1 XR anchor config: fixed prim-relative anchor (pelvis), independent of initial pose.""" +def _assert_pelvis_relative_xr_cfg(embodiment_name: str, simulation_app) -> bool: + """GR1T2 (gr1_pink) and G1 WBC share the same pelvis-anchored XrCfg semantics.""" from isaaclab.devices.openxr.xr_cfg import XrAnchorRotationMode from isaaclab_arena.assets.asset_registry import AssetRegistry from isaaclab_arena.utils.pose import Pose asset_registry = AssetRegistry() - embodiment = asset_registry.get_asset_by_name("g1_wbc_pink")() + embodiment = asset_registry.get_asset_by_name(embodiment_name)() xr_cfg = embodiment.get_xr_cfg() - # G1 uses a fixed prim-relative XrCfg: anchor offset and rotation are constant - expected_pos = (0.0, 0.0, -1.0) - expected_rot = (0.0, 0.0, -0.70711, 0.70711) - expected_anchor_prim = "/World/envs/env_0/Robot/pelvis" - np.testing.assert_allclose( xr_cfg.anchor_pos, - expected_pos, + _EXPECTED_ANCHOR_POS, rtol=1e-5, - err_msg=f"G1 XR anchor_pos should be fixed offset: expected {expected_pos}, got {xr_cfg.anchor_pos}", + err_msg=f"{embodiment_name}: anchor_pos expected {_EXPECTED_ANCHOR_POS}, got {xr_cfg.anchor_pos}", ) np.testing.assert_allclose( xr_cfg.anchor_rot, - expected_rot, + _EXPECTED_ANCHOR_ROT, rtol=1e-5, - err_msg=f"G1 XR anchor_rot should be fixed: expected {expected_rot}, got {xr_cfg.anchor_rot}", + err_msg=f"{embodiment_name}: anchor_rot expected {_EXPECTED_ANCHOR_ROT}, got {xr_cfg.anchor_rot}", + ) + assert xr_cfg.anchor_prim_path == _EXPECTED_ANCHOR_PRIM, ( + f"{embodiment_name}: anchor_prim_path expected {_EXPECTED_ANCHOR_PRIM}, got {xr_cfg.anchor_prim_path}" ) - assert ( - 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_rotation_mode == XrAnchorRotationMode.FOLLOW_PRIM_SMOOTHED - ), "G1 XR anchor_rotation_mode should be FOLLOW_PRIM_SMOOTHED" - - # With initial pose set, config is unchanged (anchor is relative to pelvis prim, not world) + assert xr_cfg.fixed_anchor_height is True, f"{embodiment_name}: fixed_anchor_height should be True" + assert xr_cfg.anchor_rotation_mode == XrAnchorRotationMode.FOLLOW_PRIM_SMOOTHED, ( + f"{embodiment_name}: anchor_rotation_mode should be FOLLOW_PRIM_SMOOTHED" + ) + + # Anchor offsets are relative to the pelvis prim, not recomputed from world initial pose. robot_pose = Pose(position_xyz=(0.5, 1.0, 0.0), rotation_xyzw=(0.0, 0.0, 0.0, 1.0)) embodiment.set_initial_pose(robot_pose) xr_cfg_after = embodiment.get_xr_cfg() np.testing.assert_allclose( xr_cfg_after.anchor_pos, - expected_pos, + _EXPECTED_ANCHOR_POS, rtol=1e-5, - err_msg="G1 XR anchor_pos should remain fixed after set_initial_pose", + err_msg=f"{embodiment_name}: anchor_pos should stay fixed after set_initial_pose", ) np.testing.assert_allclose( xr_cfg_after.anchor_rot, - expected_rot, + _EXPECTED_ANCHOR_ROT, rtol=1e-5, - err_msg="G1 XR anchor_rot should remain fixed after set_initial_pose", + err_msg=f"{embodiment_name}: anchor_rot should stay fixed after set_initial_pose", ) return True -def _test_xr_anchor_multiple_positions(simulation_app) -> bool: - """Test XR anchor with multiple different robot positions.""" - from isaaclab_arena.assets.asset_registry import AssetRegistry - from isaaclab_arena.utils.pose import Pose +def _test_gr1_pink_xr_anchor(simulation_app) -> bool: + return _assert_pelvis_relative_xr_cfg("gr1_pink", simulation_app) - asset_registry = AssetRegistry() - embodiment = asset_registry.get_asset_by_name("gr1_pink")() - test_positions = [ - (0.0, 0.0, 0.0), - (1.0, 0.0, 0.0), - (0.0, 1.0, 0.0), - (1.0, 1.0, 1.0), - (-1.0, -1.0, 0.0), - ] - - for pos in test_positions: - robot_pose = Pose(position_xyz=pos, rotation_xyzw=(0.0, 0.0, 0.0, 1.0)) - embodiment.set_initial_pose(robot_pose) - - xr_cfg = embodiment.get_xr_cfg() - - # Verify XR anchor moved with robot - expected_pos = tuple( - robot_pos + offset_pos for robot_pos, offset_pos in zip(pos, embodiment._xr_offset.position_xyz) - ) - - np.testing.assert_allclose( - xr_cfg.anchor_pos, - expected_pos, - rtol=1e-5, - err_msg=f"XR anchor incorrect for robot at {pos}: expected {expected_pos}, got {xr_cfg.anchor_pos}", - ) - return True - - -# Public test functions that pytest will discover -def test_gr1t2_xr_anchor_pose(): - """Test GR1T2 XR anchor pose behavior.""" - result = run_simulation_app_function( - _test_gr1t2_xr_anchor_pose, - headless=HEADLESS, - ) - assert result, "GR1T2 XR anchor pose test failed" +def _test_g1_wbc_pink_xr_anchor(simulation_app) -> bool: + return _assert_pelvis_relative_xr_cfg("g1_wbc_pink", simulation_app) -def test_g1_xr_anchor_pose(): - """Test G1 XR anchor pose behavior.""" +def test_gr1_pink_xr_anchor_pose(): + """GR1T2 Pink uses a fixed pelvis-relative XR anchor.""" result = run_simulation_app_function( - _test_g1_xr_anchor_pose, + _test_gr1_pink_xr_anchor, headless=HEADLESS, ) - assert result, "G1 XR anchor pose test failed" + assert result, "gr1_pink XR anchor test failed" -def test_xr_anchor_multiple_positions(): - """Test XR anchor with multiple robot positions.""" +def test_g1_wbc_pink_xr_anchor_pose(): + """G1 WBC Pink uses the same pelvis-relative XR anchor pattern as GR1T2.""" result = run_simulation_app_function( - _test_xr_anchor_multiple_positions, + _test_g1_wbc_pink_xr_anchor, headless=HEADLESS, ) - assert result, "Multiple positions XR anchor test failed" + assert result, "g1_wbc_pink XR anchor test failed" if __name__ == "__main__": - test_gr1t2_xr_anchor_pose() - test_g1_xr_anchor_pose() - test_xr_anchor_multiple_positions() + test_gr1_pink_xr_anchor_pose() + test_g1_wbc_pink_xr_anchor_pose() From bb720a11fe97e9c810ebbefbe0ed033b5927b27e Mon Sep 17 00:00:00 2001 From: Qian Lin Date: Thu, 16 Apr 2026 10:24:07 +0800 Subject: [PATCH 5/6] lint --- isaaclab_arena/tests/test_xr_anchor_pose.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/isaaclab_arena/tests/test_xr_anchor_pose.py b/isaaclab_arena/tests/test_xr_anchor_pose.py index 02784d10b..573361a7f 100644 --- a/isaaclab_arena/tests/test_xr_anchor_pose.py +++ b/isaaclab_arena/tests/test_xr_anchor_pose.py @@ -39,13 +39,13 @@ def _assert_pelvis_relative_xr_cfg(embodiment_name: str, simulation_app) -> bool rtol=1e-5, err_msg=f"{embodiment_name}: anchor_rot expected {_EXPECTED_ANCHOR_ROT}, got {xr_cfg.anchor_rot}", ) - assert xr_cfg.anchor_prim_path == _EXPECTED_ANCHOR_PRIM, ( - f"{embodiment_name}: anchor_prim_path expected {_EXPECTED_ANCHOR_PRIM}, got {xr_cfg.anchor_prim_path}" - ) + assert ( + xr_cfg.anchor_prim_path == _EXPECTED_ANCHOR_PRIM + ), f"{embodiment_name}: anchor_prim_path expected {_EXPECTED_ANCHOR_PRIM}, got {xr_cfg.anchor_prim_path}" assert xr_cfg.fixed_anchor_height is True, f"{embodiment_name}: fixed_anchor_height should be True" - assert xr_cfg.anchor_rotation_mode == XrAnchorRotationMode.FOLLOW_PRIM_SMOOTHED, ( - f"{embodiment_name}: anchor_rotation_mode should be FOLLOW_PRIM_SMOOTHED" - ) + assert ( + xr_cfg.anchor_rotation_mode == XrAnchorRotationMode.FOLLOW_PRIM_SMOOTHED + ), f"{embodiment_name}: anchor_rotation_mode should be FOLLOW_PRIM_SMOOTHED" # Anchor offsets are relative to the pelvis prim, not recomputed from world initial pose. robot_pose = Pose(position_xyz=(0.5, 1.0, 0.0), rotation_xyzw=(0.0, 0.0, 0.0, 1.0)) From eef98a4648d11d5b9679f589c9953591c5538613 Mon Sep 17 00:00:00 2001 From: qianlin <53278415+qianl-nv@users.noreply.github.com> Date: Thu, 16 Apr 2026 10:38:07 +0800 Subject: [PATCH 6/6] Update isaaclab_arena/tests/test_xr_anchor_pose.py import Co-authored-by: greptile-apps[bot] <165735046+greptile-apps[bot]@users.noreply.github.com> --- isaaclab_arena/tests/test_xr_anchor_pose.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/isaaclab_arena/tests/test_xr_anchor_pose.py b/isaaclab_arena/tests/test_xr_anchor_pose.py index 573361a7f..a91aceac4 100644 --- a/isaaclab_arena/tests/test_xr_anchor_pose.py +++ b/isaaclab_arena/tests/test_xr_anchor_pose.py @@ -18,7 +18,7 @@ def _assert_pelvis_relative_xr_cfg(embodiment_name: str, simulation_app) -> bool: """GR1T2 (gr1_pink) and G1 WBC share the same pelvis-anchored XrCfg semantics.""" - from isaaclab.devices.openxr.xr_cfg import XrAnchorRotationMode + from isaaclab_teleop.xr_cfg import XrAnchorRotationMode from isaaclab_arena.assets.asset_registry import AssetRegistry from isaaclab_arena.utils.pose import Pose