urkit 0.1.0__py3-none-any.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
Files changed (27) hide show
  1. urkit/__init__.py +99 -0
  2. urkit/__main__.py +116 -0
  3. urkit/cli/__init__.py +6 -0
  4. urkit/cli/colors.py +99 -0
  5. urkit/cli/connection_monitor.py +154 -0
  6. urkit/cli/points.py +293 -0
  7. urkit/cli/teach.py +1181 -0
  8. urkit/config.py +78 -0
  9. urkit/connection.py +580 -0
  10. urkit/exceptions.py +51 -0
  11. urkit/geometry.py +413 -0
  12. urkit/gripper/__init__.py +36 -0
  13. urkit/gripper/base.py +116 -0
  14. urkit/gripper/digital.py +140 -0
  15. urkit/gripper/presets.py +113 -0
  16. urkit/gripper/robotiq.py +322 -0
  17. urkit/gripper/robotiq_preamble.py +1356 -0
  18. urkit/io.py +311 -0
  19. urkit/motion.py +504 -0
  20. urkit/points.py +277 -0
  21. urkit/robot.py +1510 -0
  22. urkit/telemetry.py +177 -0
  23. urkit-0.1.0.dist-info/METADATA +612 -0
  24. urkit-0.1.0.dist-info/RECORD +27 -0
  25. urkit-0.1.0.dist-info/WHEEL +5 -0
  26. urkit-0.1.0.dist-info/entry_points.txt +2 -0
  27. urkit-0.1.0.dist-info/top_level.txt +1 -0
