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>
This commit is contained in:
2026-03-25 11:32:28 +08:00
parent 05a146bca6
commit bfe28b188a
12 changed files with 82 additions and 882 deletions

View File

@@ -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
"""Sim-to-VR H264 stereo streaming manager."""
@@ -46,6 +46,8 @@ class StreamingManager:
def enabled(self) -> bool:
return self._enabled
_debug_saved = False
def send(self, scene, frame_count: int = 0) -> None:
"""Send one stereo frame from scene cameras. Auto-disables on failure."""
if not self._enabled:
@@ -53,6 +55,21 @@ class StreamingManager:
try:
left_rgb = scene["vr_left_eye"].data.output["rgb"][0].cpu().numpy()
right_rgb = scene["vr_right_eye"].data.output["rgb"][0].cpu().numpy()
# Save debug images once (frame 5 to ensure rendering is stable)
if frame_count == 150 and not self._debug_saved:
self._debug_saved = True
try:
from PIL import Image
import numpy as np
Image.fromarray(left_rgb[..., :3]).save("/tmp/vr_left_debug.png")
Image.fromarray(right_rgb[..., :3]).save("/tmp/vr_right_debug.png")
sbs = np.concatenate([left_rgb, right_rgb], axis=1)
Image.fromarray(sbs[..., :3]).save("/tmp/vr_sbs_debug.png")
print(f"[STREAM DIAG] Saved debug frames: left={left_rgb.shape}, right={right_rgb.shape}")
except Exception as e:
print(f"[STREAM DIAG] Save failed: {e}")
if not self._streamer.send_frame(left_rgb, right_rgb):
logger.warning("[Stream] Connection lost. Disabling streaming.")
self._enabled = False