diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index a454e0f..0148d2f 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -66,6 +66,23 @@ def parse_r3_buttons(data): 'Start': bool(btn1 & 0x04), } +def scale_rotation(R, alpha): + """Scale a rotation matrix's angle by alpha (0=identity, 1=original). Pure numpy.""" + cos_theta = np.clip((np.trace(R) - 1.0) / 2.0, -1.0, 1.0) + theta = np.arccos(cos_theta) + if theta < 1e-6: + return np.eye(3) + axis = np.array([ + R[2, 1] - R[1, 2], + R[0, 2] - R[2, 0], + R[1, 0] - R[0, 1] + ]) / (2.0 * np.sin(theta)) + scaled = theta * alpha + K = np.array([[0, -axis[2], axis[1]], + [axis[2], 0, -axis[0]], + [-axis[1], axis[0], 0]]) + return np.eye(3) + np.sin(scaled) * K + (1.0 - np.cos(scaled)) * (K @ K) + # Previous button state for edge detection _r3_prev_buttons = {} @@ -120,6 +137,10 @@ if __name__ == '__main__': 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)') + parser.add_argument('--settle-time', type=float, default=0.5, + help='Seconds to ramp tracking after calibration (default: 0.5)') + parser.add_argument('--rot-blend', type=float, default=0.3, + help='Rotation tracking blend (0=position-only, 1=full rotation, default: 0.3)') # 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') @@ -311,6 +332,7 @@ if __name__ == '__main__': # Relative-mode calibration state calibrated = False + calibration_time = 0.0 # When calibration was captured (for settle ramp) 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 @@ -325,6 +347,7 @@ if __name__ == '__main__': vp_ref_left = _cal_tele.left_wrist_pose.copy() vp_ref_right = _cal_tele.right_wrist_pose.copy() calibrated = True + calibration_time = time.time() 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]}") @@ -382,6 +405,7 @@ if __name__ == '__main__': vp_ref_left = _cal_tele.left_wrist_pose.copy() vp_ref_right = _cal_tele.right_wrist_pose.copy() calibrated = True + calibration_time = time.time() i_error_left = np.zeros(3) i_error_right = np.zeros(3) logger_mp.info(f"[calibration] Re-calibrated on resume. " @@ -470,20 +494,29 @@ if __name__ == '__main__': # --- Apply relative-mode calibration --- if calibrated: - # Position: robot_ref + (vp_current - vp_ref) + # Settle ramp: smoothly blend from 0% to 100% delta over settle_time + settle_elapsed = now - calibration_time + settle_alpha = min(1.0, settle_elapsed / max(args.settle_time, 0.01)) + + # Position: robot_ref + settle_alpha * (vp_current - vp_ref) + left_delta_pos = tele_data.left_wrist_pose[:3, 3] - vp_ref_left[:3, 3] + right_delta_pos = tele_data.right_wrist_pose[:3, 3] - vp_ref_right[:3, 3] + 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] + left_wrist_adjusted[:3, 3] = robot_ref_left[:3, 3] + settle_alpha * left_delta_pos + right_wrist_adjusted[:3, 3] = robot_ref_right[:3, 3] + settle_alpha * right_delta_pos + + # Rotation: compute delta, scale by rot_blend (and settle_alpha) + left_delta_R = tele_data.left_wrist_pose[:3, :3] @ vp_ref_left[:3, :3].T + right_delta_R = tele_data.right_wrist_pose[:3, :3] @ vp_ref_right[:3, :3].T + rot_alpha = args.rot_blend * settle_alpha + if rot_alpha < 0.01: + left_wrist_adjusted[:3, :3] = robot_ref_left[:3, :3] + right_wrist_adjusted[:3, :3] = robot_ref_right[:3, :3] + else: + left_wrist_adjusted[:3, :3] = scale_rotation(left_delta_R, rot_alpha) @ robot_ref_left[:3, :3] + right_wrist_adjusted[:3, :3] = scale_rotation(right_delta_R, rot_alpha) @ robot_ref_right[:3, :3] else: left_wrist_adjusted = tele_data.left_wrist_pose.copy() right_wrist_adjusted = tele_data.right_wrist_pose.copy()