UR机器人C语言和Python编程控制

---

一、通过 TCP/IP 进行远程控制

机械臂的网口通信有几个端口可以使用，这个网页最下方有一个xlsx格式的表格附件,里面详细描述了UR机械臂传输数据的详细格式。
链接: 图表链接

可以看到CB系列30003通信端口频率为125HZ，并且可以使用URScript，所以速度较快控制较全面。但是官方推荐使用RTDE(real time data exchange)，是封装好的API

二、UR机器人通信端口类型

参考链接： https://blog.csdn.net/hangl_ciom/article/details/88619438

2.1、Modbus TCP端口（502端口）

1、Modbus是OS模型第7层上的应用层报文传输协议，它在连接至不同类型总线或网络的设备之间提供客户端/服务器通信，互联网组织保留系统端口502访问Modbus，Modbus是一个请求/应答协议，并且提供功能码规定的服务。
2、UR机器人既可以作为Modbus TCP服务器，也可以作为Modbus TCP客户端，两者的主要区别在于消息请求方的不同。
3、通过UR机器人的相关Modbus地址，我们可以访问机器人的很多信息，进而可以在上位机上解析这些信息并显示。
4、这个MODBUS端口有一点要注意，需要随时读取随时建立连接，即建立的连接只有马上使用才可以，过一会就会被自动断开，分析应该是状态端口不允许被某一程序长时间占用，或者这个是UDP端口。
通过MODBUS端口读取机器人的当前状态，实际上就是读取机器人相应地址的寄存器数据。需要读取状态寄存器时，向MODBUS端口发送命令，命令一般为一个12个字节的命令包，格式如下：

构造好以上格式的数据包，发送到状态端口（如上所述，发送前建立连接，发送接收数据完成后立即关闭连接）
发出命令包后等待接收返回数据，前9个字节为信息头，之后即为返回的需要的寄存器的值，数目为需要的寄存器数目，每个寄存器数据占两个字节，同样高位在前。

2.2、C语言上位机编程端口（30001/30002/30003端口）

2.2.1、机器人运动控制

上位机作为客户端，通过30001/30002/30003其中一个特定的编程端口，与机器人控制器建立TCP/IP连接，就可以在上位机上按照URScript语言格式编写脚本程序，直接发送给机器人控制器，机器人就可以执行相应动作了。
注意：位置单位均为m，角度单位均为弧度，速度单位为m/s，加速度单位为m/s2。指令后面必须加回车换行，即"\r\n"。

1、停止指令
  命令格式：stopl(1)

char* cmd = "stopl(1)\r\n";
SendCommand(cmd,strlen(cmd));

2、 MOVEL移动指令
命令格式：movel(p[x, y, z, u, v, w], a = avalue, v = vvalue)
其中x y z为三个轴的目标位置坐标，单位为米
u v w为旋转向量，单位为弧度
avalue与vvalue分别为移动时的加速度和速度

char cmd[1024];
sprintf_s(cmd,"movel(p["
	"%lf, %lf, %lf, %lf, %lf, %lf"
	"], a = %lf, v = %lf)\r\n",
	x,y,z,u,v,w,a,v);
SendCommand(cmd,strlen(cmd));

3、 MOVEJ移动指令
命令格式：movej(p[g1, g2, g3, g4, g5, g6], a = avalue, v = vvalue)
与movel指令格式一样，但参数是关节角度。

char cmd[1024];
sprintf_s(cmd,"movej(["
	"%lf, %lf, %lf, %lf, %lf, %lf"
	"], a = %lf, v = %lf)\r\n",
	g1,g2,g3,g4,g5,g6,a,v);
SendCommand(cmd,strlen(cmd));

2.2.2、机器人状态读取

一旦客户端打开端口，就会按照一定的频率收到来自机器人的信息。客户端通过这三个端口收到的机器人信息稍有不同，通过30003端口收到的信息是最丰富的，包含了通过30001和30002收到的大部分信息。通过30003每次收到的数据包是1044个字节（当前最新是1116字节如下图），以标准格式排列，极少数情况下客户端会收到小于1044个字节。

数据解析例子：

第1-4字节（上图中偏移地址：0x00)：00 00 04 54，整数型数据，即0x454，1108，这就是接收数据的字节长度。
第13字节（上图中偏移地址：0x0c）开始，连续6个Double型数据，即关节目标位置q target，数据为：2.466446,-0.586911,1.581819,-2.725837,4.662427,-0.580726；

数据解析代码链接: https://blog.csdn.net/hangl_ciom/article/details/104439042

