-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathrobotiq_two_finger_gripper.py
216 lines (170 loc) · 6.23 KB
/
robotiq_two_finger_gripper.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
#! /usr/bin/env python
"""
Python library to control Robotiq Two Finger Gripper connected to UR robot via
Python-URX
Tested using a UR5 Version CB3 and Robotiq 2-Finger Gripper Version 85
SETUP
You must install the driver first and then power on the gripper from the
gripper UI. The driver can be found here:
http://support.robotiq.com/pages/viewpage.action?pageId=5963876
FAQ
Q: Why does this class group all the commands together and run them as a single
program as opposed to running each line seperately (like most of URX)?
A: The gripper is controlled by connecting to the robot's computer (TCP/IP) and
then communicating with the gripper via a socket (127.0.0.1:63352). The scope
of the socket is at the program level. It will be automatically closed
whenever a program finishes. Therefore it's important that we run all commands
as a single program.
DOCUMENTATION
- This code was developed by downloading the gripper package "DCU-1.0.10" from
http://support.robotiq.com/pages/viewpage.action?pageId=5963876. Or more
directly from http://support.robotiq.com/download/attachments/5963876/DCU-1.0.10.zip
- The file robotiq_2f_gripper_programs_CB3/rq_script.script was referenced to
create this class
FUTURE FEATURES
Though I haven't developed it yet, if you look in
robotiq_2f_gripper_programs_CB3/advanced_template_test.script and view function
"rq_get_var" there is an example of how to determine the current state of the
gripper and if it's holding an object.
""" # noqa
import logging
import os
import time
from urx.urscript import URScript
# Gripper Variables
ACT = "ACT"
GTO = "GTO"
ATR = "ATR"
ARD = "ARD"
FOR = "FOR"
SPE = "SPE"
OBJ = "OBJ"
STA = "STA"
FLT = "FLT"
POS = "POS"
SOCKET_HOST = "127.0.0.1"
SOCKET_PORT = 63352
SOCKET_NAME = "gripper_socket"
class RobotiqScript(URScript):
def __init__(self,
socket_host=SOCKET_HOST,
socket_port=SOCKET_PORT,
socket_name=SOCKET_NAME):
self.socket_host = socket_host
self.socket_port = socket_port
self.socket_name = socket_name
super(RobotiqScript, self).__init__()
# Reset connection to gripper
self._socket_close(self.socket_name)
self._socket_open(self.socket_host,
self.socket_port,
self.socket_name)
def _import_rq_script(self):
dir_path = os.path.dirname(os.path.realpath(__file__))
rq_script = os.path.join(dir_path, 'rq_script.script')
with open(rq_script, 'rb') as f:
rq_script = f.read()
self.add_header_to_program(rq_script)
def _rq_get_var(self, var_name, nbytes):
self._socket_send_string("GET {}".format(var_name))
self._socket_read_byte_list(nbytes)
def _get_gripper_fault(self):
self._rq_get_var(FLT, 2)
def _get_gripper_object(self):
self._rq_get_var(OBJ, 1)
def _get_gripper_status(self):
self._rq_get_var(STA, 1)
def _set_gripper_activate(self):
self._socket_set_var(GTO, 1, self.socket_name)
def _set_gripper_force(self, value):
"""
FOR is the variable
range is 0 - 255
0 is no force
255 is full force
"""
value = self._constrain_unsigned_char(value)
self._socket_set_var(FOR, value, self.socket_name)
def _set_gripper_position(self, value):
"""
SPE is the variable
range is 0 - 255
0 is no speed
255 is full speed
"""
value = self._constrain_unsigned_char(value)
self._socket_set_var(POS, value, self.socket_name)
def _set_gripper_speed(self, value):
"""
SPE is the variable
range is 0 - 255
0 is no speed
255 is full speed
"""
value = self._constrain_unsigned_char(value)
self._socket_set_var(SPE, value, self.socket_name)
def _set_robot_activate(self):
self._socket_set_var(ACT, 1, self.socket_name)
class Robotiq_Two_Finger_Gripper(object):
def __init__(self,
robot,
payload=0.85,
speed=255,
force=50,
socket_host=SOCKET_HOST,
socket_port=SOCKET_PORT,
socket_name=SOCKET_NAME):
self.robot = robot
self.payload = payload
self.speed = speed
self.force = force
self.socket_host = socket_host
self.socket_port = socket_port
self.socket_name = socket_name
self.logger = logging.getLogger(u"robotiq")
def _get_new_urscript(self):
"""
Set up a new URScript to communicate with gripper
"""
urscript = RobotiqScript(socket_host=self.socket_host,
socket_port=self.socket_port,
socket_name=self.socket_name)
# Set input and output voltage ranges
urscript._set_analog_inputrange(0, 0)
urscript._set_analog_inputrange(1, 0)
urscript._set_analog_inputrange(2, 0)
urscript._set_analog_inputrange(3, 0)
urscript._set_analog_outputdomain(0, 0)
urscript._set_analog_outputdomain(1, 0)
urscript._set_tool_voltage(0)
urscript._set_runstate_outputs()
# Set payload, speed and force
urscript._set_payload(self.payload)
urscript._set_gripper_speed(self.speed)
urscript._set_gripper_force(self.force)
# Initialize the gripper
urscript._set_robot_activate()
urscript._set_gripper_activate()
# Wait on activation to avoid USB conflicts
urscript._sleep(0.1)
return urscript
def gripper_action(self, value):
"""
Activate the gripper to a given value from 0 to 255
0 is open
255 is closed
"""
urscript = self._get_new_urscript()
# Move to the position
sleep = 2.0
urscript._set_gripper_position(value)
urscript._sleep(sleep)
# Send the script
self.robot.send_program(urscript())
# sleep the code the same amount as the urscript to ensure that
# the action completes
time.sleep(sleep)
def open_gripper(self):
self.gripper_action(0)
def close_gripper(self):
self.gripper_action(255)