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