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

重构xr teleop

Browse Source
This commit is contained in:
yt lee
2026-03-23 22:06:13 +08:00
parent d756e3ae0b
commit 05a146bca6
23 changed files with 3195 additions and 761 deletions
Download Patch File Download Diff File Expand all files Collapse all files

91
scripts/environments/teleoperation/xr_teleop/diagnostics.py Normal file
View File

@@ -0,0 +1,91 @@
# SPDX-FileCopyrightText: Copyright (c) 2022-2025, The Isaac Lab Project Developers.
# SPDX-License-Identifier: BSD-3-Clause
"""Periodic diagnostics reporter for teleoperation debugging."""
import numpy as np
class DiagnosticsReporter:
"""Prints robot state info every N frames during teleoperation."""
def __init__(self, interval: int = 30, is_dual_arm: bool = False):
self.interval = interval
self.is_dual_arm = is_dual_arm
self._left_arm_idx: list[int] | None = None
self._right_arm_idx: list[int] | None = None
self._joint_names_printed = False
def _ensure_indices(self, robot):
if self._left_arm_idx is None:
jnames = robot.joint_names
self._left_arm_idx = [i for i, n in enumerate(jnames) if n.startswith("l_joint")]
self._right_arm_idx = [i for i, n in enumerate(jnames) if n.startswith("r_joint")]
def report(self, env, obs, frame: int, last_act: np.ndarray | None = None,
xr_client=None, read_gripper_contact=None):
"""Print diagnostics if frame is on interval boundary."""
if frame % self.interval != 0:
return
np.set_printoptions(precision=4, suppress=True, floatmode='fixed')
policy_obs = obs["policy"]
joint_pos = policy_obs["joint_pos"][0].cpu().numpy()
if last_act is None:
last_act = policy_obs["actions"][0].cpu().numpy()
robot = env.unwrapped.scene["robot"]
self._ensure_indices(robot)
# Print full joint table on first report
if not self._joint_names_printed:
jnames = robot.joint_names
print(f"\n{'=' * 70}")
print(f" ALL {len(jnames)} JOINT NAMES AND POSITIONS")
print(f"{'=' * 70}")
for i, name in enumerate(jnames):
print(f" [{i:2d}] {name:30s} = {joint_pos[i]:+.4f}")
print(f" Deduced left arm indices: {self._left_arm_idx}")
print(f"{'=' * 70}\n")
self._joint_names_printed = True
arm_joints = joint_pos[self._left_arm_idx]
right_arm_joints = joint_pos[self._right_arm_idx]
joy_str = "N/A"
if xr_client is not None:
try:
joy_dbg = xr_client.get_joystick("left")
joy_str = f"[{joy_dbg[0]:+.2f}, {joy_dbg[1]:+.2f}]"
except Exception:
pass
if self.is_dual_arm:
eef_pos_left = policy_obs["eef_pos_left"][0].cpu().numpy()
eef_pos_right = policy_obs["eef_pos_right"][0].cpu().numpy()
contact_str = read_gripper_contact() if read_gripper_contact else "N/A"
print(f"\n---------------- [ROBOT STATE frame={frame}] ----------------")
print(f"| Left Arm Joints (rad): {arm_joints}")
print(f"| Right Arm Joints (rad): {right_arm_joints}")
print(f"| Left EEF Pos (world, m): {eef_pos_left}")
print(f"| Right EEF Pos (world, m): {eef_pos_right}")
print(f"| Left Gripper Contact: {contact_str}")
print(f"| Cmd left_arm(abs): {last_act[:7]}")
print(f"| Cmd wheel vel (rad/s): {last_act[7:11]}")
print(f"| Cmd left_gripper: {last_act[11:12]} (+1=open -1=close)")
print(f"| Cmd right_arm(abs): {last_act[12:19]}")
print(f"| Cmd right_gripper: {last_act[19:]} (+1=open -1=close)")
print(f"| Joystick (x=turn,y=fwd): {joy_str}")
print(f"----------------------------------------------------------------")
else:
eef_pos = policy_obs["eef_pos"][0].cpu().numpy()
print(f"\n---------------- [ROBOT STATE frame={frame}] ----------------")
print(f"| Left Arm Joints (rad): {arm_joints}")
print(f"| EEF Pos (world, m): {eef_pos}")
print(f"| Cmd arm(abs pos+quat): {last_act[:7]}")
print(f"| Cmd wheel vel (rad/s): {last_act[7:11]}")
print(f"| Cmd gripper: {last_act[11:]} (+1=open -1=close)")
print(f"| Joystick (x=turn,y=fwd): {joy_str}")
print(f"----------------------------------------------------------------")