From 7e0f4a3e758015ad12af638f936f26322f0906fe Mon Sep 17 00:00:00 2001 From: yt lee Date: Fri, 6 Mar 2026 11:11:06 +0800 Subject: [PATCH] 1 --- README.md | 6 +- .../teleoperation/teleop_xr_agent.py | 185 ++++++++++++------ .../manager_based/il/open_drybox/__init__.py | 11 +- .../mindrobot_left_arm_ik_env_cfg.py | 10 + 4 files changed, 151 insertions(+), 61 deletions(-) diff --git a/README.md b/README.md index ead3f36..a0a8060 100644 --- a/README.md +++ b/README.md @@ -132,4 +132,8 @@ Some examples of packages that can likely be excluded are: "/extscache/omni.graph.*" // Graph UI tools "/extscache/omni.services.*" // Services tools ... -``` \ No newline at end of file +``` + + + +python scripts/environments/teleoperation/teleop_se3_agent.py --task Isaac-MindRobot-LeftArm-IK-Rel-v0 --teleop_device spacemouse \ No newline at end of file diff --git a/scripts/environments/teleoperation/teleop_xr_agent.py b/scripts/environments/teleoperation/teleop_xr_agent.py index 3dfd51e..2071de0 100644 --- a/scripts/environments/teleoperation/teleop_xr_agent.py +++ b/scripts/environments/teleoperation/teleop_xr_agent.py @@ -62,6 +62,7 @@ simulation_app = app_launcher.app import gymnasium as gym import numpy as np import torch +from scipy.spatial.transform import Rotation as R from isaaclab.envs import ManagerBasedRLEnvCfg @@ -72,6 +73,7 @@ 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 # ===================================================================== @@ -79,11 +81,12 @@ from xr_utils.geometry import R_HEADSET_TO_WORLD class XrTeleopController: """Teleop controller for PICO XR headset.""" - def __init__(self, arm="left", pos_sensitivity=1.0, rot_sensitivity=0.3): + def __init__(self, arm="left", pos_sensitivity=1.0, rot_sensitivity=0.3, is_absolute=False): self.xr_client = XrClient() self.pos_sensitivity = pos_sensitivity self.rot_sensitivity = rot_sensitivity self.arm = arm + self.is_absolute = is_absolute self.controller_name = "left_controller" if arm == "left" else "right_controller" self.grip_name = "left_grip" if arm == "left" else "right_grip" @@ -92,9 +95,14 @@ class XrTeleopController: # Coordinate transform matrix self.R_headset_world = R_HEADSET_TO_WORLD - # Raw XR tracking space poses (NOT transformed) + # Raw XR tracking space poses self.prev_xr_pos = None self.prev_xr_quat = None + + # Absolute target states + self.target_eef_pos = None + self.target_eef_quat = None # wxyz + self.grip_active = False self.frame_count = 0 @@ -113,10 +121,30 @@ class XrTeleopController: def close(self): self.xr_client.close() - def advance(self) -> torch.Tensor: + def get_zero_action(self, trigger, current_eef_pos=None, current_eef_quat=None): + if self.is_absolute: + action = torch.zeros(8) + # Stay at the current valid pose, or fallback to the cached target + if 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()) + elif self.target_eef_pos is not None and self.target_eef_quat is not None: + action[:3] = torch.tensor(self.target_eef_pos.copy()) + action[3:7] = torch.tensor(self.target_eef_quat.copy()) + else: + action[3] = 1.0 # default w=1 for quat + action[7] = 1.0 if trigger > 0.5 else -1.0 + else: + action = torch.zeros(7) + action[6] = 1.0 if trigger > 0.5 else -1.0 + return action + + def advance(self, current_eef_pos=None, current_eef_quat=None) -> torch.Tensor: """ - Reads the XR controller and returns the 7D action tensor: - [dx, dy, dz, droll, dpitch, dyaw, gripper_cmd] + Reads the XR controller. + Relative bounds return 7D action tensor: [dx, dy, dz, drx, dry, drz, gripper] + Absolute bounds return 8D action tensor: [x, y, z, qw, qx, qy, qz, gripper] + Note: current_eef_quat expects wxyz. """ # XR buttons check (e.g. A or B for reset) try: @@ -131,85 +159,114 @@ class XrTeleopController: 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) + return self.get_zero_action(0.0, current_eef_pos, current_eef_quat) - # Skip transformation if quaternion is invalid (e.g. before headset truly connects) + # Skip transformation if quaternion is invalid 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 + return self.get_zero_action(trigger, current_eef_pos, current_eef_quat) # 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 时才移动机械臂 + # 握持键作为离合器 (Clutch) 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 + # Ensure target tracks the current pose while we are not grabbing + if self.is_absolute and 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() # wxyz + return self.get_zero_action(trigger, current_eef_pos, current_eef_quat) 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() + if self.is_absolute and 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() # wxyz self.grip_active = True - action = torch.zeros(7) - action[6] = 1.0 if trigger > 0.5 else -1.0 - return action + return self.get_zero_action(trigger, current_eef_pos, current_eef_quat) - # ========== 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 + # XR +X (Right) -> World -Y + # XR +Y (Up) -> World +Z + # XR +Z (Back) -> World -X + raw_to_world_delta_pos = np.array([-xr_delta_pos[2], -xr_delta_pos[0], xr_delta_pos[1]]) + delta_pos = raw_to_world_delta_pos * self.pos_sensitivity - # ========== 2. Rotation Delta (calculate in XR frame, map to World) ========== - # compute pure relative angular difference in local XR tracking space + # Rotation Delta 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 + + raw_to_world_delta_rot = np.array([-xr_delta_rot[2], -xr_delta_rot[0], xr_delta_rot[1]]) + delta_rot = raw_to_world_delta_rot * 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 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) + if self.is_absolute: + # Apply delta integrally to target pose + if self.target_eef_pos is None: + # Failsafe if we don't have eef_pos injected yet + 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 += delta_pos + + # Multiply rotation. Target quat is wxyz. Delta rot is rotvec in world frame. + # scipy Rotation uses xyzw ! So convert wxyz -> xyzw, multiply, then -> wxyz + target_r = R.from_quat([self.target_eef_quat[1], self.target_eef_quat[2], self.target_eef_quat[3], self.target_eef_quat[0]]) + delta_r = R.from_rotvec(delta_rot) + new_r = delta_r * target_r # Apply delta rotation in global space + new_rot_xyzw = new_r.as_quat() + self.target_eef_quat = np.array([new_rot_xyzw[3], new_rot_xyzw[0], new_rot_xyzw[1], new_rot_xyzw[2]]) + + action = torch.tensor([ + self.target_eef_pos[0], self.target_eef_pos[1], self.target_eef_pos[2], + self.target_eef_quat[0], self.target_eef_quat[1], self.target_eef_quat[2], self.target_eef_quat[3], + gripper_action], dtype=torch.float32) + + self.prev_xr_pos = raw_pose[:3].copy() + self.prev_xr_quat = raw_pose[3:].copy() + + else: + # Relative clamping limits (used by relative mode to avoid divergence) + max_pos_delta = 0.04 + if pos_norm > max_pos_delta: + delta_pos = (raw_to_world_delta_pos * max_pos_delta / pos_norm) * self.pos_sensitivity + + max_rot_delta = 0.02 + if rot_norm > max_rot_delta: + delta_rot = (raw_to_world_delta_rot * max_rot_delta / rot_norm) * self.rot_sensitivity + + 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) + + self.prev_xr_pos = raw_pose[:3].copy() + self.prev_xr_quat = raw_pose[3:].copy() + + self.frame_count += 1 - # ========== 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"| Task Mode: {'ABSOLUTE' if self.is_absolute else 'RELATIVE'}") 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()}") + if self.is_absolute: + print(f"| Targ Pos (dx,dy,dz): {action[:3].numpy()}") + print(f"| Targ Quat (wxyz): {action[3:7].numpy()}") + else: + 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("===============================================================") @@ -240,11 +297,15 @@ def main() -> None: return # 3. Teleoperation Interface Initialization + is_abs_mode = "Abs" in args_cli.task + print(f"\n[INFO] Connecting to PICO XR Headset using {args_cli.arm} controller...") + print(f"[INFO] Using IK Mode: {'ABSOLUTE' if is_abs_mode else 'RELATIVE'}") 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 + rot_sensitivity=args_cli.sensitivity * (1.0 if is_abs_mode else 0.3), # Absolute rotation handles 1:1 better than relative + is_absolute=is_abs_mode ) should_reset = False @@ -265,14 +326,20 @@ def main() -> None: print(" 🕹️ Press B or Y to reset the environment.") print("=" * 50 + "\n") - # 4. Simulation loop device = env.unwrapped.device sim_frame = 0 + obs, _ = env.reset() + while simulation_app.is_running(): try: with torch.inference_mode(): - # Get action from XR Controller [1, 7] - action_np = teleop_interface.advance() + # Extract tracking pose to seed absolute IK + policy_obs = obs["policy"] + eef_pos = policy_obs["eef_pos"][0].cpu().numpy() + eef_quat = policy_obs["eef_quat"][0].cpu().numpy() + + # Get action from XR Controller + action_np = teleop_interface.advance(current_eef_pos=eef_pos, current_eef_quat=eef_quat) actions = action_np.unsqueeze(0).repeat(env.num_envs, 1).to(device) # Step environment @@ -298,16 +365,16 @@ def main() -> None: 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}") + # Find arm joint indices dynamically by looking at the first 6-7 joints that aren't fingers or hands + arm_idx = [i for i, n in enumerate(jnames) if "finger" not in n and "hand" not in n][:7] + print(f" Deduced 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")] + env._arm_idx_cache = [i for i, n in enumerate(jnames) if "finger" not in n and "hand" not in n][:7] arm_idx = env._arm_idx_cache arm_joints = joint_pos[arm_idx] diff --git a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/__init__.py b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/__init__.py index 804a0a4..2d2a6f9 100644 --- a/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/__init__.py +++ b/source/mindbot/mindbot/tasks/manager_based/il/open_drybox/__init__.py @@ -37,4 +37,13 @@ gym.register( }, disable_env_checker=True, ) -# ...existing code... \ No newline at end of file + +gym.register( + id="Isaac-MindRobot-LeftArm-IK-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.mindrobot_left_arm_ik_env_cfg:MindRobotLeftArmIKAbsEnvCfg", + "robomimic_bc_cfg_entry_point": f"{__name__}.agents:robomimic/bc_rnn_low_dim.json", + }, + disable_env_checker=True, +) \ No newline at end of file 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 3dd4211..d162115 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 @@ -305,3 +305,13 @@ class MindRobotLeftArmIKEnvCfg(ManagerBasedRLEnvCfg): self.gripper_joint_names = ["left_hand_joint_left", "left_hand_joint_right"] self.gripper_open_val = 0.0 self.gripper_threshold = 0.005 + + +@configclass +class MindRobotLeftArmIKAbsEnvCfg(MindRobotLeftArmIKEnvCfg): + """MindRobot 左臂 IK-Abs 遥操作环境配置（绝对坐标系，适合 VR 控制器）。""" + + def __post_init__(self): + super().__post_init__() + # Switch controller to absolute mode + self.actions.arm_action.controller.use_relative_mode = False