VR

scripts/environments/teleoperation/teleop_xr_agent.py

@@ -188,40 +188,37 @@ class XrTeleopController:
             self.grip_active = True
             return self.get_zero_action(trigger, current_eef_pos, current_eef_quat)
 
-        xr_delta_pos = raw_pose[:3] - self.prev_xr_pos
-        pos_norm = np.linalg.norm(xr_delta_pos)
-        
-        # PICO -> Isaac World mapping:
-        # 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
+        # Since OpenXR headset zero is not guaranteed to match robot zero, we compute the
+        # raw transformation in World Frame, but apply it relatively to the stored target.
 
-        # Rotation Delta
-        xr_delta_rot = quat_diff_as_rotvec_xyzw(self.prev_xr_quat, raw_pose[3:])
-        rot_norm = np.linalg.norm(xr_delta_rot)
-        
-        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
+        # 1. First, find current XR pose projected into World frame
+        xr_world_pos, xr_world_quat_xyzw = transform_xr_pose(raw_pose[:3], raw_pose[3:])
+        prev_xr_world_pos, prev_xr_world_quat_xyzw = transform_xr_pose(self.prev_xr_pos, self.prev_xr_quat)
 
-        # Gripper
+        # 2. Extract Delta POS in World frame
+        world_delta_pos = (xr_world_pos - prev_xr_world_pos) * self.pos_sensitivity
+        pos_norm = np.linalg.norm((raw_pose[:3] - self.prev_xr_pos))
+
+        # 3. Extract Delta ROT in World frame
+        world_delta_rot = quat_diff_as_rotvec_xyzw(prev_xr_world_quat_xyzw, xr_world_quat_xyzw) * self.rot_sensitivity
+        rot_norm = np.linalg.norm(quat_diff_as_rotvec_xyzw(self.prev_xr_quat, raw_pose[3:]))
+
+        # 4. Gripper
         gripper_action = 1.0 if trigger > 0.5 else -1.0
 
         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
+            # Position update (simple translation in world frame)
+            self.target_eef_pos += world_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
+            # Rotation update: apply delta_R (in world frame) to target_R (in world frame)
+            # R_new = R_delta @ R_target
             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
+            delta_r = R.from_rotvec(world_delta_rot)
+            new_r = delta_r * target_r
             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]])
 
@@ -237,15 +234,15 @@ class XrTeleopController:
             # 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
+                world_delta_pos = (world_delta_pos * max_pos_delta / pos_norm)
 
             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
+                world_delta_rot = (world_delta_rot * max_rot_delta / rot_norm)
 
             action = torch.tensor([
-                delta_pos[0], delta_pos[1], delta_pos[2], 
-                delta_rot[0], delta_rot[1], delta_rot[2], 
+                world_delta_pos[0], world_delta_pos[1], world_delta_pos[2], 
+                world_delta_rot[0], world_delta_rot[1], world_delta_rot[2], 
                 gripper_action], dtype=torch.float32)
                 
             self.prev_xr_pos = raw_pose[:3].copy()
@@ -259,8 +256,8 @@ class XrTeleopController:
             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(f"| XR Delta Pos (world):     {world_delta_pos}  (norm={pos_norm:.4f})")
+            print(f"| XR Delta Rot (world):     {world_delta_rot}  (norm={rot_norm:.4f})")
             if self.is_absolute:
                 print(f"| Targ Pos (dx,dy,dz):      {action[:3].numpy()}")
                 print(f"| Targ Quat (wxyz):         {action[3:7].numpy()}")