Fix XR teleop: body-frame IK control for mobile chassis

Switch arm IK from world-frame to body-frame control so that
pushing the XR controller forward always moves the arm in the
robot's forward direction, regardless of chassis rotation.

Key changes:
- dual_arm_agent: convert EEF observations to body frame before
  passing to XR controller; send body-frame IK targets directly
  (removed convert_action_world_to_root)
- xr_controller: XR deltas treated as body-frame deltas (no yaw
  rotation needed — VR view tracks robot heading naturally)
- streaming: add debug frame save for stereo alignment diagnostics
- mindrobot_2i_cfg: IdealPDActuator for trunk, disabled gravity
- Author headers updated to Yutang Li, SIAT

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>

scripts/environments/teleoperation/xr_teleop/dual_arm_agent.py

@@ -1,4 +1,4 @@
-# SPDX-FileCopyrightText: Copyright (c) 2022-2025, The Isaac Lab Project Developers.
+# Copyright (c) 2025, Yutang Li, SIAT (yt.li2@siat.ac.cn)
 # SPDX-License-Identifier: BSD-3-Clause
 
 """Dual-arm XR teleoperation agent with chassis control."""
@@ -67,24 +67,48 @@ class DualArmXrAgent(BaseTeleopAgent):
         policy_obs = obs["policy"]
         robot = self.env.unwrapped.scene["robot"]
 
+        # Get robot root pose for world→body EEF conversion
+        root_pos_w = robot.data.root_pos_w[0].cpu().numpy()
+        root_quat_w = robot.data.root_quat_w[0].cpu().numpy()
+
         # Read chassis
         wheel_cmd, self._v_fwd, self._omega = self.chassis.get_commands(self.shared_client)
 
-        # Left arm
-        eef_pos_left = policy_obs["eef_pos_left"][0].cpu().numpy()
-        eef_quat_left = policy_obs["eef_quat_left"][0].cpu().numpy()
-        left_action = self.teleop_left.advance(current_eef_pos=eef_pos_left, current_eef_quat=eef_quat_left)
+        # Convert EEF observations from world frame to body frame
+        from .frame_utils import world_pose_to_root_frame
+        eef_pos_left_w = policy_obs["eef_pos_left"][0].cpu().numpy()
+        eef_quat_left_w = policy_obs["eef_quat_left"][0].cpu().numpy()
+        eef_pos_left_b, eef_quat_left_b = world_pose_to_root_frame(
+            eef_pos_left_w, eef_quat_left_w, root_pos_w, root_quat_w)
 
-        # Right arm
-        eef_pos_right = policy_obs["eef_pos_right"][0].cpu().numpy()
-        eef_quat_right = policy_obs["eef_quat_right"][0].cpu().numpy()
-        right_action = self.teleop_right.advance(current_eef_pos=eef_pos_right, current_eef_quat=eef_quat_right)
+        eef_pos_right_w = policy_obs["eef_pos_right"][0].cpu().numpy()
+        eef_quat_right_w = policy_obs["eef_quat_right"][0].cpu().numpy()
+        eef_pos_right_b, eef_quat_right_b = world_pose_to_root_frame(
+            eef_pos_right_w, eef_quat_right_w, root_pos_w, root_quat_w)
 
-        # Joint-locking: only convert when grip active
+        # XR controller works in body frame:
+        # - current_eef is body frame (converted above)
+        # - XR delta is treated as body-frame delta directly (no rotation)
+        #   because in VR teleop the user's visual frame tracks the robot heading
+        # - output is body frame → send directly to IK
+        left_action = self.teleop_left.advance(current_eef_pos=eef_pos_left_b, current_eef_quat=eef_quat_left_b)
+        right_action = self.teleop_right.advance(current_eef_pos=eef_pos_right_b, current_eef_quat=eef_quat_right_b)
+
+        # Joint-locking: target is already in body frame, use directly
         if self.teleop_left.grip_active or self._last_root_left is None:
-            self._last_root_left = convert_action_world_to_root(left_action, robot)[:7].clone()
+            self._last_root_left = left_action[:7].clone()
         if self.teleop_right.grip_active or self._last_root_right is None:
-            self._last_root_right = convert_action_world_to_root(right_action, robot)[:7].clone()
+            self._last_root_right = right_action[:7].clone()
+
+        # Diagnostic: print once per second when left grip active
+        _cnt = getattr(self, '_diag_cnt', 0) + 1
+        self._diag_cnt = _cnt
+        if self.teleop_left.grip_active and _cnt % 30 == 0:
+            import math
+            print(f"[FRAME DIAG] root_quat_w={root_quat_w}")
+            print(f"  eef_left_world=({eef_pos_left_w[0]:.3f},{eef_pos_left_w[1]:.3f},{eef_pos_left_w[2]:.3f})")
+            print(f"  eef_left_body =({eef_pos_left_b[0]:.3f},{eef_pos_left_b[1]:.3f},{eef_pos_left_b[2]:.3f})")
+            print(f"  ik_cmd_body   =({self._last_root_left[0]:.3f},{self._last_root_left[1]:.3f},{self._last_root_left[2]:.3f})")
 
         # left_arm(7) | wheel(4) | left_grip(1) | right_arm(7) | right_grip(1)
         return torch.cat([