2.3、python上位机编程端口（30001/30002/30003端口）

2.3.1、socket客户端（连接机械臂）

import socket

HOST = '172.22.0.155'  # ur机械臂ip地址，根据自己实际情况修改
PORT = 30003  # 端口，根据自己实际情况修改
robot = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
robot.connect((HOST, PORT))

2.3.2、控制机械臂运动（发送控制指令）

# 配置好加速度a，速度v，时间t的关系，防止卡顿。指令后面必须加"\n"。
# send发送的指令需要为bytes型的，而不是直接的str型，所以加上‘b’或者在后面加".encode()"
command = b"movel(p[-0.2,-0.6,0.300,0,0,0],a=1,v=0.25,t=6)\n"
command = "movel(p[-0.2,-0.6,0.300,0,0,0],a=1,v=0.25,t=6)\n".encode()
robot.send(command)
robot.close()

如果你连续发送两条运动指令，应该确定第一条完成才可发送下一条指令。

2.3.3、返回的数据解析

根据2.2.2中的UR官方Excel表格，将字节所表示的名称和字节类型放入字典中，根据版本不同会有所变化。按照字典中的格式解析，解析之后将解析的数据再放入字典中。

dic= {'MessageSize': 'i', 'Time': 'd', 'q target': '6d', 'qd target': '6d', 'qdd target': '6d','I target': '6d',
      'M target': '6d', 'q actual': '6d', 'qd actual': '6d', 'I actual': '6d', 'I control': '6d',
      'Tool vector actual': '6d', 'TCP speed actual': '6d', 'TCP force': '6d', 'Tool vector target': '6d',
      'TCP speed target': '6d', 'Digital input bits': 'd', 'Motor temperatures': '6d', 'Controller Timer': 'd',
      'Test value': 'd', 'Robot Mode': 'd', 'Joint Modes': '6d', 'Safety Mode': 'd', 'empty1': '6d', 
      'Tool Accelerometer values': '3d', 'empty2': '6d', 'Speed scaling': 'd', 'Linear momentum norm': 'd', 
      'SoftwareOnly': 'd', 'softwareOnly2': 'd', 'V main': 'd', 'V robot': 'd', 'I robot': 'd', 'V actual': '6d', 
      'Digital outputs': 'd', 'Program state': 'd', 'Elbow position': '3d', 'Elbow velocity': '3d'}

data = robot.recv(1108)
names=[]
ii=range(len(dic))
for key,i in zip(dic,ii):
    fmtsize=struct.calcsize(dic[key])
    data1,data=data[0:fmtsize],data[fmtsize:]
    fmt="!"+dic[key]
    names.append(struct.unpack(fmt, data1))
    dic[key]=dic[key],struct.unpack(fmt, data1)
print(names)
print(dic)

a=dic["q actual"]
a2=np.array(a[1])  # 6个关节“q actual”（关节角度值，弧度表示）
print(a2*180/math.pi)  # 弧度转角度

另一个解析代码：

while (True):
       try: 
           # 为了读取掉前面的数据
           packet_1  = robot.recv(4)
           packet_2  = robot.recv(8)
           packet_3  = robot.recv(48)
           packet_4  = robot.recv(48)
           packet_5  = robot.recv(48)
           packet_6  = robot.recv(48)
           packet_7  = robot.recv(48) 
           packet_8  = robot.recv(48)
           packet_9  = robot.recv(48)
           packet_10 = robot.recv(48)
           packet_11 = robot.recv(48)
           
           #获取末端当前x坐标
           packet_12 = robot.recv(8)
           packet_12 = packet_12.encode("hex") #convert the data from \x hex notation to plain hex
           mx = str(packet_12)
           mx = struct.unpack('!d', packet_12.decode('hex'))[0]
           print "mx = ", round(mx ,4)
           
           #获取末端当前y坐标
           packet_13 = robot.recv(8)
           packet_13 = packet_13.encode("hex") #convert the data from \x hex notation to plain hex
           my = str(packet_13)
           my = struct.unpack('!d', packet_13.decode('hex'))[0]
           print "my = ", round(my ,4)
 		   
 		   #获取末端当前z坐标
           packet_14 = robot.recv(8)
           packet_14 = packet_14.encode("hex") #convert the data from \x hex notation to plain hex
           mz = str(packet_14)
           mz = struct.unpack('!d', packet_14.decode('hex'))[0]
           print "mz = ", round(mz ,4)
           
           #获取末端当前rx坐标
           packet_15 = robot.recv(8)
           packet_15 = packet_15.encode("hex") #convert the data from \x hex notation to plain hex
           mrx = str(packet_15)
           mrx = struct.unpack('!d', packet_15.decode('hex'))[0]
           print "mrx = ", round(mrx,3)
           
           #获取末端当前ry坐标
           packet_16 = robot.recv(8)
           packet_16 = packet_16.encode("hex") #convert the data from \x hex notation to plain hex
           mry = str(packet_16)
           mry = struct.unpack('!d', packet_16.decode('hex'))[0]
           print "mry = ", round(mry,3)
           
           #获取末端当前rz坐标
           packet_17 = robot.recv(8)
           packet_17 = packet_17.encode("hex") #convert the data from \x hex notation to plain hex
           mrz = str(packet_17)
           mrz = struct.unpack('!d', packet_17.decode('hex'))[0]
           print "mrz = ", round(mrz,3)

		   #精度控制，根据情况设置 toler_dist 
           if abs(round(mx,4) - round(x,4)) < toler_dist and abs(round(my,4) - round(y,4)) < toler_dist and abs(round(mz,4) - round(z,4)) < toler_dist:
               break

		   #注意：因为这里并没有把robot反馈的所有字节都获取，所以在进行下一次判断时应该关闭socket然后重新连接
           robot.close()
           # initRobot函数就是第一部分socket客户端的内容
           initRobot() 

