机器人抓取（二）—— ROS 控制 onrobot RG2 / RG6 机械手（第二代）

机械臂为UR5 CB3 。机械手onrobot第二代rg6夹抓。上位机为Jetson AGX Xavier ubuntu 18.04.
接上篇：机器人抓取（一）—— ROS MoveIt! 程序控制真实UR5机器人运动（python）

1. Onrobot RG6 机械手 (采坑记录，可跳过)

rg2/6 有两种控制模式：Compute Box+urcap 和 teach Mode (without installed OnRobot UR Caps)。
目前github上所能找到的rg2/6控制资料都是基于 teach mode ，控制 DO8,DO9(或 TO0，TO1)端口实现机械手的开合控制。而第二代rg6机械手所用的控制模式是Compute Box。因此，目前github 及ros论坛上的所有rg2的控制程序都不能 work。 例如Using ROS Rviz to control the UR5 with OnRobot RG2 Gripper
原因：teach Mode模式下，ur机器人的工具端口直接与rg2/6机械手相连，因此可以采用控制机器人工具端口的方式实现夹抓的开合。然而在compute box模式下，rg2/6机械手连接到了 onrobot 的 Compute Box 上（机械手根本就没有与 ur 的工具端口相连），compute box 通过网线与机器人控制柜通信。

1.1 rg6 安装与配置

第二代 rg2/6机械手带有快换（Quick Changer），操作模式采用 Compute Box（CB），相应的接线方式如下：

OnRobot URCap安装与设置，可参考相应手册。此处不再赘述。

机器人与 PC（我用的 Jetson AGX Xavier）通过网线连接。注意：机器人控制器的以太网端口已经被占用，则使用标准 4 端口以太网交换机就可以同时使用两个网络设备。

注意：
机器人重新启动后，系统会检查 OnRobot 产品。程序加载过程中 ，保存的设置会恢复。检查通过 Quick Changer（用于 I/O）执行， 可能会耗时 5 秒。因此，在启动程序前，至少等待 5 秒钟。

1.2 OnRobot URCap ：URScript

据onrobot 手册介绍，OnRobot 支持URScript 命令 - 可以配合其他脚本使用。OnRobot URCap 启用时，将会有一个定义的 RG 脚本函数：

rg_grip(rg_width, force, tool_index=0, blocking=False, depth_compensation=False, popupmsg=True)
 # blocking=True 此函数等待夹爪完成夹持命令 .popupmsg: 未来函数，不需要填充)

所有输入参数与 UR图形化编程语言PolyScope 的 RG Grip 命令的参数相同，其中包括指尖补偿。 例如，用于快速释放工件的相对运动可按如下来完成：

 rg_grip(rg_Width+5, 40, rg_index_get())  #将夹爪打开 5mm，目标力设置为 40N，rg_index_get() 获取 rg 夹爪编号。如果只有一个夹爪，默认为 0.

然而rg_grip 指令只有编写在 ur 机器人的示教器的 script 中才有效，用socket 从 pc 直接发送该指令到机器人，无效。ur+的工程师说是因为，该函数是一个non-native script functions（即URScript手册中没有这个函数），URControl process中没有该函数的定义。
ur+工程师给出解决方法有两个：
1.通过Dashboard Server远程启动这个指令。太难，未测试。
2.首先要 copy 该函数的定义，将该函数的定义一起发送。在机器人示教器中编写 RG Grip 指令，可以在生成的.script文件中找到该函数的定义。经测试，此方法无效。

参考：
Start a script via socket
URScript control of Onrobot RG2 Robot Gripper

2. 通过 I/O端口控制 rg6 开合

前面说过，因第二代 rg6 机械手采用了 compute box 的控制方式，并未连接 ur 机器人的工具端口，因此无法再通过控制 ur 机器人工具端口的方式控制机械手。
踩过各种坑后，最终摸索出了一条曲线救国方案：用机器人的数字 I/O 端口控制onrobot gripper。注：此方法只能控制机械手张开或闭合，无法控制机械手张开到指定宽度。

实现步骤如下：

1. 连接机器人和 compute box 的 I/O 端口。
   

2. onrobot 提供网页客户端编程 weblogic。网线连接 pc ，ur5 和 compute box，打开浏览器，输入 compute box ip 192.168.1.1 可以登录 onrobot web 客户端。
   编写 WebLogic 程序，监听compute box DI端口（即 ur的DO端口）信号，输出相应动作
   

