From 206830c5308fff0173640f8da5d56a922a940f38 Mon Sep 17 00:00:00 2001 From: Joe DiPrima Date: Tue, 17 Feb 2026 22:53:29 -0600 Subject: [PATCH] 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 --- teleop/robot_control/robot_arm_ik.py | 23 ++++++++++ teleop/teleop_hand_and_arm.py | 68 +++++++++++++++++++++++++--- 2 files changed, 85 insertions(+), 6 deletions(-) diff --git a/teleop/robot_control/robot_arm_ik.py b/teleop/robot_control/robot_arm_ik.py index c7645d2..017ba99 100644 --- a/teleop/robot_control/robot_arm_ik.py +++ b/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) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index e630ba8..a454e0f 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/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,16 +468,33 @@ if __name__ == '__main__': dt = min(now - last_loop_time, 0.1) # clamp to prevent huge jumps after stalls last_loop_time = now - left_wrist_adjusted = tele_data.left_wrist_pose.copy() - right_wrist_adjusted = tele_data.right_wrist_pose.copy() + # --- 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() if args.ki > 0.0 and INTEGRAL_ENABLED: # 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