Add EE pose transform to ego coordinate

Author: qianl-nv

isaaclab_arena/embodiments/g1/g1.py

@@ -34,6 +34,9 @@
 from isaaclab_arena_g1.g1_env.mdp import g1_observations as g1_observations_mdp
 from isaaclab_arena_g1.g1_env.mdp.actions.g1_decoupled_wbc_joint_action_cfg import G1DecoupledWBCJointActionCfg
 from isaaclab_arena_g1.g1_env.mdp.actions.g1_decoupled_wbc_pink_action_cfg import G1DecoupledWBCPinkActionCfg
+from isaaclab_arena_g1.g1_env.mdp.actions.g1_decoupled_wbc_pink_world_frame_action_cfg import (
+    G1DecoupledWBCPinkWorldFrameActionCfg,
+)
 
 
 class G1EmbodimentBase(EmbodimentBase):
@@ -115,9 +118,16 @@ def __init__(
         initial_pose: Pose | None = None,
         camera_offset: Pose | None = _DEFAULT_G1_CAMERA_OFFSET,
         use_tiled_camera: bool = False,  # Default to regular for single env
+        use_world_frame_actions: bool = False,  # Set to True for VR/motion controller teleop
     ):
         super().__init__(enable_cameras, initial_pose)
-        self.action_config = G1WBCPinkActionCfg()
+        
+        # Use world frame action config if needed (e.g., for VR controllers)
+        if use_world_frame_actions:
+            self.action_config = G1DecoupledWBCPinkWorldFrameActionCfg()
+        else:
+            self.action_config = G1WBCPinkActionCfg()
+            
         self.observation_config = G1WBCPinkObservationsCfg()
         self.observation_config.policy.concatenate_terms = self.concatenate_observation_terms
         self.observation_config.wbc.concatenate_terms = self.concatenate_observation_terms

isaaclab_arena_environments/galileo_g1_locomanip_pick_and_place_environment.py

@@ -21,7 +21,23 @@ def get_env(self, args_cli: argparse.Namespace):
         background = self.asset_registry.get_asset_by_name("galileo_locomanip")()
         pick_up_object = self.asset_registry.get_asset_by_name(args_cli.object)()
         blue_sorting_bin = self.asset_registry.get_asset_by_name("blue_sorting_bin")()
-        embodiment = self.asset_registry.get_asset_by_name(args_cli.embodiment)(enable_cameras=args_cli.enable_cameras)
+        
+        # Check if we need world frame actions (for VR/motion controllers)
+        use_world_frame_actions = (
+            args_cli.teleop_device in ["motion_controllers", "openxr"]
+            and args_cli.embodiment == "g1_wbc_pink"
+        )
+        
+        # Create embodiment with appropriate action config
+        if use_world_frame_actions:
+            embodiment = self.asset_registry.get_asset_by_name(args_cli.embodiment)(
+                enable_cameras=args_cli.enable_cameras,
+                use_world_frame_actions=True
+            )
+        else:
+            embodiment = self.asset_registry.get_asset_by_name(args_cli.embodiment)(
+                enable_cameras=args_cli.enable_cameras
+            )
 
         if args_cli.teleop_device is not None:
             teleop_device = self.device_registry.get_device_by_name(args_cli.teleop_device)()

isaaclab_arena_g1/g1_env/mdp/actions/__init__.py

@@ -2,3 +2,11 @@
 # All rights reserved.
 #
 # SPDX-License-Identifier: Apache-2.0
+
+from .g1_decoupled_wbc_pink_world_frame_action import G1DecoupledWBCPinkWorldFrameAction
+from .g1_decoupled_wbc_pink_world_frame_action_cfg import G1DecoupledWBCPinkWorldFrameActionCfg
+
+__all__ = [
+    "G1DecoupledWBCPinkWorldFrameAction",
+    "G1DecoupledWBCPinkWorldFrameActionCfg",
+]

isaaclab_arena_g1/g1_env/mdp/actions/g1_decoupled_wbc_pink_world_frame_action.py

