hangX/mindbot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
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:
yt lee
2026-03-25 11:32:28 +08:00
parent 05a146bca6
commit bfe28b188a
12 changed files with 82 additions and 882 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
scripts/environments/teleoperation/xr_teleop/base_agent.py
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
"""Base teleoperation agent with main loop, env management, and reset handling."""

2
scripts/environments/teleoperation/xr_teleop/chassis.py
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
"""Chassis (mobile base) control via XR joystick."""

2
scripts/environments/teleoperation/xr_teleop/diagnostics.py
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
"""Periodic diagnostics reporter for teleoperation debugging."""

48
scripts/environments/teleoperation/xr_teleop/dual_arm_agent.py
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
"""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([

2
scripts/environments/teleoperation/xr_teleop/dual_arm_head_agent.py
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
"""Dual-arm + head tracking + trunk + stereo streaming XR agent."""

2
scripts/environments/teleoperation/xr_teleop/frame_utils.py
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
"""Coordinate frame utilities for XR teleoperation."""

2
scripts/environments/teleoperation/xr_teleop/head_tracker.py
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
"""Head tracking via XR headset (HMD) for robot head joint control."""

19
scripts/environments/teleoperation/xr_teleop/streaming.py
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

17
scripts/environments/teleoperation/xr_teleop/xr_controller.py
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
"""XR teleoperation controller with absolute IK target accumulation and grip clutch."""
@@ -50,6 +50,7 @@ class XrTeleopController:
self.grip_engage_threshold = 0.8
self.grip_release_threshold = 0.2
self.callbacks = {}
self._root_yaw_rad = 0.0 # current chassis heading, updated each frame
def add_callback(self, name: str, func: Callable):
self.callbacks[name] = func
@@ -63,6 +64,10 @@ class XrTeleopController:
self.target_eef_quat = None
self.require_grip_reengage = True
def set_root_yaw(self, yaw_rad: float):
"""Update the chassis heading so XR deltas are in body frame."""
self._root_yaw_rad = yaw_rad
def close(self):
if self._owns_client:
self.xr_client.close()
@@ -143,6 +148,16 @@ class XrTeleopController:
prev_xr_world_pos, prev_xr_world_quat_xyzw = transform_xr_pose(self.prev_xr_pos, self.prev_xr_quat)
world_delta_pos = (xr_world_pos - prev_xr_world_pos) * self.pos_sensitivity
# In VR teleop, user "inhabits" the robot: their physical forward (XR +X)
# maps directly to robot forward (body +X). No rotation needed — the VR view
# already rotates with the robot, so XR deltas naturally align with body frame.
# Diagnostic
if self.frame_count % 30 == 0 and self.target_eef_pos is not None:
print(f"[XR {self.arm}] delta=({world_delta_pos[0]:+.4f},{world_delta_pos[1]:+.4f},{world_delta_pos[2]:+.4f}) "
f"target=({self.target_eef_pos[0]:.3f},{self.target_eef_pos[1]:.3f},{self.target_eef_pos[2]:.3f})")
pos_norm = np.linalg.norm(world_delta_pos)
if pos_norm > self.max_pos_per_step:
world_delta_pos *= self.max_pos_per_step / pos_norm