Add torso retargetter and remove padding hack

Author: qianl-nv

isaaclab_arena/assets/retargeter_library.py

@@ -17,17 +17,47 @@
 # limitations under the License.
 
 from abc import ABC, abstractmethod
+from dataclasses import dataclass
+from typing import Any
+
+import torch
 
 from isaaclab.devices.openxr.retargeters import (
     G1LowerBodyStandingMotionControllerRetargeterCfg,
     G1TriHandUpperBodyMotionControllerGripperRetargeterCfg,
     GR1T2RetargeterCfg,
 )
-from isaaclab.devices.retargeter_base import RetargeterCfg
+from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg
 
 from isaaclab_arena.assets.register import register_retargeter
 
 
+class DummyTorsoRetargeter(RetargeterBase):
+    """Dummy retargeter that returns zero torso orientation commands.
+    
+    This is used to pad the action space for G1 WBC Pink with motion controllers,
+    which don't provide torso orientation commands.
+    """
+
+    def __init__(self, cfg: "DummyTorsoRetargeterCfg"):
+        super().__init__(cfg)
+
+    def retarget(self, data: Any) -> torch.Tensor:
+        """Return zeros for torso orientation (roll, pitch, yaw)."""
+        return torch.zeros(3, device=self._sim_device)
+
+    def get_requirements(self) -> list[RetargeterBase.Requirement]:
+        """This retargeter doesn't require any device data."""
+        return []
+
+
+@dataclass
+class DummyTorsoRetargeterCfg(RetargeterCfg):
+    """Configuration for dummy torso retargeter."""
+
+    retargeter_type: type[RetargeterBase] = DummyTorsoRetargeter
+
+
 class RetargetterBase(ABC):
     device: str
     embodiment: str
@@ -125,9 +155,14 @@ def get_retargeter_cfg(
     ) -> list[RetargeterCfg]:
         """Get motion controller retargeter configuration for G1.
         
-        Returns a list of retargeters for upper body (with gripper) and lower body.
+        Returns a list of retargeters:
+        - Upper body (with gripper): outputs 16 dims [gripper(2), left_wrist(7), right_wrist(7)]
+        - Lower body: outputs 4 dims [nav_cmd(3), hip_height(1)]
+        - Dummy torso: outputs 3 dims [torso_orientation_rpy(3)] all zeros
+        Total: 23 dims to match g1_wbc_pink action space
         """
         return [
             G1TriHandUpperBodyMotionControllerGripperRetargeterCfg(sim_device=sim_device),
             G1LowerBodyStandingMotionControllerRetargeterCfg(sim_device=sim_device),
+            DummyTorsoRetargeterCfg(sim_device=sim_device),
         ]

isaaclab_arena/scripts/imitation_learning/teleop.py

@@ -238,13 +238,6 @@ def stop_teleoperation() -> None:
 
                 # Only apply teleop commands when active
                 if teleoperation_active:
-                    # Pad action if teleop device outputs fewer dims than environment expects
-                    # actions[:16] are EEF-related, actions[16:] are WBC-related (may need padding)
-                    expected_dim = env.action_space.shape[-1]
-                    if action.shape[0] < expected_dim:
-                        padding = torch.zeros(expected_dim - action.shape[0], device=action.device, dtype=action.dtype)
-                        action = torch.cat([action, padding])
-
                     # process actions
                     actions = action.repeat(env.num_envs, 1)
                     # apply actions