Add start-time calibration for relative-mode arm tracking

Captures VP wrist poses and robot FK poses when tracking starts (A/r),
then maps all subsequent VP input as deltas from the user's reference
onto the robot's reference frame. This eliminates the arm skew that
occurred because the system was mapping absolute VP positions directly.

- Add compute_fk_pose() returning full 4x4 SE(3) matrices from Pinocchio FK
- Calibrate on every START transition (initial + resume after pause)
- Position: target = robot_ref + (vp_current - vp_ref)
- Rotation: target_R = (vp_R @ vp_ref_R^T) @ robot_ref_R
- Arm reset (X) auto-pauses and invalidates calibration
- I-term error now uses calibrated target instead of raw VP
- --no-calibration flag preserves old absolute-mode behavior

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

teleop/robot_control/robot_arm_ik.py

@@ -326,6 +326,29 @@ class G1_29_ArmIK:
         right_ee_pos = self.reduced_robot.data.oMf[self.R_hand_id].translation.copy()
         return left_ee_pos, right_ee_pos
 
+    def compute_fk_pose(self, current_lr_arm_q):
+        """Compute full FK SE(3) poses for both end-effectors.
+
+        Args:
+            current_lr_arm_q: 14-dim numpy array of current joint angles
+
+        Returns:
+            left_ee_pose: 4x4 numpy array, homogeneous transform of L_ee in waist frame
+            right_ee_pose: 4x4 numpy array, homogeneous transform of R_ee in waist frame
+        """
+        q = np.array(current_lr_arm_q)
+        pin.forwardKinematics(self.reduced_robot.model, self.reduced_robot.data, q)
+        pin.updateFramePlacements(self.reduced_robot.model, self.reduced_robot.data)
+        left_se3 = self.reduced_robot.data.oMf[self.L_hand_id]
+        right_se3 = self.reduced_robot.data.oMf[self.R_hand_id]
+        left_pose = np.eye(4)
+        left_pose[:3, :3] = left_se3.rotation.copy()
+        left_pose[:3, 3] = left_se3.translation.copy()
+        right_pose = np.eye(4)
+        right_pose[:3, :3] = right_se3.rotation.copy()
+        right_pose[:3, 3] = right_se3.translation.copy()
+        return left_pose, right_pose
+
 class G1_23_ArmIK:
     def __init__(self, Unit_Test = False, Visualization = False):
         np.set_printoptions(precision=5, suppress=True, linewidth=200)

teleop/teleop_hand_and_arm.py

@@ -118,6 +118,8 @@ if __name__ == '__main__':
     parser.add_argument('--ki', type=float, default=0.3, help='Integral gain for drift correction (0.0 = disabled)')
     parser.add_argument('--i-clamp', type=float, default=0.10, help='Anti-windup clamp for I term in meters (default: 0.10)')
     parser.add_argument('--i-decay', type=float, default=0.995, help='Per-frame decay factor for I term (default: 0.995)')
+    parser.add_argument('--no-calibration', action='store_true',
+                        help='Disable start-time calibration (use absolute VP poses)')
     # record mode and task info
     parser.add_argument('--record', action = 'store_true', help = 'Enable data recording mode')
     parser.add_argument('--task-dir', type = str, default = './utils/data/', help = 'path to save data')
@@ -307,6 +309,26 @@ if __name__ == '__main__':
         last_loop_time = time.time()
         i_frame_count = 0
 
+        # Relative-mode calibration state
+        calibrated = False
+        vp_ref_left = None       # 4x4: VP wrist pose at calibration moment
+        vp_ref_right = None
+        robot_ref_left = None    # 4x4: Robot FK pose at calibration moment
+        robot_ref_right = None
+        prev_start = False
+
+        # Initial calibration: capture reference poses on first START
+        if not args.no_calibration:
+            _cal_tele = tv_wrapper.get_tele_data()
+            _cal_arm_q = arm_ctrl.get_current_dual_arm_q()
+            robot_ref_left, robot_ref_right = arm_ik.compute_fk_pose(_cal_arm_q)
+            vp_ref_left = _cal_tele.left_wrist_pose.copy()
+            vp_ref_right = _cal_tele.right_wrist_pose.copy()
+            calibrated = True
+            prev_start = True
+            logger_mp.info(f"[calibration] Initial reference captured. "
+                           f"Robot L={robot_ref_left[:3,3]}, R={robot_ref_right[:3,3]}")
+
         # main loop. robot start to follow VR user's motion
         while not STOP:
             start_time = time.time()
