├── .gitignore ├── README.md ├── __pycache__ ├── camera_D435i.cpython-39.pyc ├── camera_calibration.cpython-38.pyc ├── camera_calibration.cpython-39.pyc ├── main.cpython-38.pyc ├── opencv_camera.cpython-38.pyc ├── robotcontrol.cpython-38.pyc ├── robotcontrol.cpython-39.pyc ├── util.cpython-38.pyc └── util.cpython-39.pyc ├── camera_D435i.py ├── camera_calibration.py ├── control_RG2_master ├── __pycache__ │ └── util.cpython-38.pyc ├── rtde.py ├── serialize.py ├── test_main.py └── util.py ├── depth_calibration.py ├── main.py ├── opencv_camera.py ├── robotcontrol.py └── util.py /.gitignore: -------------------------------------------------------------------------------- 1 | photo 2 | point_photo 3 | data 4 | .idea -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # UR5机器人手眼标定----基于python opencv(眼在手外) 2 | ## 环境 3 | 系统:WIN11 4 | IDE:pycharm 5 | opencv版本:4.6.0(低于4.1.0没有camera_calibration函数) 6 | 7 | ## main.py 8 | 主要负责拍照部分,弹出窗口后,先输入拍照数量 9 | 然后按下s拍照 10 | 拍到足够数目的照片后会调用camera_calibration.py进行运算 11 | ## camera_calibration.py 12 | 自动读取拍照得到的数据进行运算 13 | 核心的函数是camera_calibration() 14 | 参数说明: 15 | R_base2gripper: 从基座到手臂末端的旋转矩阵,由机器人读数得到的rxryrz得到 16 | 程序读到的是旋转矢量,需要先转换成旋转矩阵,然后和xyz拼接在一起,求逆矩阵,取求出来的逆矩阵的[:3,:3],然后再转置传入! 17 | 18 | t_base2gripper: 从基座到手臂末端的平移向量,由机器人读数得到的xyz得到单位是mm,直接传入 19 | 20 | R_target2cam: 标定板在相机坐标系下的旋转矩阵,由对照片进行标定后的得到,先用solvePnP函数获得,solvePnP得到的是旋转矢量,先用罗德里格斯公式 21 | 转成旋转矩阵,然后再转置再传入 22 | 23 | t_target2cam: 标定板在相机坐标系下的平移向量,由对照片进行标定后的得到,先用solvePnP函数获得,需要乘上负的标定板旋转矩阵的转置再传入 24 | 25 | method=cv.CALIB_HAND_EYE_TSAI: 指定用什么方法来进行计算,TSAI最快 26 | 27 | # 更新日志 28 | 2024/5/14 优化了代码，相机调用线程化 -------------------------------------------------------------------------------- /__pycache__/camera_D435i.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DHR0703/python_opencv_camera_calibration/afe44d94d95a755cd2b761512b8c84b9268b333c/__pycache__/camera_D435i.cpython-39.pyc -------------------------------------------------------------------------------- /__pycache__/camera_calibration.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DHR0703/python_opencv_camera_calibration/afe44d94d95a755cd2b761512b8c84b9268b333c/__pycache__/camera_calibration.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/camera_calibration.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DHR0703/python_opencv_camera_calibration/afe44d94d95a755cd2b761512b8c84b9268b333c/__pycache__/camera_calibration.cpython-39.pyc -------------------------------------------------------------------------------- /__pycache__/main.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DHR0703/python_opencv_camera_calibration/afe44d94d95a755cd2b761512b8c84b9268b333c/__pycache__/main.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/opencv_camera.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DHR0703/python_opencv_camera_calibration/afe44d94d95a755cd2b761512b8c84b9268b333c/__pycache__/opencv_camera.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/robotcontrol.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DHR0703/python_opencv_camera_calibration/afe44d94d95a755cd2b761512b8c84b9268b333c/__pycache__/robotcontrol.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/robotcontrol.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DHR0703/python_opencv_camera_calibration/afe44d94d95a755cd2b761512b8c84b9268b333c/__pycache__/robotcontrol.cpython-39.pyc -------------------------------------------------------------------------------- /__pycache__/util.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DHR0703/python_opencv_camera_calibration/afe44d94d95a755cd2b761512b8c84b9268b333c/__pycache__/util.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/util.cpython-39.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DHR0703/python_opencv_camera_calibration/afe44d94d95a755cd2b761512b8c84b9268b333c/__pycache__/util.cpython-39.pyc -------------------------------------------------------------------------------- /camera_D435i.py: -------------------------------------------------------------------------------- 1 | import threading 2 | import time 3 | import pyrealsense2 as rs 4 | import numpy as np 5 | import cv2 6 | 7 | 8 | 9 | class RealSenseCamera: 10 | def __init__(self, output_dir="catch_result/photo"): 11 | self._display_thread = None 12 | self.last_depth_image = None 13 | self.last_color_image = None 14 | self.pipeline = rs.pipeline() 15 | self.config = rs.config() 16 | # 获取设备产品线，用于设置支持分辨率 17 | self.pipeline_wrapper = rs.pipeline_wrapper(self.pipeline) 18 | self.pipeline_profile = self.config.resolve(self.pipeline_wrapper) 19 | self.device = self.pipeline_profile.get_device() 20 | # 创建对齐对象（深度对齐颜色） 21 | self.align = rs.align(rs.stream.color) 22 | self.output_dir = output_dir 23 | self.is_running = False 24 | 25 | self.image_ready = threading.Condition(threading.Lock()) # 添加条件锁 26 | 27 | # 设置配置：RGB与深度流均为640x480 28 | self.config.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 30) 29 | self.config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) 30 | 31 | # 创建深度处理管道，包括temporal和spatial滤波器 32 | self.depth_pipeline = rs.pipeline_profile() 33 | # 孔洞填充过滤器 34 | self.hole_filling = rs.hole_filling_filter() 35 | # 时间滤波器 36 | self.temporal = rs.temporal_filter() 37 | # 边缘滤波 38 | self.spatial = rs.spatial_filter() 39 | self.spatial.set_option(rs.option.filter_magnitude, 5) 40 | self.spatial.set_option(rs.option.filter_smooth_alpha, 0.5) 41 | self.spatial.set_option(rs.option.filter_smooth_delta, 8) 42 | # 创建深度处理管道，包括temporal和spatial滤波器 43 | self.depth_pipeline = rs.pipeline_profile() 44 | self.depth_filters = [ 45 | self.hole_filling, 46 | self.temporal, 47 | self.spatial 48 | ] 49 | 50 | self.colorizer = rs.colorizer() 51 | 52 | def start_display_and_capture(self): 53 | self.is_running = True 54 | self.pipeline.start(self.config) 55 | self._display_thread = threading.Thread(target=self._display_and_capture) 56 | self._display_thread.start() 57 | 58 | def _display_and_capture(self): 59 | try: 60 | time.sleep(3) 61 | while self.is_running: 62 | frames = self.pipeline.wait_for_frames() 63 | frames = self.align.process(frames) 64 | 65 | # 获取并处理深度数据 66 | depth_frame = frames.get_depth_frame() 67 | for filter_ in self.depth_filters: 68 | depth_frame = filter_.process(depth_frame) 69 | # 保存原深度图 70 | depth = np.asanyarray(depth_frame.get_data()) 71 | 72 | # 在深度图像上应用颜色图 73 | depth_colormap = self.colorizer.colorize(depth_frame) 74 | depth_image = np.asanyarray(depth_colormap.get_data()) 75 | 76 | # 获取彩色图像 77 | color_frame = frames.get_color_frame() 78 | color_image = np.asanyarray(color_frame.get_data()) 79 | # 因为opencv使用的是BGR,但是相机用的是RGB,所以要转换 80 | color_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR) 81 | 82 | # 将RGB图像和深度图像堆叠在一起 83 | combined_image = np.hstack((color_image, depth_image)) 84 | 85 | # 显示堆叠后的图像 86 | cv2.imshow("RGB & Depth Images", combined_image) 87 | 88 | with self.image_ready: # 在这里获取锁 89 | self.last_color_image = color_image 90 | self.last_depth_image = depth 91 | 92 | self.image_ready.notify_all() # 通知所有等待的线程，图像已准备好 93 | 94 | cv2.waitKey(1) 95 | except Exception as e: 96 | print(f"Error during display and capture: {e}") 97 | self.is_running = False 98 | finally: 99 | cv2.destroyAllWindows() 100 | self.pipeline.stop() 101 | 102 | def save_screenshot(self, photo_path) -> None: 103 | with self.image_ready: 104 | while not (self.last_color_image is not None and self.last_depth_image is not None): 105 | self.image_ready.wait() # 等待图像准备好 106 | # 保存图片 107 | # 保存待识别的图片 108 | cv2.imwrite(photo_path, self.last_color_image) 109 | 110 | def stop(self): 111 | self.is_running = False 112 | -------------------------------------------------------------------------------- /camera_calibration.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | @Time ： 2022/8/20 14:26 4 | @Auth ： 邓浩然 5 | @File ：camera_calibration.py 6 | @IDE ：PyCharm 7 | @Description：标定部分的代码,需要先完成拍照操作 8 | """ 9 | 10 | import cv2 as cv 11 | import numpy as np 12 | import glob 13 | 14 | # 相机内参矩阵 15 | CAMERA_INTRINSIC = [[609.441, 0, 321.084], 16 | [0, 607.96, 238.077], 17 | [0, 0, 1]] 18 | # 不使用e来表示小数,让控制台输出更加人性化 19 | np.set_printoptions(suppress=True) 20 | # 数据txt位置 21 | xyz_path = 'data/xyz.txt' 22 | rxryrz_path = 'data/RxRyRz.txt' 23 | rvecsfilePath = 'data/rvecs.txt' 24 | tvecsfilePath = 'data/tvecs.txt' 25 | # 照片存储位置: 26 | photo_path = 'photo/' 27 | # 点阵图片位置 28 | point_photo_path = 'point_photo/' 29 | 30 | RESULT_PATH = 'data/result.txt' 31 | 32 | # 棋盘标定板参数 33 | w = 11 34 | h = 8 35 | checker_size = 15 36 | 37 | 38 | # 把矩阵写入到文件里,按照指定格式 39 | def write_vec3d(fpath, mat): 40 | # 打开文件 41 | file = open(fpath, 'w') 42 | # 按照指定格式写入文件 43 | for num in mat: 44 | file.write('Vec3d(' + str(num[0]) + ',' + str(num[1]) + ',' + str(num[2]) + '),\n') 45 | 46 | 47 | # 标定并把数据写入文件 48 | def camera_calibration(): 49 | # 找棋盘格角点 50 | # 设置寻找亚像素角点的参数，采用的停止准则是最大循环次数30和最大误差容限0.001 51 | criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001) # 阈值 52 | # 世界坐标系中的棋盘格点,例如(0,0,0), (1,0,0), (2,0,0) ....,(8,5,0)，去掉Z坐标，记为二维矩阵 53 | objp = np.zeros((w * h, 3), np.float32) 54 | objp[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2) 55 | # checker_size是棋盘格子的大小,单位是mm 56 | objp = objp * checker_size 57 | # 储存棋盘格角点的世界坐标和图像坐标对 58 | # 在世界坐标系中的三维点 59 | objpoints = [] 60 | # 在图像平面的二维点 61 | imgpoints = [] 62 | # 加载photo文件夹下所有的jpg图像,要注意读取的顺序必须和拍摄顺序一致 63 | images = glob.glob(photo_path + '*.png') 64 | # 文件名,使用ASCII编码 65 | name_num = 97 66 | i = 0 67 | for fname in images: 68 | 69 | img = cv.imread(fname) 70 | # 获取画面中心点 71 | # 获取图像的长宽 72 | 73 | gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) 74 | u, v = img.shape[:2] 75 | # 找到棋盘格角点,SB函数的鲁棒性更好,噪点忍受能力高 76 | # ret, corners = cv.findChessboardCorners(gray, (w, h), None) 77 | ret, corners = cv.findChessboardCornersSB(gray, (w, h), None) 78 | # 如果找到足够点对，将其存储起来 79 | if ret: 80 | print(f"第{i + 1}张图片生成点阵") 81 | i = i + 1 82 | # 在原角点的基础上寻找亚像素角点 83 | cv.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) 84 | # 追加进入世界三维点和平面二维点中 85 | objpoints.append(objp) 86 | imgpoints.append(corners) 87 | # 将角点在图像上显示 88 | cv.drawChessboardCorners(img, (w, h), corners, ret) 89 | cv.namedWindow('findCorners', cv.WINDOW_NORMAL | cv.WINDOW_KEEPRATIO | cv.WINDOW_GUI_EXPANDED) 90 | cv.resizeWindow('findCorners', 640, 480) 91 | cv.imshow('findCorners', img) 92 | cv.imwrite(point_photo_path + chr(name_num) + ".png", img) 93 | name_num += 1 94 | cv.waitKey(500) 95 | cv.destroyAllWindows() 96 | # 标定 97 | print('正在计算') 98 | # 标定 99 | ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, 100 | None) 101 | 102 | print("retval:", ret) 103 | # 内参数矩阵 104 | print("cameraMatrix内参矩阵:\n", camera_matrix) 105 | # 畸变系数 distortion cofficients = (k_1,k_2,p_1,p_2,k_3) 106 | print("distCoeffs畸变值:\n", np.array(dist_coeffs)) 107 | # 旋转向量 # 外参数 108 | print("rvecs旋转向量外参:\n", np.array(rvecs)) 109 | # 平移向量 # 外参数 110 | print("tvecs平移向量外参:\n", np.array(tvecs)) 111 | newcameramtx, roi = cv.getOptimalNewCameraMatrix(camera_matrix, dist_coeffs, (u, v), 0, (u, v)) 112 | print('newcameramtx内参', newcameramtx) 113 | 114 | # N*3*1旋转矢量 115 | rvec = np.zeros((len(rvecs), 3, 1), np.double) 116 | # N*3*1平移XYZ 117 | tvec = np.zeros((len(tvecs), 3, 1), np.double) 118 | # 使用solvePnP获得新的标定板在相机坐标系下的位姿信息 119 | for num_photo in range(len(images)): 120 | retval, rv, tv = cv.solvePnP(np.array(objpoints[num_photo]), 121 | np.array(imgpoints[num_photo]), 122 | camera_matrix, dist_coeffs) 123 | rvec[num_photo] = rv 124 | tvec[num_photo] = tv 125 | # 写入旋转向量,把N*3*1转成N*3写入TXT 126 | np.savetxt(rvecsfilePath, np.squeeze(rvec), fmt='%.8f', delimiter=',') 127 | # 写入平移向量,把N*3*1转成N*3写入TXT 128 | np.savetxt(tvecsfilePath, np.squeeze(tvec), fmt='%.8f', delimiter=',') 129 | 130 | # 读取xyz 131 | xyz = np.loadtxt(xyz_path, dtype=np.double, delimiter=',') 132 | # 读取RxRyRz 133 | rxryrz = np.loadtxt(rxryrz_path, dtype=np.double, delimiter=',') 134 | 135 | R_base2gripper = [] 136 | t_base2gripper = [] 137 | R_target2cam = [] 138 | t_target2cam = [] 139 | 140 | # 开始为各个参数赋值 141 | for i in range(len(xyz)): 142 | # 处理R_base2gripper和t_base2gripper 143 | # 创建新的4*4的矩阵用来存放R和T拼接矩阵 144 | rtarray = np.zeros((4, 4), np.double) 145 | # 旋转向量转旋转矩阵,dst是是旋转矩阵,jacobian是雅可比矩阵 146 | dst, jacobian = cv.Rodrigues(rxryrz[i]) 147 | # 存入旋转矩阵 148 | rtarray[:3, :3] = dst 149 | # 传入平移向量 150 | rtarray[:3, 3] = xyz[i] 151 | rtarray[3, 3] = 1 152 | # 求逆矩阵 153 | rb2e = np.linalg.inv(rtarray) 154 | # 放入用来传给calibrateHandEye的参数中 155 | R_base2gripper.append(rb2e[:3, :3].T) 156 | t_base2gripper.append(xyz[i]) 157 | 158 | # 处理R_target2cam和t_target2cam 159 | # 获标定板在相机坐标系下的旋转矩阵,把旋转向量转成旋转矩阵 160 | dst1, jacobian1 = cv.Rodrigues(rvec[i]) 161 | # 相机坐标系旋转矩阵转置 162 | R_target2cam.append(dst1.T) 163 | # 写入相机坐标系平移向量,平移向量需要乘原本的负的旋转矩阵 164 | t_target2cam.append(np.matmul(-dst1.T, tvec[i])) 165 | 166 | # 核心方法,前面都是为了得到该方法的参数,获得转换矩阵 167 | r_cam2gripper, t_cam2gripper = cv.calibrateHandEye(R_base2gripper, t_base2gripper, R_target2cam, t_target2cam, 168 | method=cv.CALIB_HAND_EYE_TSAI) 169 | # 拼接出转换矩阵 170 | rt = np.vstack((np.hstack((r_cam2gripper, t_cam2gripper)), np.array([0, 0, 0, 1]))) 171 | # results = np.zeros((3, 4, 4)) 172 | for i in range(len(t_base2gripper)): 173 | print(str(i) + " ") 174 | base = np.column_stack((R_base2gripper[i], t_base2gripper[i])) 175 | base = np.row_stack((base, np.array([0, 0, 0, 1]))) 176 | 177 | gripper = np.column_stack((R_target2cam[i], t_target2cam[i])) 178 | gripper = np.row_stack((gripper, np.array([0, 0, 0, 1]))) 179 | 180 | result = np.matmul(np.matmul(base, rt), gripper) 181 | # result[i] = result 182 | print(repr(result)) 183 | print('相机相对于末端的变换矩阵为：') 184 | print(rt) 185 | # 内参数矩阵 186 | print("cameraMatrix内参矩阵:\n", camera_matrix) 187 | # np.savetxt(RESULT_PATH, results, delimiter=',') 188 | -------------------------------------------------------------------------------- /control_RG2_master/__pycache__/util.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DHR0703/python_opencv_camera_calibration/afe44d94d95a755cd2b761512b8c84b9268b333c/control_RG2_master/__pycache__/util.cpython-38.pyc -------------------------------------------------------------------------------- /control_RG2_master/rtde.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016, Universal Robots A/S, 2 | # All rights reserved. 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # * Redistributions of source code must retain the above copyright 6 | # notice, this list of conditions and the following disclaimer. 7 | # * Redistributions in binary form must reproduce the above copyright 8 | # notice, this list of conditions and the following disclaimer in the 9 | # documentation and/or other materials provided with the distribution. 10 | # * Neither the name of the Universal Robots A/S nor the names of its 11 | # contributors may be used to endorse or promote products derived 12 | # from this software without specific prior written permission. 13 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | # DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY 17 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | import struct 25 | import socket 26 | import select 27 | import sys 28 | import logging 29 | 30 | if sys.version_info[0] < 3: 31 | import serialize 32 | else: 33 | import serialize 34 | 35 | DEFAULT_TIMEOUT = 1.0 36 | 37 | LOGNAME = 'rtde' 38 | _log = logging.getLogger(LOGNAME) 39 | 40 | 41 | class Command: 42 | RTDE_REQUEST_PROTOCOL_VERSION = 86 # ascii V 43 | RTDE_GET_URCONTROL_VERSION = 118 # ascii v 44 | RTDE_TEXT_MESSAGE = 77 # ascii M 45 | RTDE_DATA_PACKAGE = 85 # ascii U 46 | RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS = 79 # ascii O 47 | RTDE_CONTROL_PACKAGE_SETUP_INPUTS = 73 # ascii I 48 | RTDE_CONTROL_PACKAGE_START = 83 # ascii S 49 | RTDE_CONTROL_PACKAGE_PAUSE = 80 # ascii P 50 | 51 | 52 | RTDE_PROTOCOL_VERSION = 2 53 | 54 | class ConnectionState: 55 | DISCONNECTED = 0 56 | CONNECTED = 1 57 | STARTED = 2 58 | PAUSED = 3 59 | 60 | class RTDEException(Exception): 61 | def __init__(self, msg): 62 | self.msg = msg 63 | def __str__(self): 64 | return repr(self.msg) 65 | 66 | class RTDE(object): 67 | def __init__(self, hostname, port=30004): 68 | self.hostname = hostname 69 | self.port = port 70 | self.__conn_state = ConnectionState.DISCONNECTED 71 | self.__sock = None 72 | self.__output_config = None 73 | self.__input_config = {} 74 | 75 | def connect(self): 76 | if self.__sock: 77 | return 78 | 79 | self.__buf = b'' # buffer data in binary format 80 | try: 81 | self.__sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 82 | self.__sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 83 | self.__sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) 84 | self.__sock.settimeout(DEFAULT_TIMEOUT) 85 | self.__sock.connect((self.hostname, self.port)) 86 | self.__conn_state = ConnectionState.CONNECTED 87 | except (socket.timeout, socket.error): 88 | self.__sock = None 89 | raise 90 | if not self.negotiate_protocol_version(): 91 | raise RTDEException('Unable to negotiate protocol version') 92 | 93 | def disconnect(self): 94 | if self.__sock: 95 | self.__sock.close() 96 | self.__sock = None 97 | self.__conn_state = ConnectionState.DISCONNECTED 98 | 99 | def is_connected(self): 100 | return self.__conn_state is not ConnectionState.DISCONNECTED 101 | 102 | def get_controller_version(self): 103 | cmd = Command.RTDE_GET_URCONTROL_VERSION 104 | version = self.__sendAndReceive(cmd) 105 | if version: 106 | _log.info('Controller version: ' + str(version.major) + '.' + str(version.minor) + '.' + str(version.bugfix)+ '.' + str(version.build)) 107 | if version.major == 3 and version.minor <= 2 and version.bugfix < 19171: 108 | _log.error("Please upgrade your controller to minimally version 3.2.19171") 109 | sys.exit() 110 | return version.major, version.minor, version.bugfix, version.build 111 | return None, None, None, None 112 | 113 | def negotiate_protocol_version(self): 114 | cmd = Command.RTDE_REQUEST_PROTOCOL_VERSION 115 | payload = struct.pack('>H', RTDE_PROTOCOL_VERSION) 116 | success = self.__sendAndReceive(cmd, payload) 117 | return success 118 | 119 | def send_input_setup(self, variables, types=[]): 120 | cmd = Command.RTDE_CONTROL_PACKAGE_SETUP_INPUTS 121 | payload = bytearray(','.join(variables), 'utf-8') 122 | result = self.__sendAndReceive(cmd, payload) 123 | if len(types)!=0 and not self.__list_equals(result.types, types): 124 | _log.error('Data type inconsistency for input setup: ' + 125 | str(types) + ' - ' + 126 | str(result.types)) 127 | return None 128 | result.names = variables 129 | self.__input_config[result.id] = result 130 | return serialize.DataObject.create_empty(variables, result.id) 131 | 132 | def send_output_setup(self, variables, types=[], frequency=125): 133 | cmd = Command.RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS 134 | payload = struct.pack('>d', frequency) 135 | payload = payload + (','.join(variables).encode('utf-8')) 136 | result = self.__sendAndReceive(cmd, payload) 137 | if len(types)!=0 and not self.__list_equals(result.types, types): 138 | _log.error('Data type inconsistency for output setup: ' + 139 | str(types) + ' - ' + 140 | str(result.types)) 141 | return False 142 | result.names = variables 143 | self.__output_config = result 144 | return True 145 | 146 | def send_start(self): 147 | cmd = Command.RTDE_CONTROL_PACKAGE_START 148 | success = self.__sendAndReceive(cmd) 149 | if success: 150 | _log.info('RTDE synchronization started') 151 | self.__conn_state = ConnectionState.STARTED 152 | else: 153 | _log.error('RTDE synchronization failed to start') 154 | return success 155 | 156 | def send_pause(self): 157 | cmd = Command.RTDE_CONTROL_PACKAGE_PAUSE 158 | success = self.__sendAndReceive(cmd) 159 | if success: 160 | _log.info('RTDE synchronization paused') 161 | self.__conn_state = ConnectionState.PAUSED 162 | else: 163 | _log.error('RTDE synchronization failed to pause') 164 | return success 165 | 166 | def send(self, input_data): 167 | if self.__conn_state != ConnectionState.STARTED: 168 | _log.error('Cannot send when RTDE synchronization is inactive') 169 | return 170 | if not input_data.recipe_id in self.__input_config: 171 | _log.error('Input configuration id not found: ' + str(input_data.recipe_id)) 172 | return 173 | config = self.__input_config[input_data.recipe_id] 174 | return self.__sendall(Command.RTDE_DATA_PACKAGE, config.pack(input_data)) 175 | 176 | def receive(self, binary=False): 177 | if self.__output_config is None: 178 | _log.error('Output configuration not initialized') 179 | return None 180 | if self.__conn_state != ConnectionState.STARTED: 181 | _log.error('Cannot receive when RTDE synchronization is inactive') 182 | return None 183 | return self.__recv(Command.RTDE_DATA_PACKAGE, binary) 184 | 185 | def send_message(self, message, source = "Python Client", type = serialize.Message.INFO_MESSAGE): 186 | cmd = Command.RTDE_TEXT_MESSAGE 187 | fmt = '>B%dsB%dsB' % (len(message), len(source)) 188 | payload = struct.pack(fmt, len(message), message, len(source), source, type) 189 | return self.__sendall(cmd, payload) 190 | 191 | def __on_packet(self, cmd, payload): 192 | if cmd == Command.RTDE_REQUEST_PROTOCOL_VERSION: 193 | return self.__unpack_protocol_version_package(payload) 194 | elif cmd == Command.RTDE_GET_URCONTROL_VERSION: 195 | return self.__unpack_urcontrol_version_package(payload) 196 | elif cmd == Command.RTDE_TEXT_MESSAGE: 197 | return self.__unpack_text_message(payload) 198 | elif cmd == Command.RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS: 199 | return self.__unpack_setup_outputs_package(payload) 200 | elif cmd == Command.RTDE_CONTROL_PACKAGE_SETUP_INPUTS: 201 | return self.__unpack_setup_inputs_package(payload) 202 | elif cmd == Command.RTDE_CONTROL_PACKAGE_START: 203 | return self.__unpack_start_package(payload) 204 | elif cmd == Command.RTDE_CONTROL_PACKAGE_PAUSE: 205 | return self.__unpack_pause_package(payload) 206 | elif cmd == Command.RTDE_DATA_PACKAGE: 207 | return self.__unpack_data_package(payload, self.__output_config) 208 | else: 209 | _log.error('Unknown package command: ' + str(cmd)) 210 | 211 | def __sendAndReceive(self, cmd, payload=b''): 212 | if self.__sendall(cmd, payload): 213 | return self.__recv(cmd) 214 | else: 215 | return None 216 | 217 | def __sendall(self, command, payload=b''): 218 | fmt = '>HB' 219 | size = struct.calcsize(fmt) + len(payload) 220 | buf = struct.pack(fmt, size, command) + payload 221 | 222 | if self.__sock is None: 223 | _log.error('Unable to send: not connected to Robot') 224 | return False 225 | 226 | _, writable, _ = select.select([], [self.__sock], [], DEFAULT_TIMEOUT) 227 | if len(writable): 228 | self.__sock.sendall(buf) 229 | return True 230 | else: 231 | self.__trigger_disconnected() 232 | return False 233 | 234 | def has_data(self): 235 | timeout = 0 236 | readable, _, _ = select.select([self.__sock], [], [], timeout) 237 | return len(readable)!=0 238 | 239 | def __recv(self, command, binary=False): 240 | while self.is_connected(): 241 | readable, _, xlist = select.select([self.__sock], [], [self.__sock], DEFAULT_TIMEOUT) 242 | if len(readable): 243 | more = self.__sock.recv(4096) 244 | if len(more) == 0: 245 | self.__trigger_disconnected() 246 | return None 247 | self.__buf = self.__buf + more 248 | 249 | if len(xlist) or len(readable) == 0: # Effectively a timeout of DEFAULT_TIMEOUT seconds 250 | _log.info('lost connection with controller') 251 | self.__trigger_disconnected() 252 | return None 253 | 254 | # unpack_from requires a buffer of at least 3 bytes 255 | while len(self.__buf) >= 3: 256 | # Attempts to extract a packet 257 | packet_header = serialize.ControlHeader.unpack(self.__buf) 258 | 259 | if len(self.__buf) >= packet_header.size: 260 | packet, self.__buf = self.__buf[3:packet_header.size], self.__buf[packet_header.size:] 261 | data = self.__on_packet(packet_header.command, packet) 262 | if len(self.__buf) >= 3 and command == Command.RTDE_DATA_PACKAGE: 263 | next_packet_header = serialize.ControlHeader.unpack(self.__buf) 264 | if next_packet_header.command == command: 265 | _log.info('skipping package(1)') 266 | continue 267 | if packet_header.command == command: 268 | if(binary): 269 | return packet[1:] 270 | 271 | return data 272 | else: 273 | _log.info('skipping package(2)') 274 | else: 275 | break 276 | return None 277 | 278 | def __trigger_disconnected(self): 279 | _log.info("RTDE disconnected") 280 | self.disconnect() #clean-up 281 | 282 | def __unpack_protocol_version_package(self, payload): 283 | if len(payload) != 1: 284 | _log.error('RTDE_REQUEST_PROTOCOL_VERSION: Wrong payload size') 285 | return None 286 | result = serialize.ReturnValue.unpack(payload) 287 | return result.success 288 | 289 | def __unpack_urcontrol_version_package(self, payload): 290 | if len(payload) != 16: 291 | _log.error('RTDE_GET_URCONTROL_VERSION: Wrong payload size') 292 | return None 293 | version = serialize.ControlVersion.unpack(payload) 294 | return version 295 | 296 | def __unpack_text_message(self, payload): 297 | if len(payload) < 1: 298 | _log.error('RTDE_TEXT_MESSAGE: No payload') 299 | return None 300 | msg = serialize.Message.unpack(payload) 301 | if(msg.level == serialize.Message.EXCEPTION_MESSAGE or 302 | msg.level == serialize.Message.ERROR_MESSAGE): 303 | _log.error(msg.source + ': ' + msg.message) 304 | elif msg.level == serialize.Message.WARNING_MESSAGE: 305 | _log.warning(msg.source + ': ' + msg.message) 306 | elif msg.level == serialize.Message.INFO_MESSAGE: 307 | _log.info(msg.source + ': ' + msg.message) 308 | 309 | def __unpack_setup_outputs_package(self, payload): 310 | if len(payload) < 1: 311 | _log.error('RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS: No payload') 312 | return None 313 | output_config = serialize.DataConfig.unpack_recipe(payload) 314 | return output_config 315 | 316 | def __unpack_setup_inputs_package(self, payload): 317 | if len(payload) < 1: 318 | _log.error('RTDE_CONTROL_PACKAGE_SETUP_INPUTS: No payload') 319 | return None 320 | input_config = serialize.DataConfig.unpack_recipe(payload) 321 | return input_config 322 | 323 | def __unpack_start_package(self, payload): 324 | if len(payload) != 1: 325 | _log.error('RTDE_CONTROL_PACKAGE_START: Wrong payload size') 326 | return None 327 | result = serialize.ReturnValue.unpack(payload) 328 | return result.success 329 | 330 | def __unpack_pause_package(self, payload): 331 | if len(payload) != 1: 332 | _log.error('RTDE_CONTROL_PACKAGE_PAUSE: Wrong payload size') 333 | return None 334 | result = serialize.ReturnValue.unpack(payload) 335 | return result.success 336 | 337 | def __unpack_data_package(self, payload, output_config): 338 | if output_config is None: 339 | _log.error('RTDE_DATA_PACKAGE: Missing output configuration') 340 | return None 341 | output = output_config.unpack(payload) 342 | return output 343 | 344 | def __list_equals(self, l1, l2): 345 | if len(l1) != len(l2): 346 | return False 347 | for i in range(len((l1))): 348 | if l1[i] != l2[i]: 349 | return False 350 | return True 351 | -------------------------------------------------------------------------------- /control_RG2_master/serialize.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016, Universal Robots A/S, 2 | # All rights reserved. 3 | # Redistribution and use in source and binary forms, with or without 4 | # modification, are permitted provided that the following conditions are met: 5 | # * Redistributions of source code must retain the above copyright 6 | # notice, this list of conditions and the following disclaimer. 7 | # * Redistributions in binary form must reproduce the above copyright 8 | # notice, this list of conditions and the following disclaimer in the 9 | # documentation and/or other materials provided with the distribution. 10 | # * Neither the name of the Universal Robots A/S nor the names of its 11 | # contributors may be used to endorse or promote products derived 12 | # from this software without specific prior written permission. 13 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | # DISCLAIMED. IN NO EVENT SHALL UNIVERSAL ROBOTS A/S BE LIABLE FOR ANY 17 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | import struct 25 | 26 | 27 | class ControlHeader(object): 28 | __slots__ = ['command', 'size',] 29 | 30 | @staticmethod 31 | def unpack(buf): 32 | rmd = ControlHeader() 33 | (rmd.size, rmd.command) = struct.unpack_from('>HB', buf) 34 | return rmd 35 | 36 | 37 | class ControlVersion(object): 38 | __slots__ = ['major', 'minor', 'bugfix', 'build'] 39 | 40 | @staticmethod 41 | def unpack(buf): 42 | rmd = ControlVersion() 43 | (rmd.major, rmd.minor, rmd.bugfix, rmd.build) = struct.unpack_from('>IIII', buf) 44 | return rmd 45 | 46 | 47 | class ReturnValue(object): 48 | __slots__ = ['success'] 49 | 50 | @staticmethod 51 | def unpack(buf): 52 | rmd = ReturnValue() 53 | rmd.success = bool(struct.unpack_from('>B', buf)[0]) 54 | return rmd 55 | 56 | 57 | class Message(object): 58 | __slots__ = ['level', 'message', 'source'] 59 | EXCEPTION_MESSAGE = 0 60 | ERROR_MESSAGE = 1 61 | WARNING_MESSAGE = 2 62 | INFO_MESSAGE = 3 63 | 64 | @staticmethod 65 | def unpack(buf): 66 | rmd = Message() 67 | offset = 0 68 | msg_length = struct.unpack_from(">B", buf, offset)[0] 69 | offset = offset + 1 70 | rmd.message = buf[offset:offset+msg_length] 71 | offset = offset + msg_length 72 | 73 | src_length = struct.unpack_from(">B", buf, offset)[0] 74 | offset = offset + 1 75 | rmd.source = buf[offset:offset+src_length] 76 | offset = offset + src_length 77 | rmd.level = struct.unpack_from(">B", buf, offset)[0] 78 | 79 | return rmd 80 | 81 | 82 | def get_item_size(data_type): 83 | if data_type.startswith('VECTOR6'): 84 | return 6 85 | elif data_type.startswith('VECTOR3'): 86 | return 3 87 | return 1 88 | 89 | def unpack_field(data, offset, data_type): 90 | size = get_item_size(data_type) 91 | if(data_type == 'VECTOR6D' or 92 | data_type == 'VECTOR3D'): 93 | return [float(data[offset+i]) for i in range(size)] 94 | elif(data_type == 'VECTOR6UINT32'): 95 | return [int(data[offset+i]) for i in range(size)] 96 | elif(data_type == 'DOUBLE'): 97 | return float(data[offset]) 98 | elif(data_type == 'UINT32' or 99 | data_type == 'UINT64'): 100 | return int(data[offset]) 101 | elif(data_type == 'VECTOR6INT32'): 102 | return [int(data[offset+i]) for i in range(size)] 103 | elif(data_type == 'INT32' or 104 | data_type == 'UINT8'): 105 | return int(data[offset]) 106 | elif(data_type == 'BOOL'): 107 | return bool(data[offset]) 108 | raise ValueError('unpack_field: unknown data type: ' + data_type) 109 | 110 | 111 | class DataObject(object): 112 | recipe_id = None 113 | def pack(self, names, types): 114 | if len(names) != len(types): 115 | raise ValueError('List sizes are not identical.') 116 | l = [] 117 | if(self.recipe_id is not None): 118 | l.append(self.recipe_id) 119 | for i in range(len(names)): 120 | if self.__dict__[names[i]] is None: 121 | raise ValueError('Uninitialized parameter: ' + names[i]) 122 | if types[i].startswith('VECTOR'): 123 | l.extend(self.__dict__[names[i]]) 124 | else: 125 | l.append(self.__dict__[names[i]]) 126 | return l 127 | 128 | @staticmethod 129 | def unpack(data, names, types): 130 | if len(names) != len(types): 131 | raise ValueError('List sizes are not identical.') 132 | obj = DataObject() 133 | offset = 0 134 | obj.recipe_id = data[0] 135 | for i in range(len(names)): 136 | obj.__dict__[names[i]] = unpack_field(data[1:], offset, types[i]) 137 | offset += get_item_size(types[i]) 138 | return obj 139 | 140 | @staticmethod 141 | def create_empty(names, recipe_id): 142 | obj = DataObject() 143 | for i in range(len(names)): 144 | obj.__dict__[names[i]] = None 145 | obj.recipe_id = recipe_id 146 | return obj 147 | 148 | 149 | class DataConfig(object): 150 | __slots__ = ['id', 'names', 'types', 'fmt'] 151 | @staticmethod 152 | def unpack_recipe(buf): 153 | rmd = DataConfig(); 154 | rmd.id = struct.unpack_from('>B', buf)[0] 155 | rmd.types = buf.decode('utf-8')[1:].split(',') 156 | rmd.fmt = '>B' 157 | for i in rmd.types: 158 | if i=='INT32': 159 | rmd.fmt += 'i' 160 | elif i=='UINT32': 161 | rmd.fmt += 'I' 162 | elif i=='VECTOR6D': 163 | rmd.fmt += 'd'*6 164 | elif i=='VECTOR3D': 165 | rmd.fmt += 'd'*3 166 | elif i=='VECTOR6INT32': 167 | rmd.fmt += 'i'*6 168 | elif i=='VECTOR6UINT32': 169 | rmd.fmt += 'I'*6 170 | elif i=='DOUBLE': 171 | rmd.fmt += 'd' 172 | elif i=='UINT64': 173 | rmd.fmt += 'Q' 174 | elif i=='UINT8': 175 | rmd.fmt += 'B' 176 | elif i =='BOOL': 177 | rmd.fmt += '?' 178 | elif i=='IN_USE': 179 | raise ValueError('An input parameter is already in use.') 180 | else: 181 | raise ValueError('Unknown data type: ' + i) 182 | return rmd 183 | 184 | def pack(self, state): 185 | l = state.pack(self.names, self.types) 186 | return struct.pack(self.fmt, *l) 187 | 188 | def unpack(self, data): 189 | li = struct.unpack_from(self.fmt, data) 190 | return DataObject.unpack(li, self.names, self.types) 191 | 192 | -------------------------------------------------------------------------------- /control_RG2_master/test_main.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import socket 3 | import time 4 | import struct 5 | import util 6 | import rtde 7 | 8 | HOST = "192.168.1.111" 9 | PORT = 30003 10 | 11 | 12 | def get_current_tcp(): 13 | tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 14 | tcp_socket.connect((HOST, PORT)) 15 | data = tcp_socket.recv(1108) 16 | position = struct.unpack('!6d', data[444:492]) 17 | tcp_socket.close() 18 | return np.asarray(position) 19 | 20 | 21 | def get_current_pos(): # x, y, theta 22 | tcp = get_current_tcp() 23 | rpy = util.rv2rpy(tcp[3], tcp[4], tcp[5]) 24 | return np.asarray([tcp[0], tcp[1], rpy[-1]]) 25 | 26 | 27 | def get_current_pos_same_with_simulation(): 28 | tcp = get_current_tcp() 29 | rpy = util.rv2rpy(tcp[3], tcp[4], tcp[5]) 30 | return np.asarray([tcp[1], tcp[0], rpy[-1]]) 31 | 32 | 33 | def move_to_tcp(target_tcp): 34 | tool_acc = 0.1 # Safe: 0.5 35 | tool_vel = 0.1 # Safe: 0.2 36 | tool_pos_tolerance = [0.001, 0.001, 0.001, 0.05, 0.05, 0.05] 37 | tcp_command = "movel(p[%f,%f,%f,%f,%f,%f],a=%f,v=%f,t=0,r=0)\n" % ( 38 | target_tcp[0], target_tcp[1], target_tcp[2], target_tcp[3], target_tcp[4], 39 | target_tcp[5], 40 | tool_acc, tool_vel) 41 | tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 42 | tcp_socket.connect((HOST, PORT)) 43 | tcp_socket.send(str.encode(tcp_command)) # 利用字符串的encode方法编码成bytes，默认为utf-8类型 44 | tcp_socket.close() 45 | # 确保已达到目标点，就可以紧接着发送下一条指令 46 | actual_pos = get_current_tcp() 47 | target_rpy = util.rv2rpy(target_tcp[3], target_tcp[4], target_tcp[5]) 48 | rpy = util.rv2rpy(actual_pos[3], actual_pos[4], actual_pos[5]) 49 | while not (all([np.abs(actual_pos[j] - target_tcp[j]) < tool_pos_tolerance[j] for j in range(3)]) 50 | and all([np.abs(rpy[j] - target_rpy[j]) < tool_pos_tolerance[j + 3] for j in range(3)])): 51 | actual_pos = get_current_tcp() 52 | rpy = util.rv2rpy(actual_pos[3], actual_pos[4], actual_pos[5]) 53 | time.sleep(0.01) 54 | 55 | 56 | def increase_move(delta_x, delta_y, delta_z, delta_theta): 57 | tcp = get_current_tcp() 58 | rpy = util.rv2rpy(tcp[3], tcp[4], tcp[5]) 59 | rpy[2] = rpy[2] + delta_theta 60 | target_rv = util.rpy2rv(rpy) 61 | target_tcp = np.asarray([tcp[0] + delta_x, tcp[1] + delta_y, tcp[2] + delta_z, 62 | target_rv[0], target_rv[1], target_rv[2]]) 63 | move_to_tcp(target_tcp) 64 | 65 | 66 | def get_digital_output(): 67 | tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 68 | tcp_socket.connect((HOST, PORT)) 69 | data = tcp_socket.recv(1108) 70 | tool = struct.unpack('!d', data[1044:1052])[0] 71 | tcp_socket.close() 72 | return tool 73 | 74 | 75 | def operate_gripper(target_width): 76 | tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 77 | tcp_socket.connect((HOST, PORT)) 78 | 79 | tcp_command = "def rg2ProgOpen():\n" 80 | tcp_command += "\ttextmsg(\"inside RG2 function called\")\n" 81 | 82 | tcp_command += '\ttarget_width={}\n'.format(target_width) 83 | tcp_command += "\ttarget_force=40\n" 84 | tcp_command += "\tpayload=1.0\n" 85 | tcp_command += "\tset_payload1=False\n" 86 | tcp_command += "\tdepth_compensation=False\n" 87 | tcp_command += "\tslave=False\n" 88 | 89 | tcp_command += "\ttimeout = 0\n" 90 | tcp_command += "\twhile get_digital_in(9) == False:\n" 91 | tcp_command += "\t\ttextmsg(\"inside while\")\n" 92 | tcp_command += "\t\tif timeout > 400:\n" 93 | tcp_command += "\t\t\tbreak\n" 94 | tcp_command += "\t\tend\n" 95 | tcp_command += "\t\ttimeout = timeout+1\n" 96 | tcp_command += "\t\tsync()\n" 97 | tcp_command += "\tend\n" 98 | tcp_command += "\ttextmsg(\"outside while\")\n" 99 | 100 | tcp_command += "\tdef bit(input):\n" 101 | tcp_command += "\t\tmsb=65536\n" 102 | tcp_command += "\t\tlocal i=0\n" 103 | tcp_command += "\t\tlocal output=0\n" 104 | tcp_command += "\t\twhile i<17:\n" 105 | tcp_command += "\t\t\tset_digital_out(8,True)\n" 106 | tcp_command += "\t\t\tif input>=msb:\n" 107 | tcp_command += "\t\t\t\tinput=input-msb\n" 108 | tcp_command += "\t\t\t\tset_digital_out(9,False)\n" 109 | tcp_command += "\t\t\telse:\n" 110 | tcp_command += "\t\t\t\tset_digital_out(9,True)\n" 111 | tcp_command += "\t\t\tend\n" 112 | tcp_command += "\t\t\tif get_digital_in(8):\n" 113 | tcp_command += "\t\t\t\tout=1\n" 114 | tcp_command += "\t\t\tend\n" 115 | tcp_command += "\t\t\tsync()\n" 116 | tcp_command += "\t\t\tset_digital_out(8,False)\n" 117 | tcp_command += "\t\t\tsync()\n" 118 | tcp_command += "\t\t\tinput=input*2\n" 119 | tcp_command += "\t\t\toutput=output*2\n" 120 | tcp_command += "\t\t\ti=i+1\n" 121 | tcp_command += "\t\tend\n" 122 | tcp_command += "\t\treturn output\n" 123 | tcp_command += "\tend\n" 124 | tcp_command += "\ttextmsg(\"outside bit definition\")\n" 125 | 126 | tcp_command += "\ttarget_width=target_width+0.0\n" 127 | tcp_command += "\tif target_force>40:\n" 128 | tcp_command += "\t\ttarget_force=40\n" 129 | tcp_command += "\tend\n" 130 | 131 | tcp_command += "\tif target_force<4:\n" 132 | tcp_command += "\t\ttarget_force=4\n" 133 | tcp_command += "\tend\n" 134 | tcp_command += "\tif target_width>110:\n" 135 | tcp_command += "\t\ttarget_width=110\n" 136 | tcp_command += "\tend\n" 137 | tcp_command += "\tif target_width<0:\n" 138 | tcp_command += "\t\ttarget_width=0\n" 139 | tcp_command += "\tend\n" 140 | tcp_command += "\trg_data=floor(target_width)*4\n" 141 | tcp_command += "\trg_data=rg_data+floor(target_force/2)*4*111\n" 142 | tcp_command += "\tif slave:\n" 143 | tcp_command += "\t\trg_data=rg_data+16384\n" 144 | tcp_command += "\tend\n" 145 | 146 | tcp_command += "\ttextmsg(\"about to call bit\")\n" 147 | tcp_command += "\tbit(rg_data)\n" 148 | tcp_command += "\ttextmsg(\"called bit\")\n" 149 | 150 | tcp_command += "\tif depth_compensation:\n" 151 | tcp_command += "\t\tfinger_length = 55.0/1000\n" 152 | tcp_command += "\t\tfinger_heigth_disp = 5.0/1000\n" 153 | tcp_command += "\t\tcenter_displacement = 7.5/1000\n" 154 | 155 | tcp_command += "\t\tstart_pose = get_forward_kin()\n" 156 | tcp_command += "\t\tset_analog_inputrange(2, 1)\n" 157 | tcp_command += "\t\tzscale = (get_analog_in(2)-0.026)/2.976\n" 158 | tcp_command += "\t\tzangle = zscale*1.57079633-0.087266462\n" 159 | tcp_command += "\t\tzwidth = 5+110*sin(zangle)\n" 160 | 161 | tcp_command += "\t\tstart_depth = cos(zangle)*finger_length\n" 162 | 163 | tcp_command += "\t\tsync()\n" 164 | tcp_command += "\t\tsync()\n" 165 | tcp_command += "\t\ttimeout = 0\n" 166 | 167 | tcp_command += "\t\twhile get_digital_in(9) == True:\n" 168 | tcp_command += "\t\t\ttimeout=timeout+1\n" 169 | tcp_command += "\t\t\tsync()\n" 170 | tcp_command += "\t\t\tif timeout > 20:\n" 171 | tcp_command += "\t\t\t\tbreak\n" 172 | tcp_command += "\t\t\tend\n" 173 | tcp_command += "\t\tend\n" 174 | tcp_command += "\t\ttimeout = 0\n" 175 | tcp_command += "\t\twhile get_digital_in(9) == False:\n" 176 | tcp_command += "\t\t\tzscale = (get_analog_in(2)-0.026)/2.976\n" 177 | tcp_command += "\t\t\tzangle = zscale*1.57079633-0.087266462\n" 178 | tcp_command += "\t\t\tzwidth = 5+110*sin(zangle)\n" 179 | tcp_command += "\t\t\tmeasure_depth = cos(zangle)*finger_length\n" 180 | tcp_command += "\t\t\tcompensation_depth = (measure_depth - start_depth)\n" 181 | tcp_command += "\t\t\ttarget_pose = pose_trans(start_pose,p[0,0,-compensation_depth,0,0,0])\n" 182 | tcp_command += "\t\t\tif timeout > 400:\n" 183 | tcp_command += "\t\t\t\tbreak\n" 184 | tcp_command += "\t\t\tend\n" 185 | tcp_command += "\t\t\ttimeout=timeout+1\n" 186 | tcp_command += "\t\t\tservoj(get_inverse_kin(target_pose),0,0,0.008,0.033,1700)\n" 187 | tcp_command += "\t\tend\n" 188 | tcp_command += "\t\tnspeed = norm(get_actual_tcp_speed())\n" 189 | tcp_command += "\t\twhile nspeed > 0.001:\n" 190 | tcp_command += "\t\t\tservoj(get_inverse_kin(target_pose),0,0,0.008,0.033,1700)\n" 191 | tcp_command += "\t\t\tnspeed = norm(get_actual_tcp_speed())\n" 192 | tcp_command += "\t\tend\n" 193 | tcp_command += "\tend\n" 194 | tcp_command += "\tif depth_compensation==False:\n" 195 | tcp_command += "\t\ttimeout = 0\n" 196 | tcp_command += "\t\twhile get_digital_in(9) == True:\n" 197 | tcp_command += "\t\t\ttimeout = timeout+1\n" 198 | tcp_command += "\t\t\tsync()\n" 199 | tcp_command += "\t\t\tif timeout > 20:\n" 200 | tcp_command += "\t\t\t\tbreak\n" 201 | tcp_command += "\t\t\tend\n" 202 | tcp_command += "\t\tend\n" 203 | tcp_command += "\t\ttimeout = 0\n" 204 | tcp_command += "\t\twhile get_digital_in(9) == False:\n" 205 | tcp_command += "\t\t\ttimeout = timeout+1\n" 206 | tcp_command += "\t\t\tsync()\n" 207 | tcp_command += "\t\t\tif timeout > 400:\n" 208 | tcp_command += "\t\t\t\tbreak\n" 209 | tcp_command += "\t\t\tend\n" 210 | tcp_command += "\t\tend\n" 211 | tcp_command += "\tend\n" 212 | tcp_command += "\tif set_payload1:\n" 213 | tcp_command += "\t\tif slave:\n" 214 | tcp_command += "\t\t\tif get_analog_in(3) < 2:\n" 215 | tcp_command += "\t\t\t\tzslam=0\n" 216 | tcp_command += "\t\t\telse:\n" 217 | tcp_command += "\t\t\t\tzslam=payload\n" 218 | tcp_command += "\t\t\tend\n" 219 | tcp_command += "\t\telse:\n" 220 | tcp_command += "\t\t\tif get_digital_in(8) == False:\n" 221 | tcp_command += "\t\t\t\tzmasm=0\n" 222 | tcp_command += "\t\t\telse:\n" 223 | tcp_command += "\t\t\t\tzmasm=payload\n" 224 | tcp_command += "\t\t\tend\n" 225 | tcp_command += "\t\tend\n" 226 | tcp_command += "\t\tzsysm=0.0\n" 227 | tcp_command += "\t\tzload=zmasm+zslam+zsysm\n" 228 | tcp_command += "\t\tset_payload(zload)\n" 229 | tcp_command += "\tend\n" 230 | 231 | tcp_command += "end\n" 232 | 233 | tcp_socket.send(str.encode(tcp_command)) # 利用字符串的encode方法编码成bytes，默认为utf-8类型 234 | tcp_socket.close() 235 | time.sleep(1) 236 | # gripper_fully_closed = check_grasp() 237 | 238 | 239 | def check_grasp(): 240 | con = rtde.RTDE(HOST, 30004) 241 | con.connect() 242 | output_names = ['tool_analog_input0'] 243 | output_types = ['DOUBLE'] 244 | con.send_output_setup(output_names, output_types, frequency=125) 245 | con.send_start() 246 | state = con.receive(True) 247 | voltage = struct.unpack('!d', state) 248 | return voltage[0] > 0.3 249 | 250 | 251 | def move_down(): 252 | tcp = get_current_tcp() 253 | tcp[2] = 0.065 254 | move_to_tcp(tcp) 255 | 256 | 257 | def move_up(): 258 | tcp = get_current_tcp() 259 | tcp[2] = 0.13 260 | move_to_tcp(tcp) 261 | 262 | 263 | def grasp(): 264 | operate_gripper(100) 265 | move_down() 266 | operate_gripper(0) 267 | move_up() 268 | return check_grasp() 269 | 270 | 271 | def go_home(): 272 | operate_gripper(100) 273 | move_to_tcp([111.13/1000,486.79/1000,432.98/1000,2.226,-2.229,0.013]) 274 | 275 | 276 | def rotation_matrix_from_vectors(vec1, vec2): 277 | """ Find the rotation matrix that aligns vec1 to vec2 278 | :param vec1: A 3d "source" vector 279 | :param vec2: A 3d "destination" vector 280 | :return mat: A transform matrix (3x3) which when applied to vec1, aligns it with vec2. 281 | """ 282 | a, b = (vec1 / np.linalg.norm(vec1)).reshape(3), (vec2 / np.linalg.norm(vec2)).reshape(3) 283 | v = np.cross(a, b) 284 | if any(v): # if not all zeros then 285 | c = np.dot(a, b) 286 | s = np.linalg.norm(v) 287 | kmat = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]]) 288 | return np.eye(3) + kmat + kmat.dot(kmat) * ((1 - c) / (s ** 2)) 289 | 290 | else: 291 | return np.eye(3) # cross of all zeros only occurs on identical directions 292 | 293 | 294 | if __name__ == '__main__': 295 | print(get_current_tcp()) 296 | -------------------------------------------------------------------------------- /control_RG2_master/util.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | # 旋转矢量转旋转矩阵 5 | def rv2rm(rx, ry, rz): 6 | theta = np.linalg.norm([rx, ry, rz]) 7 | kx = rx / theta 8 | ky = ry / theta 9 | kz = rz / theta 10 | 11 | c = np.cos(theta) 12 | s = np.sin(theta) 13 | v = 1 - c 14 | 15 | R = np.zeros((3, 3)) 16 | R[0][0] = kx * kx * v + c 17 | R[0][1] = kx * ky * v - kz * s 18 | R[0][2] = kx * kz * v + ky * s 19 | 20 | R[1][0] = ky * kx * v + kz * s 21 | R[1][1] = ky * ky * v + c 22 | R[1][2] = ky * kz * v - kx * s 23 | 24 | R[2][0] = kz * kx * v - ky * s 25 | R[2][1] = kz * ky * v + kx * s 26 | R[2][2] = kz * kz * v + c 27 | 28 | return R 29 | 30 | 31 | # 旋转矩阵转rpy 32 | def rm2rpy(R): 33 | sy = np.sqrt(R[0][0] * R[0][0] + R[1][0] * R[1][0]) 34 | singular = sy < 1e-6 35 | 36 | if not singular: 37 | x = np.arctan2(R[2][1], R[2][2]) 38 | y = np.arctan2(-R[2][0], sy) 39 | z = np.arctan2(R[1][0], R[0][0]) 40 | else: 41 | x = np.arctan2(-R[1][2], R[1][1]) 42 | y = np.arctan2(-R[2][0], sy) 43 | z = 0 44 | 45 | return np.asarray([x, y, z]) 46 | 47 | 48 | # rpy转旋转矩阵 49 | def rpy2rm(rpy): 50 | # Rx = np.zeros((3, 3), dtype=rpy.dtype) 51 | # Ry = np.zeros((3, 3), dtype=rpy.dtype) 52 | # Rz = np.zeros((3, 3), dtype=rpy.dtype) 53 | 54 | R0 = np.zeros((3, 3), dtype=rpy.dtype) 55 | 56 | thetaX = rpy[0] 57 | thetaY = rpy[1] 58 | thetaZ = rpy[2] 59 | 60 | cx = np.cos(thetaX) 61 | sx = np.sin(thetaX) 62 | 63 | cy = np.cos(thetaY) 64 | sy = np.sin(thetaY) 65 | 66 | cz = np.cos(thetaZ) 67 | sz = np.sin(thetaZ) 68 | 69 | R0[0][0] = cz * cy 70 | R0[0][1] = cz * sy * sx - sz * cx 71 | R0[0][2] = cz * sy * cx + sz * sx 72 | R0[1][0] = sz * cy 73 | R0[1][1] = sz * sy * sx + cz * cx 74 | R0[1][2] = sz * sy * cx - cz * sx 75 | R0[2][0] = -sy 76 | R0[2][1] = cy * sx 77 | R0[2][2] = cy * cx 78 | return R0 79 | 80 | 81 | # 旋转矩阵转旋转矢量 82 | def rm2rv(R): 83 | theta = np.arccos((R[0][0] + R[1][1] + R[2][2] - 1) / 2) 84 | K = (1 / (2 * np.sin(theta))) * np.asarray([R[2][1] - R[1][2], R[0][2] - R[2][0], R[1][0] - R[0][1]]) 85 | r = theta * K 86 | return r 87 | 88 | 89 | def rv2rpy(rx, ry, rz): 90 | R = rv2rm(rx, ry, rz) 91 | rpy = rm2rpy(R) 92 | return rpy 93 | 94 | 95 | def rpy2rv(rpy): 96 | R = rpy2rm(rpy) 97 | rv = rm2rv(R) 98 | return rv 99 | -------------------------------------------------------------------------------- /depth_calibration.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | @Time ： 2022/11/12 11:31 4 | @Auth ： 邓浩然 5 | @File ：depth_calibration.py 6 | @IDE ：PyCharm 7 | @Description：负责对深度相机进行校准的程序 8 | """ 9 | import cv2 10 | import torch 11 | import numpy 12 | import numpy as np 13 | import opencv_camera 14 | 15 | def save_txt(): 16 | kkkl = cv2.imread("depth.png", -1) 17 | kkkl = np.array(kkkl) 18 | k = [] 19 | for i in range(480): 20 | for j in range(640): 21 | b = [] 22 | b.append(i) 23 | b.append(j) 24 | b.append(kkkl[i, j]) 25 | k.append(b) 26 | k = np.array(k) 27 | # print(k) 28 | np.savetxt("target_point_cloud.txt", k) 29 | 30 | 31 | def linear_regression(x, y): 32 | N = len(x) 33 | sumx = sum(x) 34 | sumy = sum(y) 35 | sumx2 = sum(x ** 2) 36 | sumxy = sum(x * y) 37 | 38 | A = np.mat([[N, sumx], [sumx, sumx2]]) 39 | b = np.array([sumy, sumxy]) 40 | 41 | return np.linalg.solve(A, b) 42 | 43 | 44 | def adjusting(): 45 | save_txt() 46 | filename = "color.png" 47 | # model_type = "DPT_Large" # MiDaS v3 - Large (highest accuracy, slowest inference speed) 48 | model_type = "DPT_Hybrid" # MiDaS v3 - Hybrid (medium accuracy, medium inference speed) 49 | 50 | # model_type = "MiDaS_small" # MiDaS v2.1 - Small (lowest accuracy, highest inference speed) 51 | 52 | midas = torch.hub.load('C:/Users/deng/.cache/torch/hub/intel-isl_MiDaS_master', model_type, source='local') 53 | 54 | device = torch.device("cuda") if torch.cuda.is_available() else torch.device("cpu") 55 | midas.to(device) 56 | midas.eval() 57 | 58 | midas_transforms = torch.hub.load('C:/Users/deng/.cache/torch/hub/intel-isl_MiDaS_master', "transforms", 59 | source='local') 60 | 61 | if model_type == "DPT_Large" or model_type == "DPT_Hybrid": 62 | transform = midas_transforms.dpt_transform 63 | else: 64 | transform = midas_transforms.small_transform 65 | 66 | img = cv2.imread(filename) 67 | img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) 68 | 69 | input_batch = transform(img).to(device) 70 | 71 | with torch.no_grad(): 72 | prediction = midas(input_batch) 73 | 74 | prediction = torch.nn.functional.interpolate( 75 | prediction.unsqueeze(1), 76 | size=img.shape[:2], 77 | mode="bicubic", 78 | align_corners=False, 79 | ).squeeze() 80 | prediction = prediction 81 | output = prediction.cpu().numpy() 82 | print(output.shape) 83 | 84 | f = open("target_point_cloud.txt", "r") 85 | file = f.readlines() 86 | data = [] 87 | print() 88 | for i in file: 89 | i = i.strip("\n").split(" ") 90 | datalist = [] 91 | for a in i: 92 | datalist.append(float(a)) 93 | datalist.append(output[int(float(i[0]))][int(float(i[1]))]) 94 | data.append(datalist) 95 | # print(data) 96 | X = [] 97 | Y = [] 98 | for aaa in data: 99 | X.append(aaa[3]) 100 | Y.append(aaa[2]) 101 | X = numpy.array(X) 102 | Y = numpy.array(Y) 103 | 104 | # X = numpy.array([8.19,2.72,6.39,8.71,4.7,2.66,3.78]) 105 | # Y = numpy.array([7.01,2.78,6.4,6.71,4.1,4.23,4.05]) 106 | print(X) 107 | print(Y) 108 | # def residuals(p,x): 109 | # k,b = p 110 | # #return Y-(k*X+b) 111 | # return k*x+b 112 | # 113 | # def error(p,x,y): 114 | # return residuals(p, x)- y 115 | # r = optimize.leastsq(error, [1, 1],args=(X,Y)) 116 | # k, b = r[0] 117 | # print("k = ", k ,"b = ", b) 118 | 119 | a0, a1 = linear_regression(X, Y) 120 | print("k = ", a1, "b = ", a0) 121 | return a1, a0 122 | # XX = numpy.arange(1,10,0.1) 123 | # YY = k*XX+b 124 | # plt.plot(XX,YY) 125 | # plt.plot(X,Y,"o") 126 | # plt.xlabel("X") 127 | # plt.ylabel("Y") 128 | # output = output.astype(numpy.uint16) 129 | # cv2.imwrite("./new/"+ssum+"_1.png", output) 130 | # cv2.imwrite("./new/"+ssum+"_2.png", cv2.cvtColor(img, cv2.COLOR_RGB2BGR)) 131 | # convertPNG("./new/"+ssum+"_1.png", ssum)#C:/Users/BAMBOO/Desktop/source pics/rgbd_6/color 132 | 133 | 134 | if __name__ == '__main__': 135 | ca = opencv_camera.Camera() 136 | ca.run() 137 | adjusting() 138 | -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | @Time ： 2024/3/1 14:26 4 | @Auth ： 邓浩然 5 | @File ：main.py 6 | @IDE ：PyCharm 7 | @Description：主程序,主要包含了拍照部分的功能 8 | """ 9 | import os 10 | import numpy as np 11 | import socket 12 | import struct 13 | import keyboard 14 | import camera_D435i 15 | import camera_calibration 16 | import robotcontrol 17 | 18 | """ 19 | 标定安装设置:X:0 Y:0 Z:0 20 | 抓取安装设置X:0 Y:0 Z:200 21 | """ 22 | 23 | # UR-5机器人的IP和端口,IP可以在机器人的网络设置处看到,端口号建议看手册 24 | HOST = "192.168.31.32" 25 | PORT = 30003 26 | 27 | # 数据txt位置 28 | XYZ_PATH = 'data/xyz.txt' 29 | RXRYRZ_PATH = 'data/RxRyRz.txt' 30 | 31 | # 照片存储位置: 32 | PHOTO_PATH = 'photo/' 33 | POINT_PHOTO_PATH = 'point_photo/' 34 | 35 | 36 | # 注意安装位置都为0 37 | # 获得当前位姿,前三个是XYZ单位是mm 后三个是RX RY RZ旋转向量 38 | def get_current_tcp(): 39 | tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 40 | tcp_socket.connect((HOST, PORT)) 41 | data = tcp_socket.recv(1108) 42 | position = struct.unpack('!6d', data[444:492]) 43 | tcp_socket.close() 44 | pos_list = np.asarray(position) 45 | # 单位转化,机械臂传过来的单位是米,我们需要毫米 46 | pos_list[0:3] = pos_list[0:3] * 1000 47 | 48 | return pos_list 49 | 50 | 51 | # 删除上次的拍照照片 52 | def delete_photo(): 53 | for files in os.listdir(PHOTO_PATH): 54 | if files.endswith(".png"): 55 | os.remove(os.path.join(PHOTO_PATH, files)) 56 | for files in os.listdir(POINT_PHOTO_PATH): 57 | if files.endswith(".png"): 58 | os.remove(os.path.join(POINT_PHOTO_PATH, files)) 59 | 60 | 61 | # 拍照的方法,按s拍照,按q退出 62 | def get_photo(photos_num): 63 | # 创建相机线程 64 | camera = camera_D435i.RealSenseCamera() 65 | # 线程开始运行 66 | camera.start_display_and_capture() 67 | # 递增，拍照次数 68 | num = 0 69 | # 用于存储位姿信息 70 | pose = np.zeros((photos_num, 6)) 71 | # 文件名,使用ASCII编码 72 | name_num = 97 73 | # 删除上次的拍照照片 74 | delete_photo() 75 | # 循环读取每一帧 76 | while num < photos_num: 77 | keyboard.wait('S') 78 | # 拍照操作 79 | camera.save_screenshot(f"photo/{chr(name_num)}.png") 80 | # 获得并输出位姿 81 | pose_tcp = get_current_tcp() 82 | print('机械臂位姿: ', np.around(pose_tcp, decimals=3)) 83 | # 把位姿写入数组 84 | pose[num] = pose_tcp 85 | print('第', (num + 1), '张照片拍摄完成') 86 | print("-------------------------") 87 | # 循环标志+1 88 | num += 1 89 | # 文件名ASCII码+1 90 | name_num += 1 91 | 92 | # 写入xyz数据到xyz.txt 93 | np.savetxt(XYZ_PATH, pose[:, 0:3], delimiter=',') 94 | # 写入rxryrz数据到rxryrz.txt 95 | np.savetxt(RXRYRZ_PATH, pose[:, 3:6], delimiter=',') 96 | camera.stop() 97 | 98 | 99 | 100 | if __name__ == '__main__': 101 | ro = robotcontrol.RobotControl() 102 | # ro.reset() 103 | # ro.moveJ_Angle([-60, -128, -60, -15, 90, -95, ]) 104 | # 自由移动模式 105 | ro.rtde_c.teachMode() 106 | # 拍几张照片 107 | get_photo(8) 108 | camera_calibration.camera_calibration() 109 | -------------------------------------------------------------------------------- /opencv_camera.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | @Time ： 2022/9/4 16:40 4 | @Auth ： 邓浩然 5 | @File ：opencv_camera.py 6 | @IDE ：PyCharm 7 | @Description：负责拍照的类,使用线程 8 | """ 9 | import time 10 | import pyrealsense2 as rs 11 | import numpy as np 12 | import cv2 13 | 14 | 15 | class Camera: 16 | def __init__(self): 17 | self.flag = True 18 | # 用于存放帧集合 19 | self.frame_list = [] 20 | # 时间滤波器 21 | self.temporal = rs.temporal_filter() 22 | # 孔洞填充过滤器 23 | # self.hole_filling = rs.hole_filling_filter() 24 | # 空间过滤器 25 | self.spatial = rs.spatial_filter() 26 | self.spatial.set_option(rs.option.filter_magnitude, 1) 27 | self.spatial.set_option(rs.option.filter_smooth_alpha, 0.25) 28 | self.spatial.set_option(rs.option.filter_smooth_delta, 50) 29 | # 叠加孔洞填充 30 | self.spatial.set_option(rs.option.holes_fill, 3) 31 | # 深度图着色器 32 | self.colorizer = rs.colorizer() 33 | # 点云处理工具 34 | self.pc = rs.pointcloud() 35 | 36 | # 负责对照片进行处理 37 | def take_photo(self, color_image): 38 | # 时间滤波 39 | temp_filtered = None 40 | for x in range(10): 41 | temp_filtered = self.temporal.process(self.frame_list[x]) 42 | filtered_depth = self.spatial.process(temp_filtered) 43 | filtered_depth_image = np.asanyarray(filtered_depth.get_data()) 44 | 45 | # 保存待识别的图片 46 | cv2.imwrite('color.png', color_image) 47 | # 保存深度图 48 | cv2.imwrite('depth.png', filtered_depth_image) 49 | 50 | def run(self): 51 | # 配置深度和颜色流 52 | pipeline = rs.pipeline() 53 | camera_config = rs.config() 54 | 55 | # 获取设备产品线，用于设置支持分辨率 56 | pipeline_wrapper = rs.pipeline_wrapper(pipeline) 57 | pipeline_profile = camera_config.resolve(pipeline_wrapper) 58 | device = pipeline_profile.get_device() 59 | 60 | # 创建对齐对象（深度对齐颜色） 61 | align = rs.align(rs.stream.color) 62 | found_rgb = False 63 | for s in device.sensors: 64 | if s.get_info(rs.camera_info.name) == 'RGB Camera': 65 | found_rgb = True 66 | break 67 | if not found_rgb: 68 | print("The demo requires Depth camera with Color sensor") 69 | exit(0) 70 | 71 | # 深度参数 72 | camera_config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) 73 | # 颜色参数 74 | camera_config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) 75 | 76 | # 开始流式传输 77 | pipeline.start(camera_config) 78 | time.sleep(2) 79 | try: 80 | while len(self.frame_list) < 10: 81 | # 等待一对连贯的帧：深度和颜色 82 | frames = pipeline.wait_for_frames() 83 | # 对齐后再获取 84 | aligned_frames = align.process(frames) 85 | 86 | # 需要进行拍照,就开始存放帧 87 | self.frame_list.append(aligned_frames.get_depth_frame()) 88 | 89 | # 获得对齐后的深度和颜色帧 90 | depth_frame = aligned_frames.get_depth_frame() 91 | color_frame = aligned_frames.get_color_frame() 92 | 93 | if not depth_frame or not color_frame: 94 | continue 95 | 96 | # 将图像转换为 numpy 数组,也成为了图像 97 | color_image = np.asanyarray(color_frame.get_data()) 98 | 99 | # 在深度图像上应用颜色图 100 | depth_colormap = self.colorizer.colorize(depth_frame).get_data() 101 | 102 | # 需要进行拍照,我们会获取10帧来进行彩色图,深度图和点云的计算 103 | if len(self.frame_list) == 10: 104 | # 存完了10帧，停止拍照 105 | self.take_photo(color_image) 106 | # 拍摄完成 107 | print('拍照完成') 108 | 109 | images = np.hstack((color_image, depth_colormap)) 110 | 111 | # 显示图像 112 | cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE) 113 | cv2.imshow('RealSense', images) 114 | cv2.waitKey(1) 115 | 116 | finally: 117 | # 停止流式传输 118 | pipeline.stop() 119 | -------------------------------------------------------------------------------- /robotcontrol.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import util 3 | import math 4 | import rtde_control 5 | import rtde_receive 6 | import rtde_io 7 | 8 | # UR5连接参数 9 | HOST = "192.168.31.32" 10 | PORT = 30003 11 | 12 | 13 | class RobotControl: 14 | 15 | def __init__(self): 16 | # 初始化socket来获得数据 17 | self.tool_acc = 0.5 # Safe: 0.5 18 | self.tool_vel = 0.2 # Safe: 0.2 19 | # UR官方的RTDE接口,可用于控制和读取数据 20 | # rtde_c复制UR5的控制 21 | self.rtde_c = rtde_control.RTDEControlInterface(HOST) 22 | # rtde_r负责UR5的数据读取 23 | self.rtde_r = rtde_receive.RTDEReceiveInterface(HOST) 24 | # rtde_io负责控制机器臂的数字输出等 25 | self.rtde_io = rtde_io.RTDEIOInterface(HOST) 26 | 27 | def __del__(self): 28 | self.rtde_r.disconnect() 29 | self.rtde_c.disconnect() 30 | 31 | def get_current_tcp(self): 32 | """ 获得XYZ RXRYRZ,XYZ单位是M,示教器上单位是mm.RXYZ和示教器上一致 """ 33 | return self.rtde_r.getActualTCPPose() 34 | 35 | def get_speed(self): 36 | """ 获得机器臂的运动速度 """ 37 | return self.rtde_r.getActualTCPSpeed() 38 | 39 | def get_current_angle(self): 40 | """ 获得各个关节的角度,返回数组,依次为机座,肩部,肘部,手腕1 2 3 """ 41 | # 获得弧度数组 42 | actual = np.array(self.rtde_r.getActualQ()) 43 | # 转化为角度 44 | actual = actual * 180 / math.pi 45 | return actual 46 | 47 | def get_current_radian(self): 48 | """ 返回各个关节的弧度，返回数组,依次为机座,肩部,肘部,手腕1 2 3 """ 49 | return self.rtde_r.getActualQ() 50 | 51 | def get_current_pos(self): 52 | """ x, y, theta """ 53 | tcp = self.get_current_tcp() 54 | rpy = util.rv2rpy(tcp[3], tcp[4], tcp[5]) 55 | return np.asarray([tcp[0], tcp[1], rpy[-1]]) 56 | 57 | def get_current_pos_same_with_simulation(self): 58 | tcp = self.get_current_tcp() 59 | rpy = util.rv2rpy(tcp[3], tcp[4], tcp[5]) 60 | return np.asarray([tcp[1], tcp[0], rpy[-1]]) 61 | 62 | def move_up(self, z): 63 | """机械臂末端向上移动多少mm""" 64 | tcp = self.get_current_tcp() 65 | tcp[2] = tcp[2] + z / 1000 66 | self.rtde_c.moveL(tcp, speed=self.tool_vel, acceleration=self.tool_acc) 67 | 68 | def move_down(self, z): 69 | """机械臂末端向下移动多少mm""" 70 | tcp = self.get_current_tcp() 71 | tcp[2] = tcp[2] - z / 1000 72 | self.rtde_c.moveL(tcp, speed=self.tool_vel, acceleration=self.tool_acc) 73 | 74 | def reset(self, tool_vel=0.8, tool_acc=0.5): 75 | """机器臂复位""" 76 | self.rtde_c.moveJ( 77 | q=[-0.785398157435008, -1.570796314870016, -1.570796314870016, -1.570796314870016, 1.5707963705062866, 0.0], 78 | speed=tool_vel, acceleration=tool_acc) 79 | 80 | def moveJ_Angle(self, angles, tool_vel=0.8, tool_acc=0.5): 81 | """机器臂复位""" 82 | self.rtde_c.moveJ(q=[angles[0] / 180 * math.pi, angles[1] / 180 * math.pi, angles[2] / 180 * math.pi, 83 | angles[3] / 180 * math.pi, angles[4] / 180 * math.pi, angles[5] / 180 * math.pi], 84 | speed=tool_vel, acceleration=tool_acc) 85 | -------------------------------------------------------------------------------- /util.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | # 旋转矢量转旋转矩阵 5 | def rv2rm(rx, ry, rz): 6 | theta = np.linalg.norm([rx, ry, rz]) 7 | kx = rx / theta 8 | ky = ry / theta 9 | kz = rz / theta 10 | 11 | c = np.cos(theta) 12 | s = np.sin(theta) 13 | v = 1 - c 14 | 15 | R = np.zeros((3, 3)) 16 | R[0][0] = kx * kx * v + c 17 | R[0][1] = kx * ky * v - kz * s 18 | R[0][2] = kx * kz * v + ky * s 19 | 20 | R[1][0] = ky * kx * v + kz * s 21 | R[1][1] = ky * ky * v + c 22 | R[1][2] = ky * kz * v - kx * s 23 | 24 | R[2][0] = kz * kx * v - ky * s 25 | R[2][1] = kz * ky * v + kx * s 26 | R[2][2] = kz * kz * v + c 27 | 28 | return R 29 | 30 | 31 | # 旋转矩阵转rpy 32 | def rm2rpy(R): 33 | sy = np.sqrt(R[0][0] * R[0][0] + R[1][0] * R[1][0]) 34 | singular = sy < 1e-6 35 | 36 | if not singular: 37 | x = np.arctan2(R[2][1], R[2][2]) 38 | y = np.arctan2(-R[2][0], sy) 39 | z = np.arctan2(R[1][0], R[0][0]) 40 | else: 41 | x = np.arctan2(-R[1][2], R[1][1]) 42 | y = np.arctan2(-R[2][0], sy) 43 | z = 0 44 | 45 | return np.asarray([x, y, z]) 46 | 47 | 48 | # rpy转旋转矩阵 49 | def rpy2rm(rpy): 50 | # Rx = np.zeros((3, 3), dtype=rpy.dtype) 51 | # Ry = np.zeros((3, 3), dtype=rpy.dtype) 52 | # Rz = np.zeros((3, 3), dtype=rpy.dtype) 53 | 54 | R0 = np.zeros((3, 3), dtype=rpy.dtype) 55 | 56 | thetaX = rpy[0] 57 | thetaY = rpy[1] 58 | thetaZ = rpy[2] 59 | 60 | cx = np.cos(thetaX) 61 | sx = np.sin(thetaX) 62 | 63 | cy = np.cos(thetaY) 64 | sy = np.sin(thetaY) 65 | 66 | cz = np.cos(thetaZ) 67 | sz = np.sin(thetaZ) 68 | 69 | R0[0][0] = cz * cy 70 | R0[0][1] = cz * sy * sx - sz * cx 71 | R0[0][2] = cz * sy * cx + sz * sx 72 | R0[1][0] = sz * cy 73 | R0[1][1] = sz * sy * sx + cz * cx 74 | R0[1][2] = sz * sy * cx - cz * sx 75 | R0[2][0] = -sy 76 | R0[2][1] = cy * sx 77 | R0[2][2] = cy * cx 78 | return R0 79 | 80 | 81 | # 旋转矩阵转旋转矢量 82 | def rm2rv(R): 83 | theta = np.arccos((R[0][0] + R[1][1] + R[2][2] - 1) / 2) 84 | K = (1 / (2 * np.sin(theta))) * np.asarray([R[2][1] - R[1][2], R[0][2] - R[2][0], R[1][0] - R[0][1]]) 85 | r = theta * K 86 | return r 87 | 88 | 89 | def rv2rpy(rx, ry, rz): 90 | R = rv2rm(rx, ry, rz) 91 | rpy = rm2rpy(R) 92 | return rpy 93 | 94 | 95 | def rpy2rv(rpy): 96 | R = rpy2rm(rpy) 97 | rv = rm2rv(R) 98 | return rv 99 | --------------------------------------------------------------------------------