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:
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user