|
|
|
|
@@ -1,856 +0,0 @@
|
|
|
|
|
#!/usr/bin/env python3
|
|
|
|
|
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
|
|
|
|
|
# All rights reserved.
|
|
|
|
|
#
|
|
|
|
|
# SPDX-License-Identifier: BSD-3-Clause
|
|
|
|
|
|
|
|
|
|
"""
|
|
|
|
|
Script to run teleoperation with Isaac Lab manipulation environments using PICO XR Controllers.
|
|
|
|
|
|
|
|
|
|
Only absolute IK mode is supported (Isaac-MindRobot-LeftArm-IK-Abs-v0).
|
|
|
|
|
The controller computes delta pose in Isaac Sim world frame and accumulates
|
|
|
|
|
an absolute EEF target, which is then converted to robot root frame before
|
|
|
|
|
being sent to the DifferentialIK action manager.
|
|
|
|
|
"""
|
|
|
|
|
|
|
|
|
|
import argparse
|
|
|
|
|
import logging
|
|
|
|
|
import sys
|
|
|
|
|
import os
|
|
|
|
|
from collections.abc import Callable
|
|
|
|
|
|
|
|
|
|
# Ensure xr_utils (next to this script) is importable when running directly
|
|
|
|
|
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
|
|
|
|
|
|
|
|
|
|
from isaaclab.app import AppLauncher
|
|
|
|
|
|
|
|
|
|
logger = logging.getLogger(__name__)
|
|
|
|
|
|
|
|
|
|
# add argparse arguments
|
|
|
|
|
parser = argparse.ArgumentParser(
|
|
|
|
|
description="Teleoperation for Isaac Lab environments with PICO XR Controller (absolute IK mode only)."
|
|
|
|
|
)
|
|
|
|
|
parser.add_argument("--num_envs", type=int, default=1, help="Number of environments to simulate.")
|
|
|
|
|
parser.add_argument(
|
|
|
|
|
"--task",
|
|
|
|
|
type=str,
|
|
|
|
|
default="Isaac-MindRobot-LeftArm-IK-Abs-v0",
|
|
|
|
|
help="Name of the task (must be an Abs IK task).",
|
|
|
|
|
)
|
|
|
|
|
parser.add_argument(
|
|
|
|
|
"--sensitivity", type=float, default=None,
|
|
|
|
|
help="Set both pos and rot sensitivity (overridden by --pos_sensitivity/--rot_sensitivity).",
|
|
|
|
|
)
|
|
|
|
|
parser.add_argument("--pos_sensitivity", type=float, default=None, help="Position sensitivity. Default: 1.0.")
|
|
|
|
|
parser.add_argument("--rot_sensitivity", type=float, default=None, help="Rotation sensitivity. Default: 0.3.")
|
|
|
|
|
parser.add_argument("--arm", type=str, default="left", choices=["left", "right"], help="Which arm/controller to use.")
|
|
|
|
|
parser.add_argument("--base_speed", type=float, default=5.0, help="Max wheel speed (rad/s) for joystick full forward.")
|
|
|
|
|
parser.add_argument("--base_turn", type=float, default=2.0, help="Max wheel differential (rad/s) for joystick full left/right.")
|
|
|
|
|
parser.add_argument("--drive_speed", type=float, default=0.5, help="Max robot linear speed (m/s) for direct base control. Default: 0.5")
|
|
|
|
|
parser.add_argument("--drive_turn", type=float, default=1.5, help="Max robot yaw rate (rad/s) for direct base control. Default: 1.5")
|
|
|
|
|
parser.add_argument(
|
|
|
|
|
"--stream-to", type=str, default=None, dest="stream_to",
|
|
|
|
|
help="PICO VR headset IP for sim stereo streaming. Streams H264 SBS to the Unity client TCP server. "
|
|
|
|
|
"Requires vr_left_eye/vr_right_eye cameras in the env scene config.",
|
|
|
|
|
)
|
|
|
|
|
parser.add_argument("--stream-port", type=int, default=12345, dest="stream_port", help="TCP port for video streaming.")
|
|
|
|
|
parser.add_argument("--stream-bitrate", type=int, default=20_000_000, dest="stream_bitrate", help="H264 bitrate (bps).")
|
|
|
|
|
parser.add_argument(
|
|
|
|
|
"--debug-viewports", action="store_true", dest="debug_viewports", default=False,
|
|
|
|
|
help="Enable debug viewport windows (Left Hand, Right Hand, Head, Chest). Off by default when --stream-to is used.",
|
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
AppLauncher.add_app_launcher_args(parser)
|
|
|
|
|
args_cli = parser.parse_args()
|
|
|
|
|
app_launcher_args = vars(args_cli)
|
|
|
|
|
app_launcher_args["xr"] = False
|
|
|
|
|
|
|
|
|
|
app_launcher = AppLauncher(app_launcher_args)
|
|
|
|
|
simulation_app = app_launcher.app
|
|
|
|
|
|
|
|
|
|
"""Rest everything follows."""
|
|
|
|
|
|
|
|
|
|
import gymnasium as gym
|
|
|
|
|
import numpy as np
|
|
|
|
|
import torch
|
|
|
|
|
from scipy.spatial.transform import Rotation as R
|
|
|
|
|
|
|
|
|
|
from isaaclab.envs import ManagerBasedRLEnvCfg
|
|
|
|
|
|
|
|
|
|
import isaaclab_tasks # noqa: F401
|
|
|
|
|
import mindbot.tasks # noqa: F401
|
|
|
|
|
from isaaclab_tasks.utils import parse_env_cfg
|
|
|
|
|
|
|
|
|
|
from xr_utils import XrClient, transform_xr_pose, quat_diff_as_rotvec_xyzw, is_valid_quaternion
|
|
|
|
|
from xr_utils.geometry import R_HEADSET_TO_WORLD
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# =====================================================================
|
|
|
|
|
# Robot Camera Viewport Utilities
|
|
|
|
|
# =====================================================================
|
|
|
|
|
|
|
|
|
|
# Resolved prim paths for env_0 — must match CameraCfg prim_path with
|
|
|
|
|
# {ENV_REGEX_NS} → /World/envs/env_0
|
|
|
|
|
_ROBOT_CAM_PRIMS: dict[str, str] = {
|
|
|
|
|
"Left Hand": "/World/envs/env_0/Robot/rm_65_fb_left/Link_6/robot___left_8_02/cam_left_hand",
|
|
|
|
|
"Right Hand": "/World/envs/env_0/Robot/rm_65_b_right/Link6/robot___right_8_2/cam_right_hand",
|
|
|
|
|
"Head": "/World/envs/env_0/Robot/robot_head/ZED_X/base_link/ZED_X/CameraLeft",
|
|
|
|
|
"Chest": "/World/envs/env_0/Robot/robot_trunk/cam_chest",
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
# Stores created viewport window objects
|
|
|
|
|
_robot_viewports: dict[str, object] = {}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def create_robot_viewports() -> None:
|
|
|
|
|
"""Create 4 viewport windows and bind each to a robot camera prim.
|
|
|
|
|
|
|
|
|
|
Must be called after env.reset() so all prims exist on the USD stage.
|
|
|
|
|
"""
|
|
|
|
|
try:
|
|
|
|
|
import omni.kit.viewport.utility as vp_util
|
|
|
|
|
except ImportError:
|
|
|
|
|
logger.warning("[Viewport] omni.kit.viewport.utility not available — skipping viewport creation.")
|
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
for name, cam_path in _ROBOT_CAM_PRIMS.items():
|
|
|
|
|
vp_win = vp_util.create_viewport_window(
|
|
|
|
|
f"Robot {name} View", width=640, height=360
|
|
|
|
|
)
|
|
|
|
|
vp_win.viewport_api.camera_path = cam_path
|
|
|
|
|
_robot_viewports[name] = vp_win
|
|
|
|
|
print(f"[INFO] Viewport 'Robot {name} View' bound to: {cam_path}")
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def rebind_robot_viewports() -> None:
|
|
|
|
|
"""Re-bind all robot viewports to their camera paths.
|
|
|
|
|
|
|
|
|
|
Call this after every env reset so viewports stay locked to the cameras.
|
|
|
|
|
"""
|
|
|
|
|
for name, vp_win in _robot_viewports.items():
|
|
|
|
|
cam_path = _ROBOT_CAM_PRIMS[name]
|
|
|
|
|
vp_win.viewport_api.camera_path = cam_path
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# =====================================================================
|
|
|
|
|
# Helpers
|
|
|
|
|
# =====================================================================
|
|
|
|
|
|
|
|
|
|
def _quat_wxyz_to_rotation(quat_wxyz: np.ndarray) -> R:
|
|
|
|
|
"""Convert Isaac-style wxyz quaternion to scipy Rotation."""
|
|
|
|
|
return R.from_quat([quat_wxyz[1], quat_wxyz[2], quat_wxyz[3], quat_wxyz[0]])
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def _rotation_to_quat_wxyz(rot: R) -> np.ndarray:
|
|
|
|
|
"""Convert scipy Rotation to Isaac-style wxyz quaternion."""
|
|
|
|
|
q = rot.as_quat()
|
|
|
|
|
return np.array([q[3], q[0], q[1], q[2]])
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def world_pose_to_root_frame(
|
|
|
|
|
pos_w: np.ndarray,
|
|
|
|
|
quat_wxyz: np.ndarray,
|
|
|
|
|
root_pos_w: np.ndarray,
|
|
|
|
|
root_quat_wxyz: np.ndarray,
|
|
|
|
|
) -> tuple[np.ndarray, np.ndarray]:
|
|
|
|
|
"""Express a world-frame pose in the robot root frame."""
|
|
|
|
|
root_rot = _quat_wxyz_to_rotation(root_quat_wxyz)
|
|
|
|
|
pose_rot = _quat_wxyz_to_rotation(quat_wxyz)
|
|
|
|
|
pos_root = root_rot.inv().apply(pos_w - root_pos_w)
|
|
|
|
|
quat_root = _rotation_to_quat_wxyz(root_rot.inv() * pose_rot)
|
|
|
|
|
return pos_root, quat_root
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# =====================================================================
|
|
|
|
|
# Teleoperation Controller
|
|
|
|
|
# =====================================================================
|
|
|
|
|
|
|
|
|
|
class XrTeleopController:
|
|
|
|
|
"""Teleop controller for PICO XR headset (absolute IK mode).
|
|
|
|
|
|
|
|
|
|
Accumulates an absolute EEF target in Isaac Sim world frame.
|
|
|
|
|
Grip button acts as clutch; B/Y buttons trigger environment reset.
|
|
|
|
|
"""
|
|
|
|
|
|
|
|
|
|
def __init__(self, arm="left", pos_sensitivity=1.0, rot_sensitivity=0.3,
|
|
|
|
|
max_pos_per_step=0.05, max_rot_per_step=0.08, xr_client=None):
|
|
|
|
|
self.xr_client = xr_client if xr_client is not None else XrClient()
|
|
|
|
|
self._owns_client = xr_client is None # only close if we created it
|
|
|
|
|
self.pos_sensitivity = pos_sensitivity
|
|
|
|
|
self.rot_sensitivity = rot_sensitivity
|
|
|
|
|
# Hard caps per physics step to prevent IK divergence.
|
|
|
|
|
# max_rot_per_step ~4.6°, max_pos_per_step 5 cm.
|
|
|
|
|
self.max_pos_per_step = max_pos_per_step
|
|
|
|
|
self.max_rot_per_step = max_rot_per_step
|
|
|
|
|
self.arm = arm
|
|
|
|
|
|
|
|
|
|
self.controller_name = "left_controller" if arm == "left" else "right_controller"
|
|
|
|
|
self.grip_name = "left_grip" if arm == "left" else "right_grip"
|
|
|
|
|
self.trigger_name = "left_trigger" if arm == "left" else "right_trigger"
|
|
|
|
|
|
|
|
|
|
self.R_headset_world = R_HEADSET_TO_WORLD
|
|
|
|
|
|
|
|
|
|
self.prev_xr_pos = None
|
|
|
|
|
self.prev_xr_quat = None
|
|
|
|
|
self.target_eef_pos = None # world frame
|
|
|
|
|
self.target_eef_quat = None # world frame, wxyz
|
|
|
|
|
|
|
|
|
|
self.grip_active = False
|
|
|
|
|
self.frame_count = 0
|
|
|
|
|
self.reset_button_latched = False
|
|
|
|
|
self.require_grip_reengage = False
|
|
|
|
|
self.grip_engage_threshold = 0.8
|
|
|
|
|
self.grip_release_threshold = 0.2
|
|
|
|
|
self.callbacks = {}
|
|
|
|
|
|
|
|
|
|
def add_callback(self, name: str, func: Callable):
|
|
|
|
|
self.callbacks[name] = func
|
|
|
|
|
|
|
|
|
|
def reset(self):
|
|
|
|
|
self.prev_xr_pos = None
|
|
|
|
|
self.prev_xr_quat = None
|
|
|
|
|
self.grip_active = False
|
|
|
|
|
self.frame_count = 0
|
|
|
|
|
self.target_eef_pos = None
|
|
|
|
|
self.target_eef_quat = None
|
|
|
|
|
# Require grip release after reset to avoid driving to stale pose.
|
|
|
|
|
self.require_grip_reengage = True
|
|
|
|
|
|
|
|
|
|
def close(self):
|
|
|
|
|
if self._owns_client:
|
|
|
|
|
self.xr_client.close()
|
|
|
|
|
|
|
|
|
|
def get_zero_action(self, trigger, current_eef_pos=None, current_eef_quat=None) -> torch.Tensor:
|
|
|
|
|
"""Return 8D hold action [x, y, z, qw, qx, qy, qz, gripper] at frozen target pose."""
|
|
|
|
|
action = torch.zeros(8)
|
|
|
|
|
if self.target_eef_pos is not None and self.target_eef_quat is not None:
|
|
|
|
|
# Prefer frozen world-frame target so IK holds a fixed world position
|
|
|
|
|
# even if the robot chassis drifts slightly under gravity.
|
|
|
|
|
action[:3] = torch.tensor(self.target_eef_pos.copy())
|
|
|
|
|
action[3:7] = torch.tensor(self.target_eef_quat.copy())
|
|
|
|
|
elif current_eef_pos is not None and current_eef_quat is not None:
|
|
|
|
|
action[:3] = torch.tensor(current_eef_pos.copy())
|
|
|
|
|
action[3:7] = torch.tensor(current_eef_quat.copy())
|
|
|
|
|
else:
|
|
|
|
|
action[3] = 1.0 # identity quaternion
|
|
|
|
|
action[7] = 1.0 if trigger > 0.5 else -1.0
|
|
|
|
|
return action
|
|
|
|
|
|
|
|
|
|
def advance(self, current_eef_pos=None, current_eef_quat=None) -> torch.Tensor:
|
|
|
|
|
"""Read XR controller and return 8D absolute action [x, y, z, qw, qx, qy, qz, gripper].
|
|
|
|
|
|
|
|
|
|
Position and quaternion are in Isaac Sim world frame.
|
|
|
|
|
Caller must convert to robot root frame before sending to IK.
|
|
|
|
|
"""
|
|
|
|
|
try:
|
|
|
|
|
# Only left controller Y button resets (not right controller B)
|
|
|
|
|
if self.arm == "left":
|
|
|
|
|
reset_pressed = self.xr_client.get_button("Y")
|
|
|
|
|
else:
|
|
|
|
|
reset_pressed = False
|
|
|
|
|
if reset_pressed and not self.reset_button_latched:
|
|
|
|
|
if "RESET" in self.callbacks:
|
|
|
|
|
self.callbacks["RESET"]()
|
|
|
|
|
self.reset_button_latched = reset_pressed
|
|
|
|
|
except Exception:
|
|
|
|
|
pass
|
|
|
|
|
|
|
|
|
|
try:
|
|
|
|
|
raw_pose = self.xr_client.get_pose(self.controller_name)
|
|
|
|
|
grip = self.xr_client.get_key_value(self.grip_name)
|
|
|
|
|
trigger = self.xr_client.get_key_value(self.trigger_name)
|
|
|
|
|
except Exception:
|
|
|
|
|
return self.get_zero_action(0.0, current_eef_pos, current_eef_quat)
|
|
|
|
|
|
|
|
|
|
if not is_valid_quaternion(raw_pose[3:]):
|
|
|
|
|
return self.get_zero_action(trigger, current_eef_pos, current_eef_quat)
|
|
|
|
|
|
|
|
|
|
# Wait for grip release after reset
|
|
|
|
|
if self.require_grip_reengage:
|
|
|
|
|
if grip <= self.grip_release_threshold:
|
|
|
|
|
self.require_grip_reengage = False
|
|
|
|
|
else:
|
|
|
|
|
if current_eef_pos is not None and current_eef_quat is not None:
|
|
|
|
|
self.target_eef_pos = current_eef_pos.copy()
|
|
|
|
|
self.target_eef_quat = current_eef_quat.copy()
|
|
|
|
|
return self.get_zero_action(trigger, current_eef_pos, current_eef_quat)
|
|
|
|
|
|
|
|
|
|
# Grip button as clutch with hysteresis
|
|
|
|
|
grip_pressed = grip > self.grip_release_threshold if self.grip_active else grip >= self.grip_engage_threshold
|
|
|
|
|
|
|
|
|
|
if not grip_pressed:
|
|
|
|
|
self.prev_xr_pos = None
|
|
|
|
|
self.prev_xr_quat = None
|
|
|
|
|
# Only snapshot target on the transition frame (grip just released) or first ever frame.
|
|
|
|
|
# After that, keep it frozen so IK holds a fixed world-frame position.
|
|
|
|
|
if self.grip_active or self.target_eef_pos is None:
|
|
|
|
|
if current_eef_pos is not None and current_eef_quat is not None:
|
|
|
|
|
self.target_eef_pos = current_eef_pos.copy()
|
|
|
|
|
self.target_eef_quat = current_eef_quat.copy()
|
|
|
|
|
self.grip_active = False
|
|
|
|
|
return self.get_zero_action(trigger, current_eef_pos, current_eef_quat)
|
|
|
|
|
|
|
|
|
|
if not self.grip_active:
|
|
|
|
|
self.prev_xr_pos = raw_pose[:3].copy()
|
|
|
|
|
self.prev_xr_quat = raw_pose[3:].copy()
|
|
|
|
|
if current_eef_pos is not None and current_eef_quat is not None:
|
|
|
|
|
self.target_eef_pos = current_eef_pos.copy()
|
|
|
|
|
self.target_eef_quat = current_eef_quat.copy()
|
|
|
|
|
self.grip_active = True
|
|
|
|
|
return self.get_zero_action(trigger, current_eef_pos, current_eef_quat)
|
|
|
|
|
|
|
|
|
|
# Project current and previous XR poses into Isaac Sim world frame
|
|
|
|
|
xr_world_pos, xr_world_quat_xyzw = transform_xr_pose(raw_pose[:3], raw_pose[3:])
|
|
|
|
|
prev_xr_world_pos, prev_xr_world_quat_xyzw = transform_xr_pose(self.prev_xr_pos, self.prev_xr_quat)
|
|
|
|
|
|
|
|
|
|
# Delta position (world frame), clamped per step
|
|
|
|
|
world_delta_pos = (xr_world_pos - prev_xr_world_pos) * self.pos_sensitivity
|
|
|
|
|
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
|
|
|
|
|
|
|
|
|
|
# Delta rotation (world frame), clamped per step
|
|
|
|
|
world_delta_rot = quat_diff_as_rotvec_xyzw(prev_xr_world_quat_xyzw, xr_world_quat_xyzw) * self.rot_sensitivity
|
|
|
|
|
rot_norm = np.linalg.norm(world_delta_rot)
|
|
|
|
|
if rot_norm > self.max_rot_per_step:
|
|
|
|
|
world_delta_rot *= self.max_rot_per_step / rot_norm
|
|
|
|
|
|
|
|
|
|
gripper_action = 1.0 if trigger > 0.5 else -1.0
|
|
|
|
|
|
|
|
|
|
# Accumulate absolute target
|
|
|
|
|
if self.target_eef_pos is None:
|
|
|
|
|
self.target_eef_pos = np.zeros(3)
|
|
|
|
|
self.target_eef_quat = np.array([1.0, 0.0, 0.0, 0.0])
|
|
|
|
|
|
|
|
|
|
self.target_eef_pos += world_delta_pos
|
|
|
|
|
|
|
|
|
|
# Apply rotation delta in world frame so controller direction = EEF world direction
|
|
|
|
|
target_r = _quat_wxyz_to_rotation(self.target_eef_quat)
|
|
|
|
|
self.target_eef_quat = _rotation_to_quat_wxyz(R.from_rotvec(world_delta_rot) * target_r)
|
|
|
|
|
|
|
|
|
|
self.prev_xr_pos = raw_pose[:3].copy()
|
|
|
|
|
self.prev_xr_quat = raw_pose[3:].copy()
|
|
|
|
|
self.frame_count += 1
|
|
|
|
|
|
|
|
|
|
action = torch.tensor([
|
|
|
|
|
*self.target_eef_pos,
|
|
|
|
|
*self.target_eef_quat,
|
|
|
|
|
gripper_action,
|
|
|
|
|
], dtype=torch.float32)
|
|
|
|
|
|
|
|
|
|
if self.frame_count % 30 == 0:
|
|
|
|
|
np.set_printoptions(precision=4, suppress=True, floatmode='fixed')
|
|
|
|
|
print("\n====================== [VR TELEOP DEBUG] ======================")
|
|
|
|
|
print(f"| Raw VR Pos (OpenXR): {np.array(raw_pose[:3])}")
|
|
|
|
|
print(f"| Raw VR Quat (xyzw): {np.array(raw_pose[3:])}")
|
|
|
|
|
print(f"| XR Delta Pos (world): {world_delta_pos} (norm={np.linalg.norm(world_delta_pos):.4f})")
|
|
|
|
|
print(f"| XR Delta Rot (world): {world_delta_rot} (norm={np.linalg.norm(world_delta_rot):.4f})")
|
|
|
|
|
print(f"| Targ Pos (world): {action[:3].numpy()}")
|
|
|
|
|
print(f"| Targ Quat (world, wxyz): {action[3:7].numpy()}")
|
|
|
|
|
print(f"| Gripper & Trigger: Grip={grip:.2f}, Trig={trigger:.2f}")
|
|
|
|
|
print("===============================================================")
|
|
|
|
|
|
|
|
|
|
return action
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# =====================================================================
|
|
|
|
|
# Head Tracking Utility
|
|
|
|
|
# =====================================================================
|
|
|
|
|
|
|
|
|
|
def get_head_joint_targets(
|
|
|
|
|
xr_client, neutral_rot: R | None
|
|
|
|
|
) -> tuple[np.ndarray, R | None]:
|
|
|
|
|
"""Extract head joint targets [yaw, pitch] from HMD pose.
|
|
|
|
|
|
|
|
|
|
At first call (neutral_rot is None) the current headset orientation
|
|
|
|
|
is recorded as the neutral reference and zeros are returned.
|
|
|
|
|
Subsequent calls return yaw/pitch relative to that neutral pose.
|
|
|
|
|
|
|
|
|
|
Returns:
|
|
|
|
|
(joint_targets, neutral_rot) where joint_targets is shape (2,)
|
|
|
|
|
float32 [head_yaw_Joint_rad, head_pitch_Joint_rad].
|
|
|
|
|
"""
|
|
|
|
|
try:
|
|
|
|
|
raw_pose = xr_client.get_pose("headset")
|
|
|
|
|
if not is_valid_quaternion(raw_pose[3:]):
|
|
|
|
|
return np.zeros(2, dtype=np.float32), neutral_rot
|
|
|
|
|
_, head_world_quat_xyzw = transform_xr_pose(raw_pose[:3], raw_pose[3:])
|
|
|
|
|
head_rot = R.from_quat(head_world_quat_xyzw)
|
|
|
|
|
if neutral_rot is None:
|
|
|
|
|
# First call — record neutral and hold zero
|
|
|
|
|
return np.zeros(2, dtype=np.float32), head_rot
|
|
|
|
|
# Relative rotation from neutral heading
|
|
|
|
|
rel_rot = head_rot * neutral_rot.inv()
|
|
|
|
|
# ZYX Euler: [0]=yaw (Z), [1]=pitch (Y)
|
|
|
|
|
euler_zyx = rel_rot.as_euler("ZYX")
|
|
|
|
|
yaw = float(np.clip(euler_zyx[0], -1.57, 1.57)) # ±90°
|
|
|
|
|
pitch = float(np.clip(euler_zyx[1], -0.52, 0.52)) # ±30°
|
|
|
|
|
return np.array([yaw, pitch], dtype=np.float32), neutral_rot
|
|
|
|
|
except Exception:
|
|
|
|
|
return np.zeros(2, dtype=np.float32), neutral_rot
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# =====================================================================
|
|
|
|
|
# Main Execution Loop
|
|
|
|
|
# =====================================================================
|
|
|
|
|
|
|
|
|
|
def main() -> None:
|
|
|
|
|
"""Run teleoperation with PICO XR Controller against Isaac Lab environment."""
|
|
|
|
|
|
|
|
|
|
env_cfg = parse_env_cfg(args_cli.task, num_envs=args_cli.num_envs)
|
|
|
|
|
env_cfg.env_name = args_cli.task
|
|
|
|
|
|
|
|
|
|
if not isinstance(env_cfg, ManagerBasedRLEnvCfg):
|
|
|
|
|
raise ValueError(f"Teleoperation requires ManagerBasedRLEnvCfg. Got: {type(env_cfg)}")
|
|
|
|
|
if "Abs" not in args_cli.task:
|
|
|
|
|
raise ValueError(
|
|
|
|
|
f"Task '{args_cli.task}' is not an absolute IK task. "
|
|
|
|
|
"Only Abs tasks are supported (e.g. Isaac-MindRobot-LeftArm-IK-Abs-v0)."
|
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
env_cfg.terminations.time_out = None
|
|
|
|
|
|
|
|
|
|
try:
|
|
|
|
|
env = gym.make(args_cli.task, cfg=env_cfg).unwrapped
|
|
|
|
|
except Exception as e:
|
|
|
|
|
logger.error(f"Failed to create environment '{args_cli.task}': {e}")
|
|
|
|
|
simulation_app.close()
|
|
|
|
|
return
|
|
|
|
|
|
|
|
|
|
# Detect single-arm vs dual-arm based on action manager term names
|
|
|
|
|
is_dual_arm = "left_arm_action" in env.action_manager._terms
|
|
|
|
|
has_head = "head_action" in env.action_manager._terms
|
|
|
|
|
|
|
|
|
|
# -- Sim-to-VR stereo streaming setup --
|
|
|
|
|
streamer = None
|
|
|
|
|
if args_cli.stream_to:
|
|
|
|
|
try:
|
|
|
|
|
from mindbot.utils.sim_video_streamer import SimVideoStreamer
|
|
|
|
|
# Get camera resolution from scene config (vr_left_eye)
|
|
|
|
|
scene = env.unwrapped.scene
|
|
|
|
|
if "vr_left_eye" not in scene.sensors:
|
|
|
|
|
logger.warning("[Stream] vr_left_eye camera not found in scene. Streaming disabled.")
|
|
|
|
|
else:
|
|
|
|
|
cam = scene["vr_left_eye"]
|
|
|
|
|
cam_w = cam.cfg.width
|
|
|
|
|
cam_h = cam.cfg.height
|
|
|
|
|
streamer = SimVideoStreamer(
|
|
|
|
|
host=args_cli.stream_to,
|
|
|
|
|
port=args_cli.stream_port,
|
|
|
|
|
width=cam_w,
|
|
|
|
|
height=cam_h,
|
|
|
|
|
fps=30,
|
|
|
|
|
bitrate=args_cli.stream_bitrate,
|
|
|
|
|
)
|
|
|
|
|
if streamer.connect():
|
|
|
|
|
print(f"[INFO] Sim stereo streaming to {args_cli.stream_to}:{args_cli.stream_port} "
|
|
|
|
|
f"({cam_w*2}x{cam_h} SBS @ 30fps)")
|
|
|
|
|
else:
|
|
|
|
|
logger.error("[Stream] Failed to connect. Streaming disabled.")
|
|
|
|
|
streamer = None
|
|
|
|
|
except ImportError:
|
|
|
|
|
logger.error("[Stream] mindbot.utils.sim_video_streamer not found. pip install av?")
|
|
|
|
|
except Exception as e:
|
|
|
|
|
logger.error(f"[Stream] Init failed: {e}")
|
|
|
|
|
|
|
|
|
|
# Sensitivity: explicit args override --sensitivity, which overrides defaults
|
|
|
|
|
pos_sens = 1.0
|
|
|
|
|
rot_sens = 0.3
|
|
|
|
|
if args_cli.sensitivity is not None:
|
|
|
|
|
pos_sens = args_cli.sensitivity
|
|
|
|
|
rot_sens = args_cli.sensitivity
|
|
|
|
|
if args_cli.pos_sensitivity is not None:
|
|
|
|
|
pos_sens = args_cli.pos_sensitivity
|
|
|
|
|
if args_cli.rot_sensitivity is not None:
|
|
|
|
|
rot_sens = args_cli.rot_sensitivity
|
|
|
|
|
|
|
|
|
|
if is_dual_arm:
|
|
|
|
|
print(f"\n[INFO] Dual-arm mode detected. Using both controllers.")
|
|
|
|
|
print(f"[INFO] IK Mode: ABSOLUTE")
|
|
|
|
|
print(f"[INFO] Sensitivity: pos={pos_sens:.3f} rot={rot_sens:.3f}")
|
|
|
|
|
shared_client = XrClient()
|
|
|
|
|
teleop_left = XrTeleopController(arm="left", pos_sensitivity=pos_sens, rot_sensitivity=rot_sens, xr_client=shared_client)
|
|
|
|
|
teleop_right = XrTeleopController(arm="right", pos_sensitivity=pos_sens, rot_sensitivity=rot_sens, xr_client=shared_client)
|
|
|
|
|
teleop_interface = teleop_left # primary interface for callbacks/reset
|
|
|
|
|
teleop_right_ref = teleop_right
|
|
|
|
|
else:
|
|
|
|
|
print(f"\n[INFO] Connecting to PICO XR Headset using {args_cli.arm} controller...")
|
|
|
|
|
print(f"[INFO] IK Mode: ABSOLUTE")
|
|
|
|
|
print(f"[INFO] Sensitivity: pos={pos_sens:.3f} rot={rot_sens:.3f}")
|
|
|
|
|
teleop_interface = XrTeleopController(
|
|
|
|
|
arm=args_cli.arm,
|
|
|
|
|
pos_sensitivity=pos_sens,
|
|
|
|
|
rot_sensitivity=rot_sens,
|
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
should_reset = False
|
|
|
|
|
|
|
|
|
|
def request_reset():
|
|
|
|
|
nonlocal should_reset
|
|
|
|
|
should_reset = True
|
|
|
|
|
print("[INFO] Reset requested via XR button.")
|
|
|
|
|
|
|
|
|
|
teleop_interface.add_callback("RESET", request_reset)
|
|
|
|
|
|
|
|
|
|
def clear_ik_target_state():
|
|
|
|
|
"""Sync IK controller's internal target to current EEF pose to avoid jumps after reset."""
|
|
|
|
|
if is_dual_arm:
|
|
|
|
|
for term_key in ["left_arm_action", "right_arm_action"]:
|
|
|
|
|
arm_action_term = env.action_manager._terms[term_key]
|
|
|
|
|
ee_pos_b, ee_quat_b = arm_action_term._compute_frame_pose()
|
|
|
|
|
arm_action_term._raw_actions.zero_()
|
|
|
|
|
arm_action_term._processed_actions.zero_()
|
|
|
|
|
arm_action_term._ik_controller._command.zero_()
|
|
|
|
|
arm_action_term._ik_controller.ee_pos_des[:] = ee_pos_b
|
|
|
|
|
arm_action_term._ik_controller.ee_quat_des[:] = ee_quat_b
|
|
|
|
|
else:
|
|
|
|
|
arm_action_term = env.action_manager._terms["arm_action"]
|
|
|
|
|
ee_pos_b, ee_quat_b = arm_action_term._compute_frame_pose()
|
|
|
|
|
arm_action_term._raw_actions.zero_()
|
|
|
|
|
arm_action_term._processed_actions.zero_()
|
|
|
|
|
arm_action_term._ik_controller._command.zero_()
|
|
|
|
|
arm_action_term._ik_controller.ee_pos_des[:] = ee_pos_b
|
|
|
|
|
arm_action_term._ik_controller.ee_quat_des[:] = ee_quat_b
|
|
|
|
|
|
|
|
|
|
def convert_action_world_to_root(action_tensor: torch.Tensor) -> torch.Tensor:
|
|
|
|
|
"""Convert absolute IK action from world frame to robot root frame."""
|
|
|
|
|
robot = env.unwrapped.scene["robot"]
|
|
|
|
|
root_pos_w = robot.data.root_pos_w[0].detach().cpu().numpy()
|
|
|
|
|
root_quat_w = robot.data.root_quat_w[0].detach().cpu().numpy()
|
|
|
|
|
pos_root, quat_root = world_pose_to_root_frame(
|
|
|
|
|
action_tensor[:3].numpy(), action_tensor[3:7].numpy(),
|
|
|
|
|
root_pos_w, root_quat_w,
|
|
|
|
|
)
|
|
|
|
|
out = action_tensor.clone()
|
|
|
|
|
out[:3] = torch.tensor(pos_root, dtype=torch.float32)
|
|
|
|
|
out[3:7] = torch.tensor(quat_root, dtype=torch.float32)
|
|
|
|
|
return out
|
|
|
|
|
|
|
|
|
|
def get_chassis_commands() -> tuple[torch.Tensor, float, float]:
|
|
|
|
|
"""Read left joystick → (wheel_cmd 4D, v_fwd m/s, omega rad/s).
|
|
|
|
|
|
|
|
|
|
wheel_cmd : differential wheel velocities for the action vector (training data).
|
|
|
|
|
v_fwd : desired robot forward speed in m/s (for direct base control).
|
|
|
|
|
omega : desired robot yaw rate in rad/s (for direct base control).
|
|
|
|
|
"""
|
|
|
|
|
try:
|
|
|
|
|
joy = teleop_interface.xr_client.get_joystick("left")
|
|
|
|
|
jy = float(joy[1])
|
|
|
|
|
jx = float(joy[0])
|
|
|
|
|
except Exception:
|
|
|
|
|
return torch.zeros(4), 0.0, 0.0
|
|
|
|
|
# Wheel velocity commands for action vector (rad/s)
|
|
|
|
|
v_w = jy * args_cli.base_speed
|
|
|
|
|
omega_w = jx * args_cli.base_turn
|
|
|
|
|
right_vel = v_w - omega_w
|
|
|
|
|
left_vel = v_w + omega_w
|
|
|
|
|
wheel_cmd = torch.tensor([right_vel, left_vel, left_vel, right_vel], dtype=torch.float32)
|
|
|
|
|
if abs(v_w) > 0.1 or abs(omega_w) > 0.1:
|
|
|
|
|
print(f"[WHEEL] joy=({jx:+.2f},{jy:+.2f}) v_wheel={v_w:.2f} ω_wheel={omega_w:.2f} → {wheel_cmd.tolist()}")
|
|
|
|
|
# Direct base velocity commands (m/s, rad/s)
|
|
|
|
|
v_fwd = jy * args_cli.drive_speed
|
|
|
|
|
omega = -jx * args_cli.drive_turn # jx<0 (left push) → positive yaw = left turn
|
|
|
|
|
return wheel_cmd, v_fwd, omega
|
|
|
|
|
|
|
|
|
|
def apply_base_velocity(v_fwd: float, omega: float) -> None:
|
|
|
|
|
"""Directly set robot root linear+angular velocity to bypass isotropic friction.
|
|
|
|
|
|
|
|
|
|
PhysX uses isotropic friction, so skid-steer lateral slip is impossible through
|
|
|
|
|
wheel torques alone. This function overrides the base velocity each step to give
|
|
|
|
|
clean translational + rotational motion regardless of friction.
|
|
|
|
|
The write is buffered and applied at the start of the next env.step().
|
|
|
|
|
"""
|
|
|
|
|
if abs(v_fwd) < 1e-4 and abs(omega) < 1e-4:
|
|
|
|
|
return
|
|
|
|
|
robot = env.unwrapped.scene["robot"]
|
|
|
|
|
rq = robot.data.root_quat_w # [N, 4] wxyz
|
|
|
|
|
w_q, x_q, y_q, z_q = rq[:, 0], rq[:, 1], rq[:, 2], rq[:, 3]
|
|
|
|
|
# Robot forward direction in world frame (rotate [1,0,0] by root quaternion)
|
|
|
|
|
fwd_x = 1.0 - 2.0 * (y_q * y_q + z_q * z_q)
|
|
|
|
|
fwd_y = 2.0 * (x_q * y_q + w_q * z_q)
|
|
|
|
|
vel = torch.zeros(env.num_envs, 6, device=device)
|
|
|
|
|
vel[:, 0] = v_fwd * fwd_x # world x linear
|
|
|
|
|
vel[:, 1] = v_fwd * fwd_y # world y linear
|
|
|
|
|
# vel[:, 2] = 0 # z: let physics handle gravity/contact
|
|
|
|
|
vel[:, 5] = omega # world z angular (yaw)
|
|
|
|
|
robot.write_root_velocity_to_sim(vel)
|
|
|
|
|
|
|
|
|
|
obs, _ = env.reset()
|
|
|
|
|
clear_ik_target_state()
|
|
|
|
|
teleop_interface.reset()
|
|
|
|
|
|
|
|
|
|
# Read contact sensor data directly from OmniGraph node USD attributes.
|
|
|
|
|
# The USD has action graphs that execute each physics step and write outputs to:
|
|
|
|
|
# outputs:inContact (bool) outputs:value (float, Newtons)
|
|
|
|
|
# /root in USD → /World/envs/env_0/Robot in the loaded scene.
|
|
|
|
|
_CONTACT_OG_NODES = {
|
|
|
|
|
"left_r": (
|
|
|
|
|
"/World/envs/env_0/Robot"
|
|
|
|
|
"/l_zhuajia___m2_1_cs/isaac_read_contact_sensor_node"
|
|
|
|
|
),
|
|
|
|
|
"left_l": (
|
|
|
|
|
"/World/envs/env_0/Robot"
|
|
|
|
|
"/l_zhuajia___m3_1_cs/isaac_read_contact_sensor_node"
|
|
|
|
|
),
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
def _og_contact_reading(stage, node_path: str) -> tuple[bool, float]:
|
|
|
|
|
"""Read one OmniGraph contact node output via USD attribute API."""
|
|
|
|
|
prim = stage.GetPrimAtPath(node_path)
|
|
|
|
|
if not prim.IsValid():
|
|
|
|
|
return False, float("nan")
|
|
|
|
|
in_contact_attr = prim.GetAttribute("outputs:inContact")
|
|
|
|
|
force_attr = prim.GetAttribute("outputs:value")
|
|
|
|
|
in_contact = in_contact_attr.Get() if in_contact_attr.IsValid() else False
|
|
|
|
|
force = force_attr.Get() if force_attr.IsValid() else 0.0
|
|
|
|
|
return bool(in_contact), float(force or 0.0)
|
|
|
|
|
|
|
|
|
|
# One-time diagnostic: run after first env.reset() so all prims exist
|
|
|
|
|
def _diag_contact_nodes() -> None:
|
|
|
|
|
import omni.usd
|
|
|
|
|
stage = omni.usd.get_context().get_stage()
|
|
|
|
|
print("\n[DIAG] OmniGraph contact node check:")
|
|
|
|
|
for name, path in _CONTACT_OG_NODES.items():
|
|
|
|
|
prim = stage.GetPrimAtPath(path)
|
|
|
|
|
if prim.IsValid():
|
|
|
|
|
has_in = prim.GetAttribute("outputs:inContact").IsValid()
|
|
|
|
|
has_val = prim.GetAttribute("outputs:value").IsValid()
|
|
|
|
|
print(f" [{name}] ✓ prim valid | outputs:inContact={has_in} outputs:value={has_val}")
|
|
|
|
|
else:
|
|
|
|
|
# Try to find similar prims to hint correct path
|
|
|
|
|
parent_path = "/".join(path.split("/")[:-1])
|
|
|
|
|
parent = stage.GetPrimAtPath(parent_path)
|
|
|
|
|
print(f" [{name}] ✗ NOT FOUND at: {path}")
|
|
|
|
|
if parent.IsValid():
|
|
|
|
|
children = [c.GetName() for c in parent.GetChildren()]
|
|
|
|
|
print(f" parent exists, children: {children}")
|
|
|
|
|
else:
|
|
|
|
|
gp_path = "/".join(path.split("/")[:-2])
|
|
|
|
|
gp = stage.GetPrimAtPath(gp_path)
|
|
|
|
|
print(f" grandparent '{gp_path}' valid={gp.IsValid()}")
|
|
|
|
|
|
|
|
|
|
def read_gripper_contact() -> str:
|
|
|
|
|
"""Return contact status string by reading OmniGraph node USD attributes."""
|
|
|
|
|
try:
|
|
|
|
|
import omni.usd
|
|
|
|
|
stage = omni.usd.get_context().get_stage()
|
|
|
|
|
c_r, f_r = _og_contact_reading(stage, _CONTACT_OG_NODES["left_r"])
|
|
|
|
|
c_l, f_l = _og_contact_reading(stage, _CONTACT_OG_NODES["left_l"])
|
|
|
|
|
if f_r != f_r: # nan → prim not found
|
|
|
|
|
return "prim not found (run with diag)"
|
|
|
|
|
sym_r = "✓" if c_r else "✗"
|
|
|
|
|
sym_l = "✓" if c_l else "✗"
|
|
|
|
|
return f"{sym_r}R={f_r:.1f}N {sym_l}L={f_l:.1f}N"
|
|
|
|
|
except Exception as _e:
|
|
|
|
|
return f"N/A ({_e})"
|
|
|
|
|
|
|
|
|
|
# One-time diagnostic: verify OmniGraph contact sensor node paths
|
|
|
|
|
_diag_contact_nodes()
|
|
|
|
|
|
|
|
|
|
# Create debug viewport windows (always on)
|
|
|
|
|
create_robot_viewports()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if is_dual_arm:
|
|
|
|
|
teleop_right_ref.reset()
|
|
|
|
|
|
|
|
|
|
print("\n" + "=" * 50)
|
|
|
|
|
print(" Teleoperation Started!")
|
|
|
|
|
if is_dual_arm:
|
|
|
|
|
print(" LEFT controller → left arm")
|
|
|
|
|
print(" RIGHT controller → right arm")
|
|
|
|
|
print(" TRIGGER: open/close gripper")
|
|
|
|
|
print(" GRIP (hold): move the arm")
|
|
|
|
|
print(" Left joystick: Y=forward/back, X=turn")
|
|
|
|
|
print(" Y (left controller): reset environment")
|
|
|
|
|
print("=" * 50 + "\n")
|
|
|
|
|
|
|
|
|
|
device = env.unwrapped.device
|
|
|
|
|
sim_frame = 0
|
|
|
|
|
obs, _ = env.reset()
|
|
|
|
|
clear_ik_target_state()
|
|
|
|
|
last_root_left = None
|
|
|
|
|
last_root_right = None
|
|
|
|
|
neutral_head_rot = None # calibrated on first headset sample after reset
|
|
|
|
|
|
|
|
|
|
# trunk_action uses use_default_offset=False, so action = absolute position target.
|
|
|
|
|
trunk_cmd = torch.tensor([0.1], dtype=torch.float32)
|
|
|
|
|
|
|
|
|
|
while simulation_app.is_running():
|
|
|
|
|
try:
|
|
|
|
|
with torch.inference_mode():
|
|
|
|
|
if should_reset:
|
|
|
|
|
obs, _ = env.reset()
|
|
|
|
|
clear_ik_target_state()
|
|
|
|
|
teleop_interface.reset()
|
|
|
|
|
if is_dual_arm:
|
|
|
|
|
teleop_right_ref.reset()
|
|
|
|
|
rebind_robot_viewports()
|
|
|
|
|
should_reset = False
|
|
|
|
|
sim_frame = 0
|
|
|
|
|
last_root_left = None
|
|
|
|
|
last_root_right = None
|
|
|
|
|
neutral_head_rot = None # re-calibrate on next headset sample
|
|
|
|
|
continue
|
|
|
|
|
|
|
|
|
|
policy_obs = obs["policy"]
|
|
|
|
|
wheel_np, v_fwd, omega_base = get_chassis_commands()
|
|
|
|
|
|
|
|
|
|
# Head tracking: HMD yaw/pitch relative to neutral pose
|
|
|
|
|
head_cmd_np, neutral_head_rot = get_head_joint_targets(
|
|
|
|
|
teleop_interface.xr_client, neutral_head_rot
|
|
|
|
|
)
|
|
|
|
|
head_cmd = torch.tensor(head_cmd_np, dtype=torch.float32)
|
|
|
|
|
|
|
|
|
|
if is_dual_arm:
|
|
|
|
|
eef_pos_left = policy_obs["eef_pos_left"][0].cpu().numpy()
|
|
|
|
|
eef_quat_left = policy_obs["eef_quat_left"][0].cpu().numpy()
|
|
|
|
|
eef_pos_right = policy_obs["eef_pos_right"][0].cpu().numpy()
|
|
|
|
|
eef_quat_right = policy_obs["eef_quat_right"][0].cpu().numpy()
|
|
|
|
|
|
|
|
|
|
left_action = teleop_left.advance(current_eef_pos=eef_pos_left, current_eef_quat=eef_quat_left)
|
|
|
|
|
right_action = teleop_right_ref.advance(current_eef_pos=eef_pos_right, current_eef_quat=eef_quat_right)
|
|
|
|
|
|
|
|
|
|
# Only recompute root-frame IK target when grip is active; otherwise freeze joints
|
|
|
|
|
if teleop_left.grip_active or last_root_left is None:
|
|
|
|
|
last_root_left = convert_action_world_to_root(left_action)[:7].clone()
|
|
|
|
|
if teleop_right_ref.grip_active or last_root_right is None:
|
|
|
|
|
last_root_right = convert_action_world_to_root(right_action)[:7].clone()
|
|
|
|
|
|
|
|
|
|
# Action layout depends on task:
|
|
|
|
|
# with head: left_arm(7) | wheel(4) | left_gripper(1) | right_arm(7) | right_gripper(1) | head(2) | trunk(1) = 23D
|
|
|
|
|
# without head: left_arm(7) | wheel(4) | left_gripper(1) | right_arm(7) | right_gripper(1) = 20D
|
|
|
|
|
parts = [
|
|
|
|
|
last_root_left, wheel_np, left_action[7:8],
|
|
|
|
|
last_root_right, right_action[7:8],
|
|
|
|
|
]
|
|
|
|
|
if has_head:
|
|
|
|
|
parts.append(head_cmd)
|
|
|
|
|
parts.append(trunk_cmd)
|
|
|
|
|
action_np = torch.cat(parts)
|
|
|
|
|
else:
|
|
|
|
|
eef_pos = policy_obs["eef_pos"][0].cpu().numpy()
|
|
|
|
|
eef_quat = policy_obs["eef_quat"][0].cpu().numpy()
|
|
|
|
|
|
|
|
|
|
action_np = teleop_interface.advance(current_eef_pos=eef_pos, current_eef_quat=eef_quat)
|
|
|
|
|
|
|
|
|
|
if teleop_interface.grip_active or last_root_left is None:
|
|
|
|
|
last_root_left = convert_action_world_to_root(action_np)[:7].clone()
|
|
|
|
|
|
|
|
|
|
# Action layout: arm(7) | wheel(4) | gripper(1)
|
|
|
|
|
action_np = torch.cat([last_root_left, wheel_np, action_np[7:8]])
|
|
|
|
|
|
|
|
|
|
actions = action_np.unsqueeze(0).repeat(env.num_envs, 1).to(device)
|
|
|
|
|
obs, _, _, _, _ = env.step(actions)
|
|
|
|
|
# Direct base velocity override: bypasses isotropic friction limitation
|
|
|
|
|
# so skid-steer turning works correctly.
|
|
|
|
|
apply_base_velocity(v_fwd, omega_base)
|
|
|
|
|
|
|
|
|
|
# Stream sim stereo to VR headset (if enabled)
|
|
|
|
|
if streamer is not None:
|
|
|
|
|
try:
|
|
|
|
|
scene = env.unwrapped.scene
|
|
|
|
|
left_rgb = scene["vr_left_eye"].data.output["rgb"][0].cpu().numpy()
|
|
|
|
|
right_rgb = scene["vr_right_eye"].data.output["rgb"][0].cpu().numpy()
|
|
|
|
|
if not streamer.send_frame(left_rgb, right_rgb):
|
|
|
|
|
logger.warning("[Stream] Connection lost. Disabling streaming.")
|
|
|
|
|
streamer = None
|
|
|
|
|
except Exception as e:
|
|
|
|
|
if sim_frame % 300 == 0:
|
|
|
|
|
logger.warning(f"[Stream] Frame error: {e}")
|
|
|
|
|
|
|
|
|
|
sim_frame += 1
|
|
|
|
|
if sim_frame % 30 == 0:
|
|
|
|
|
np.set_printoptions(precision=4, suppress=True, floatmode='fixed')
|
|
|
|
|
policy_obs = obs["policy"]
|
|
|
|
|
joint_pos = policy_obs["joint_pos"][0].cpu().numpy()
|
|
|
|
|
last_act = policy_obs["actions"][0].cpu().numpy()
|
|
|
|
|
|
|
|
|
|
if sim_frame == 30:
|
|
|
|
|
robot = env.unwrapped.scene["robot"]
|
|
|
|
|
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}")
|
|
|
|
|
arm_idx = [i for i, n in enumerate(jnames) if n.startswith("l_joint")]
|
|
|
|
|
print(f" Deduced left arm indices: {arm_idx}")
|
|
|
|
|
print(f"{'='*70}\n")
|
|
|
|
|
|
|
|
|
|
if not hasattr(env, '_arm_idx_cache'):
|
|
|
|
|
robot = env.unwrapped.scene["robot"]
|
|
|
|
|
jnames = robot.joint_names
|
|
|
|
|
env._arm_idx_cache = [i for i, n in enumerate(jnames) if n.startswith("l_joint")]
|
|
|
|
|
env._right_arm_idx_cache = [i for i, n in enumerate(jnames) if n.startswith("r_joint")]
|
|
|
|
|
arm_joints = joint_pos[env._arm_idx_cache]
|
|
|
|
|
right_arm_joints = joint_pos[env._right_arm_idx_cache]
|
|
|
|
|
|
|
|
|
|
try:
|
|
|
|
|
joy_dbg = teleop_interface.xr_client.get_joystick("left")
|
|
|
|
|
joy_str = f"[{joy_dbg[0]:+.2f}, {joy_dbg[1]:+.2f}]"
|
|
|
|
|
except Exception:
|
|
|
|
|
joy_str = "N/A"
|
|
|
|
|
|
|
|
|
|
if is_dual_arm:
|
|
|
|
|
eef_pos_left = policy_obs["eef_pos_left"][0].cpu().numpy()
|
|
|
|
|
eef_quat_left = policy_obs["eef_quat_left"][0].cpu().numpy()
|
|
|
|
|
eef_pos_right = policy_obs["eef_pos_right"][0].cpu().numpy()
|
|
|
|
|
|
|
|
|
|
print(f"\n---------------- [ROBOT STATE frame={sim_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: {read_gripper_contact()}")
|
|
|
|
|
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()
|
|
|
|
|
eef_quat = policy_obs["eef_quat"][0].cpu().numpy()
|
|
|
|
|
|
|
|
|
|
if not hasattr(env, '_prev_eef_quat'):
|
|
|
|
|
env._prev_eef_quat = eef_quat.copy()
|
|
|
|
|
prev_q = env._prev_eef_quat
|
|
|
|
|
r_prev = R.from_quat([prev_q[1], prev_q[2], prev_q[3], prev_q[0]])
|
|
|
|
|
r_curr = R.from_quat([eef_quat[1], eef_quat[2], eef_quat[3], eef_quat[0]])
|
|
|
|
|
delta_rotvec = (r_curr * r_prev.inv()).as_rotvec()
|
|
|
|
|
delta_angle_deg = np.degrees(np.linalg.norm(delta_rotvec))
|
|
|
|
|
delta_axis = delta_rotvec / (np.linalg.norm(delta_rotvec) + 1e-9)
|
|
|
|
|
env._prev_eef_quat = eef_quat.copy()
|
|
|
|
|
approach_vec = r_curr.apply(np.array([1.0, 0.0, 0.0]))
|
|
|
|
|
|
|
|
|
|
print(f"\n---------------- [ROBOT STATE frame={sim_frame}] ----------------")
|
|
|
|
|
print(f"| Left Arm Joints (rad): {arm_joints}")
|
|
|
|
|
print(f"| EEF Pos (world, m): {eef_pos}")
|
|
|
|
|
print(f"| EEF approach vec (world): [{approach_vec[0]:+.3f}, {approach_vec[1]:+.3f}, {approach_vec[2]:+.3f}] (夹爪朝向)")
|
|
|
|
|
print(f"| EEF rot since last print: {delta_angle_deg:+.2f}° around [{delta_axis[0]:+.3f},{delta_axis[1]:+.3f},{delta_axis[2]:+.3f}] (world)")
|
|
|
|
|
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"----------------------------------------------------------------")
|
|
|
|
|
|
|
|
|
|
except Exception as e:
|
|
|
|
|
logger.error(f"Error during simulation step: {e}")
|
|
|
|
|
break
|
|
|
|
|
|
|
|
|
|
if streamer is not None:
|
|
|
|
|
streamer.close()
|
|
|
|
|
teleop_interface.close()
|
|
|
|
|
if is_dual_arm:
|
|
|
|
|
teleop_right_ref.close()
|
|
|
|
|
shared_client.close()
|
|
|
|
|
env.close()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if __name__ == "__main__":
|
|
|
|
|
main()
|
|
|
|
|
simulation_app.close()
|
