diff --git a/Massage/Massage.pyc b/Massage/Massage.pyc old mode 100644 new mode 100755 diff --git a/Massage/MassageControl/hardware/__pycache__/dobot_api.cpython-38.pyc b/Massage/MassageControl/hardware/__pycache__/dobot_api.cpython-38.pyc index 42d5db1..5119330 100644 Binary files a/Massage/MassageControl/hardware/__pycache__/dobot_api.cpython-38.pyc and b/Massage/MassageControl/hardware/__pycache__/dobot_api.cpython-38.pyc differ diff --git a/Massage/MassageControl/hardware/__pycache__/dobot_nova5.cpython-39.pyc b/Massage/MassageControl/hardware/__pycache__/dobot_nova5.cpython-39.pyc index c53eae5..f69cf28 100644 Binary files a/Massage/MassageControl/hardware/__pycache__/dobot_nova5.cpython-39.pyc and b/Massage/MassageControl/hardware/__pycache__/dobot_nova5.cpython-39.pyc differ diff --git a/Massage/MassageControl/hardware/__pycache__/remote_cam.cpython-39.pyc b/Massage/MassageControl/hardware/__pycache__/remote_cam.cpython-39.pyc new file mode 100644 index 0000000..395d656 Binary files /dev/null and b/Massage/MassageControl/hardware/__pycache__/remote_cam.cpython-39.pyc differ diff --git a/Massage/MassageControl/hardware/capture.py b/Massage/MassageControl/hardware/capture.py new file mode 100644 index 0000000..81f99b6 --- /dev/null +++ b/Massage/MassageControl/hardware/capture.py @@ -0,0 +1,124 @@ +import os +import sys +import cv2 +import threading +import numpy as np +import time + +current_file_path = os.path.abspath(__file__) +dobot_nova5_path = os.path.dirname(os.path.dirname(current_file_path)) +sys.path.append(dobot_nova5_path) + +from hardware.dobot_nova5 import dobot_nova5 +from hardware.remote_cam import ToolCamera + +class Getpose: + def __init__(self): + self.running = True + self.arm = dobot_nova5() + self.arm.start() + self.arm.init() + self.cam = ToolCamera(host='127.0.0.1') + self.cam.start() + time.sleep(2) + + self.pose_data = [] + self.image_data = [] + # self.arm.start_drag() + self.rgb = None + + def test_show(self): + self.rgb, self.depth, intrinsics = self.cam.get_latest_frame() + if self.rgb is not None: + cv2.imshow("Video Feed", self.rgb) + cv2.waitKey(1000) + + def add_data(self): + angle = self.arm.getAngel() + self.pose_data.append(angle) + print(f"Current angle: {angle}") + + def save_data(self): + if self.pose_data: + data = np.array(self.pose_data) + np.savetxt('pose.txt', data, fmt='%.5f') + + +if __name__ == "__main__": + save_directory = '/home/jsfb/jsfb_ws/global_config/captured_images' + if not os.path.exists(save_directory): + os.makedirs(save_directory) + + myTest = Getpose() + + myTest.arm.start_drag() + + time.sleep(0.5) + try: + count = 1 + i = 1 + while True: + # 读取摄像头的帧 + r, d, intrinsics = myTest.cam.get_latest_frame() + # ret, frame = cam.read() + frame = r + + # if not ret: + # print("Error: Could not read frame from video device.") + # break + + # 显示摄像头的帧 + cv2.imshow('found', frame) + + # 等待按键事件 + key = cv2.waitKey(1) & 0xFF + + + # if key == ord('r'): + # pose = increase_dof(poselist[index]) + # index+=1 + # code = cp.arm.move_joint(pose, 4,wait=True) + # if code == -1: + # print("运动到拍照位置失败") + # else: + + # 按下's'键保存照片 + if key == ord('s'): + img_path = f'photo{i}' + if not os.path.exists(os.path.join(save_directory, img_path)): + os.makedirs(os.path.join(save_directory, img_path)) + filename = os.path.join(save_directory, f'{img_path}/rgb_image.png') + cv2.imwrite(filename, frame) # 保存照片 + filename1 = os.path.join(save_directory, f'{img_path}/depth_image.png') + cv2.imwrite(filename1, d) # 保存照片 + angle = myTest.arm.Get_feedInfo_angle() + print("angle = ",angle) + filename2 = os.path.join(save_directory, f'{img_path}/angle.txt') + np.savetxt(filename2, angle.shape(-1,6),delimiter=',') + print(f'Saved {filename}') + + # pose = increase_dof(poselist[index]) + # index+=1 + # code = cp.arm.move_joint(poselist[index], 4,wait=True) + # if code == -1: + # print("运动到拍照位置失败") + count += 1 # 增加照片计数 + i += 1 + # 按下'q'键退出循环 + elif key == ord('q'): + break + + + except KeyboardInterrupt: + # 处理Ctrl+C中断 + print("\nCamera session ended by user.") + finally: + # 释放资源 + myTest.arm.stop_drag() + time.sleep(0.5) + myTest.arm.disableRobot() + myTest.cam.stop() + cv2.destroyAllWindows() + + # 总结拍摄的照片 + print(f'Total number of images captured: {count}') \ No newline at end of file diff --git a/Massage/MassageControl/hardware/dobot_nova5.py b/Massage/MassageControl/hardware/dobot_nova5.py index b4f3c9e..c22cc70 100644 --- a/Massage/MassageControl/hardware/dobot_nova5.py +++ b/Massage/MassageControl/hardware/dobot_nova5.py @@ -871,24 +871,24 @@ if __name__ == "__main__": posJ_ready = [-45,55.5785,-120.6882,5.111,90,-90] dobot.RunPoint_P_inJoint(posJ_ready) - # sleep(1) + # # sleep(1) dobot.chooseRightFrame() # print("Arm pose1:",dobot.getPose()) - # print("Arm pose:",dobot.getPose(user=1,tool=0)) + # # print("Arm pose:",dobot.getPose(user=1,tool=0)) dobot.setEndEffector(i=8,tool_i="{0.0,0.0,154.071,0.0,0.0,0.0}") dobot.chooseEndEffector(i=8) print("Arm pose2:",dobot.getPose(user=1,tool=8)) - print(dobot.get_arm_position()) + # print(dobot.get_arm_position()) - # dobot.RunPoint_P_inPose([300, -135, 224.34, -180, 0, -90]) - cur_pose = dobot.getPose(user=1,tool=8) - cur_pose[0] += 30 - target_pose = cur_pose - print("target",target_pose) - dobot.RunPoint_P_inPose(target_pose) - dobot.ServoPose(dobot.getPose(user=1,tool=8)) - print("new ready joint",dobot.feedbackData.QActual) - # dobot.start_drag() - # dobot.stop_drag() + # # dobot.RunPoint_P_inPose([300, -135, 224.34, -180, 0, -90]) + # cur_pose = dobot.getPose(user=1,tool=8) + # cur_pose[0] += 30 + # target_pose = cur_pose + # print("target",target_pose) + # dobot.RunPoint_P_inPose(target_pose) + # dobot.ServoPose(dobot.getPose(user=1,tool=8)) + # print("new ready joint",dobot.feedbackData.QActual) + # # dobot.start_drag() + dobot.stop_drag() dobot.disableRobot() \ No newline at end of file diff --git a/Massage/MassageControl/hardware/remote_cam.py b/Massage/MassageControl/hardware/remote_cam.py index e69de29..266710a 100644 --- a/Massage/MassageControl/hardware/remote_cam.py +++ b/Massage/MassageControl/hardware/remote_cam.py @@ -0,0 +1,235 @@ +import requests +import base64 +import cv2 +import open3d as o3d +import numpy as np +import paramiko +import time +class ToolCamera: + def __init__(self, host): + """ + 初始化 CameraClient 类。 + """ + self.host = host + self.port = 22 # SSH端口号，默认是22 + self.username = "jsfb" # SSH用户名 + self.password = "jsfb" # SSH密码 + self.root_password = "jsfb" + + # 要执行的命令 + self.start_command = "systemctl restart cam_server" + self.stop_command = "systemctl stop cam_server" + + def start(self): + self.execute_command_on_remote( + host=self.host, + username=self.username, + password=self.password, + root_password=self.root_password, + command=self.start_command + ) + + def stop(self): + self.execute_command_on_remote( + host=self.host, + username=self.username, + password=self.password, + root_password=self.root_password, + command=self.stop_command + ) + + def execute_command_on_remote(self, host, username, password, root_password, command, port=22): + # 创建SSH客户端 + ssh = paramiko.SSHClient() + ssh.set_missing_host_key_policy(paramiko.AutoAddPolicy()) + + try: + # 连接到远程服务器，指定端口 + ssh.connect(hostname=host, port=port, username=username, password=password) + + # 构建完整的命令字符串，将root密码传递给sudo + sudo_command = f'echo {root_password} | sudo -S {command}' + + # 获取通道对象 + transport = ssh.get_transport() + channel = transport.open_session() + channel.get_pty() # 获取伪终端 + channel.exec_command(sudo_command) # 执行命令 + + # 检查命令是否在后台执行 + while True: + # 如果命令在后台运行, channel.exit_status_ready() 将会一直是 False + if channel.exit_status_ready(): + break + + # 非阻塞方式读取输出 + if channel.recv_ready(): + output = channel.recv(1024).decode('utf-8') + print(output) + + # 非阻塞方式读取错误输出 + if channel.recv_stderr_ready(): + error = channel.recv_stderr(1024).decode('utf-8') + print(error) + + # 延时，防止CPU占用过高 + time.sleep(0.1) + + return channel.recv_exit_status() + finally: + # 关闭SSH连接 + ssh.close() + + def get_latest_frame(self): + """ + 发送请求到服务器以获取最新的RGB和深度图像数据。 + + 返回: + tuple: 包含RGB图像和深度图像的元组 (rgb_image, depth_image)，如果请求失败则返回 (None, None)。 + """ + try: + # 发送GET请求到服务器 + response = requests.get("http://" + self.host + ":8000/get_latest_frame", timeout=(3, 5)) + + # 检查请求是否成功 + if response.status_code == 200: + data = response.json() + + # 获取Base64编码的RGB和深度图像数据 + rgb_image_base64 = data['rgb_image'] + depth_image_base64 = data['depth_image'] + camera_intrinsics = data['camera_intrinsics'] + + # 解码Base64为二进制数据 + rgb_image_data = base64.b64decode(rgb_image_base64) + depth_image_data = base64.b64decode(depth_image_base64) + + # 转换为NumPy数组 + rgb_image_np = np.frombuffer(rgb_image_data, np.uint8) + depth_image_np = np.frombuffer(depth_image_data, np.uint8) + + # 解码为OpenCV图像 + rgb_image = cv2.imdecode(rgb_image_np, cv2.IMREAD_COLOR) + depth_image = cv2.imdecode(depth_image_np, cv2.IMREAD_UNCHANGED) + + + return rgb_image, depth_image, camera_intrinsics + else: + print(f"Failed to retrieve data from server, status code: {response.status_code}") + return None, None, None + except Exception as e: + print(f"Exception occurred: {e}") + return None, None, None + + def display_images(self, rgb_image, depth_image): + """ + 显示RGB和深度图像。 + + 参数: + rgb_image (numpy.ndarray): RGB图像。 + depth_image (numpy.ndarray): 深度图像。 + """ + if rgb_image is not None and depth_image is not None: + cv2.imshow('RGB Image', rgb_image) + cv2.imshow('Depth Image', depth_image) + cv2.waitKey(0) + cv2.destroyAllWindows() + else: + print("No image data to display.") + + def display_rgb_point_cloud(self, rgb_image, depth_image, camera_intrinsics): + """ + 显示RGB点云，将RGB图像和深度图像转换为点云并显示。 + + 参数: + rgb_image (np.ndarray): RGB图像。 + depth_image (np.ndarray): 深度图像。 + camera_intrinsics (dict): 相机的内参字典，包含 'fx', 'fy', 'cx', 'cy'。 + + 返回: + None + """ + # 获取RGB图像和深度图像的尺寸 + rgb_h, rgb_w = rgb_image.shape[:2] + depth_h, depth_w = depth_image.shape[:2] + + # 计算裁剪区域 + start_x = (rgb_w - depth_w) // 2 + start_y = (rgb_h - depth_h) // 2 + + # 裁剪RGB图像以匹配深度图像的尺寸 + rgb_image_cropped = rgb_image[start_y:start_y + depth_h, start_x:start_x + depth_w] + rgb_image_cropped = cv2.cvtColor(rgb_image_cropped, cv2.COLOR_RGB2BGR) + + # 将深度图像转换为浮点型并将单位从毫米转换为米（假设深度图像以毫米为单位） + depth_image = depth_image.astype(np.float32) / 1000.0 + + # 创建点云的空数组 + points = [] + colors = [] + + # 相机内参 + fx = camera_intrinsics['fx'] + fy = camera_intrinsics['fy'] + cx = camera_intrinsics['cx'] + cy = camera_intrinsics['cy'] + + # 遍历每个像素，将其转换为3D点 + for v in range(depth_h): + for u in range(depth_w): + z = depth_image[v, u] + if z > 0: # 排除无效深度 + x = (u - cx) * z / fx + y = (v - cy) * z / fy + points.append([x, y, z]) + colors.append(rgb_image_cropped[v, u] / 255.0) # 颜色归一化到[0,1] + + # 将点云和颜色转换为NumPy数组 + points = np.array(points) + colors = np.array(colors) + + # 创建Open3D点云对象 + point_cloud = o3d.geometry.PointCloud() + point_cloud.points = o3d.utility.Vector3dVector(points) + point_cloud.colors = o3d.utility.Vector3dVector(colors) + + # 显示点云 + o3d.visualization.draw_geometries([point_cloud]) + +if __name__ == "__main__": + import time + import os + # 初始化客户端并指定服务器地址 + cam = ToolCamera(host='127.0.0.1') + # cam.stop() + cam.start() + time.sleep(2) + # 获取最新的RGB和深度图像 + rgb_image, depth_image, camera_intrinsics = cam.get_latest_frame() + print(camera_intrinsics) + + max_depth = np.max(depth_image) + print(np.min(depth_image),np.max(depth_image)) + # print(depth_image[200, 320]) + # depth_image = (depth_image / max_depth * 65535).astype(np.uint16) + # print(np.min(depth_image),np.max(depth_image)) + # 对图像进行水平翻转 + # rgb_image = cv2.flip(rgb_image, 1) + # depth_image = cv2.flip(depth_image, 1) + + # 显示图像 + cam.display_images(rgb_image, depth_image) + cv2.imwrite("aucpuncture2point/configs/using_img/color.png",rgb_image) + cv2.imwrite("aucpuncture2point/configs/using_img/depth.png",depth_image) + # 获取当前时间并格式化为文件夹名 (格式: 年-月-日_时-分-秒) + current_time = time.strftime("%Y-%m-%d_%H-%M-%S") + base_dir = "/home/jsfb/jsfb_ws/collected_data/test_images" + # 创建一个根据时间命名的新文件夹 + folder_path = os.path.join(base_dir, current_time) + os.makedirs(folder_path, exist_ok=True) # 如果文件夹不存在则创建 + + cv2.imwrite(os.path.join(folder_path, "color.png"), rgb_image) + cv2.imwrite(os.path.join(folder_path, "depth.png"), depth_image) + + # 显示RGB点云 + # cam.display_rgb_point_cloud(rgb_image, depth_image, camera_intrinsics) diff --git a/Massage/MassageControl/tools/auto_visual_valibration.py b/Massage/MassageControl/tools/auto_visual_valibration.py index e5858b1..d6bb8b9 100644 --- a/Massage/MassageControl/tools/auto_visual_valibration.py +++ b/Massage/MassageControl/tools/auto_visual_valibration.py @@ -10,7 +10,7 @@ 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 +from hardware.dobot_nova5 import dobot_nova5 np.set_printoptions(precision=8, suppress=True) # 设置寻找亚像素角点的参数，采用的停止准则是最大循环次数30和最大误差容限0.001 @@ -41,16 +41,20 @@ class Calibration: self.size = None self.intrinsics_mtx = None self.disorder = None + time.sleep(2) rgb_image, depth_image, camera_intrinsics = self.cam.get_latest_frame() + self.save_directory = '/home/jsfb/jsfb_ws/global_config/captured_images' + if rgb_image is None : print(f'===============相机连接失败请检查并重试==============') else: - print(f'===============相机连接成功==============') self.size = rgb_image.shape[:2] + print(f'===============相机连接成功============== ',self.size) + self.intrinsics = camera_intrinsics - self.arm = AuboC5() - self.arm.init() + self.arm = dobot_nova5("192.168.5.1") + self.arm.start() self.obj_points = [] print(f'===============机械臂连接成功==============') @@ -62,24 +66,29 @@ class Calibration: 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) + pose = [-45.0009079,55.5785141,-120.68821716,5.11103201,-90.00195312,-90.00085449] + code = self.arm.RunPoint_P_inJoint(pose) if code == -1: print("运动到拍照位置失败") #拍照 self.check_corners() #运动到下一个位置 # 目前只设计了44条轨迹 - for i in range (0,45): + for i in range (0,18): next_pose = self.get_next_pose_ugly(i) - self.arm.move_joint(next_pose, 4,wait=True) + code=self.arm.RunPoint_P_inJoint(next_pose) + time.sleep(0.5) if code == -1: print("运动到拍照位置失败") else: self.check_corners() - + time.sleep(0.5) + def check_corners(self): rgb, depth, intrinsics = self.cam.get_latest_frame() + # img_path = f'photo{i}' + # filename = os.path.join(self.save_directory, f'{img_path}/rgb_image.png') + # rgb=cv2.imread(filename) gray = cv2.cvtColor(rgb, cv2.COLOR_BGR2GRAY) ret, corners = cv2.findChessboardCorners(gray, self.board_size, None) if ret: @@ -115,14 +124,12 @@ class Calibration: 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) + arm_position,arm_orientation = self.arm.get_arm_position() + rotation_matrix = R.from_quat(arm_orientation).as_matrix() + translation_vector = arm_position homo_mat = np.eye(4) # 创建 4x4 单位矩阵 homo_mat[:3, :3] = rotation_matrix - homo_mat[:3, 3] = translation_vector.flatten() + homo_mat[:3, 3] = translation_vector return homo_mat def camera_calib(self): @@ -132,6 +139,7 @@ class Calibration: if ret: print("内参矩阵:\n", mtx) # 内参数矩阵 print("畸变系数:\n", dist) # 畸变系数 distortion cofficients = (k_1,k_2,p_1,p_2,k_3) + print("重投影误差:\n",ret) print("++++++++++相机标定完成++++++++++++++") self.intrinsics_mtx = mtx self.disorder = dist @@ -185,52 +193,24 @@ class Calibration: 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] + [-4.513801574707031250e+01,5.518265151977539062e+01,-1.237858963012695312e+02,9.085088729858398438e+00,-9.337700653076171875e+01,-9.000016784667968750e+01], + [-4.594435882568359375e+01,3.636156082153320312e+01,-1.064173660278320312e+02,-2.125140428543090820e+00,-9.337675476074218750e+01,-9.000016784667968750e+01], + [-4.595432662963867188e+01,1.059147930145263672e+01,-8.284246063232421875e+01,-1.484527587890625000e+01,-9.337491607666015625e+01,-9.000013732910156250e+01], + [-4.638331604003906250e+01,-6.824899196624755859e+00,-6.758448791503906250e+01,-2.286166954040527344e+01,-9.337348937988281250e+01,-9.000016784667968750e+01], + [-4.636801910400390625e+01,-1.807893753051757812e+01,-6.532709503173828125e+01,-2.261142921447753906e+01,-9.336659240722656250e+01,-9.000010681152343750e+01], + [-4.637482833862304688e+01,-4.540402412414550781e+00,-1.098018112182617188e+02,3.086268234252929688e+01,-9.305430603027343750e+01,-9.000022125244140625e+01], + [-4.640084075927734375e+01,8.068140029907226562e+00,-1.362033843994140625e+02,6.259086990356445312e+01,-9.334789276123046875e+01,-9.000019073486328125e+01], + [-4.640056610107421875e+01,8.061657905578613281e+00,-1.511797027587890625e+02,8.927394104003906250e+01,-9.331819915771484375e+01,-9.000019073486328125e+01], + [-4.638466262817382812e+01,3.287191867828369141e+00,-1.566032409667968750e+02,1.035896453857421875e+02,-9.330465698242187500e+01,-9.000022125244140625e+01], + [-3.178509473800659180e+00,6.977911472320556641e+00,-1.417592010498046875e+02,6.005132293701171875e+01,-1.280102996826171875e+02,-9.000010681152343750e+01], + [-1.424913024902343750e+01,-2.980865538120269775e-01,-1.193396759033203125e+02,3.752756500244140625e+01,-1.278784713745117188e+02,-8.999975585937500000e+01], + [-2.393187522888183594e+01,-2.973175048828125000e-01,-1.121088027954101562e+02,3.928178024291992188e+01,-1.183067703247070312e+02,-8.999983215332031250e+01], + [-3.699544525146484375e+01,-2.981689572334289551e-01,-1.078408889770507812e+02,3.178039932250976562e+01,-1.024006271362304688e+02,-8.999983215332031250e+01], + [-6.734904479980468750e+01,-4.368630886077880859e+00,-1.035898971557617188e+02,2.342595481872558594e+01,-7.085527801513671875e+01,-8.999980926513671875e+01], + [-8.632910919189453125e+01,-1.039798259735107422e+01,-1.038105545043945312e+02,2.330961036682128906e+01,-5.476583099365234375e+01,-8.999980926513671875e+01], + [-8.156823730468750000e+01,5.878097534179687500e+00,-1.439221954345703125e+02,6.846669006347656250e+01,-4.482905197143554688e+01,-8.999922943115234375e+01], + [-8.601764678955078125e+01,3.570293045043945312e+01,-1.308228912353515625e+02,2.318510818481445312e+01,-5.378013610839843750e+01,-8.999922943115234375e+01], + [-7.132132053375244141e+00,3.722338104248046875e+01,-1.065378265380859375e+02,-3.337371826171875000e+00,-1.185582199096679688e+02,-8.999931335449218750e+01] ] return poselist[index] @@ -241,7 +221,7 @@ if __name__ == '__main__': # 标定板的width 对应的角点的个数 6 # 标定板的height 对应的角点的个数 3 calibration_board_size = [6,3] - calibration_board_square_size = 0.027 #unit - meter + calibration_board_square_size = 0.03 #unit - meter systemPath = "/home/jsfb/Documents/" now = datetime.now() @@ -252,7 +232,9 @@ if __name__ == '__main__': calib = Calibration(calibration_board_size,calibration_board_square_size,systemPath) calib.collect_data() + print("=================数据采集完成===================") R, t,intrin = calib.calibrate() + print("旋转矩阵：") print(R) print("平移向量：") diff --git a/Massage/controller.service b/Massage/controller.service old mode 100644 new mode 100755 diff --git a/Massage/test_manual.py b/Massage/test_manual.py old mode 100644 new mode 100755 diff --git a/Massage/test_manual.pyc b/Massage/test_manual.pyc old mode 100644 new mode 100755 diff --git a/Massage/tools/__pycache__/log.cpython-38.pyc b/Massage/tools/__pycache__/log.cpython-38.pyc new file mode 100644 index 0000000..92cc5b1 Binary files /dev/null and b/Massage/tools/__pycache__/log.cpython-38.pyc differ diff --git a/Restart_Speaker.py b/Restart_Speaker.py old mode 100644 new mode 100755 diff --git a/Restart_Speaker.pyc b/Restart_Speaker.pyc old mode 100644 new mode 100755 diff --git a/clear_pyc.sh b/clear_pyc.sh old mode 100644 new mode 100755 diff --git a/example_startup.pyc b/example_startup.pyc old mode 100644 new mode 100755 diff --git a/py2json.py b/py2json.py old mode 100644 new mode 100755 diff --git a/py2json.pyc b/py2json.pyc old mode 100644 new mode 100755 diff --git a/restart_language.sh b/restart_language.sh old mode 100644 new mode 100755 diff --git a/版本说明.txt b/版本说明.txt old mode 100644 new mode 100755