urkit/gripper/robotiq_preamble.py ADDED
@@ -0,0 +1,1356 @@
1
+ """Robotiq gripper preamble — URScript function library.
2
+
3
+ This module provides the ``ROBOTIQ_PREAMBLE`` constant, a self-contained
4
+ block of URScript that defines all helper functions for communicating
5
+ with a Robotiq 2-Finger gripper over the robot's internal socket
6
+ (127.0.0.1:63352).
7
+
8
+ The preamble must be prepended to every ``sendCustomScriptFunction`` call
9
+ so the robot has the function definitions and a live socket context
10
+ (before each ``rq_*`` function executes).
11
+
12
+ Source: Robotiq Universal Robots URCap (official reference implementation).
13
+ """
14
+
15
+ from __future__ import annotations
16
+
17
+ NEW_LINE = "\n"
18
+ QUOTATION = "\""
19
+
20
+ ROBOTIQ_PREAMBLE: str = "\n".join([
21
+ "#aliases for the gripper variable names",
22
+ "ACT = 1",
23
+ "GTO = 2",
24
+ "ATR = 3",
25
+ "ARD = 4",
26
+ "FOR = 5",
27
+ "SPE = 6",
28
+ "OBJ = 7",
29
+ "STA = 8",
30
+ "FLT = 9",
31
+ "POS = 10",
32
+ "PRE = 11",
33
+ "LBP = 12",
34
+ "LRD = 13",
35
+ "LBL = 14",
36
+ "LGN = 15",
37
+ "MSC = 16",
38
+ "MOD = 17",
39
+ "",
40
+ "gripper_1_connected = False",
41
+ "gripper_2_connected = False",
42
+ "gripper_3_connected = False",
43
+ "gripper_4_connected = False",
44
+ "",
45
+ "gripper_1_socket_open = False",
46
+ "gripper_2_socket_open = False",
47
+ "gripper_3_socket_open = False",
48
+ "gripper_4_socket_open = False",
49
+ "",
50
+ "def rq_init_comm_if_connected(gripper_sid=9, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
51
+ " if(not is_gripper_socket_open(gripper_socket)):",
52
+ " open_gripper_socket(gripper_socket)",
53
+ " end",
54
+ "",
55
+ " is_gripper_connected = rq_is_gripper_connected(gripper_sid, gripper_socket)",
56
+ " if(is_gripper_connected):",
57
+ " rq_set_gripper_connected(gripper_socket)",
58
+ " end",
59
+ "",
60
+ " return is_gripper_connected",
61
+ "end",
62
+ "",
63
+ "def open_gripper_socket(gripper_socket):",
64
+ " is_open = socket_open(" + QUOTATION + "127.0.0.1" + QUOTATION + ",63352, gripper_socket)",
65
+ " set_gripper_socket_open(gripper_socket, is_open)",
66
+ "end",
67
+ "",
68
+ "def is_gripper_socket_open(gripper_socket):",
69
+ " if(gripper_socket == " + QUOTATION + "1" + QUOTATION + "):",
70
+ " return gripper_1_socket_open",
71
+ " elif(gripper_socket == " + QUOTATION + "2" + QUOTATION + "):",
72
+ " return gripper_2_socket_open",
73
+ " elif(gripper_socket == " + QUOTATION + "3" + QUOTATION + "):",
74
+ " return gripper_3_socket_open",
75
+ " elif(gripper_socket == " + QUOTATION + "4" + QUOTATION + "):",
76
+ " return gripper_4_socket_open",
77
+ " else:",
78
+ " return False",
79
+ " end",
80
+ "end",
81
+ "",
82
+ "def set_gripper_socket_open(gripper_socket, is_open):",
83
+ " if(gripper_socket == " + QUOTATION + "1" + QUOTATION + "):",
84
+ " gripper_1_socket_open = is_open",
85
+ " elif(gripper_socket == " + QUOTATION + "2" + QUOTATION + "):",
86
+ " gripper_2_socket_open = is_open",
87
+ " elif(gripper_socket == " + QUOTATION + "3" + QUOTATION + "):",
88
+ " gripper_3_socket_open = is_open",
89
+ " elif(gripper_socket == " + QUOTATION + "4" + QUOTATION + "):",
90
+ " gripper_4_socket_open = is_open",
91
+ " else:",
92
+ " end",
93
+ "end",
94
+ "",
95
+ "def rq_is_gripper_connected(gripper_sid=9, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
96
+ " socket_set_var(" + QUOTATION + "SID" + QUOTATION + ", gripper_sid, gripper_socket)",
97
+ " ack = socket_read_byte_list(3, gripper_socket, 0.1)",
98
+ " return is_ack(ack)",
99
+ "end",
100
+ "",
101
+ "def rq_set_gripper_connected(gripper_id=" + QUOTATION + "1" + QUOTATION + "):",
102
+ " if(gripper_id == " + QUOTATION + "1" + QUOTATION + "):",
103
+ " gripper_1_connected = True",
104
+ " end",
105
+ "",
106
+ " if (gripper_id == " + QUOTATION + "2" + QUOTATION + "):",
107
+ " gripper_2_connected = True",
108
+ " end",
109
+ "",
110
+ " if (gripper_id == " + QUOTATION + "3" + QUOTATION + "):",
111
+ " gripper_3_connected = True",
112
+ " end",
113
+ "",
114
+ " if (gripper_id == " + QUOTATION + "4" + QUOTATION + "):",
115
+ " gripper_4_connected = True",
116
+ " end",
117
+ "end",
118
+ "",
119
+ "def rq_wait_for_gripper_connected():",
120
+ " gripper_socket = " + QUOTATION + "gripper_conn_socket" + QUOTATION + "",
121
+ " socket_open(" + QUOTATION + "127.0.0.1" + QUOTATION + ",63352, gripper_socket)",
122
+ "",
123
+ " retryCtr = 1",
124
+ " sid_list = rq_get_sid(gripper_socket)",
125
+ " gripper_is_connected = rq_is_any_gripper_connected(sid_list)",
126
+ "",
127
+ " while(not gripper_is_connected and retryCtr < 2000):",
128
+ " retryCtr = retryCtr + 1",
129
+ " sid_list = rq_get_sid(gripper_socket)",
130
+ " gripper_is_connected = rq_is_any_gripper_connected(sid_list)",
131
+ " end",
132
+ "",
133
+ " socket_close(gripper_socket)",
134
+ "end",
135
+ "",
136
+ "def rq_is_any_gripper_connected(sid_list):",
137
+ " is_gripper_1_connected = rq_is_gripper1_in_sid_list(sid_list)",
138
+ " is_gripper_2_connected = rq_is_gripper2_in_sid_list(sid_list)",
139
+ " is_gripper_3_connected = rq_is_gripper3_in_sid_list(sid_list)",
140
+ " is_gripper_4_connected = rq_is_gripper4_in_sid_list(sid_list)",
141
+ "",
142
+ " if(is_gripper_1_connected or is_gripper_2_connected or is_gripper_3_connected or is_gripper_4_connected):",
143
+ " return True",
144
+ " else:",
145
+ " return False",
146
+ " end",
147
+ "end",
148
+ "",
149
+ "def rq_is_gripper_in_sid_list(gripper_sid, sid_list):",
150
+ " sid_list_length = sid_list[0]",
151
+ " sid_list_empty_length = 2",
152
+ "",
153
+ " if (sid_list_length <= sid_list_empty_length):",
154
+ " return False",
155
+ " end",
156
+ "",
157
+ " sid1 = sid_list[2]",
158
+ " sid2 = sid_list[5]",
159
+ " sid3 = sid_list[8]",
160
+ " sid4 = sid_list[11]",
161
+ "",
162
+ " if(sid1 == gripper_sid or sid2 == gripper_sid or sid3 == gripper_sid or sid4 == gripper_sid):",
163
+ " return True",
164
+ " else:",
165
+ " return False",
166
+ " end",
167
+ "end",
168
+ "",
169
+ "def rq_is_gripper1_in_sid_list(sid_list):",
170
+ " gripper_1_sid_ascii = 57",
171
+ " return rq_is_gripper_in_sid_list(gripper_1_sid_ascii, sid_list)",
172
+ "end",
173
+ "",
174
+ "def rq_is_gripper2_in_sid_list(sid_list):",
175
+ " gripper_2_sid_ascii = 50",
176
+ " return rq_is_gripper_in_sid_list(gripper_2_sid_ascii, sid_list)",
177
+ "end",
178
+ "",
179
+ "def rq_is_gripper3_in_sid_list(sid_list):",
180
+ " gripper_3_sid_ascii = 51",
181
+ " return rq_is_gripper_in_sid_list(gripper_3_sid_ascii, sid_list)",
182
+ "end",
183
+ "",
184
+ "def rq_is_gripper4_in_sid_list(sid_list):",
185
+ " gripper_4_sid_ascii = 52",
186
+ " return rq_is_gripper_in_sid_list(gripper_4_sid_ascii, sid_list)",
187
+ "end",
188
+ "",
189
+ "def rq_set_sid(gripper_sid=9, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
190
+ " socket_set_var(" + QUOTATION + "SID" + QUOTATION + ", gripper_sid, gripper_socket)",
191
+ " sync()",
192
+ " ack = socket_read_byte_list(3, gripper_socket)",
193
+ " return is_ack(ack)",
194
+ "end",
195
+ "",
196
+ "def rq_get_sid(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
197
+ " socket_send_string(" + QUOTATION + "GET SID" + QUOTATION + ", gripper_socket)",
198
+ " sync()",
199
+ " sid_list = socket_read_byte_list(17, gripper_socket)",
200
+ " sync()",
201
+ " return sid_list",
202
+ "end",
203
+ "",
204
+ "def rq_activate(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
205
+ " rq_gripper_act = 0",
206
+ "",
207
+ " if (not rq_is_gripper_activated(gripper_socket)):",
208
+ " rq_reset(gripper_socket)",
209
+ "",
210
+ " while(socket_get_var(" + QUOTATION + "ACT" + QUOTATION + ",gripper_socket) == 1):",
211
+ " sleep(0.1)",
212
+ " rq_reset(gripper_socket)",
213
+ " end",
214
+ " end",
215
+ "",
216
+ " rq_set_var(ACT,1, gripper_socket)",
217
+ "end",
218
+ "",
219
+ "def rq_activate_and_wait(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
220
+ " rq_activate(gripper_socket)",
221
+ " sleep(1.0)",
222
+ "",
223
+ " while(not rq_is_gripper_activated(gripper_socket)):",
224
+ " # wait for activation completed",
225
+ " end",
226
+ " sleep(0.5)",
227
+ "end",
228
+ "",
229
+ "def rq_activate_all_grippers(reset=False):",
230
+ " if(gripper_1_connected):",
231
+ " rq_reset_and_activate(" + QUOTATION + "1" + QUOTATION + ", reset)",
232
+ " end",
233
+ "",
234
+ " if(gripper_2_connected):",
235
+ " rq_reset_and_activate(" + QUOTATION + "2" + QUOTATION + ", reset)",
236
+ " end",
237
+ "",
238
+ " if(gripper_3_connected):",
239
+ " rq_reset_and_activate(" + QUOTATION + "3" + QUOTATION + ", reset)",
240
+ " end",
241
+ "",
242
+ " if(gripper_4_connected):",
243
+ " rq_reset_and_activate(" + QUOTATION + "4" + QUOTATION + ", reset)",
244
+ " end",
245
+ "",
246
+ " sleep(0.2)",
247
+ "end",
248
+ "",
249
+ "def rq_reset_and_activate(gripper_socket=" + QUOTATION + "1" + QUOTATION + ", reset=False):",
250
+ " if(reset):",
251
+ " rq_reset(gripper_socket)",
252
+ " sleep(0.5)",
253
+ " rq_activate_and_wait(gripper_socket)",
254
+ " elif(not rq_is_gripper_activated(gripper_socket)):",
255
+ " rq_activate_and_wait(gripper_socket)",
256
+ " end",
257
+ "end",
258
+ "",
259
+ "def rq_scan_block():",
260
+ " gripper_socket = " + QUOTATION + "scn_block_socket" + QUOTATION + "",
261
+ " socket_open(" + QUOTATION + "127.0.0.1" + QUOTATION + ", 63352, gripper_socket)",
262
+ " socket_set_var(" + QUOTATION + "SCN_BLOCK" + QUOTATION + ", 1, gripper_socket)",
263
+ " sync()",
264
+ " ack_test = socket_read_byte_list(3, gripper_socket)",
265
+ "",
266
+ " retry_counter = 0",
267
+ "",
268
+ " while(not is_ack(ack_test) and retry_counter < 5):",
269
+ " socket_set_var(" + QUOTATION + "SCN_BLOCK" + QUOTATION + ", 1, gripper_socket)",
270
+ " sync()",
271
+ " ack_test = socket_read_byte_list(3, gripper_socket)",
272
+ " retry_counter = retry_counter + 1",
273
+ " end",
274
+ "",
275
+ " socket_close(" + QUOTATION + "scn_block_socket" + QUOTATION + ")",
276
+ "end",
277
+ "",
278
+ "def rq_reset(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
279
+ " rq_gripper_act = 0",
280
+ " rq_obj_detect = 0",
281
+ " rq_mov_complete = 0",
282
+ "",
283
+ " rq_set_var(ACT,0, gripper_socket)",
284
+ " rq_set_var(ATR,0, gripper_socket)",
285
+ "end",
286
+ "",
287
+ "def rq_auto_release_open_and_wait(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
288
+ " rq_set_var(ARD,0, gripper_socket)",
289
+ " rq_set_var(ACT,1, gripper_socket)",
290
+ " rq_set_var(ATR,0, gripper_socket)",
291
+ " sleep(0.1)",
292
+ " rq_set_var(ATR,1, gripper_socket)",
293
+ "",
294
+ " rq_wait_autorelease_completed(gripper_socket)",
295
+ "end",
296
+ "",
297
+ "def rq_auto_release_close_and_wait(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
298
+ " rq_set_var(ARD,1, gripper_socket)",
299
+ " rq_set_var(ACT,1, gripper_socket)",
300
+ " rq_set_var(ATR,0, gripper_socket)",
301
+ " sleep(0.1)",
302
+ " rq_set_var(ATR,1, gripper_socket)",
303
+ "",
304
+ " rq_wait_autorelease_completed(gripper_socket)",
305
+ "end",
306
+ "",
307
+ "def rq_wait_autorelease_completed(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
308
+ " retryCounter = 1",
309
+ " gFLT = rq_get_var(FLT, 2, gripper_socket)",
310
+ "",
311
+ " while(not is_FLT_autorelease_in_progress(gFLT) and retryCounter <= 20):",
312
+ " retryCounter = retryCounter + 1",
313
+ " gFLT = rq_get_var(FLT, 2, gripper_socket)",
314
+ " sleep(0.1)",
315
+ " end",
316
+ "",
317
+ " retryCounter = 1",
318
+ " gFLT = rq_get_var(FLT, 2, gripper_socket)",
319
+ "",
320
+ " while(not is_FLT_autorelease_completed(gFLT) and retryCounter <= 100):",
321
+ " retryCounter = retryCounter + 1",
322
+ " gFLT = rq_get_var(FLT, 2, gripper_socket)",
323
+ " sleep(0.1)",
324
+ " end",
325
+ "end",
326
+ "",
327
+ "def rq_set_force(force, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
328
+ " force = floor(scale(force, [0, 255], [0,255]))",
329
+ " rq_set_var(FOR, force, gripper_socket)",
330
+ "end",
331
+ "",
332
+ "def rq_set_speed(speed, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
333
+ " speed = floor(scale(speed, [0, 255], [0,255]))",
334
+ " rq_set_var(SPE, speed, gripper_socket)",
335
+ "end",
336
+ "",
337
+ "def rq_open(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
338
+ " rq_move(0, gripper_socket)",
339
+ "end",
340
+ "",
341
+ "def rq_close(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
342
+ " rq_move(255, gripper_socket)",
343
+ "end",
344
+ "",
345
+ "def rq_open_and_wait(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
346
+ " rq_move_and_wait(0, gripper_socket)",
347
+ "end",
348
+ "",
349
+ "def rq_close_and_wait(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
350
+ " rq_move_and_wait(255, gripper_socket)",
351
+ "end",
352
+ "",
353
+ "def rq_move(pos, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
354
+ " rq_mov_complete = 0",
355
+ " rq_obj_detect = 0",
356
+ "",
357
+ " rq_set_pos(pos, gripper_socket)",
358
+ " rq_go_to(gripper_socket)",
359
+ "end",
360
+ "",
361
+ "def rq_move_and_wait(pos, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
362
+ " rq_move(pos, gripper_socket)",
363
+ "",
364
+ " while (not rq_is_motion_complete(gripper_socket)):",
365
+ " # wait for motion completed",
366
+ " sleep(0.01)",
367
+ " sync()",
368
+ " end",
369
+ "",
370
+ " # following code used for compatibility with previous versions",
371
+ " rq_is_object_detected(gripper_socket)",
372
+ "",
373
+ " if (rq_obj_detect != 1):",
374
+ " rq_mov_complete = 1",
375
+ " end",
376
+ "end",
377
+ "",
378
+ "def rq_wait_for_pos_request(pos, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
379
+ " gPRE = rq_get_var(PRE, 3, gripper_socket)",
380
+ " pre = (gPRE[1] - 48)*100 + (gPRE[2] -48)*10 + gPRE[3] - 48",
381
+ "",
382
+ " while (pre != pos):",
383
+ " rq_set_var(POS, pos, gripper_socket)",
384
+ " gPRE = rq_get_var(PRE, 3, gripper_socket)",
385
+ " pre = (gPRE[1] - 48)*100 + (gPRE[2] -48)*10 + gPRE[3] - 48",
386
+ " sync()",
387
+ " end",
388
+ "end",
389
+ "",
390
+ "def rq_wait_for_pos(pos, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
391
+ " rq_wait_for_pos_request(pos, gripper_socket)",
392
+ "",
393
+ " # Wait for the gripper motion to complete",
394
+ " while (not rq_is_motion_complete(gripper_socket)):",
395
+ " # wait for motion completed",
396
+ " sleep(0.01)",
397
+ " sync()",
398
+ " rq_go_to(gripper_socket)",
399
+ " end",
400
+ "",
401
+ " # following code used for compatibility with previous versions",
402
+ " rq_is_object_detected(gripper_socket)",
403
+ "",
404
+ " if (rq_obj_detect != 1):",
405
+ " rq_mov_complete = 1",
406
+ " end",
407
+ "end",
408
+ "",
409
+ "def rq_wait(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
410
+ " # Wait for the gripper motion to complete",
411
+ " while (not rq_is_motion_complete(gripper_socket)):",
412
+ " # wait for motion completed",
413
+ " sleep(0.01)",
414
+ " sync()",
415
+ " end",
416
+ "",
417
+ " # following code used for compatibility with previous versions",
418
+ " rq_is_object_detected(gripper_socket)",
419
+ "",
420
+ " if (rq_obj_detect != 1):",
421
+ " rq_mov_complete = 1",
422
+ " end",
423
+ "end",
424
+ "",
425
+ "def rq_wait_for_object_detected(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
426
+ " # Wait the object detection",
427
+ " while (not rq_is_object_detected(gripper_socket)):",
428
+ " # wait for motion completed",
429
+ " sleep(0.01)",
430
+ " sync()",
431
+ " end",
432
+ "end",
433
+ "",
434
+ "# set the position",
435
+ "def rq_set_pos(pos, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
436
+ " pos = floor(scale(pos, [0, 255], [0, 255]))",
437
+ " rq_set_var(POS, pos, gripper_socket)",
438
+ " rq_wait_for_pos_request(pos, gripper_socket)",
439
+ "end",
440
+ "",
441
+ "# set the position, speed and force",
442
+ "def rq_set_pos_spd_for(pos, speed, force, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
443
+ " enter_critical",
444
+ " rq_send_pos_spd_for(pos, speed, force, gripper_socket)",
445
+ " ack = socket_read_byte_list(3, gripper_socket)",
446
+ " exit_critical",
447
+ "",
448
+ " sync()",
449
+ "",
450
+ " while(is_not_ack(ack)):",
451
+ " enter_critical",
452
+ " rq_send_pos_spd_for(pos, speed, force, gripper_socket)",
453
+ " ack = socket_read_byte_list(3, gripper_socket)",
454
+ " exit_critical",
455
+ "",
456
+ " sync()",
457
+ " end",
458
+ "",
459
+ " rq_wait_for_pos_request(pos, gripper_socket)",
460
+ "end",
461
+ "",
462
+ "def rq_set_gripper_max_current_mA(current_mA, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
463
+ " current = floor(current_mA / 10)",
464
+ " rq_set_var(MSC, current, gripper_socket)",
465
+ " sleep(1.5)",
466
+ "end",
467
+ "",
468
+ "def rq_set_gripper_mode(mode, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
469
+ " rq_set_var(MOD, mode, gripper_socket)",
470
+ "end",
471
+ "",
472
+ "def rq_set_gripper_max_cur(current_mA, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
473
+ " rq_set_gripper_max_current_mA(current_mA, gripper_socket)",
474
+ "end",
475
+ "",
476
+ "def rq_get_gripper_max_current_mA(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
477
+ " socket_send_string(" + QUOTATION + "GET MSC" + QUOTATION + ",gripper_socket)",
478
+ " sync()",
479
+ " var_value = socket_read_byte_list(3, gripper_socket)",
480
+ "",
481
+ " current = rq_list_of_bytes_to_value(var_value)",
482
+ "",
483
+ " if(current == -1):",
484
+ " current_mA = current",
485
+ " else:",
486
+ " current_mA = current * 10",
487
+ " end",
488
+ "",
489
+ " return current_mA",
490
+ "end",
491
+ "",
492
+ "def rq_get_gripper_max_cur(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
493
+ " return rq_get_gripper_max_current_mA(gripper_socket)",
494
+ "end",
495
+ "",
496
+ "def rq_list_of_bytes_to_value(list_of_bytes):",
497
+ " value = -1",
498
+ "",
499
+ " # response list length",
500
+ " if (list_of_bytes[0] == 1):",
501
+ " value = list_of_bytes[1] - 48",
502
+ " elif (list_of_bytes[0] == 2):",
503
+ " value = (list_of_bytes[1] - 48) * 10 + (list_of_bytes[2] - 48)",
504
+ " elif (list_of_bytes[0] == 3):",
505
+ " value = (list_of_bytes[1] - 48) * 100 + (list_of_bytes[2] - 48) * 10 + (list_of_bytes[3] - 48)",
506
+ " end",
507
+ "",
508
+ " return value",
509
+ "end",
510
+ "",
511
+ "# send the position, speed and force",
512
+ "def rq_send_pos_spd_for(pos, speed, force, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
513
+ " pos = floor(scale(pos, [0, 255], [0, 255]))",
514
+ " speed = floor(scale(speed, [0, 255], [0,255]))",
515
+ " force = floor(scale(force, [0, 255], [0,255]))",
516
+ "",
517
+ " socket_send_string(" + QUOTATION + "SET POS" + QUOTATION + ", gripper_socket)",
518
+ " socket_send_byte(32, gripper_socket)",
519
+ " socket_send_string(pos, gripper_socket)",
520
+ " socket_send_byte(32, gripper_socket)",
521
+ " socket_send_string(" + QUOTATION + "SPE" + QUOTATION + ", gripper_socket)",
522
+ " socket_send_byte(32, gripper_socket)",
523
+ " socket_send_string(speed, gripper_socket)",
524
+ " socket_send_byte(32, gripper_socket)",
525
+ " socket_send_string(" + QUOTATION + "FOR" + QUOTATION + ", gripper_socket)",
526
+ " socket_send_byte(32, gripper_socket)",
527
+ " socket_send_string(force, gripper_socket)",
528
+ " socket_send_byte(10, gripper_socket)",
529
+ "end",
530
+ "",
531
+ "def rq_is_motion_complete(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
532
+ " rq_mov_complete = 0",
533
+ "",
534
+ " gOBJ = rq_get_var(OBJ, 1, gripper_socket)",
535
+ " sleep(0.01)",
536
+ "",
537
+ " if (is_OBJ_gripper_at_position(gOBJ)):",
538
+ " rq_mov_complete = 1",
539
+ " return True",
540
+ " end",
541
+ "",
542
+ " if (is_OBJ_object_detected(gOBJ)):",
543
+ " rq_mov_complete = 1",
544
+ " return True",
545
+ " end",
546
+ "",
547
+ " return False",
548
+ "",
549
+ "end",
550
+ "",
551
+ "def rq_is_gripper_activated(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
552
+ " gSTA = rq_get_var(STA, 1, gripper_socket)",
553
+ "",
554
+ " if(is_STA_gripper_activated(gSTA)):",
555
+ " rq_gripper_act = 1",
556
+ " return True",
557
+ " else:",
558
+ " rq_gripper_act = 0",
559
+ " return False",
560
+ " end",
561
+ "end",
562
+ "",
563
+ "def rq_is_object_detected(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
564
+ " gOBJ = rq_get_var(OBJ, 1, gripper_socket)",
565
+ "",
566
+ " if(is_OBJ_object_detected(gOBJ)):",
567
+ " rq_obj_detect = 1",
568
+ " return True",
569
+ " else:",
570
+ " rq_obj_detect = 0",
571
+ " return False",
572
+ " end",
573
+ "end",
574
+ "",
575
+ "def rq_current_pos(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
576
+ " enter_critical",
577
+ " rq_pos = socket_get_var(" + QUOTATION + "POS" + QUOTATION + ",gripper_socket)",
578
+ " exit_critical",
579
+ " sync()",
580
+ " return rq_pos",
581
+ "end",
582
+ "",
583
+ "def rq_motor_current(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
584
+ " enter_critical",
585
+ " rq_current = socket_get_var(" + QUOTATION + "COU" + QUOTATION + ",gripper_socket)",
586
+ " exit_critical",
587
+ " sync()",
588
+ " return rq_current * 10",
589
+ "end",
590
+ "",
591
+ "def rq_print_connected_grippers():",
592
+ " if(gripper_1_connected):",
593
+ " textmsg(" + QUOTATION + "Gripper 1 : " + QUOTATION + ", " + QUOTATION + "connected and socket open." + QUOTATION + ")",
594
+ " end",
595
+ "",
596
+ " if (gripper_2_connected):",
597
+ " textmsg(" + QUOTATION + "Gripper 2 : " + QUOTATION + ", " + QUOTATION + "connected and socket open." + QUOTATION + ")",
598
+ " end",
599
+ "",
600
+ " if (gripper_3_connected):",
601
+ " textmsg(" + QUOTATION + "Gripper 3 : " + QUOTATION + ", " + QUOTATION + "connected and socket open." + QUOTATION + ")",
602
+ " end",
603
+ "",
604
+ " if (gripper_4_connected):",
605
+ " textmsg(" + QUOTATION + "Gripper 4 : " + QUOTATION + ", " + QUOTATION + "connected and socket open." + QUOTATION + ")",
606
+ " end",
607
+ "end",
608
+ "",
609
+ "def rq_print_gripper_fault_code(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
610
+ " gFLT = rq_get_var(FLT, 2, gripper_socket)",
611
+ "",
612
+ " if(is_FLT_no_fault(gFLT)):",
613
+ " textmsg(" + QUOTATION + "Gripper Fault : " + QUOTATION + ", " + QUOTATION + "No Fault (0x00)" + QUOTATION + ")",
614
+ " elif (is_FLT_action_delayed(gFLT)):",
615
+ " textmsg(" + QUOTATION + "Gripper Fault : " + QUOTATION + ", " + QUOTATION + "Priority Fault: Action delayed, initialization must be completed prior to action (0x05)" + QUOTATION + ")",
616
+ " elif (is_FLT_not_activated(gFLT)):",
617
+ " textmsg(" + QUOTATION + "Gripper Fault : " + QUOTATION + ", " + QUOTATION + "Priority Fault: The activation must be set prior to action (0x07)" + QUOTATION + ")",
618
+ " elif (is_FLT_autorelease_in_progress(gFLT)):",
619
+ " textmsg(" + QUOTATION + "Gripper Fault : " + QUOTATION + ", " + QUOTATION + "Minor Fault: Automatic release in progress (0x0B)" + QUOTATION + ")",
620
+ " elif (is_FLT_overcurrent(gFLT)):",
621
+ " textmsg(" + QUOTATION + "Gripper Fault : " + QUOTATION + ", " + QUOTATION + "Minor Fault: Overcurrent protection triggered (0x0E)" + QUOTATION + ")",
622
+ " elif (is_FLT_autorelease_completed(gFLT)):",
623
+ " textmsg(" + QUOTATION + "Gripper Fault : " + QUOTATION + ", " + QUOTATION + "Major Fault: Automatic release completed (0x0F)" + QUOTATION + ")",
624
+ " else:",
625
+ " textmsg(" + QUOTATION + "Gripper Fault : " + QUOTATION + ", " + QUOTATION + "Unknown Fault" + QUOTATION + ")",
626
+ " end",
627
+ "end",
628
+ "",
629
+ "def rq_print_gripper_num_cycles(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
630
+ " socket_send_string(" + QUOTATION + "GET NCY" + QUOTATION + ",gripper_socket)",
631
+ " sync()",
632
+ " string_from_server = socket_read_string(gripper_socket)",
633
+ " sync()",
634
+ "",
635
+ " if(string_from_server == " + QUOTATION + "0" + QUOTATION + "):",
636
+ " textmsg(" + QUOTATION + "Gripper Cycle Number : " + QUOTATION + ", " + QUOTATION + "Number of cycles is unreachable." + QUOTATION + ")",
637
+ " else:",
638
+ " textmsg(" + QUOTATION + "Gripper Cycle Number : " + QUOTATION + ", string_from_server)",
639
+ " end",
640
+ "end",
641
+ "",
642
+ "def rq_print_gripper_driver_state(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
643
+ " socket_send_string(" + QUOTATION + "GET DST" + QUOTATION + ",gripper_socket)",
644
+ " sync()",
645
+ " string_from_server = socket_read_string(gripper_socket)",
646
+ " sync()",
647
+ "",
648
+ " if(string_from_server == " + QUOTATION + "0" + QUOTATION + "):",
649
+ " textmsg(" + QUOTATION + "Gripper Driver State : " + QUOTATION + ", " + QUOTATION + "RQ_STATE_INIT" + QUOTATION + ")",
650
+ " elif(string_from_server == " + QUOTATION + "1" + QUOTATION + "):",
651
+ " textmsg(" + QUOTATION + "Gripper Driver State : " + QUOTATION + ", " + QUOTATION + "RQ_STATE_LISTEN" + QUOTATION + ")",
652
+ " elif(string_from_server == " + QUOTATION + "2" + QUOTATION + "):",
653
+ " textmsg(" + QUOTATION + "Gripper Driver State : " + QUOTATION + ", " + QUOTATION + "RQ_STATE_READ_INFO" + QUOTATION + ")",
654
+ " elif(string_from_server == " + QUOTATION + "3" + QUOTATION + "):",
655
+ " textmsg(" + QUOTATION + "Gripper Driver State : " + QUOTATION + ", " + QUOTATION + "RQ_STATE_ACTIVATION" + QUOTATION + ")",
656
+ " else:",
657
+ " textmsg(" + QUOTATION + "Gripper Driver State : " + QUOTATION + ", " + QUOTATION + "RQ_STATE_RUN" + QUOTATION + ")",
658
+ " end",
659
+ "end",
660
+ "",
661
+ "def rq_print_gripper_serial_number(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
662
+ " socket_send_string(" + QUOTATION + "GET SNU" + QUOTATION + ",gripper_socket)",
663
+ " sync()",
664
+ " string_from_server = socket_read_string(gripper_socket)",
665
+ " sync()",
666
+ " textmsg(" + QUOTATION + "Gripper Serial Number : " + QUOTATION + ", string_from_server)",
667
+ "end",
668
+ "",
669
+ "def rq_print_gripper_firmware_version(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
670
+ " socket_send_string(" + QUOTATION + "GET FWV" + QUOTATION + ",gripper_socket)",
671
+ " sync()",
672
+ " string_from_server = socket_read_string(gripper_socket)",
673
+ " sync()",
674
+ " textmsg(" + QUOTATION + "Gripper Firmware Version : " + QUOTATION + ", string_from_server)",
675
+ "end",
676
+ "",
677
+ "def rq_print_gripper_driver_version(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
678
+ " socket_send_string(" + QUOTATION + "GET VER" + QUOTATION + ",gripper_socket)",
679
+ " sync()",
680
+ " string_from_server = socket_read_string(gripper_socket)",
681
+ " sync()",
682
+ " textmsg(" + QUOTATION + "Gripper Driver Version : " + QUOTATION + ", string_from_server)",
683
+ "end",
684
+ "",
685
+ "def rq_print_gripper_probleme_connection(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
686
+ " socket_send_string(" + QUOTATION + "GET PCO" + QUOTATION + ",gripper_socket)",
687
+ " sync()",
688
+ " string_from_server = socket_read_string(gripper_socket)",
689
+ " sync()",
690
+ " if (string_from_server == " + QUOTATION + "0" + QUOTATION + "):",
691
+ " textmsg(" + QUOTATION + "Gripper Connection State : " + QUOTATION + ", " + QUOTATION + "No connection problem detected" + QUOTATION + ")",
692
+ " else:",
693
+ " textmsg(" + QUOTATION + "Gripper Connection State : " + QUOTATION + ", " + QUOTATION + "Connection problem detected" + QUOTATION + ")",
694
+ " end",
695
+ "end",
696
+ "",
697
+ "# Returns True if list_of_bytes is [3, 'a', 'c', 'k']",
698
+ "def is_ack(list_of_bytes):",
699
+ "",
700
+ " # list length is not 3",
701
+ " if (list_of_bytes[0] != 3):",
702
+ " return False",
703
+ " end",
704
+ "",
705
+ " # first byte not is 'a'?",
706
+ " if (list_of_bytes[1] != 97):",
707
+ " return False",
708
+ " end",
709
+ "",
710
+ " # first byte not is 'c'?",
711
+ " if (list_of_bytes[2] != 99):",
712
+ " return False",
713
+ " end",
714
+ "",
715
+ " # first byte not is 'k'?",
716
+ " if (list_of_bytes[3] != 107):",
717
+ " return False",
718
+ " end",
719
+ "",
720
+ " return True",
721
+ "end",
722
+ "",
723
+ "# Returns True if list_of_bytes is not [3, 'a', 'c', 'k']",
724
+ "def is_not_ack(list_of_bytes):",
725
+ " if (is_ack(list_of_bytes)):",
726
+ " return False",
727
+ " else:",
728
+ " return True",
729
+ " end",
730
+ "end",
731
+ "",
732
+ "def is_STA_gripper_activated (list_of_bytes):",
733
+ "",
734
+ " # list length is not 1",
735
+ " if (list_of_bytes[0] != 1):",
736
+ " return False",
737
+ " end",
738
+ "",
739
+ " # byte is '3'?",
740
+ " if (list_of_bytes[1] == 51):",
741
+ " return True",
742
+ " end",
743
+ "",
744
+ " return False",
745
+ "end",
746
+ "",
747
+ "# Returns True if list_of_byte is [1, '1'] or [1, '2']",
748
+ "# Used to test OBJ = 0x1 or OBJ = 0x2",
749
+ "def is_OBJ_object_detected (list_of_bytes):",
750
+ "",
751
+ " # list length is not 1",
752
+ " if (list_of_bytes[0] != 1):",
753
+ " return False",
754
+ " end",
755
+ "",
756
+ " # byte is '2'?",
757
+ " if (list_of_bytes[1] == 50):",
758
+ " return True",
759
+ " end",
760
+ "",
761
+ " # byte is '1'?",
762
+ " if (list_of_bytes[1] == 49):",
763
+ " return True",
764
+ " end",
765
+ "",
766
+ " return False",
767
+ "",
768
+ "end",
769
+ "",
770
+ "# Returns True if list_of_byte is [1, '3']",
771
+ "# Used to test OBJ = 0x3",
772
+ "def is_OBJ_gripper_at_position (list_of_bytes):",
773
+ "",
774
+ " # list length is not 1",
775
+ " if (list_of_bytes[0] != 1):",
776
+ " return False",
777
+ " end",
778
+ "",
779
+ " # byte is '3'?",
780
+ " if (list_of_bytes[1] == 51):",
781
+ " return True",
782
+ " end",
783
+ "",
784
+ " return False",
785
+ "end",
786
+ "",
787
+ "def is_not_OBJ_gripper_at_position (list_of_bytes):",
788
+ "",
789
+ " if (is_OBJ_gripper_at_position(list_of_bytes)):",
790
+ " return False",
791
+ " else:",
792
+ " return True",
793
+ " end",
794
+ "end",
795
+ "",
796
+ "#### GTO Section ####",
797
+ "def rq_stop(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
798
+ " rq_set_var(GTO, 0, gripper_socket)",
799
+ "end",
800
+ "",
801
+ "def rq_set_GTO_and_wait(value, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
802
+ " rq_set_var(GTO ,value, gripper_socket)",
803
+ " while(not is_GTO(value, rq_get_var(GTO, 1, gripper_socket))):",
804
+ " sync()",
805
+ " end",
806
+ "end",
807
+ "",
808
+ "def rq_go_to(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
809
+ " rq_set_var(GTO, 1, gripper_socket)",
810
+ "end",
811
+ "",
812
+ "",
813
+ "def is_GTO(goto_value, list_of_bytes):",
814
+ " zero_ascii = 48",
815
+ " if (list_of_bytes[0] != 1):",
816
+ " return False",
817
+ " end",
818
+ "",
819
+ " if (list_of_bytes[1] == zero_ascii + goto_value):",
820
+ " return True",
821
+ " else:",
822
+ " return False",
823
+ " end",
824
+ "end",
825
+ "#### GTO Section ####",
826
+ "",
827
+ "def is_FLT_no_fault(list_of_bytes):",
828
+ "",
829
+ " # list length is not 2",
830
+ " if (list_of_bytes[0] != 2):",
831
+ " return False",
832
+ " end",
833
+ "",
834
+ " # first byte is '0'?",
835
+ " if (list_of_bytes[1] != 48):",
836
+ " return False",
837
+ " end",
838
+ "",
839
+ " # second byte is '0'?",
840
+ " if (list_of_bytes[2] != 48):",
841
+ " return False",
842
+ " end",
843
+ "",
844
+ " return True",
845
+ "",
846
+ "end",
847
+ "",
848
+ "def is_FLT_action_delayed(list_of_bytes):",
849
+ "",
850
+ " # list length is not 2",
851
+ " if (list_of_bytes[0] != 2):",
852
+ " return False",
853
+ " end",
854
+ "",
855
+ " # first byte is '0'?",
856
+ " if (list_of_bytes[1] != 48):",
857
+ " return False",
858
+ " end",
859
+ "",
860
+ " # second byte is '5'?",
861
+ " if (list_of_bytes[2] != 53):",
862
+ " return False",
863
+ " end",
864
+ "",
865
+ " return True",
866
+ "end",
867
+ "",
868
+ "def is_FLT_not_activated(list_of_bytes):",
869
+ "",
870
+ " # list length is not 2",
871
+ " if (list_of_bytes[0] != 2):",
872
+ " return False",
873
+ " end",
874
+ "",
875
+ " # first byte is '0'?",
876
+ " if (list_of_bytes[1] != 48):",
877
+ " return False",
878
+ " end",
879
+ "",
880
+ " # second byte is '7'?",
881
+ " if (list_of_bytes[2] != 55):",
882
+ " return False",
883
+ " end",
884
+ "",
885
+ " return True",
886
+ "end",
887
+ "",
888
+ "def is_FLT_autorelease_in_progress(list_of_bytes):",
889
+ "",
890
+ " # list length is not 2",
891
+ " if (list_of_bytes[0] != 2):",
892
+ " return False",
893
+ " end",
894
+ "",
895
+ " # first byte is '1'?",
896
+ " if (list_of_bytes[1] != 49):",
897
+ " return False",
898
+ " end",
899
+ "",
900
+ " # second byte is '1'?",
901
+ " if (list_of_bytes[2] != 49):",
902
+ " return False",
903
+ " end",
904
+ "",
905
+ " return True",
906
+ "",
907
+ "end",
908
+ "",
909
+ "def is_FLT_overcurrent(list_of_bytes):",
910
+ "",
911
+ " # list length is not 2",
912
+ " if (list_of_bytes[0] != 2):",
913
+ " return False",
914
+ " end",
915
+ "",
916
+ " # first byte is '1'?",
917
+ " if (list_of_bytes[1] != 49):",
918
+ " return False",
919
+ " end",
920
+ "",
921
+ " # second byte is '4'?",
922
+ " if (list_of_bytes[2] != 52):",
923
+ " return False",
924
+ " end",
925
+ "",
926
+ " return True",
927
+ "",
928
+ "end",
929
+ "",
930
+ "def is_FLT_autorelease_completed(list_of_bytes):",
931
+ "",
932
+ " # list length is not 2",
933
+ " if (list_of_bytes[0] != 2):",
934
+ " return False",
935
+ " end",
936
+ "",
937
+ " # first byte is '1'?",
938
+ " if (list_of_bytes[1] != 49):",
939
+ " return False",
940
+ " end",
941
+ "",
942
+ " # second byte is '5'?",
943
+ " if (list_of_bytes[2] != 53):",
944
+ " return False",
945
+ " end",
946
+ "",
947
+ " return True",
948
+ "",
949
+ "end",
950
+ "",
951
+ "def rq_set_var(var_name, var_value, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
952
+ "",
953
+ " var_name_string = " + QUOTATION + "" + QUOTATION + "",
954
+ "",
955
+ " if (var_name == ACT):",
956
+ " var_name_string = " + QUOTATION + "ACT" + QUOTATION + "",
957
+ " elif (var_name == GTO):",
958
+ " var_name_string = " + QUOTATION + "GTO" + QUOTATION + "",
959
+ " elif (var_name == ATR):",
960
+ " var_name_string = " + QUOTATION + "ATR" + QUOTATION + "",
961
+ " elif (var_name == ARD):",
962
+ " var_name_string = " + QUOTATION + "ARD" + QUOTATION + "",
963
+ " elif (var_name == FOR):",
964
+ " var_name_string = " + QUOTATION + "FOR" + QUOTATION + "",
965
+ " elif (var_name == SPE):",
966
+ " var_name_string = " + QUOTATION + "SPE" + QUOTATION + "",
967
+ " elif (var_name == POS):",
968
+ " var_name_string = " + QUOTATION + "POS" + QUOTATION + "",
969
+ " elif (var_name == LBP):",
970
+ " var_name_string = " + QUOTATION + "LBP" + QUOTATION + "",
971
+ " elif (var_name == LRD):",
972
+ " var_name_string = " + QUOTATION + "LRD" + QUOTATION + "",
973
+ " elif (var_name == LBL):",
974
+ " var_name_string = " + QUOTATION + "LBL" + QUOTATION + "",
975
+ " elif (var_name == LGN):",
976
+ " var_name_string = " + QUOTATION + "LGN" + QUOTATION + "",
977
+ " elif (var_name == MSC):",
978
+ " var_name_string = " + QUOTATION + "MSC" + QUOTATION + "",
979
+ " elif (var_name == MOD):",
980
+ " var_name_string = " + QUOTATION + "MOD" + QUOTATION + "",
981
+ " end",
982
+ "",
983
+ " enter_critical",
984
+ " socket_set_var(var_name_string, var_value, gripper_socket)",
985
+ " ack = socket_read_byte_list(3, gripper_socket)",
986
+ " exit_critical",
987
+ "",
988
+ " sync()",
989
+ "",
990
+ " while(is_not_ack(ack)):",
991
+ " enter_critical",
992
+ " socket_set_var(var_name_string , var_value, gripper_socket)",
993
+ " sync()",
994
+ " ack = socket_read_byte_list(3, gripper_socket)",
995
+ " exit_critical",
996
+ "",
997
+ " sync()",
998
+ " end",
999
+ "end",
1000
+ "",
1001
+ "",
1002
+ "def rq_get_var(var_name, nbr_bytes, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
1003
+ " enter_critical",
1004
+ "",
1005
+ " if (var_name == FLT):",
1006
+ " socket_send_string(" + QUOTATION + "GET FLT" + QUOTATION + ", gripper_socket)",
1007
+ " elif (var_name == OBJ):",
1008
+ " socket_send_string(" + QUOTATION + "GET OBJ" + QUOTATION + ", gripper_socket)",
1009
+ " elif (var_name == STA):",
1010
+ " socket_send_string(" + QUOTATION + "GET STA" + QUOTATION + ", gripper_socket)",
1011
+ " elif (var_name == PRE):",
1012
+ " socket_send_string(" + QUOTATION + "GET PRE" + QUOTATION + ", gripper_socket)",
1013
+ " elif (var_name == GTO):",
1014
+ " socket_send_string(" + QUOTATION + "GET GTO" + QUOTATION + ", gripper_socket)",
1015
+ " else:",
1016
+ " end",
1017
+ "",
1018
+ " var_value = socket_read_byte_list(nbr_bytes, gripper_socket)",
1019
+ "",
1020
+ " exit_critical",
1021
+ "",
1022
+ " sync()",
1023
+ "",
1024
+ " return var_value",
1025
+ "end",
1026
+ "",
1027
+ "def rq_is_object_validated(gripper_selected, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
1028
+ " if(gripper_selected):",
1029
+ " if(rq_is_object_detected(gripper_socket)):",
1030
+ " return True",
1031
+ " else:",
1032
+ " return False",
1033
+ " end",
1034
+ " else:",
1035
+ " return True",
1036
+ " end",
1037
+ "end",
1038
+ "",
1039
+ "############################################",
1040
+ "# normalized functions (maps 0-100 to 0-255)",
1041
+ "############################################",
1042
+ "def rq_set_force_norm(force_norm, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
1043
+ " force_gripper = norm_to_gripper(force_norm)",
1044
+ " rq_set_force(force_gripper, gripper_socket)",
1045
+ "end",
1046
+ "",
1047
+ "def rq_set_speed_norm(speed_norm, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
1048
+ " speed_gripper = norm_to_gripper(speed_norm)",
1049
+ " rq_set_speed(speed_gripper, gripper_socket)",
1050
+ "end",
1051
+ "",
1052
+ "def rq_move_norm(pos_norm, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
1053
+ " pos_gripper = norm_to_gripper(pos_norm)",
1054
+ " rq_move(pos_gripper, gripper_socket)",
1055
+ "end",
1056
+ "",
1057
+ "def rq_move_and_wait_norm(pos_norm, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
1058
+ " pos_gripper = norm_to_gripper(pos_norm)",
1059
+ " rq_move_and_wait(pos_gripper, gripper_socket)",
1060
+ "end",
1061
+ "",
1062
+ "def rq_set_pos_norm(pos_norm, gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
1063
+ " pos_gripper = norm_to_gripper(pos_norm)",
1064
+ " rq_set_pos(pos_gripper, gripper_socket)",
1065
+ "end",
1066
+ "",
1067
+ "def rq_current_pos_norm(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
1068
+ " pos_gripper = rq_current_pos(gripper_socket)",
1069
+ " pos_norm = gripper_to_norm(pos_gripper)",
1070
+ " return pos_norm",
1071
+ "end",
1072
+ "",
1073
+ "def gripper_to_norm(value_gripper):",
1074
+ " value_norm = (value_gripper / 255) * 100",
1075
+ " return floor(value_norm)",
1076
+ "end",
1077
+ "",
1078
+ "def norm_to_gripper(value_norm):",
1079
+ " value_gripper = (value_norm / 100) * 255",
1080
+ " return ceil(value_gripper)",
1081
+ "end",
1082
+ "",
1083
+ "def rq_get_position():",
1084
+ " return rq_current_pos_norm()",
1085
+ "end",
1086
+ "",
1087
+ "def rq_gripper_led_on(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
1088
+ " rq_set_var(LBP,0, gripper_socket)",
1089
+ "end",
1090
+ "",
1091
+ "def rq_gripper_led_off(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
1092
+ " rq_set_var(LBP,1, gripper_socket)",
1093
+ " rq_set_var(LRD,0, gripper_socket)",
1094
+ " rq_set_var(LBL,0, gripper_socket)",
1095
+ " rq_set_var(LGN,0, gripper_socket)",
1096
+ "end",
1097
+ "",
1098
+ "def rq_gripper_led_force_red(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
1099
+ " rq_set_var(LBP,1, gripper_socket)",
1100
+ " rq_set_var(LRD,1, gripper_socket)",
1101
+ " rq_set_var(LBL,0, gripper_socket)",
1102
+ " rq_set_var(LGN,0, gripper_socket)",
1103
+ "end",
1104
+ "",
1105
+ "def rq_gripper_led_force_blue(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
1106
+ " rq_set_var(LBP,1, gripper_socket)",
1107
+ " rq_set_var(LRD,0, gripper_socket)",
1108
+ " rq_set_var(LBL,1, gripper_socket)",
1109
+ " rq_set_var(LGN,0, gripper_socket)",
1110
+ "end",
1111
+ "",
1112
+ "def rq_gripper_led_force_green(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
1113
+ " rq_set_var(LBP,1, gripper_socket)",
1114
+ " rq_set_var(LRD,0, gripper_socket)",
1115
+ " rq_set_var(LBL,0, gripper_socket)",
1116
+ " rq_set_var(LGN,1, gripper_socket)",
1117
+ "end",
1118
+ "",
1119
+ "def rq_gripper_led_force_purple(gripper_socket=" + QUOTATION + "1" + QUOTATION + "):",
1120
+ " rq_set_var(LBP,1, gripper_socket)",
1121
+ " rq_set_var(LRD,1, gripper_socket)",
1122
+ " rq_set_var(LBL,1, gripper_socket)",
1123
+ " rq_set_var(LGN,0, gripper_socket)",
1124
+ "end",
1125
+ "",
1126
+ "############################################",
1127
+ "# mm/inches functions",
1128
+ "############################################",
1129
+ "gripper_closed_norm = [100, 100, 100, 100]",
1130
+ "gripper_open_norm = [0, 0, 0, 0]",
1131
+ "gripper_closed_mm = [0, 0, 0, 0]",
1132
+ "gripper_open_mm = [50, 50, 50, 50]",
1133
+ "",
1134
+ "def rq_current_pos_mm(gripper_socket=1):",
1135
+ " pos_gripper = rq_current_pos(gripper_socket)",
1136
+ " pos_mm = gripper_to_mm(pos_gripper, gripper_socket)",
1137
+ " return round_value_2_dec(pos_mm)",
1138
+ "end",
1139
+ "",
1140
+ "def rq_current_pos_inches(gripper_socket=1):",
1141
+ " pos_gripper = rq_current_pos(gripper_socket)",
1142
+ " pos_mm = gripper_to_mm(pos_gripper, gripper_socket)",
1143
+ " pos_in = pos_mm / 25.4",
1144
+ " return round_value_2_dec(pos_in)",
1145
+ "end",
1146
+ "",
1147
+ "def rq_move_mm(pos_mm, gripper_socket=1):",
1148
+ " pos_gripper = mm_to_gripper(pos_mm, gripper_socket)",
1149
+ " rq_move(pos_gripper, gripper_socket)",
1150
+ "end",
1151
+ "",
1152
+ "def rq_move_and_wait_mm(pos_mm, gripper_socket=1):",
1153
+ " pos_gripper = mm_to_gripper(pos_mm, gripper_socket)",
1154
+ " rq_move_and_wait(pos_gripper, gripper_socket)",
1155
+ "end",
1156
+ "",
1157
+ "def rq_move_inches(pos_in, gripper_socket=1):",
1158
+ " pos_mm = pos_in * 25.4",
1159
+ " rq_move_mm(pos_mm, gripper_socket)",
1160
+ "end",
1161
+ "",
1162
+ "def rq_move_and_wait_inches(pos_in, gripper_socket=1):",
1163
+ " pos_mm = pos_in * 25.4",
1164
+ " rq_move_and_wait_mm(pos_mm, gripper_socket)",
1165
+ "end",
1166
+ "",
1167
+ "def get_closed_norm(gripper_socket):",
1168
+ " return gripper_closed_norm[gripper_socket - 1]",
1169
+ "end",
1170
+ "",
1171
+ "def get_open_norm(gripper_socket):",
1172
+ " return gripper_open_norm[gripper_socket - 1]",
1173
+ "end",
1174
+ "",
1175
+ "def get_closed_mm(gripper_socket):",
1176
+ " return gripper_closed_mm[gripper_socket - 1]",
1177
+ "end",
1178
+ "",
1179
+ "def get_open_mm(gripper_socket):",
1180
+ " return gripper_open_mm[gripper_socket - 1]",
1181
+ "end",
1182
+ "",
1183
+ "def set_closed_norm(closed_norm, gripper_socket):",
1184
+ " gripper_closed_norm[gripper_socket - 1] = closed_norm",
1185
+ "end",
1186
+ "",
1187
+ "def set_open_norm(open_norm, gripper_socket):",
1188
+ " gripper_open_norm[gripper_socket - 1] = open_norm",
1189
+ "end",
1190
+ "",
1191
+ "def set_closed_mm(closed_mm, gripper_socket):",
1192
+ " gripper_closed_mm[gripper_socket - 1] = closed_mm",
1193
+ "end",
1194
+ "",
1195
+ "def set_open_mm(open_mm, gripper_socket):",
1196
+ " gripper_open_mm[gripper_socket - 1] = open_mm",
1197
+ "end",
1198
+ "",
1199
+ "def gripper_to_mm(value_gripper, gripper_socket):",
1200
+ " closed_norm = get_closed_norm(gripper_socket)",
1201
+ " open_norm = get_open_norm(gripper_socket)",
1202
+ " closed_mm = get_closed_mm(gripper_socket)",
1203
+ " open_mm = get_open_mm(gripper_socket)",
1204
+ "",
1205
+ " value_norm = (value_gripper / 255) * 100",
1206
+ "",
1207
+ " slope = (closed_mm - open_mm) / (closed_norm - open_norm)",
1208
+ " value_mm = slope * (value_norm - closed_norm) + closed_mm",
1209
+ "",
1210
+ " if (value_mm > open_mm):",
1211
+ " value_mm_limited = open_mm",
1212
+ " elif (value_mm < closed_mm):",
1213
+ " value_mm_limited = closed_mm",
1214
+ " else:",
1215
+ " value_mm_limited = value_mm",
1216
+ " end",
1217
+ "",
1218
+ " return value_mm_limited",
1219
+ "end",
1220
+ "",
1221
+ "def mm_to_gripper(value_mm, gripper_socket):",
1222
+ " closed_norm = get_closed_norm(gripper_socket)",
1223
+ " open_norm = get_open_norm(gripper_socket)",
1224
+ " closed_mm = get_closed_mm(gripper_socket)",
1225
+ " open_mm = get_open_mm(gripper_socket)",
1226
+ "",
1227
+ " slope = (closed_norm - open_norm) / (closed_mm - open_mm)",
1228
+ " value_norm = (value_mm - closed_mm) * slope + closed_norm",
1229
+ " value_gripper = value_norm * 255 / 100",
1230
+ "",
1231
+ " if (value_gripper > 255):",
1232
+ " value_gripper_limited = 255",
1233
+ " elif (value_gripper < 0):",
1234
+ " value_gripper_limited = 0",
1235
+ " else:",
1236
+ " value_gripper_limited = round_value(value_gripper)",
1237
+ " end",
1238
+ "",
1239
+ " return value_gripper_limited",
1240
+ "end",
1241
+ "",
1242
+ "def round_value(value):",
1243
+ " value_mod = value % 1",
1244
+ "",
1245
+ " if(value_mod < 0.5):",
1246
+ " return floor(value)",
1247
+ " else:",
1248
+ " return ceil(value)",
1249
+ " end",
1250
+ "end",
1251
+ "",
1252
+ "def round_value_2_dec(value):",
1253
+ " value_x_100 = value * 100",
1254
+ " value_x_100_rounded = round_value(value_x_100)",
1255
+ " return value_x_100_rounded / 100",
1256
+ "end",
1257
+ "",
1258
+ "def clear_socket_buffer(gripper_socket=" + QUOTATION + "1" + QUOTATION + ", read_timeout = 0.1):",
1259
+ " byte_in_buffer = socket_read_byte_list(1, gripper_socket, read_timeout)",
1260
+ "",
1261
+ " while(byte_in_buffer[0] >= 1):",
1262
+ " byte_in_buffer = socket_read_byte_list(1, gripper_socket, read_timeout)",
1263
+ " end",
1264
+ "end",
1265
+ "",
1266
+ "def scale(value, rawRange, scaledRange):",
1267
+ " def computeSlope(inputRange, outputRange):",
1268
+ " outputRangeDelta = outputRange[1] - outputRange[0]",
1269
+ " inputRangeDelta = inputRange[1] - inputRange[0]",
1270
+ "",
1271
+ " if (inputRangeDelta == 0):",
1272
+ " return 0",
1273
+ " else:",
1274
+ " return outputRangeDelta / inputRangeDelta",
1275
+ " end",
1276
+ " end",
1277
+ "",
1278
+ " def computeIntercept(slope, inputRange, outputRange):",
1279
+ " return outputRange[0] - (slope * inputRange[0])",
1280
+ " end",
1281
+ "",
1282
+ " def clipScaledValue(outputScaledValue, outputRange):",
1283
+ " if (outputRange[0] < outputRange[1]):",
1284
+ " return clipWhenLowerLimitIsLessThanHigher(outputScaledValue, outputRange)",
1285
+ " else:",
1286
+ " return clipWhenLowerLimitIsGreaterThanHigherLimit(outputScaledValue, outputRange)",
1287
+ " end",
1288
+ " end",
1289
+ "",
1290
+ " def clipWhenLowerLimitIsGreaterThanHigherLimit(outputScaledValue, outputRange):",
1291
+ " if (outputScaledValue < outputRange[1]):",
1292
+ " return outputRange[1]",
1293
+ " elif (outputScaledValue > outputRange[0]):",
1294
+ " return outputRange[0]",
1295
+ " else:",
1296
+ " return outputScaledValue",
1297
+ " end",
1298
+ " end",
1299
+ "",
1300
+ " def clipWhenLowerLimitIsLessThanHigher(outputScaledValue, outputRange):",
1301
+ " if (outputScaledValue < outputRange[0]):",
1302
+ " return outputRange[0]",
1303
+ " elif (outputScaledValue > outputRange[1]):",
1304
+ " return outputRange[1]",
1305
+ " else:",
1306
+ " return outputScaledValue",
1307
+ " end",
1308
+ " end",
1309
+ "",
1310
+ " slope = computeSlope(rawRange, scaledRange)",
1311
+ " intercept = computeIntercept(slope, rawRange, scaledRange)",
1312
+ " scaledValue = slope * value + intercept",
1313
+ " return clipScaledValue(scaledValue, scaledRange)",
1314
+ "end",
1315
+ "",
1316
+ "def limit(value, range):",
1317
+ " return scale(value, range, range)",
1318
+ "end",
1319
+ "",
1320
+ "rq_obj_detect = 0",
1321
+ "#set_tool_voltage(24)",
1322
+ "#set_tool_communication(True, 115200, 0, 1, 1.5, 3.5)",
1323
+ "rq_wait_for_gripper_connected()",
1324
+ "rq_init_comm_if_connected(9, " + QUOTATION + "1" + QUOTATION + ")",
1325
+ "rq_init_comm_if_connected(2, " + QUOTATION + "2" + QUOTATION + ")",
1326
+ "rq_init_comm_if_connected(3, " + QUOTATION + "3" + QUOTATION + ")",
1327
+ "rq_init_comm_if_connected(4, " + QUOTATION + "4" + QUOTATION + ")",
1328
+ "rq_print_connected_grippers()",
1329
+ "connectivity_checked = [-1,-1,-1,-1]",
1330
+ "status_checked = [-1,-1,-1,-1]",
1331
+ "current_speed = [-1,-1,-1,-1]",
1332
+ "current_force = [-1,-1,-1,-1]",
1333
+ "set_closed_norm(100.0, 1)",
1334
+ "set_open_norm(0.0, 1)",
1335
+ "set_closed_mm(0.0, 1)",
1336
+ "set_open_mm(50.0, 1)",
1337
+ "set_closed_norm(100.0, 2)",
1338
+ "set_open_norm(0.0, 2)",
1339
+ "set_closed_mm(0.0, 2)",
1340
+ "set_open_mm(50.0, 2)",
1341
+ "set_closed_norm(100.0, 3)",
1342
+ "set_open_norm(0.0, 3)",
1343
+ "set_closed_mm(0.0, 3)",
1344
+ "set_open_mm(50.0, 3)",
1345
+ "set_closed_norm(100.0, 4)",
1346
+ "set_open_norm(0.0, 4)",
1347
+ "set_closed_mm(0.0, 4)",
1348
+ "set_open_mm(50.0, 4)",
1349
+ "",
1350
+ ])
1351
+ """Full Robotiq URScript preamble.
1352
+
1353
+ This constant contains all ``rq_*`` function definitions and initialization
1354
+ code. It must be prepended to every ``sendCustomScriptFunction`` call so
1355
+ the robot has the function definitions and a live socket context.
1356
+ """