import cv2 import os from datetime import datetime import numpy as np import sys from pathlib import Path import time from scipy.spatial.transform import Rotation as R sys.path.append(str(Path(__file__).resolve().parent.parent)) sys.path.append('/home/jsfb/jsfb_ws/MassageRobot_Dobot/Massage/MassageControl') from hardware.remote_cam import ToolCamera from hardware.aubo_C5 import AuboC5 np.set_printoptions(precision=8, suppress=True) # 设置寻找亚像素角点的参数，采用的停止准则是最大循环次数30和最大误差容限0.001 criteria = (cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 0.001) # 标定计算类 # -- 连接AuboC5 # -- 连接 ToolCamera # -- 运动到初始位置 计算下一步运动位姿 # -- 开始手眼标定 # -- 输出结果 class Calibration: def __init__(self, board_size, square_size, file_folder): """ 初始化标定计算类。 board_size: 标定板的格子数，格式为 (width, height) square_size: 格子的大小，单位为米 """ self.board_size = board_size self.square_size = square_size self.file_address = file_folder self.objp = self._create_object_points() self.image_points = [] self.robot_poses = [] self.cam = ToolCamera(host='127.0.0.1') self.cam.start() self.size = None self.intrinsics_mtx = None self.disorder = None rgb_image, depth_image, camera_intrinsics = self.cam.get_latest_frame() if rgb_image is None : print(f'===============相机连接失败请检查并重试==============') else: print(f'===============相机连接成功==============') self.size = rgb_image.shape[:2] self.intrinsics = camera_intrinsics self.arm = AuboC5() self.arm.init() self.obj_points = [] print(f'===============机械臂连接成功==============') def _create_object_points(self): """创建标定板的3D点""" objp = np.zeros((self.board_size[0] * self.board_size[1], 3), np.float32) objp[:, :2] = np.mgrid[0:self.board_size[0], 0:self.board_size[1]].T.reshape(-1, 2) * self.square_size return objp def collect_data(self): #移动到初始位置 pose = [(90) * (np.pi / 180), 4.23 * (np.pi / 180), 124.33 * (np.pi / 180), 61.56 * (np.pi / 180), -89.61 * (np.pi / 180), 0 * (np.pi / 180)] code = self.arm.move_joint(pose,4,wait=True) if code == -1: print("运动到拍照位置失败") #拍照 self.check_corners() #运动到下一个位置 # 目前只设计了44条轨迹 for i in range (0,45): next_pose = self.get_next_pose_ugly(i) self.arm.move_joint(next_pose, 4,wait=True) if code == -1: print("运动到拍照位置失败") else: self.check_corners() def check_corners(self): rgb, depth, intrinsics = self.cam.get_latest_frame() gray = cv2.cvtColor(rgb, cv2.COLOR_BGR2GRAY) ret, corners = cv2.findChessboardCorners(gray, self.board_size, None) if ret: robot_pose = self.get_pose_homomat() self.add_sample(gray,corners,robot_pose) print(f"robot_pose = {robot_pose}") pic_name = self.file_address + "/"+str(len(self.image_points)) + ".jpg" print(f"save path as: {pic_name}") # 绘制检测到的角点 (用于可视化，可选) cv2.drawChessboardCorners(rgb, self.board_size, corners, ret) cv2.imwrite(pic_name, rgb) return True return False def add_sample(self, gray, corners,robot_pose): """ 添加标定样本。 image: 捕获的图像 robot_pose: 机械臂的位姿，格式为 [x, y, z, roll, pitch, yaw] """ # 检测棋盘角点 self.obj_points.append(self.objp) corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), criteria) # 在原角点的基础上寻找亚像素角点 if [corners2]: self.image_points.append(corners2) else: self.image_points.append(corners) self.robot_poses.append(robot_pose) def get_pose_homomat(self): pose = self.arm.robot_rpc_client.getRobotInterface(self.arm.robot_name).getRobotState().getTcpPose() x, y, z, rx, ry, rz = pose r = R.from_euler('xyz', [rx, ry, rz]) rotation_matrix = r.as_matrix() translation_vector = np.array([x, y, z]).reshape(3, 1) homo_mat = np.eye(4) # 创建 4x4 单位矩阵 homo_mat[:3, :3] = rotation_matrix homo_mat[:3, 3] = translation_vector.flatten() return homo_mat def camera_calib(self): # print(f"size is {self.size}") ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(self.obj_points, self.image_points, (self.size[1], self.size[0]), None, None) # print("rvecs,tvecs:",rvecs,tvecs) if ret: print("内参矩阵:\n", mtx) # 内参数矩阵 print("畸变系数:\n", dist) # 畸变系数 distortion cofficients = (k_1,k_2,p_1,p_2,k_3) print("++++++++++相机标定完成++++++++++++++") self.intrinsics_mtx = mtx self.disorder = dist else: print("-----------相机标定失败--------------") return rvecs, tvecs def split_robot_pose(self): """ 将包含 4x4 齐次矩阵的 robot_pose 列表拆分成 R_arm 和 t_arm。 Args: robot_pose: 包含 4x4 齐次矩阵的 Python list。 Returns: R_arm: 旋转矩阵的 Python list, 每个元素是 3x3 numpy 数组。 t_arm: 平移向量的 Python list, 每个元素是 3x1 numpy 数组。 """ R_arm = [] t_arm = [] for pose_matrix in self.robot_poses: # # 1. 提取旋转矩阵 (3x3) rotation_matrix = pose_matrix[:3, :3] R_arm.append(rotation_matrix) # R_arm.append(pose_matrix[:3]) # 2. 提取平移向量 (3x1) translation_vector = pose_matrix[:3, 3] # 获取齐次矩阵的平移部分 (1x3 numpy 数组) translation_vector = translation_vector.reshape(3, 1) # 转换为 3x1 的列向量 t_arm.append(translation_vector) return R_arm, t_arm def calibrate(self): rvecs, tvecs = self.camera_calib() r_arm, t_arm= self.split_robot_pose() rvecs_array = np.array(rvecs) tvecs_array = np.array(tvecs) t_arma = np.array(t_arm) r_arma = np.array(r_arm) print(rvecs_array.shape) print(tvecs_array.shape) print(t_arma.shape) print(r_arma.shape) R, t = cv2.calibrateHandEye(r_arma, t_arma, rvecs_array, tvecs_array, cv2.CALIB_HAND_EYE_TSAI) print("+++++++++++手眼标定完成+++++++++++++++") return R, t ,self.intrinsics_mtx #得到图片以后 根据pose判断方向： def get_next_pose_future(self): return None def get_next_pose_ugly(self,index): poselist =[ [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1267845650875392, -1.5116296651522887, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.4592697875924587, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0220648099678793, -1.5116296651522887, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9697049324080494, -1.4592697875924587, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9173450548482196, -1.4069099100326288, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1267845650875392, -1.6163494202719486, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.6687092978317786, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.2315043202071991, -1.7210691753916085, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.109331272567596, -1.4941763726323454, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1442378576074825, -1.424363202552572, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.3545500324727988, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.2140510276872558, -1.2847368623930255, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.109331272567596, -1.633802712791892, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1442378576074825, -1.7036158828716652, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0395181024878226, -1.4941763726323454, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.004611517447936, -1.424363202552572, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9697049324080493, -1.3545500324727988, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9347983473681627, -1.2847368623930255, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0], [1.5707963267948966, 0.06510078109938851, 2.16996785900455, 1.0395181024878226, -1.4941763726323454, 0.0], [1.5707963267948966, 0.05637413483941686, 2.16996785900455, 1.004611517447936, -1.424363202552572, 0.0], [1.5707963267948966, 0.047647488579445216, 2.16996785900455, 0.9697049324080493, -1.3545500324727988, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0], [1.562069680534925, 0.07382742735936015, 2.16996785900455, 1.0395181024878226, -1.4941763726323454, 0.0], [1.5533430342749535, 0.07382742735936015, 2.16996785900455, 1.004611517447936, -1.424363202552572, 0.0], [1.544616388014982, 0.07382742735936015, 2.16996785900455, 0.9697049324080493, -1.3545500324727988, 0.0], [1.5358897417550104, 0.07382742735936015, 2.16996785900455, 0.9347983473681627, -1.2847368623930255, 0.0], [1.5271630954950388, 0.07382742735936015, 2.16996785900455, 0.8998917623282761, -1.2149236923132523, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0], [1.579522973054868, 0.07382742735936015, 2.16996785900455, 1.109331272567596, -1.5988961277520053, 0.0], [1.5882496193148397, 0.07382742735936015, 2.16996785900455, 1.1442378576074825, -1.633802712791892, 0.0], [1.5969762655748112, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.6687092978317786, 0.0], [1.6057029118347828, 0.07382742735936015, 2.16996785900455, 1.2140510276872558, -1.7036158828716652, 0.0], [1.6144295580947543, 0.07382742735936015, 2.16996785900455, 1.2489576127271425, -1.7385224679115518, 0.0], [1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0], [1.579522973054868, 0.07382742735936015, 2.16996785900455, 1.1267845650875392, -1.581442835232062, 0.0], [1.5882496193148397, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.5988961277520053, 0.0], [1.5969762655748112, 0.07382742735936015, 2.16996785900455, 1.2315043202071991, -1.6163494202719486, 0.0], [1.6057029118347828, 0.07382742735936015, 2.16996785900455, 1.283864197767029, -1.633802712791892, 0.0], [1.6144295580947543, 0.07382742735936015, 2.16996785900455, 1.336224075326859, -1.6512560053118353, 0.0] ] return poselist[index] if __name__ == '__main__': # 标定板的width 对应的角点的个数 6 # 标定板的height 对应的角点的个数 3 calibration_board_size = [6,3] calibration_board_square_size = 0.027 #unit - meter systemPath = "/home/jsfb/Documents/" now = datetime.now() date_time_str = now.strftime("%d_%H_%M") folder_name = f"calibration_data_{date_time_str}" systemPath+=folder_name os.makedirs(systemPath, exist_ok=True) calib = Calibration(calibration_board_size,calibration_board_square_size,systemPath) calib.collect_data() R, t,intrin = calib.calibrate() print("旋转矩阵：") print(R) print("平移向量：") print(t) print(f"内参： {intrin}")