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

spacemouse ok

Browse Source
This commit is contained in:
yt lee
2026-03-05 22:41:56 +08:00
parent 2c9db33517
commit cc64e654ff
6 changed files with 837 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

335
scripts/environments/teleoperation/teleop_xr_agent.py Normal file
View File

@@ -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()

33
scripts/environments/teleoperation/xr_utils/__init__.py Normal file
View File

@@ -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",
]

203
scripts/environments/teleoperation/xr_utils/geometry.py Normal file
View File

@@ -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

169
scripts/environments/teleoperation/xr_utils/xr_client.py Normal file
View File

@@ -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.")

153
source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_cfg.py
View File

@@ -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."""

21
source/mindbot/mindbot/tasks/manager_based/il/open_drybox/mindrobot_left_arm_ik_env_cfg.py
View File

@@ -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={