三、电脑与UR仿真软件连接

3.1、确定UR机器人的IP和端口号

点击设置机器人

点击网络，记下IP地址

3.2、测试网络连接

打开电脑端TCP网络调试助手软件，填写对应的IP和端口号以客户端的方式连接成后会出现如下图所示的信息显示。

在输入框输入如下命令后点击发送

movel(p[-0.100,-0.400,0.200,0.000,3.140,-0.00],a=10,v=0.25,t=6)\n

机器人就会根据指令进行运动

至此完成了连接的测试。

四、实际机器人连接方法

如果有实际的机器人就要使用网线连接电脑与机器人工控柜底下的网口。设置的机器人的IP地址需要与电脑的IP地址前三位相同最后一位0-255。下图是PC端的IP设置，如果不成功就关闭无线网再试。设置好机器人和电脑的IP地址以后，机器人界面会显示绿色√网络连接成功。

五、一些UR相关的问题

原文连接：https://www.gg-robot.com/asdisp2-65b095fb-61929-.html
本文选择部分重要的几个。

1、优傲机器人开机后，能够自动运行程序吗？如何实现？

正常情况下，优傲机器人开机到运行程序，需要完成以下步骤：开机、初始化机器人(开、启动)、加载程序、启动程序、回初始点(自动)、启动程序。为了自动完成以上步骤，需要在“安装设置”窗口“默认程序”选项卡设置自动加载程序，设置DI信号用于自动初始化和自动启动程序。另外，记住保存时一定要存在default.installation文件内。为了避开回初始点的操作，可以把初始点的路点类型设为相对路点或可变路点，但是一定要保证机器人在当前位置回初始点的运动过程是安全的。

2、优傲机器人拖拽示教的功能如何实现？

实现拖拽示教，首先需要在示教器完成初始化过程：在“设置机器人”窗口，选择“初始化机器人”，点“开”和“启动”按钮，然后用手指压住示教器背面的黑色“自由驱动”按钮，再用另一只手拉住机械臂，就可以拖拽示教了。

3、优傲机器人运动范围是一个完整的球形，它有没有盲区或死点？

优傲机器人6个关节都可以±360°旋转，所以工作空间是在机器人工作范围内的完整的球形空间。但是，要注意机器人在运动到接近于运动范围边界的时候，不能任意指定其姿态。机械臂基座所在的正上方和正下方的圆柱体空间，要尽可能避免将工具移向圆柱体空间，这样会造成工具运动速度很慢时某个关节却运动过快，从而导致机器人报“保护性停止”。在空间内某些位置，机器人在直线运动时会产生“奇异点”问题，导致机器人停机，用Movej指令或修改机器人姿态可以避免这个问题。

4、编程中的位姿x,y,z,rx,ry,rz分别是什么？

x，y，z是对应的TCP原点在基坐标系下的坐标，rx，ry，rz是姿态注意这不是对应的旋转欧拉角，而是旋转矢量。如果要通过旋转角获得旋转矢量就得通过旋转矩阵过度。旋转角——》旋转矩阵——》旋转矢量，可以通过opencv的函数计算旋转矢量

rvec, _ = cv2.Rodrigues(Rxyz)