通过PC驱动真实UR10e机械臂+robotiq85夹爪（Scoket）

使用ROS+Socket控制UR5机械臂与夹爪及RealsenseD405相机

原创

已于 2023-05-25 17:22:18 修改 · 4.7k 阅读

85 ·

CC 4.0 BY-SA版权

文章标签：

#python #linux

于 2023-05-25 16:40:26 首次发布

本文详细介绍了如何通过ROS和Socket通信控制UR5机械臂，包括解析机械臂状态数据，使用Python3控制85夹爪，以及集成RealsenseD405相机的过程。文章强调了Python版本对字典顺序的影响，以及在处理数据时需要注意的问题。此外，还提供了夹爪和相机的控制代码示例。

上一篇博客实现了通过ROS+moveit实现在rviz中实时控制机械臂，但是由于我的夹爪没有USB线所以无法通过现有的方法~~（指能白嫖到的）~~实现ROS中的控制。

研一的时候通过Socket通信实现过UR5的控制，而我找到了一个GitHub的大佬有通过Socket+Python3实现机械臂和夹爪控制的，并且还加了相机，白嫖党狂喜😄，本文将详细说一下该大佬的代码运行时的遇到的问题以及整个部署的实现过程。记录一下细节和踩得坑。

大佬代码连接为：GitHub - 1172534699/UR5-robotiq85-D435_control_by_python: We use TCP/IP control the UR5 robot and robotiq85 by python. And we use USB control realsenseD435 camera by python.

一、UR Socket通信

在Socket 连接中，客户端程序首先会向服务器端发送一个连接请求，服务器端会接受这个连接请求，然后通过套接字与客户端进行通信。在通信过程中，客户端和服务器端通过套接字发送和接收数据。一旦通信完成，连接就会被断开，套接字也会被关闭。基本流程如图，比较复杂，我们倒也不必深究底层实现，会用就行。就是利用socket实现数据传输。

UR提供了很多的通信端口以及对接收数据的解释具体可以查看官方提供的REMOTE CONTROL VIA TCP/IP和Overview of used ports on local host或者查看该博客UR机器人通信端口和协议对UR socket传回的数据有一个大概的认识即可。根据我们的需求对机械臂控制选用30003端口，对85夹爪的控制采用63352端口。根据机械臂版本和官方提供的excel表格以及端口选择，对于机械臂本体控制我们有下面这个表，用于后期数据的解码。

二、机械臂数据的接收和解码

参考UR机器人上位机通信-python版（一）

首先创建与UR控制器的连接，IP地址通过查看示教器设置->系统->网络即可获得

import socket
import struct
import math
import numpy as np
HOST = "192.168.56.2"    # The remote host
PORT = 30003        # The same port as used by the server
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((HOST, PORT))

根据之前的数据表，我们一次会接收1116个字节数的数据，将字节所表示的名称和字节类型放入字典中：

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','Safety Status':'d'}

这里我踩了个大坑，由于之前一直用ros所以电脑默认的python版本是2.7，而python2中字典是无序的，所以会导致输入的字典顺序和解码是用的字典顺序不同，采用python3就没有这个问题了，如果想在python2中使用需要用到OrderedDict()类，再将字典一个一个输入即可。

import collections

dic = collections.OrderedDict()

按照字典中的格式解析，解析之后将解析的数据再放入字典中，再根据索引读取数据即可，这里我们获取末端工具相对于基座坐标系的位姿。

data = s.recv(1500)
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)

a=dic["Tool vector actual"]
a2=np.array(a[1])
print(a2)

[ 0.35556756 -0.53704818  0.74460635  1.45930005  0.61347262  0.63800616]
x(m),y(m),z(m),rx(rad),ry(rad),rz(rad),

三、Socket控制85夹爪

之前ROS实现不了的这次用socket实现。

夹爪控制主要是利用Github大佬提供的robotiq_gripper.py，该程序提供了一个RobotiqGripper类可以实现夹爪的激活，校准，开合这些功能供我们使用。可能是由于环境的不同，我直接用该代码会出现很多报错，并且校准功能无法实现。根据报错发现是OrderedDict（）类有问题，具体什么问题也不知道，我换成了Dict类之后就解决了，直接贴出修改后的程序吧。代码的注释十分详细。

#!/usr/bin/env python3
import socket
import threading
import time
from enum import Enum
from typing import Union, Tuple, Dict

class RobotiqGripper:
    """
    Communicates with the gripper directly, via socket with string commands, leveraging string names for variables.
    """
    # WRITE VARIABLES (CAN ALSO READ)
    ACT = 'ACT'  # act : activate (1 while activated, can be reset to clear fault status)
    GTO = 'GTO'  # gto : go to (will perform go to with the actions set in pos, for, spe)
    ATR = 'ATR'  # atr : auto-release (emergency slow move)
    ADR = 'ADR'  # adr : auto-release direction (open(1) or close(0) during auto-release)
    FOR = 'FOR'  # for : force (0-255)
    SPE = 'SPE'  # spe : speed (0-255)
    POS = 'POS'  # pos : position (0-255), 0 = open
    # READ VARIABLES
    STA = 'STA'

最低0.47元/天解锁文章