上一篇博客实现了通过ROS+moveit实现在rviz中实时控制机械臂，但是由于我的夹爪没有USB线所以无法通过现有的方法（指能白嫖到的）实现ROS中的控制。
研一的时候通过Socket通信实现过UR5的控制，而我找到了一个GitHub的大佬有通过Socket+Python3实现机械臂和夹爪控制的，并且还加了相机，白嫖党狂喜😄，本文将详细说一下该大佬的代码运行时的遇到的问题以及整个部署的实现过程。记录一下细节和踩得坑。
目录
一、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控制器的连接，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' # status (0 = is reset, 1 = activating, 3 = active)
PRE = 'PRE' # position request (echo of last commanded position)
OBJ = 'OBJ' # object detection (0 = moving, 1 = outer grip, 2 = inner grip, 3 = no object at rest)
FLT = 'FLT' # fault (0=ok, see manual for errors if not zero)
ENCODING = 'UTF-8' # ASCII and UTF-8 both seem to work
class GripperStatus(Enum):
"""Gripper status reported by the gripper. The integer values have to match what the gripper sends."""
RESET = 0
ACTIVATING = 1
# UNUSED = 2 # This value is currently not used by the gripper firmware
ACTIVE = 3
class ObjectStatus(Enum):
"""Object status reported by the gripper. The integer values have to match what the gripper sends."""
MOVING = 0
STOPPED_OUTER_OBJECT = 1
STOPPED_INNER_OBJECT = 2
AT_DEST = 3
def __init__(self):
"""Constructor."""
self.socket = None
self.command_lock = threading.Lock()
self._min_position = 0
self._max_position = 255
self._min_speed = 0
self._max_speed = 255
self._min_force = 0
self._max_force = 255
def connect(self, hostname: str, port: int, socket_timeout: float = 2.0) -> None:
"""Connects to a gripper at the given address.
:param hostname: Hostname or ip.
:param port: Port.
:param socket_timeout: Timeout for blocking socket operations.
"""
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.socket.connect((hostname, port))
self.socket.settimeout(socket_timeout)
def disconnect(self) -> None:
"""Closes the connection with the gripper."""
self.socket.close()
def _set_vars(self, var_dict: Dict[str, Union[int, float]]):
"""Sends the appropriate command via socket to set the value of n variables, and waits for its 'ack' response.
:param var_dict: Dictionary of variables to set (variable_name, value).
:return: True on successful reception of ack, false if no ack was received, indicating the set may not
have been effective.
"""
# construct unique command
cmd = "SET"
for variable, value in var_dict.items():
cmd += f" {variable} {str(value)}"
cmd += '\n' # new line is required for the command to finish
# atomic commands send/rcv
with self.command_lock:
self.socket.sendall(cmd.encode(self.ENCODING))
data = self.socket.recv(1024)
return self._is_ack(data)
def _set_var(self, variable: str, value: Union[int, float]):
"""Sends the appropriate command via socket to set the value of a variable, and waits for its 'ack' response.
:param variable: Variable to set.
:param value: Value to set for the variable.
:return: True on successful reception of ack, false if no ack was received, indicating the set may not
have been effective.
"""
return self._set_vars(dict([(variable, value)]))
def _get_var(self, variable: str):
"""Sends the appropriate command to retrieve the value of a variable from the gripper, blocking until the
response is received or the socket times out.
:param variable: Name of the variable to retrieve.
:return: Value of the variable as integer.
"""
# atomic commands send/rcv
with self.command_lock:
cmd = f"GET {variable}\n"
self.socket.sendall(cmd.encode(self.ENCODING))
data = self.socket.recv(1024)
# expect data of the form 'VAR x', where VAR is an echo of the variable name, and X the value
# note some special variables (like FLT) may send 2 bytes, instead of an integer. We assume integer here
var_name, value_str = data.decode(self.ENCODING).split()
if var_name != variable:
raise ValueError(f"Unexpected response {data} ({data.decode(self.ENCODING)}): does not match '{variable}'")
value = int(value_str)
return value
@staticmethod
def _is_ack(data: str):
return data == b'ack'
def _reset(self):
"""
Reset the gripper.
"""
self._set_var(self.ACT, 0)
self._set_var(self.ATR, 0)
while (not self._get_var(self.ACT) == 0 or not self._get_var(self.STA) == 0):
self._set_var(self.ACT, 0)
self._set_var(self.ATR, 0)
time.sleep(0.5)
def activate(self, auto_calibrate: bool = True):
"""Resets the activation flag in the gripper, and sets it back to one, clearing previous fault flags.
:param auto_calibrate: Whether to calibrate the minimum and maximum positions based on actual motion.
"""
if not self.is_active():
self._reset()
while (not self._get_var(self.ACT) == 0 or not self._get_var(self.STA) == 0):
time.sleep(0.01)
self._set_var(self.ACT, 1)
time.sleep(1.0)
while (not self._get_var(self.ACT) == 1 or not self._get_var(self.STA) == 3):
time.sleep(0.01)
# auto-calibrate position range if desired
if auto_calibrate:
self.auto_calibrate()
def is_active(self):
"""Returns whether the gripper is active."""
status = self._get_var(self.STA)
return RobotiqGripper.GripperStatus(status) == RobotiqGripper.GripperStatus.ACTIVE
def get_min_position(self) -> int:
"""Returns the minimum position the gripper can reach (open position)."""
return self._min_position
def get_max_position(self) -> int:
"""Returns the maximum position the gripper can reach (closed position)."""
return self._max_position
def get_open_position(self) -> int:
"""Returns what is considered the open position for gripper (minimum position value)."""
return self.get_min_position()
def get_closed_position(self) -> int:
"""Returns what is considered the closed position for gripper (maximum position value)."""
return self.get_max_position()
def is_open(self):
"""Returns whether the current position is considered as being fully open."""
return self.get_current_position() <= self.get_open_position()
def is_closed(self):
"""Returns whether the current position is considered as being fully closed."""
return self.get_current_position() >= self.get_closed_position()
def get_current_position(self) -> int:
"""Returns the current position as returned by the physical hardware."""
return self._get_var(self.POS)
def auto_calibrate(self, log: bool = True) -> None:
"""Attempts to calibrate the open and closed positions, by slowly closing and opening the gripper.
:param log: Whether to print the results to log.
"""
# first try to open in case we are holding an object
(position, status) = self.move_and_wait_for_pos(self.get_open_position(), 64, 1)
if RobotiqGripper.ObjectStatus(status) != RobotiqGripper.ObjectStatus.AT_DEST:
raise RuntimeError(f"Calibration failed opening to start: {str(status)}")
# try to close as far as possible, and record the number
(position, status) = self.move_and_wait_for_pos(self.get_closed_position(), 64, 1)
if RobotiqGripper.ObjectStatus(status) != RobotiqGripper.ObjectStatus.AT_DEST:
raise RuntimeError(f"Calibration failed because of an object: {str(status)}")
assert position <= self._max_position
self._max_position = position
# try to open as far as possible, and record the number
(position, status) = self.move_and_wait_for_pos(self.get_open_position(), 64, 1)
if RobotiqGripper.ObjectStatus(status) != RobotiqGripper.ObjectStatus.AT_DEST:
raise RuntimeError(f"Calibration failed because of an object: {str(status)}")
assert position >= self._min_position
self._min_position = position
if log:
print(f"Gripper auto-calibrated to [{self.get_min_position()}, {self.get_max_position()}]")
def move(self, position: int, speed: int, force: int) -> Tuple[bool, int]:
"""Sends commands to start moving towards the given position, with the specified speed and force.
:param position: Position to move to [min_position, max_position]
:param speed: Speed to move at [min_speed, max_speed]
:param force: Force to use [min_force, max_force]
:return: A tuple with a bool indicating whether the action it was successfully sent, and an integer with
the actual position that was requested, after being adjusted to the min/max calibrated range.
"""
def clip_val(min_val, val, max_val):
return max(min_val, min(val, max_val))
clip_pos = clip_val(self._min_position, position, self._max_position)
clip_spe = clip_val(self._min_speed, speed, self._max_speed)
clip_for = clip_val(self._min_force, force, self._max_force)
# moves to the given position with the given speed and force
var_dict = dict([(self.POS, clip_pos), (self.SPE, clip_spe), (self.FOR, clip_for), (self.GTO, 1)])
return self._set_vars(var_dict), clip_pos
def move_and_wait_for_pos(self, position: int, speed: int, force: int) -> Tuple[int, ObjectStatus]: # noqa
"""Sends commands to start moving towards the given position, with the specified speed and force, and
then waits for the move to complete.
:param position: Position to move to [min_position, max_position]
:param speed: Speed to move at [min_speed, max_speed]
:param force: Force to use [min_force, max_force]
:return: A tuple with an integer representing the last position returned by the gripper after it notified
that the move had completed, a status indicating how the move ended (see ObjectStatus enum for details). Note
that it is possible that the position was not reached, if an object was detected during motion.
"""
set_ok, cmd_pos = self.move(position, speed, force)
if not set_ok:
raise RuntimeError("Failed to set variables for move.")
# wait until the gripper acknowledges that it will try to go to the requested position
while self._get_var(self.PRE) != cmd_pos:
time.sleep(0.001)
cur_obj = self._get_var(self.OBJ)
while RobotiqGripper.ObjectStatus(cur_obj) == RobotiqGripper.ObjectStatus.MOVING:
cur_obj = self._get_var(self.OBJ)
final_pos = self._get_var(self.POS)
final_obj = cur_obj
return final_pos, RobotiqGripper.ObjectStatus(final_obj)之后就可以编写程序实现夹爪的控制*★,°*:.☆(￣▽￣)/$:*.°★* 。ip地址和机械臂的一样，夹爪校准后会给出一个新的区间，之后输入的开合值会在新区间内线性化，所以对于输入来说还是0代表开255代表紧闭。
#!/usr/bin/env python3
import robotiq_gripper
import time
ip="192.168.56.2"
def log_info(gripper):
print(f"Pos: {str(gripper.get_current_position()): >3}\t"
f"Open: {gripper.is_open(): <2}\t"
f"Closed: {gripper.is_closed(): <2}\t")
print("Creating gripper...")
gripper = robotiq_gripper.RobotiqGripper()
print("Connecting to gripper...")
gripper.connect(ip, 63352)
print("Resting gripper...")
gripper._reset()
print("Activating gripper...")
gripper.activate()
time.sleep(1.5)
print("Testing gripper......")
gripper.move_and_wait_for_pos(255, 64, 20)
time.sleep(1.5)
log_info(gripper)
print("gripper had closed!")
time.sleep(1.5)
gripper.move_and_wait_for_pos(0, 64, 20)
time.sleep(1.5)
log_info(gripper)
print("gripper had opened!")
time.sleep(1.5)四、相机相关程序
我用的是realsense D405相机，其实配置方面和D435是一样的。主要参考GitHub代码和Realsense相机在linux下的配置使用，RGB与depth图像对齐该博主的环境配置和对齐代码。
首先安装realsense的python模块。
pip install pyrealsense2我将该代码也封装成类方便后续程序调用，只采用了对齐的版本，删除了非对齐版本，具体代码为：其中相机的内参是我直接读取该相机的出厂内参并没有标定。
#!/usr/bin/env python3
#coding=utf-8
import numpy as np
import os
import cv2
import time
import struct
import pyrealsense2 as rs
class RealsenseD405(object):
def __init__(self):
self.im_height = 720
self.im_width = 1280
self.intrinsics = [426.4775,0,422.0653,0,424.10095,244.10073,0,0,1]
self.USE_ROS_BAG=0 #0:使用相机# 1:使用API录制好的bag
self.ALIGN_WAY=1 #0:彩色图像对齐到深度图;1:深度图对齐到彩色图像
self.show_pic = 0
self.depth_scale = 0
#self.get_data()
# color_img, depth_img = self.get_data()
# print(color_img, depth_img)
def get_data(self):
pipeline = rs.pipeline()
config = rs.config()
if self.USE_ROS_BAG:
config.enable_device_from_file("xxx.bag")#这是打开相机API录制的视频
else:
config.enable_stream(rs.stream.color, self.im_width, self.im_height, rs.format.bgr8, 30) #10、15或者30可选,20或者25会报错，其他帧率未尝试
config.enable_stream(rs.stream.depth, self.im_width, self.im_height, rs.format.z16, 30)
#左右双目
config.enable_stream(rs.stream.infrared, 1, self.im_width, self.im_height, rs.format.y8, 30)
config.enable_stream(rs.stream.infrared, 2, self.im_width, self.im_height, rs.format.y8, 30)
if self.ALIGN_WAY:
way=rs.stream.color
else:
way=rs.stream.depth
align = rs.align(way)
profile =pipeline.start(config)
depth_sensor = profile.get_device().first_depth_sensor()
self.depth_scale = depth_sensor.get_depth_scale()
frames = pipeline.wait_for_frames()
# 对齐版本
aligned_frames = align.process(frames)
depth_frame_aligned = aligned_frames .get_depth_frame()
color_frame_aligned = aligned_frames .get_color_frame()
# if not depth_frame_aligned or not color_frame_aligned:
# continue
color_image_aligned = np.asanyarray(color_frame_aligned.get_data())
if self.USE_ROS_BAG:
color_image_aligned=cv2.cvtColor(color_image_aligned,cv2.COLOR_BGR2RGB)
depth_image_aligned = np.asanyarray(depth_frame_aligned.get_data())
depth_colormap_aligned = cv2.applyColorMap(cv2.convertScaleAbs(depth_image_aligned, alpha=0.05), cv2.COLORMAP_JET)
images_aligned = np.hstack((color_image_aligned, depth_colormap_aligned))
if self.show_pic:
cv2.imshow('aligned_images', images_aligned)
cv2.waitKey(0)
return color_image_aligned,depth_image_aligned,depth_colormap_aligned
if __name__ == "__main__":
rsD405 = RealsenseD405()五、代码整合-修改UR_Robot.py
主要还是修改GitHub代码中的UR_Robot.py这部分代码，将之前的几部分整合一下，修改的部分不多主要是根据自己配置需求修改__init__部分。这部分代码我就不贴出来了，直接看GitHub中的该代码即可。
代码的move_xxx系列函数定义了如何通过socket给ur发送运动指令，其实该部分单独拿出来用就是这样的，发送一串bytes型的指令即可，不过封装在类里面略显复杂。
#!/usr/bin/env python3
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))
strL = b"movej(p[0.346,-0.526,0.734,1.472,0.632,0.644],a=0.5,v=0.3)\n"
s.send(strL)
s.close()我贴出GitHub代码中的move_j部分代码，说明一个问题。首先只有该函数输入的是各个关节角度rad，其余的函数都是目标位姿(x,y,z,rx,ry,rz)单位是m和rad 。该代码是想实现实时获取现在机器人位置来判断是否到最终位置，但是在while循环中，并没有重新去接收新的数据，换言之他一直在接收最开始时刻的socket传回的数据，所以运行该代码会堵塞在while循环中。修改while循环，在每一次循环中都重新建立连接获取新的数据，并且关闭连接。修改while循环即可实现预期功能。
def move_j(self, joint_configuration,k_acc=1,k_vel=1,t=0,r=0):
self.tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.tcp_socket.connect((self.tcp_host_ip, self.tcp_port))
# command: movej([joint_configuration],a,v,t,r)\n
tcp_command = "movej([%f" % joint_configuration[0] #"movej([]),a=,v=,\n"
for joint_idx in range(1,6):
tcp_command = tcp_command + (",%f" % joint_configuration[joint_idx])
tcp_command = tcp_command + "],a=%f,v=%f)\n" % (k_acc*self.joint_acc, k_vel*self.joint_vel)
self.tcp_socket.send(str.encode(tcp_command))
# Block until robot reaches home state
state_data = self.tcp_socket.recv(1500)
actual_joint_positions = self.parse_tcp_state_data(state_data, 'joint_data')
while not all([np.abs(actual_joint_positions[j] - joint_configuration[j]) < self.joint_tolerance for j in range(6)]):
state_data = self.tcp_socket.recv(1500)
actual_joint_positions = self.parse_tcp_state_data(state_data, 'joint_data')
time.sleep(0.01)
self.tcp_socket.close() while not all([np.abs(actual_joint_positions[j] - joint_configuration[j]) < self.joint_tolerance for j in range(6)]):
self.tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.tcp_socket.connect((self.tcp_host_ip, self.tcp_port))
state_data = self.tcp_socket.recv(1500)
actual_joint_positions = self.parse_tcp_state_data(state_data, 'joint_data')
time.sleep(0.01)
self.tcp_socket.close()六、杂谈
该代码只能在python3下运行，而我之前一直都是用python2作为默认环境导致连opencv都没有。。。前人栽树后人乘凉，夹爪的控制一直不知道如何实现，后来发现已经有人封装好了类函数，只能说开源救命了。 其实数据解析那里也不用都解析，可以选择自己需要的那部分数据分段解析，只是我懒得改代码了😄。解析的数据不对了话看一下字典是不是有问题，如果字典没问题那可能是socket接收的数据没有完全清空的就有新数据进来，可能是收发频率的问题吧。
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
