背景描述
最近在做项目时，使用了ur六轴机器人，遇到了一些难题。起初对机械臂并不了解，通过一步步摸索，总结了一些小经验。本博客主要介绍，如何通过python与机器人建立通信，实时调整机器人位姿并保存TCP位姿和关节角位姿。
一、项目目的
我负责的分支是机器人搭载相机手眼标定，但是使用的是第三方软件进行标定，在标定的过程中，需要实时移动采集机器人的位姿，并保存到本地的json文件夹中。
痛点：
- 通过示教器移动机器人，记录机器人的TCP位姿，需要手动记录，产生大量重复性工作；
- 先通过示教器移动机器人，再使用第三方软件与机器人建立通信，得到机器人当前的位姿，也需要频繁手动开启与机器人的通信（因为操作示教器，三方软件与机器人的通信就会被终止）。
于是，经过考虑，只能脱离示教器，对机器人进行控制，通过前端界面与机器人进行通信，并控制机器人。
优点：
- 可以实时移动机器人，并读取机器人的tcp位姿或关节角位姿；
- 实时自动记录机器人的位姿信息至本地，一次连接，便可得到所有想要的位姿，非常方便。
- 与前端界面结合，非常易用。
前端交互界面采用的是Pyqt5，界面如下：
思路：
- 前端界面属于服务端，输入监听端口号，点击开启按钮，即可等待机器人的连接。
- 通过 实时 移动机器人的关节角位姿，实时发送给机器人，来控制机器人的移动。
- 当机器人移动到指定位置时，即可输出当前机器人的关节角和TCP位姿。
- 步长可以设置当前机器人关节角移动的度数。
二、使用方法
机器人端需要写好socket服务
第一步：服务端输入端口号（7777为例），点击开启：
此时，日志区会提示当前正在监听额端口号和IP。
第二步：机器人端socket程序设置连接的IP和端口号要与服务端一致
tcp_status?socket_open("192.168.xxx.xxx",7777,"socket_1")
第三步：点击示教器开始运行机器人脚本程序，运行成功以后会自动返回机器人的关节角信息，
第四步：调整机器人位姿，此时日志区会显示当前的关节角位姿和TCP位姿。
第五步：如果机器人当前位姿是你想要的位姿，那么点击记录当前位姿即可，会提示生成一个json文件。
json文件内容如下：
{ // TCP位姿 x y z rx ry rz "pose1": [ -0.063743, -0.31572, 0.155045, 2.52045, -0.73415, -2.3137 ], "pose2": [ -0.0637619, -0.315727, 0.155053, 2.87786, -0.895321, -2.7989 ], "pose3": [ -0.0136603, -0.428578, 0.0524651, 0.298622, 3.08598, -0.231548 ] }
三、关键功能源码
1. 服务端通信源码
import socket class RobotServer(object): def __init__(self): self.tcp_server_socket = socket.socket() def connect_robot(self, ip, port): self.tcp_server_socket.bind((ip, port)) self.tcp_server_socket.listen(128) print("Server has been start. Waiting clients connect...") self.conn_socket, self.ip_port = self.tcp_server_socket.accept() print("Client has been connected：", self.ip_port) def send_robot_pose(self, pose): # 基座 肩部 肘部 手腕 1 2 3 * 180/Π # pose = (1, -1.14132,-2.15684,-2.18071,-0.431003,1.56199,0.429502) self.conn_socket.send(str(pose).encode(encoding='utf-8')) def receive_robot_pose(self): recv_data = self.conn_socket.recv(1024) print("接受到的tcp数据：", recv_data.decode()) return recv_data.decode() def get_init_robot_pose(self): recv_data = self.conn_socket.recv(1024) print("接受到初始位关节角姿数据：", recv_data.decode()) return recv_data.decode() def disconnect(self): self.tcp_server_socket.close() if not getattr(self.tcp_server_socket, '_closed'): print("socket is running.") else: print("socket has been close.")
2. 前端交互源码
#!/usr/bin/python # -*- coding: utf-8 -*- import socket import math import threading from PyQt5 import QtCore, QtGui, QtWidgets from PyQt5.QtWidgets import * from PyQt5.QtCore import * from uiwin import Ctrl from communication import Server, SavePose from uiwin.ParameterName import * socket_start = Server.RobotServer() class FuncControlWindows(Ctrl.ControlWindows): def __init__(self): super(FuncControlWindows, self).__init__() self.signal_post() self.mtx = QMutex() def signal_post(self): self.start_server_bt.clicked.connect(self.start_server) self.jizuo_down.clicked.connect(lambda: self.change_value(1)) self.jizuo_up.clicked.connect(lambda: self.change_value(2)) self.jianbu_down.clicked.connect(lambda: self.change_value(3)) self.jianbu_up.clicked.connect(lambda: self.change_value(4)) self.zhoubu_down.clicked.connect(lambda: self.change_value(5)) self.zhoubu_up.clicked.connect(lambda: self.change_value(6)) self.shouwan1_down.clicked.connect(lambda: self.change_value(7)) self.shouwan1_up.clicked.connect(lambda: self.change_value(8)) self.shouwan2_down.clicked.connect(lambda: self.change_value(9)) self.shouwan2_up.clicked.connect(lambda: self.change_value(10)) self.shouwan3_down.clicked.connect(lambda: self.change_value(11)) self.shouwan3_up.clicked.connect(lambda: self.change_value(12)) self.write_pose.clicked.connect(lambda: self.write_json(self.tcp_pose)) def start_server(self): if self.start_condition() and self.start_server_bt.text() == "开启": port = int(self.port_box.text()) ip = socket.gethostbyname_ex(socket.gethostname())[2][-1] print(ip) self.thread_start = Worker(ip, port) self.thread_start.process_value.connect(self.set_init_value) self.thread_start.start() self.log_text.append(server_start_current_append_txt.format(ip, port)) self.start_server_bt.setEnabled(False) self.start_server_bt.setText("等待连接中") self.start_server_bt.setIcon(qta.icon('fa5s.spinner', color='white')) self.log_text.append(wait_client_link_append_txt) def start_condition(self): txt = self.port_box.text() if len(txt) == 0 or not txt.isdigit() or int(txt) < 0 or int(txt) > 65535: QMessageBox.critical(self, '错误', port_link_fail_txt, QMessageBox.Yes, QMessageBox.Yes) return False return True def set_init_value(self, j): i = eval(j) print(format(i[0] * 180 / math.pi, '.5f')) self.jizuo_box.setText(format(i[0] * 180 / math.pi, '.5f')) self.jianbu_box.setText(format(i[1] * 180 / math.pi, '.5f')) self.zhoubu_box.setText(format(i[2] * 180 / math.pi, '.5f')) self.shouwan1_box.setText(format(i[3] * 180 / math.pi, '.5f')) self.shouwan2_box.setText(format(i[4] * 180 / math.pi, '.5f')) self.shouwan3_box.setText(format(i[5] * 180 / math.pi, '.5f')) self.start_server_bt.setText("已连接") self.start_server_bt.setIcon(qta.icon('fa.unlink', color='white')) self.log_text.append(robot_link_success_append_txt) # self.start_server_bt.setEnabled(True) def get_init_tcp_value(self, k): # tcp位姿 [1,1,1,1,1,1] self.tcp_pose = eval(k) print("获取当前TCP位姿：{}".format(self.tcp_pose)) self.log_text.append(tcp_pose_append_txt.format(self.tcp_pose)) def send_post_message(self): jizuo_post = float(self.jizuo_box.text()) jianbu_value = float(self.jianbu_box.text()) zhoubu_value = float(self.zhoubu_box.text()) shouwan1_value = float(self.shouwan1_box.text()) shouwan2_value = float(self.shouwan2_box.text()) shouwan3_value = float(self.shouwan3_box.text()) self.tp = (1, jizuo_post/180 * math.pi, jianbu_value/180 * math.pi, zhoubu_value/180 * math.pi, shouwan1_value/180 * math.pi, shouwan2_value/180 * math.pi, shouwan3_value/180 * math.pi) print(self.tp) self.log_text.append("当前关节角位姿：" + str(self.tp)) self.pose_thread = MyThread(self.tp) self.pose_thread.robot_timely_tcp_pose.connect(self.get_init_tcp_value) self.pose_thread.start() def write_json(self, value): SavePose.create_json(list(value)) self.log_text.append(save_pose_success_append_txt) def change_value(self, i): st = self.check_socket_connect() print(st) if st: self.write_pose.setEnabled(True) if i == 1: value = self.jizuo_box.text() print(value) if self.jizuo_step_com.isChecked(): step = self.jizuo_step_spin.text() cur_value = float(value) - float(step) self.jizuo_box.setText(format(cur_value, '.5f')) else: cur_value = float(value) - 1 self.jizuo_box.setText(format(cur_value, '.5f')) elif i == 2: value = self.jizuo_box.text() if self.jizuo_step_com.isChecked(): step = self.jizuo_step_spin.text() cur_value = float(value) + float(step) self.jizuo_box.setText(format(cur_value, '.5f')) else: cur_value = float(value) + 1 self.jizuo_box.setText(format(cur_value, '.5f')) elif i == 3: value = self.jianbu_box.text() if self.jianbu_step_com.isChecked(): step = self.jianbu_step_spin.text() cur_value = float(value) - float(step) self.jianbu_box.setText(format(cur_value, '.5f')) else: cur_value = float(value) - 1 self.jianbu_box.setText(format(cur_value, '.5f')) elif i == 4: value = self.jianbu_box.text() if self.jianbu_step_com.isChecked(): step = self.jianbu_step_spin.text() cur_value = float(value) + float(step) self.jianbu_box.setText(format(cur_value, '.5f')) else: cur_value = float(value) + 1 self.jianbu_box.setText(format(cur_value, '.5f')) elif i == 5: value = self.zhoubu_box.text() if self.zhoubu_step_com.isChecked(): step = self.zhoubu_step_spin.text() cur_value = float(value) - float(step) self.zhoubu_box.setText(format(cur_value, '.5f')) else: cur_value = float(value) - 1 self.zhoubu_box.setText(format(cur_value, '.5f')) elif i == 6: value = self.zhoubu_box.text() if self.zhoubu_step_com.isChecked(): step = self.zhoubu_step_spin.text() cur_value = float(value) + float(step) self.zhoubu_box.setText(format(cur_value, '.5f')) else: cur_value = float(value) + 1 self.zhoubu_box.setText(format(cur_value, '.5f')) elif i == 7: value = self.shouwan1_box.text() if self.shouwan1_step_com.isChecked(): step = self.shouwan1_step_spin.text() cur_value = float(value) - float(step) self.shouwan1_box.setText(format(cur_value, '.5f')) else: cur_value = float(value) - 1 self.shouwan1_box.setText(format(cur_value, '.5f')) elif i == 8: value = self.shouwan1_box.text() if self.shouwan1_step_com.isChecked(): step = self.shouwan1_step_spin.text() cur_value = float(value) + float(step) self.shouwan1_box.setText(format(cur_value, '.5f')) else: cur_value = float(value) + 1 self.shouwan1_box.setText(format(cur_value, '.5f')) elif i == 9: value = self.shouwan2_box.text() if self.shouwan2_step_com.isChecked(): step = self.shouwan2_step_spin.text() cur_value = float(value) - float(step) self.shouwan2_box.setText(format(cur_value, '.5f')) else: cur_value = float(value) - 1 self.shouwan2_box.setText(format(cur_value, '.5f')) elif i == 10: value = self.shouwan2_box.text() if self.shouwan2_step_com.isChecked(): step = self.shouwan2_step_spin.text() cur_value = float(value) + float(step) self.shouwan2_box.setText(format(cur_value, '.5f')) else: cur_value = float(value) + 1 self.shouwan2_box.setText(format(cur_value, '.5f')) elif i == 11: value = self.shouwan3_box.text() if self.shouwan3_step_com.isChecked(): step = self.shouwan3_step_spin.text() cur_value = float(value) - float(step) self.shouwan3_box.setText(format(cur_value, '.5f')) else: cur_value = float(value) - 1 self.shouwan3_box.setText(format(cur_value, '.5f')) elif i == 12: value = self.shouwan3_box.text() if self.shouwan3_step_com.isChecked(): step = self.shouwan3_step_spin.text() cur_value = float(value) + float(step) self.shouwan3_box.setText(format(cur_value, '.5f')) else: cur_value = float(value) + 1 self.shouwan3_box.setText(format(cur_value, '.5f')) self.send_post_message() # 发送当前位姿信息 else: QMessageBox.warning(self, '错误', no_robot_link_txt, QMessageBox.Yes, QMessageBox.Yes) def check_socket_connect(self): print(self.jizuo_box.text()) if len(self.jizuo_box.text()) != 0 and len(self.jianbu_box.text()) != 0 and len(self.zhoubu_box.text()) != 0 \ and len(self.shouwan1_box.text()) != 0 and len(self.shouwan2_box.text()) != 0 and len(self.shouwan3_box.text()) != 0: return True return False class Worker(QThread): process_value = pyqtSignal(str) def __init__(self, ip, port): super(Worker, self).__init__() self.ip = ip self.port = port def run(self): print(self.ip, self.port) socket_start.connect_robot(self.ip, self.port) init_post = socket_start.get_init_robot_pose() self.process_value.emit(init_post) socket_start.receive_robot_pose() class MyThread(QThread): robot_timely_tcp_pose = pyqtSignal(str) def __init__(self, pose): super(MyThread, self).__init__() self.pose = pose def run(self): socket_start.send_robot_pose(self.pose) init_post = socket_start.receive_robot_pose() self.robot_timely_tcp_pose.emit(init_post)
3. 机器人脚本
格式问题，不能发。
总结
当然，代码远不止这么多，这里我只贴出了关键代码，供大家参考。
