背景描述
最近在做项目时，使用了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. 机器人脚本
格式问题，不能发。（关注“测试开发自动化” 弓中皓，获取更多学习内容）
总结
当然，代码远不止这么多，这里我只贴出了关键代码，供大家参考。