diff --git a/scripts/environments/teleoperation/teleop_xr_agent.py b/scripts/environments/teleoperation/teleop_xr_agent.py new file mode 100644 index 0000000..3dfd51e --- /dev/null +++ b/scripts/environments/teleoperation/teleop_xr_agent.py @@ -0,0 +1,335 @@ +#!/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. +This script uses XRoboToolkit to fetch XR controller poses and maps them to differential IK actions. +""" + +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." +) +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-Rel-v0", + help="Name of the task.", +) +parser.add_argument( + "--sensitivity", type=float, default=5.0, help="Sensitivity factor for pos/rot." +) +parser.add_argument( + "--arm", + type=str, + default="left", + choices=["left", "right"], + help="Which arm/controller to use.", +) +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +args_cli = parser.parse_args() +app_launcher_args = vars(args_cli) + +# Disable some rendering settings to speed up +app_launcher_args["xr"] = False + +# launch omniverse app +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 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 + +# ===================================================================== +# Teleoperation Interface for XR +# ===================================================================== + +class XrTeleopController: + """Teleop controller for PICO XR headset.""" + + def __init__(self, arm="left", pos_sensitivity=1.0, rot_sensitivity=0.3): + self.xr_client = XrClient() + self.pos_sensitivity = pos_sensitivity + self.rot_sensitivity = rot_sensitivity + 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" + + # Coordinate transform matrix + self.R_headset_world = R_HEADSET_TO_WORLD + + # Raw XR tracking space poses (NOT transformed) + self.prev_xr_pos = None + self.prev_xr_quat = None + self.grip_active = False + self.frame_count = 0 + + # Callbacks (like reset, etc) + 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 + + def close(self): + self.xr_client.close() + + def advance(self) -> torch.Tensor: + """ + Reads the XR controller and returns the 7D action tensor: + [dx, dy, dz, droll, dpitch, dyaw, gripper_cmd] + """ + # XR buttons check (e.g. A or B for reset) + try: + if self.xr_client.get_button("B") or self.xr_client.get_button("Y"): + if "RESET" in self.callbacks: + self.callbacks["RESET"]() + except: + 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 as e: + return torch.zeros(7) + + # Skip transformation if quaternion is invalid (e.g. before headset truly connects) + if not is_valid_quaternion(raw_pose[3:]): + action = torch.zeros(7) + action[6] = 1.0 if trigger > 0.5 else -1.0 + return action + + # Transform XR pose pos directly via Matrix mapping for safety check (though we can map diffs next) + pos_w = self.R_headset_world @ raw_pose[:3] + + # 握持键作为离合器 (Clutch) - 按下 Grip 时才移动机械臂 + if grip < 0.5: + self.prev_xr_pos = None + self.prev_xr_quat = None + self.grip_active = False + action = torch.zeros(7) + action[6] = 1.0 if trigger > 0.5 else -1.0 + return action + + if not self.grip_active: + # We strictly log original XR coordinates to avoid quaternion base frame corruption + self.prev_xr_pos = raw_pose[:3].copy() + self.prev_xr_quat = raw_pose[3:].copy() + self.grip_active = True + action = torch.zeros(7) + action[6] = 1.0 if trigger > 0.5 else -1.0 + return action + + # ========== 1. Position Delta (calculate in XR frame, map to World) ========== + xr_delta_pos = raw_pose[:3] - self.prev_xr_pos + # Clamp raw position delta to prevent spikes (max ~4cm per frame) + max_pos_delta = 0.04 + pos_norm = np.linalg.norm(xr_delta_pos) + if pos_norm > max_pos_delta: + xr_delta_pos = xr_delta_pos * (max_pos_delta / pos_norm) + # PICO -> Isaac World mapping: + # XR +X (Right) -> World -Y (Right is -Left) + # XR +Y (Up) -> World +Z (Up is Up) + # XR +Z (Back) -> World -X (Back is -Forward) + delta_pos = np.array([-xr_delta_pos[2], -xr_delta_pos[0], xr_delta_pos[1]]) * self.pos_sensitivity + + # ========== 2. Rotation Delta (calculate in XR frame, map to World) ========== + # compute pure relative angular difference in local XR tracking space + xr_delta_rot = quat_diff_as_rotvec_xyzw(self.prev_xr_quat, raw_pose[3:]) + # Clamp raw rotation delta to prevent spikes (max ~0.02 rad = ~1.1° per frame) + # Keeping this small is critical: DLS IK can only solve small deltas as pure rotations. + # Large deltas cause the IK to swing the whole arm instead of rotating in-place. + max_rot_delta = 0.02 + rot_norm = np.linalg.norm(xr_delta_rot) + if rot_norm > max_rot_delta: + xr_delta_rot = xr_delta_rot * (max_rot_delta / rot_norm) + # Same mapping rules apply to rotation axes: + # Rotating around XR's X (Right) -> Rotating around World's -Y (Right) + # Rotating around XR's Y (Up) -> Rotating around World's +Z (Up) + # Rotating around XR's Z (Back) -> Rotating around World's -X (Back) + delta_rot = np.array([-xr_delta_rot[2], -xr_delta_rot[0], xr_delta_rot[1]]) * self.rot_sensitivity + + # Update cache + self.prev_xr_pos = raw_pose[:3].copy() + self.prev_xr_quat = raw_pose[3:].copy() + self.frame_count += 1 + + # ========== 3. Gripper ========== + gripper_action = 1.0 if trigger > 0.5 else -1.0 + + action = torch.tensor([delta_pos[0], delta_pos[1], delta_pos[2], delta_rot[0], delta_rot[1], delta_rot[2], gripper_action], dtype=torch.float32) + + # ========== 4. Comprehensive Debug Log ========== + 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 (raw): {xr_delta_pos} (norm={pos_norm:.4f})") + print(f"| XR Delta Rot (raw): {xr_delta_rot} (norm={rot_norm:.4f})") + print("|--------------------------------------------------------------") + print(f"| Sent Action Pos (dx,dy,dz): {action[:3].numpy()}") + print(f"| Sent Action Rot (rx,ry,rz): {action[3:6].numpy()}") + print(f"| Gripper & Trigger: Grip={grip:.2f}, Trig={trigger:.2f}") + print("===============================================================") + + return action + +# ===================================================================== +# Main Execution Loop +# ===================================================================== + +def main() -> None: + """Run teleoperation with PICO XR Controller against Isaac Lab environment.""" + + # 1. Configuration parsing + 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)}") + + env_cfg.terminations.time_out = None + + # 2. Environment creation + 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 + + # 3. Teleoperation Interface Initialization + print(f"\n[INFO] Connecting to PICO XR Headset using {args_cli.arm} controller...") + teleop_interface = XrTeleopController( + arm=args_cli.arm, + pos_sensitivity=args_cli.sensitivity, + rot_sensitivity=args_cli.sensitivity * 0.3, # Rotation must be much gentler for DLS IK + ) + + 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) + + env.reset() + teleop_interface.reset() + + print("\n" + "=" * 50) + print(" 🚀 Teleoperation Started!") + print(" 🎮 Use the TRIGGER to open/close gripper.") + print(" ✊ Hold GRIP button and move the controller to move the arm.") + print(" 🕹️ Press B or Y to reset the environment.") + print("=" * 50 + "\n") + + # 4. Simulation loop + device = env.unwrapped.device + sim_frame = 0 + while simulation_app.is_running(): + try: + with torch.inference_mode(): + # Get action from XR Controller [1, 7] + action_np = teleop_interface.advance() + actions = action_np.unsqueeze(0).repeat(env.num_envs, 1).to(device) + + # Step environment + obs, _, _, _, _ = env.step(actions) + + # Print robot state every 30 frames + 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() + eef_pos = policy_obs["eef_pos"][0].cpu().numpy() + eef_quat = policy_obs["eef_quat"][0].cpu().numpy() + last_act = policy_obs["actions"][0].cpu().numpy() + + # On first print, dump ALL joint names + positions to identify indices + 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 (relative)") + print(f"{'='*70}") + for i, name in enumerate(jnames): + print(f" [{i:2d}] {name:30s} = {joint_pos[i]:+.4f}") + print(f"{'='*70}") + # Find left arm joint indices + arm_idx = [i for i, n in enumerate(jnames) if n.startswith("l_joint")] + print(f" Left arm indices: {arm_idx}") + print(f"{'='*70}\n") + + # Get arm indices (cache-friendly: find once) + 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")] + arm_idx = env._arm_idx_cache + arm_joints = joint_pos[arm_idx] + + print(f"\n---------------- [ROBOT STATE frame={sim_frame}] ----------------") + print(f"| Left Arm Joints (rad): {arm_joints}") + print(f"| EEF Pos (world): {eef_pos}") + print(f"| EEF Quat (world, wxyz): {eef_quat}") + print(f"| Last Action Sent: {last_act}") + print(f"----------------------------------------------------------------") + + if should_reset: + env.reset() + teleop_interface.reset() + should_reset = False + + except Exception as e: + logger.error(f"Error during simulation step: {e}") + break + + teleop_interface.close() + env.close() + +if __name__ == "__main__": + main() + simulation_app.close() diff --git a/scripts/environments/teleoperation/xr_utils/__init__.py b/scripts/environments/teleoperation/xr_utils/__init__.py new file mode 100644 index 0000000..92d07cd --- /dev/null +++ b/scripts/environments/teleoperation/xr_utils/__init__.py @@ -0,0 +1,33 @@ +# Copyright (c) 2022-2026, The Mindbot Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""XR teleoperation utilities for PICO 4 Ultra / XRoboToolkit integration. + +This package provides coordinate transformation, XR device communication, +and teleoperation control utilities adapted from XRoboToolkit for use +with Isaac Lab environments. +""" + +from .geometry import ( + R_HEADSET_TO_WORLD, + apply_delta_pose, + is_valid_quaternion, + quat_diff_as_angle_axis, + quat_diff_as_rotvec_xyzw, + quaternion_to_angle_axis, + transform_xr_pose, +) +from .xr_client import XrClient + +__all__ = [ + "R_HEADSET_TO_WORLD", + "apply_delta_pose", + "is_valid_quaternion", + "quat_diff_as_angle_axis", + "quat_diff_as_rotvec_xyzw", + "quaternion_to_angle_axis", + "transform_xr_pose", + "XrClient", +] diff --git a/scripts/environments/teleoperation/xr_utils/geometry.py b/scripts/environments/teleoperation/xr_utils/geometry.py new file mode 100644 index 0000000..cf38b36 --- /dev/null +++ b/scripts/environments/teleoperation/xr_utils/geometry.py @@ -0,0 +1,203 @@ +# Copyright (c) 2022-2026, The Mindbot Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +# +# Adapted from XRoboToolkit (https://github.com/NVIDIA-AI-IOT/XRoboToolkit-Teleop-Sample-Python) +# Original geometry utilities by Zhigen Zhao. + +"""Geometry utilities for XR-to-World coordinate transformations. + +This module provides the core coordinate transformation matrices and functions +needed to convert PICO 4 Ultra (OpenXR) data into Isaac Sim world coordinates. + +Coordinate Systems: + - OpenXR (PICO 4): +X Right, +Y Up, -Z Forward + - Isaac Sim (World): +X Forward, +Y Left, +Z Up +""" + +import numpy as np +from scipy.spatial.transform import Rotation as R + + +# ===================================================================== +# Core Coordinate Transform: OpenXR -> Isaac Sim World +# ===================================================================== + +R_HEADSET_TO_WORLD = np.array( + [ + [0, 0, -1], # XR -Z (forward) -> World +X (forward) + [-1, 0, 0], # XR -X (left) -> World +Y (left) + [0, 1, 0], # XR +Y (up) -> World +Z (up) + ], + dtype=np.float64, +) +"""3x3 rotation matrix that maps OpenXR coordinates to Isaac Sim world coordinates.""" + + +# ===================================================================== +# Quaternion Utilities +# ===================================================================== + +def is_valid_quaternion(quat: np.ndarray, tol: float = 1e-6) -> bool: + """Check if a quaternion is valid (unit norm). + + Args: + quat: Quaternion array of length 4 (any convention). + tol: Tolerance for norm deviation from 1.0. + + Returns: + True if the quaternion is valid. + """ + if not isinstance(quat, (list, tuple, np.ndarray)) or len(quat) != 4: + return False + if not np.all(np.isfinite(quat)): + return False + magnitude = np.sqrt(np.sum(np.square(quat))) + return abs(magnitude - 1.0) <= tol + + +def quaternion_to_angle_axis(quat_wxyz: np.ndarray, eps: float = 1e-6) -> np.ndarray: + """Convert a quaternion (w, x, y, z) to angle-axis representation. + + Args: + quat_wxyz: Quaternion in (w, x, y, z) format. + eps: Tolerance for small angles. + + Returns: + Angle-axis vector [ax*angle, ay*angle, az*angle], shape (3,). + """ + q = np.array(quat_wxyz, dtype=np.float64, copy=True) + if q[0] < 0.0: + q = -q + + w = q[0] + vec_part = q[1:] + angle = 2.0 * np.arccos(np.clip(w, -1.0, 1.0)) + + if angle < eps: + return np.zeros(3, dtype=np.float64) + + sin_half_angle = np.sin(angle / 2.0) + if sin_half_angle < eps: + return np.zeros(3, dtype=np.float64) + + axis = vec_part / sin_half_angle + return axis * angle + + +def quat_diff_as_angle_axis( + q1_wxyz: np.ndarray, q2_wxyz: np.ndarray, eps: float = 1e-6 +) -> np.ndarray: + """Calculate the rotation from q1 to q2 as an angle-axis vector. + + Computes DeltaQ such that q2 = DeltaQ * q1. + + Args: + q1_wxyz: Source quaternion (w, x, y, z). + q2_wxyz: Target quaternion (w, x, y, z). + eps: Tolerance for small rotations. + + Returns: + Angle-axis vector representing DeltaQ, shape (3,). + """ + # Use scipy for robust quaternion math (scipy uses xyzw convention) + r1 = R.from_quat([q1_wxyz[1], q1_wxyz[2], q1_wxyz[3], q1_wxyz[0]]) + r2 = R.from_quat([q2_wxyz[1], q2_wxyz[2], q2_wxyz[3], q2_wxyz[0]]) + delta = r2 * r1.inv() + rotvec = delta.as_rotvec() + if np.linalg.norm(rotvec) < eps: + return np.zeros(3, dtype=np.float64) + return rotvec + + +def quat_diff_as_rotvec_xyzw( + q1_xyzw: np.ndarray, q2_xyzw: np.ndarray, eps: float = 1e-6 +) -> np.ndarray: + """Calculate the rotation from q1 to q2 as a rotation vector. + + Same as quat_diff_as_angle_axis but takes (x, y, z, w) quaternions + directly (scipy convention), avoiding extra conversions. + + Args: + q1_xyzw: Source quaternion (x, y, z, w). + q2_xyzw: Target quaternion (x, y, z, w). + eps: Tolerance for small rotations. + + Returns: + Rotation vector (angle * axis), shape (3,). + """ + r1 = R.from_quat(q1_xyzw) + r2 = R.from_quat(q2_xyzw) + delta = r2 * r1.inv() + rotvec = delta.as_rotvec() + if np.linalg.norm(rotvec) < eps: + return np.zeros(3, dtype=np.float64) + return rotvec + + +def apply_delta_pose( + source_pos: np.ndarray, + source_quat_xyzw: np.ndarray, + delta_pos: np.ndarray, + delta_rot: np.ndarray, + eps: float = 1.0e-6, +) -> tuple: + """Apply a delta pose to a source pose. + + Args: + source_pos: Position of source frame, shape (3,). + source_quat_xyzw: Quaternion of source frame (x, y, z, w), shape (4,). + delta_pos: Position displacement, shape (3,). + delta_rot: Rotation displacement in angle-axis format, shape (3,). + eps: Tolerance to consider rotation as zero. + + Returns: + Tuple of (target_pos, target_quat_xyzw), both np.ndarray. + """ + target_pos = source_pos + delta_pos + + angle = np.linalg.norm(delta_rot) + if angle > eps: + delta_r = R.from_rotvec(delta_rot) + else: + delta_r = R.identity() + + source_r = R.from_quat(source_quat_xyzw) + target_r = delta_r * source_r + target_quat_xyzw = target_r.as_quat() + + return target_pos, target_quat_xyzw + + +# ===================================================================== +# XR Pose Transform +# ===================================================================== + +def transform_xr_pose( + pos_xr: np.ndarray, quat_xyzw_xr: np.ndarray +) -> tuple: + """Transform an XR device pose from OpenXR frame to Isaac Sim world frame. + + Uses the R_HEADSET_TO_WORLD rotation matrix to transform both position + and orientation, following the same approach as XRoboToolkit. + + Args: + pos_xr: Position in OpenXR frame, shape (3,). + quat_xyzw_xr: Quaternion in (x, y, z, w) format from XR SDK, shape (4,). + + Returns: + Tuple of (pos_world, quat_xyzw_world): + - pos_world: Position in world frame, shape (3,). + - quat_xyzw_world: Quaternion in (x, y, z, w) for scipy, shape (4,). + """ + # Transform position + pos_world = R_HEADSET_TO_WORLD @ pos_xr + + # Transform orientation: R_world = R_transform * R_xr * R_transform^T + r_xr = R.from_quat(quat_xyzw_xr) # scipy uses (x, y, z, w) + r_transform = R.from_matrix(R_HEADSET_TO_WORLD) + r_world = r_transform * r_xr * r_transform.inv() + quat_xyzw_world = r_world.as_quat() + + return pos_world, quat_xyzw_world diff --git a/scripts/environments/teleoperation/xr_utils/xr_client.py b/scripts/environments/teleoperation/xr_utils/xr_client.py new file mode 100644 index 0000000..9d2d135 --- /dev/null +++ b/scripts/environments/teleoperation/xr_utils/xr_client.py @@ -0,0 +1,169 @@ +# Copyright (c) 2022-2026, The Mindbot Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause +# +# Adapted from XRoboToolkit (https://github.com/NVIDIA-AI-IOT/XRoboToolkit-Teleop-Sample-Python) +# Original XR client by Zhigen Zhao. + +"""XR device client for PICO 4 Ultra via XRoboToolkit SDK. + +This module wraps the ``xrobotoolkit_sdk`` C++ extension to provide a +Pythonic interface for retrieving pose, button, joystick, and hand-tracking +data from PICO XR headsets over the local network. + +Usage:: + + from xr_utils import XrClient + + client = XrClient() + headset_pose = client.get_pose("headset") + left_trigger = client.get_key_value("left_trigger") + client.close() +""" + +import numpy as np +import xrobotoolkit_sdk as xrt + + +class XrClient: + """Client for PICO XR devices via the XRoboToolkit SDK.""" + + def __init__(self): + """Initialize the SDK and start listening for XR data.""" + xrt.init() + print("[XrClient] XRoboToolkit SDK initialized.") + + # ------------------------------------------------------------------ + # Pose + # ------------------------------------------------------------------ + + def get_pose(self, name: str) -> np.ndarray: + """Return the 7-DoF pose [x, y, z, qx, qy, qz, qw] of a device. + + Args: + name: One of ``"left_controller"``, ``"right_controller"``, ``"headset"``. + + Returns: + np.ndarray of shape (7,). + """ + _POSE_FUNCS = { + "left_controller": xrt.get_left_controller_pose, + "right_controller": xrt.get_right_controller_pose, + "headset": xrt.get_headset_pose, + } + if name not in _POSE_FUNCS: + raise ValueError( + f"Invalid pose name: {name}. Choose from {list(_POSE_FUNCS)}" + ) + return np.asarray(_POSE_FUNCS[name](), dtype=np.float64) + + # ------------------------------------------------------------------ + # Analog Keys (trigger / grip) + # ------------------------------------------------------------------ + + def get_key_value(self, name: str) -> float: + """Return an analog value (0.0~1.0) for a trigger or grip. + + Args: + name: One of ``"left_trigger"``, ``"right_trigger"``, + ``"left_grip"``, ``"right_grip"``. + """ + _KEY_FUNCS = { + "left_trigger": xrt.get_left_trigger, + "right_trigger": xrt.get_right_trigger, + "left_grip": xrt.get_left_grip, + "right_grip": xrt.get_right_grip, + } + if name not in _KEY_FUNCS: + raise ValueError( + f"Invalid key name: {name}. Choose from {list(_KEY_FUNCS)}" + ) + return float(_KEY_FUNCS[name]()) + + # ------------------------------------------------------------------ + # Buttons + # ------------------------------------------------------------------ + + def get_button(self, name: str) -> bool: + """Return boolean state of a face / menu button. + + Args: + name: One of ``"A"``, ``"B"``, ``"X"``, ``"Y"``, + ``"left_menu_button"``, ``"right_menu_button"``, + ``"left_axis_click"``, ``"right_axis_click"``. + """ + _BTN_FUNCS = { + "A": xrt.get_A_button, + "B": xrt.get_B_button, + "X": xrt.get_X_button, + "Y": xrt.get_Y_button, + "left_menu_button": xrt.get_left_menu_button, + "right_menu_button": xrt.get_right_menu_button, + "left_axis_click": xrt.get_left_axis_click, + "right_axis_click": xrt.get_right_axis_click, + } + if name not in _BTN_FUNCS: + raise ValueError( + f"Invalid button name: {name}. Choose from {list(_BTN_FUNCS)}" + ) + return bool(_BTN_FUNCS[name]()) + + # ------------------------------------------------------------------ + # Joystick + # ------------------------------------------------------------------ + + def get_joystick(self, side: str) -> np.ndarray: + """Return joystick (x, y) for left or right controller. + + Args: + side: ``"left"`` or ``"right"``. + + Returns: + np.ndarray of shape (2,). + """ + if side == "left": + return np.asarray(xrt.get_left_axis(), dtype=np.float64) + elif side == "right": + return np.asarray(xrt.get_right_axis(), dtype=np.float64) + raise ValueError(f"Invalid side: {side}. Choose 'left' or 'right'.") + + # ------------------------------------------------------------------ + # Hand Tracking + # ------------------------------------------------------------------ + + def get_hand_tracking(self, side: str) -> np.ndarray | None: + """Return hand tracking joint poses or None if inactive. + + Args: + side: ``"left"`` or ``"right"``. + + Returns: + np.ndarray of shape (27, 7) or None. + """ + if side == "left": + if not xrt.get_left_hand_is_active(): + return None + return np.asarray(xrt.get_left_hand_tracking_state(), dtype=np.float64) + elif side == "right": + if not xrt.get_right_hand_is_active(): + return None + return np.asarray(xrt.get_right_hand_tracking_state(), dtype=np.float64) + raise ValueError(f"Invalid side: {side}. Choose 'left' or 'right'.") + + # ------------------------------------------------------------------ + # Timestamp + # ------------------------------------------------------------------ + + def get_timestamp_ns(self) -> int: + """Return the current XR system timestamp in nanoseconds.""" + return int(xrt.get_time_stamp_ns()) + + # ------------------------------------------------------------------ + # Lifecycle + # ------------------------------------------------------------------ + + def close(self): + """Shut down the SDK connection.""" + xrt.close() + print("[XrClient] SDK connection closed.") diff --git a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_cfg.py index 192cd45..b956ba0 100644 --- a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_cfg.py +++ b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_cfg.py @@ -1,7 +1,16 @@ # Copyright (c) 2024, MindRobot Project # SPDX-License-Identifier: BSD-3-Clause -"""Configuration for MindRobot dual-arm robot.""" +"""Configuration for MindRobot dual-arm robot. + +Arms are RealMan RM-65 (6-DOF, 5kg payload, 7.2kg self-weight, 610mm reach). +Reference: https://develop.realman-robotics.com/robot4th/robotParameter/RM65OntologyParameters/ + +The following configurations are available: + +* :obj:`MINDBOT_CFG`: Base configuration with gravity enabled (general purpose). +* :obj:`MINDBOT_HIGH_PD_CFG`: Gravity disabled, stiffer PD for IK task-space control. +""" import isaaclab.sim as sim_utils from isaaclab.actuators import ImplicitActuatorCfg @@ -9,113 +18,119 @@ from isaaclab.assets import ArticulationCfg from mindbot.utils.assets import MINDBOT_ASSETS_DIR -## -# ✅ 使用你的本地 USD 绝对路径 -## - MINDBOT_CFG = ArticulationCfg( spawn=sim_utils.UsdFileCfg( usd_path=f"{MINDBOT_ASSETS_DIR}/robots/mindbot/mindbot_cd2.usd", rigid_props=sim_utils.RigidBodyPropertiesCfg( disable_gravity=False, - max_depenetration_velocity=1.0, - linear_damping=0.5, - angular_damping=0.5, + max_depenetration_velocity=5.0, ), articulation_props=sim_utils.ArticulationRootPropertiesCfg( - # Allow root to be affected by physics (gravity) so robot won't hover fix_root_link=False, - enabled_self_collisions=False, - solver_position_iteration_count=16, - solver_velocity_iteration_count=8, - stabilization_threshold=1e-6, - ), - collision_props=sim_utils.CollisionPropertiesCfg( - collision_enabled=True, - contact_offset=0.0005, - rest_offset=0, + enabled_self_collisions=True, + solver_position_iteration_count=8, + solver_velocity_iteration_count=0, ), ), init_state=ArticulationCfg.InitialStateCfg( joint_pos={ - # ====== 左臂关节（主控臂）====== - "l_joint1": 2.3562, # 135° - "l_joint2": -1.2217, # -70° - "l_joint3": -1.5708, # -90° - "l_joint4": 1.5708, # 90° - "l_joint5": 1.5708, # 90° - "l_joint6": -1.2217, # -70° - # ====== 右臂关节（静止）====== - "r_joint1": 0.0, - "r_joint2": 0.0, - "r_joint3": 0.0, - "r_joint4": 0.0, - "r_joint5": 0.0, - "r_joint6": 0.0, - # ====== 左手夹爪 ====== - "left_hand_joint_left": 0.0, # 0.0=打开, -0.03=闭合 - "left_hand_joint_right": 0.0, # 0.0=打开, 0.03=闭合 - # ====== 右手夹爪 ====== + # ====== Left arm (RM-65) — away from singularities ====== + # Elbow singularity: q3=0; Wrist singularity: q5=0. + # The pose below keeps q3≠0 and q5≠0. + "l_joint1": 1.5708, # 135° + "l_joint2": -1.5708, # -70° + "l_joint3": -1.5708, # -90° (away from elbow singularity q3=0) + "l_joint4": 1.5708, # 90° + "l_joint5": 1.5708, # 90° (away from wrist singularity q5=0) + "l_joint6": -1.5708, # -70° + # ====== Right arm (RM-65) ====== + "r_joint1": -2.3562, + "r_joint2": -1.2217, + "r_joint3": 1.5708, + "r_joint4": -1.5708, + "r_joint5": -1.5708, + "r_joint6": 1.2217, + # ====== Grippers (0=open) ====== + "left_hand_joint_left": 0.0, + "left_hand_joint_right": 0.0, "right_hand_joint_left": 0.0, "right_hand_joint_right": 0.0, - # ====== 躯干 ====== + # ====== Trunk & Head ====== "PrismaticJoint": 0.26, - # ====== 头部 ====== "head_revoluteJoint": 0.0, }, - # Lower the spawn height to sit on the ground (was 0.05m above ground) pos=(0.0, 0.0, 0.0), ), actuators={ - "left_arm": ImplicitActuatorCfg( - joint_names_expr=["l_joint[1-6]"], - effort_limit_sim=100.0, - velocity_limit_sim=100.0, - # ✅ IK 控制需要高刚度 - stiffness=10000.0, - damping=1000.0, + # RM-65 J1-J3: shoulder/elbow — max 180°/s ≈ 3.14 rad/s + "left_arm_shoulder": ImplicitActuatorCfg( + joint_names_expr=["l_joint[1-3]"], + effort_limit_sim=50.0, + velocity_limit_sim=3.14, + stiffness=80.0, + damping=4.0, ), - "right_arm": ImplicitActuatorCfg( - joint_names_expr=["r_joint[1-6]"], - effort_limit_sim=100.0, - velocity_limit_sim=100.0, - stiffness=10000.0, - damping=1000.0, + # RM-65 J4-J6: wrist — max 225°/s ≈ 3.93 rad/s + "left_arm_wrist": ImplicitActuatorCfg( + joint_names_expr=["l_joint[4-6]"], + effort_limit_sim=20.0, + velocity_limit_sim=3.93, + stiffness=80.0, + damping=4.0, + ), + "right_arm_shoulder": ImplicitActuatorCfg( + joint_names_expr=["r_joint[1-3]"], + effort_limit_sim=50.0, + velocity_limit_sim=3.14, + stiffness=80.0, + damping=4.0, + ), + "right_arm_wrist": ImplicitActuatorCfg( + joint_names_expr=["r_joint[4-6]"], + effort_limit_sim=20.0, + velocity_limit_sim=3.93, + stiffness=80.0, + damping=4.0, ), "head": ImplicitActuatorCfg( joint_names_expr=["head_revoluteJoint"], - stiffness=10000.0, - damping=1000.0, + effort_limit_sim=12.0, + stiffness=80.0, + damping=4.0, ), "trunk": ImplicitActuatorCfg( joint_names_expr=["PrismaticJoint"], - stiffness=40000.0, - damping=4000.0, - effort_limit_sim=10000.0, + effort_limit_sim=200.0, velocity_limit_sim=0.2, + stiffness=1000.0, + damping=100.0, ), "wheels": ImplicitActuatorCfg( joint_names_expr=[".*_revolute_Joint"], + effort_limit_sim=200.0, stiffness=0.0, damping=100.0, - effort_limit_sim=200.0, - velocity_limit_sim=50.0, ), - # ✅ 夹爪：高刚度确保精准控制 "grippers": ImplicitActuatorCfg( joint_names_expr=[".*_hand_joint.*"], - stiffness=10000.0, - damping=1000.0, - effort_limit_sim=50.0, - velocity_limit_sim=50.0, + effort_limit_sim=200.0, + stiffness=2e3, + damping=1e2, ), }, + soft_joint_pos_limit_factor=1.0, ) +"""Base configuration for MindRobot. Gravity enabled, low PD gains.""" -## -# ✅ 专用于 IK 控制的高 PD 版本（可选但推荐） -## MINDBOT_HIGH_PD_CFG = MINDBOT_CFG.copy() -MINDBOT_HIGH_PD_CFG.actuators["left_arm"].stiffness = 40000.0 -MINDBOT_HIGH_PD_CFG.actuators["left_arm"].damping = 4000.0 +MINDBOT_HIGH_PD_CFG.spawn.rigid_props.disable_gravity = True +MINDBOT_HIGH_PD_CFG.actuators["left_arm_shoulder"].stiffness = 400.0 +MINDBOT_HIGH_PD_CFG.actuators["left_arm_shoulder"].damping = 80.0 +MINDBOT_HIGH_PD_CFG.actuators["left_arm_wrist"].stiffness = 400.0 +MINDBOT_HIGH_PD_CFG.actuators["left_arm_wrist"].damping = 80.0 +MINDBOT_HIGH_PD_CFG.actuators["right_arm_shoulder"].stiffness = 400.0 +MINDBOT_HIGH_PD_CFG.actuators["right_arm_shoulder"].damping = 80.0 +MINDBOT_HIGH_PD_CFG.actuators["right_arm_wrist"].stiffness = 400.0 +MINDBOT_HIGH_PD_CFG.actuators["right_arm_wrist"].damping = 80.0 +"""IK task-space control configuration. Gravity disabled, stiffer PD for tracking.""" diff --git a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_left_arm_ik_env_cfg.py b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_left_arm_ik_env_cfg.py index 709dd0b..3dd4211 100644 --- a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_left_arm_ik_env_cfg.py +++ b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_left_arm_ik_env_cfg.py @@ -5,6 +5,7 @@ from __future__ import annotations from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +import torch import isaaclab.sim as sim_utils from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg @@ -118,15 +119,15 @@ class MindRobotTeleopActionsCfg: arm_action = DifferentialInverseKinematicsActionCfg( asset_name="robot", joint_names=["l_joint[1-6]"], - body_name="Link_6", # Last link of left arm + body_name="left_hand_body", controller=DifferentialIKControllerCfg( command_type="pose", use_relative_mode=True, ik_method="dls", ), - scale=0.5, + scale=1, body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg( - pos=[0.0, 0.0, 0.0] + pos=[0.0, 0.0, 0.0], ), ) @@ -190,12 +191,10 @@ class MindRobotTeleopTerminationsCfg: # Events Configuration # ===================================================================== - @configclass class MindRobotTeleopEventsCfg: """Reset events for teleoperation: R键重置时将场景恢复到初始状态。""" - # 重置所有场景实体（机器人、物体）到其 init_state 定义的初始位置/姿态 reset_scene = EventTerm( func=mdp.reset_scene_to_default, mode="reset", @@ -253,8 +252,13 @@ class MindRobotLeftArmIKEnvCfg(ManagerBasedRLEnvCfg): self.sim.dt = 0.01 # 100Hz self.sim.render_interval = 2 - # Set MindRobot - self.scene.robot = MINDBOT_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + # Set MindRobot with FIXED root for teleoperation + # The original MINDBOT_HIGH_PD_CFG has fix_root_link=False (mobile base). + # For teleoperation, the base MUST be fixed to prevent the whole robot + # from sliding/tipping when IK applies torques to the arm joints. + robot_cfg = MINDBOT_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + robot_cfg.spawn.articulation_props.fix_root_link = True + self.scene.robot = robot_cfg # Configure end-effector frame transformer marker_cfg = FRAME_MARKER_CFG.copy() @@ -266,7 +270,7 @@ class MindRobotLeftArmIKEnvCfg(ManagerBasedRLEnvCfg): visualizer_cfg=marker_cfg, target_frames=[ FrameTransformerCfg.FrameCfg( - prim_path="{ENV_REGEX_NS}/Robot/rm_65_fb_left/Link_6", + prim_path="{ENV_REGEX_NS}/Robot/rm_65_fb_left/l_hand_01/left_hand_body", name="end_effector", offset=OffsetCfg( pos=[0.0, 0.0, 0.0], @@ -275,6 +279,7 @@ class MindRobotLeftArmIKEnvCfg(ManagerBasedRLEnvCfg): ], ) + # Configure teleoperation devices self.teleop_devices = DevicesCfg( devices={