@@ -0,0 +1,114 @@
+# 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 __future__ import annotations
+
+import torch
+from typing import TYPE_CHECKING
+
+from isaaclab.utils.math import quat_apply_inverse, quat_inv, quat_mul
+
+from isaaclab_arena_g1.g1_env.mdp.actions.g1_decoupled_wbc_pink_action import G1DecoupledWBCPinkAction
+
+if TYPE_CHECKING:
+    from isaaclab.envs import ManagerBasedEnv
+
+    from .g1_decoupled_wbc_pink_world_frame_action_cfg import G1DecoupledWBCPinkWorldFrameActionCfg
+
+
+class G1DecoupledWBCPinkWorldFrameAction(G1DecoupledWBCPinkAction):
+    """Action term for G1 WBC Pink that transforms world-frame wrist poses to base-frame.
+    
+    This action term extends the base G1DecoupledWBCPinkAction to handle VR controller inputs
+    that provide wrist poses in world coordinates. It automatically transforms these to the
+    robot's base frame before passing them to the IK controller.
+    
+    This is necessary because VR controllers (like Quest) output absolute world positions,
+    but the robot's IK expects positions relative to its base. Without this transformation,
+    when the robot moves, the hands would try to stay at their original world position and
+    drift away from the body.
+    """
+
+    cfg: G1DecoupledWBCPinkWorldFrameActionCfg
+
+    def __init__(self, cfg: G1DecoupledWBCPinkWorldFrameActionCfg, env: ManagerBasedEnv):
+        """Initialize the action term.
+
+        Args:
+            cfg: The configuration for this action term.
+            env: The environment in which the action term will be applied.
+        """
+        super().__init__(cfg, env)
+
+    def process_actions(self, actions: torch.Tensor):
+        """Process the input actions with world-to-base frame transformation.
+
+        This method first transforms wrist poses from world frame to robot base frame
+        (if enabled in config), then calls the parent class's process_actions.
+
+        Args:
+            actions: The input actions tensor.
+
+            Expected action layout (23 elements):
+            [0:1] left_hand_state (0=open, 1=close)
+            [1:2] right_hand_state (0=open, 1=close)
+            [2:5] left_wrist_pos (x,y,z in world frame)
+            [5:9] left_wrist_quat (w,x,y,z in world frame)
+            [9:12] right_wrist_pos (x,y,z in world frame)
+            [12:16] right_wrist_quat (w,x,y,z in world frame)
+            [16:19] navigate_cmd (x, y, angular_z)
+            [19:20] base_height
+            [20:23] torso_orientation_rpy
+        """
+        if self.cfg.transform_to_base_frame:
+            actions = self._transform_wrist_poses_to_base_frame(actions)
+
+        # Call parent class to do the actual processing
+        super().process_actions(actions)
+
+    def _transform_wrist_poses_to_base_frame(self, actions: torch.Tensor) -> torch.Tensor:
+        """Transform wrist positions and orientations from world frame to robot base frame.
+
+        The VR controllers give world-space positions, but the IK expects positions
+        relative to the robot's base. Without this, when the robot moves, the hands
+        will try to stay at their original world position and drift away from the body.
+
+        Args:
+            actions: Input actions with wrist poses in world frame.
+
+        Returns:
+            Actions with wrist poses transformed to robot base frame.
+        """
+        # Get robot's current base pose (position and orientation in world frame)
+        robot_base_pos = self._asset.data.root_link_pos_w[0, :3]  # Shape: (3,)
+        robot_base_quat = self._asset.data.root_link_quat_w[0]  # Shape: (4,) in (w,x,y,z) format
+
+        # Clone actions to avoid modifying the input
+        actions = actions.clone()
+
+        # Batch process both wrists together for efficiency
+        # Stack positions: shape (2, 3) for left and right
+        wrist_pos_world = torch.stack([actions[0, 2:5], actions[0, 9:12]], dim=0)
+
+        # Step 1: Translate to remove base position offset
+        wrist_pos_translated = wrist_pos_world - robot_base_pos
+
+        # Step 2: Rotate into robot's local frame (batch operation)
+        wrist_pos_base = quat_apply_inverse(robot_base_quat.unsqueeze(0), wrist_pos_translated)
+
+        # Transform orientations to robot base frame (batch operation)
+        # Stack quaternions: shape (2, 4) for left and right
+        wrist_quat_world = torch.stack([actions[0, 5:9], actions[0, 12:16]], dim=0)
+        robot_base_quat_inv = quat_inv(robot_base_quat.unsqueeze(0))  # Shape: (1, 4)
+        robot_base_quat_inv = robot_base_quat_inv.expand(2, -1)  # Expand to (2, 4) for batching
+        wrist_quat_base = quat_mul(robot_base_quat_inv, wrist_quat_world)
+
+        # Update action with base-relative poses
+        actions[0, 2:5] = wrist_pos_base[0]
+        actions[0, 5:9] = wrist_quat_base[0]
+        actions[0, 9:12] = wrist_pos_base[1]
+        actions[0, 12:16] = wrist_quat_base[1]
+
+        return actions

isaaclab_arena_g1/g1_env/mdp/actions/g1_decoupled_wbc_pink_world_frame_action_cfg.py

@@ -0,0 +1,29 @@
+# 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.managers.action_manager import ActionTerm
+from isaaclab.utils import configclass
+
+from .g1_decoupled_wbc_pink_action_cfg import G1DecoupledWBCPinkActionCfg
+from .g1_decoupled_wbc_pink_world_frame_action import G1DecoupledWBCPinkWorldFrameAction
+
+
+@configclass
+class G1DecoupledWBCPinkWorldFrameActionCfg(G1DecoupledWBCPinkActionCfg):
+    """Configuration for G1 WBC Pink action with world-to-base frame transformation.
+    
+    This configuration enables automatic transformation of wrist poses from world frame
+    to robot base frame, which is necessary for VR controller inputs (e.g., Quest).
+    """
+
+    class_type: type[ActionTerm] = G1DecoupledWBCPinkWorldFrameAction
+    """Specifies the action term class type for G1 WBC Pink with world frame transformation."""
+
+    transform_to_base_frame: bool = True
+    """Whether to transform wrist poses from world frame to robot base frame.
+    
+    Set this to True when using VR controllers or other devices that output world-space poses.
+    Set to False if the input poses are already in the robot's base frame.
+    """