【UR六轴机械臂源码】python脱离示教器控制UR机械臂实时采集机器人位姿（优傲机器人）

背景描述

最近在做项目时，使用了ur六轴机器人，遇到了一些难题。起初对机械臂并不了解，通过一步步摸索，总结了一些小经验。本博客主要介绍，如何通过python与机器人建立通信，实时调整机器人位姿并保存TCP位姿和关节角位姿。

---

---

一、项目目的

我负责的分支是机器人搭载相机手眼标定，但是使用的是第三方软件进行标定，在标定的过程中，需要实时移动采集机器人的位姿，并保存到本地的json文件夹中。

痛点：

1. 通过示教器移动机器人，记录机器人的TCP位姿，需要手动记录，产生大量重复性工作；
2. 先通过示教器移动机器人，再使用第三方软件与机器人建立通信，得到机器人当前的位姿，也需要频繁手动开启与机器人的通信（因为操作示教器，三方软件与机器人的通信就会被终止）。

于是，经过考虑，只能脱离示教器，对机器人进行控制，通过前端界面与机器人进行通信，并控制机器人。

优点：

1. 可以实时移动机器人，并读取机器人的tcp位姿或关节角位姿；
2. 实时自动记录机器人的位姿信息至本地，一次连接，便可得到所有想要的位姿，非常方便。
3. 与前端界面结合，非常易用。

前端交互界面采用的是Pyqt5，界面如下：

思路：

1. 前端界面属于服务端，输入监听端口号，点击开启按钮，即可等待机器人的连接。
2. 通过 实时 移动机器人的关节角位姿，实时发送给机器人，来控制机器人的移动。
3. 当机器人移动到指定位置时，即可输出当前机器人的关节角和TCP位姿。
4. 步长可以设置当前机器人关节角移动的度数。

---

二、使用方法

机器人端需要写好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. 机器人脚本

格式问题，不能发。

总结

当然，代码远不止这么多，这里我只贴出了关键代码，供大家参考。