@@ -323,7 +345,10 @@ if __name__ == '__main__':
                     if np.all(np.abs(arm_ctrl.get_current_dual_arm_q()) < 0.1):
                         break
                     time.sleep(0.05)
-                logger_mp.info("[main] Arms reset complete, resuming tracking")
+                logger_mp.info("[main] Arms reset complete. Tracking PAUSED — press A/r to recalibrate and resume.")
+                START = False
+                prev_start = False
+                calibrated = False
                 arm_ctrl.speed_gradual_max()
                 last_loop_time = time.time()
                 continue
@@ -349,6 +374,20 @@ if __name__ == '__main__':
                 logger_mp.info("[R3] Start pressed → Toggle recording")
             _r3_prev_buttons = r3_btns
 
+            # Detect START resume transition → re-calibrate
+            if START and not prev_start and not args.no_calibration:
+                _cal_tele = tv_wrapper.get_tele_data()
+                _cal_arm_q = arm_ctrl.get_current_dual_arm_q()
+                robot_ref_left, robot_ref_right = arm_ik.compute_fk_pose(_cal_arm_q)
+                vp_ref_left = _cal_tele.left_wrist_pose.copy()
+                vp_ref_right = _cal_tele.right_wrist_pose.copy()
+                calibrated = True
+                i_error_left = np.zeros(3)
+                i_error_right = np.zeros(3)
+                logger_mp.info(f"[calibration] Re-calibrated on resume. "
+                               f"Robot L={robot_ref_left[:3,3]}, R={robot_ref_right[:3,3]}")
+            prev_start = START
+
             # Skip arm tracking while paused (buttons + reset still work)
             if not START:
                 i_error_left = np.zeros(3)
@@ -429,6 +468,23 @@ if __name__ == '__main__':
             dt = min(now - last_loop_time, 0.1)  # clamp to prevent huge jumps after stalls
             last_loop_time = now
 
+            # --- Apply relative-mode calibration ---
+            if calibrated:
+                # Position: robot_ref + (vp_current - vp_ref)
+                left_wrist_adjusted = np.eye(4)
+                right_wrist_adjusted = np.eye(4)
+                left_wrist_adjusted[:3, 3] = robot_ref_left[:3, 3] + (
+                    tele_data.left_wrist_pose[:3, 3] - vp_ref_left[:3, 3])
+                right_wrist_adjusted[:3, 3] = robot_ref_right[:3, 3] + (
+                    tele_data.right_wrist_pose[:3, 3] - vp_ref_right[:3, 3])
+                # Rotation: delta_R @ robot_ref_R  where delta_R = vp_R @ vp_ref_R^T
+                left_wrist_adjusted[:3, :3] = (
+                    tele_data.left_wrist_pose[:3, :3] @ vp_ref_left[:3, :3].T
+                ) @ robot_ref_left[:3, :3]
+                right_wrist_adjusted[:3, :3] = (
+                    tele_data.right_wrist_pose[:3, :3] @ vp_ref_right[:3, :3].T
+                ) @ robot_ref_right[:3, :3]
+            else:
                 left_wrist_adjusted = tele_data.left_wrist_pose.copy()
                 right_wrist_adjusted = tele_data.right_wrist_pose.copy()
 
@@ -436,9 +492,9 @@ if __name__ == '__main__':
                 # FK: where are the robot hands actually?
                 left_ee_pos, right_ee_pos = arm_ik.compute_fk(current_lr_arm_q)
 
-                # Error: target - actual (task-space position)
-                left_error = tele_data.left_wrist_pose[:3, 3] - left_ee_pos
-                right_error = tele_data.right_wrist_pose[:3, 3] - right_ee_pos
+                # Error: calibrated target - actual (task-space position)
+                left_error = left_wrist_adjusted[:3, 3] - left_ee_pos
+                right_error = right_wrist_adjusted[:3, 3] - right_ee_pos
 
                 # Leaky integrator with anti-windup
                 i_error_left = i_error_left * args.i_decay + left_error * dt