此时点击ur5 机器人示教器的DO6，DO7 数字输出端口可以控制机器人打开、闭合，DI0 端口显示抓取状态，DI7 端口显示的输出力模式（40N or 80N）。

3. pc 端向 ur机器人发送套接字指令修改 DO 信号，从而控制 rg6。
   python 程序示例如下：

import socket
import time

class RG():
    def __init__(self,HOST="192.168.1.2",PORT = 30003):
        self.DOport=[6,7]  # digital out port，DO6控制夹抓开合,DO7控制输出力
        self.rg_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.rg_socket.settimeout(10)
        self.rg_socket.connect((HOST, PORT))
        time.sleep(0.2)
        
    def set_digital_out(self,port,flag=True):
        rg_cmd = """
        sec sceondaryProgram()
            set_standard_digital_out({},{})
        end
        """.format(port,flag)
        self.rg_socket.send(rg_cmd.encode('utf8'))
        time.sleep(0.1)  # 注意命令发送至少要间隔0.05s，否则可能设置失败
            
    def close(self,lowForce=False):
    	self.set_digital_out(self.DOport[1],lowForce)
        self.set_digital_out(self.DOport[0],True)
        time.sleep(3)
        
    def open(self,lowForce=False):
        self.set_digital_out(self.DOport[1],lowForce)
        self.set_digital_out(self.DOport[0],False)
        time.sleep(3)    
if __name__ == '__main__':
    rg6 = RG()
    rg6.close()
    rg6.open(lowForce=True)

参考：
OnRobot Gripper - SOLVED
UR机器人与电脑进行socket通讯（python / C++）
REMOTE CONTROL VIA TCP/IP
UR机器人通信端口和协议

3. 抓取检测功能

在 weblogic 添加如下程序。

监听rg6 抓取检测结果，控制compute box DO1 端口信号，compu box DO1 连接至 ur5 DI0，因此读取 ur5 的 DI0 端口即可实现抓取检测功能。

4. rg gripper 的ros 功能包

ur机器人的 ros 驱动功能包定义了 I/O 端口的读写接口。使用方法参见 io_test.py。
下面来创建 rg6的功能包。包含夹抓开闭和抓取检测功能。

1.在工作空间src文件夹下创建 ur_rg_gripper 功能包，注意依赖 ur_msg。
2.编写控制节点文件。
gripper.py

#!/usr/bin/env python
import rospy
from ur_msgs.srv import *
from ur_msgs.msg import IOStates

class Gripper:
    def __init__(self, service='/ur_hardware_interface/set_io'):
        rospy.wait_for_service(service)
        try:
            self.set_io = rospy.ServiceProxy(service, SetIO)
            print('Connected with service')
        except rospy.ServiceException as e:
            print('Service unavailable: %s'%e)
    def open(self):
        self.set_io(1, 7, 0)
        self.set_io(1, 6, 0)
        
    def close(self):
        self.set_io(1, 7, 0)
        self.set_io(1, 6, 1)

    def grip_detect(self):
        """grasp success or not"""
        # grip_detect_pin = 0 
        try:
            return rospy.wait_for_message('/ur_hardware_interface/io_states', IOStates).digital_in_states[0].state             
        except rospy.ServiceException as e:
            print('cannot get io states: %s'%e)

gripper_node.py

#!/usr/bin/env python
import rospy
from gripper import Gripper
from std_msgs.msg import String

class GripperNode:
    def __init__(self, service='/ur_hardware_interface/set_io'):
        self.gripper = Gripper(service)
        rospy.init_node('gripper_node', anonymous=True)
        self.subscriber = rospy.Subscriber('/gripper/command', String, self.callback, queue_size=1)
        self.time_wait = 0
        print('Created gripper_node')
        rospy.spin()
    def callback(self, data):
        if data.data == 'open':
            self.gripper.open()
        elif data.data == 'close':
            self.gripper.close()
        elif data.data == 'detect':
            if self.gripper.grip_detect():
                print 'grasp success'
            else:
                print "failed to grasp"

if __name__ == '__main__':
    node = GripperNode()

3.运行

$ roslaunch ur_robot_driver ur5_bringup.launch
$ python gripper_node.py

$ rostopic pub /gripper/command std_msgs/String 'close'
$ rostopic pub /gripper/command std_msgs/String 'open'
$ rostopic pub /gripper/command std_msgs/String 'detect'