1

README.md

@@ -132,4 +132,8 @@ Some examples of packages that can likely be excluded are:
 "<path-to-isaac-sim>/extscache/omni.graph.*"        // Graph UI tools
 "<path-to-isaac-sim>/extscache/omni.services.*"     // Services tools
 ...
-```
+```
+
+
+
+python scripts/environments/teleoperation/teleop_se3_agent.py --task Isaac-MindRobot-LeftArm-IK-Rel-v0 --teleop_device spacemouse

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]
 

source/mindbot/mindbot/tasks/manager_based/il/open_drybox/__init__.py

@@ -37,4 +37,13 @@ gym.register(
     },
     disable_env_checker=True,
 )
-# ...existing code...
+
+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,
+)

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