-
Notifications
You must be signed in to change notification settings - Fork 2
/
iros_5.py
123 lines (88 loc) · 3.85 KB
/
iros_5.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
import time
import copy
import math
import cv2
import imutils
from matplotlib import pyplot as plt
import iros_interface_cmds as ic
import iros_waypoints as iw
#import vision_copy as vc
# Pre Determined stuff
act_usb=78
pos_u = [-400, -400] # Location to go to for the USB
height_u = 80
pos_light = [-450, -400] #
height_light = 60 #
act_light= 20
usb1_down = {"x":67.21,"y":-107.33,"z":135.22,"rx":-127.32,"ry":-118.51,"rz":41.32}
usb1_up = {"x":67.21,"y":-110.09,"z":131.14,"rx":-120.49,"ry":-118.49,"rz":41.29}
usb_joints_waypoint = {"x":-44.27,"y":-96.15,"z":104.97,"rx":-34.63,"ry":-46.86,"rz":-207.27}
def begin(c,ser_ee):
obj = raw_input('Light or USB? (l/u)?')
if (obj == "u"):
demand_Grip = dict(iw.ee_home)
demand_Grip["act"]=act_usb
msg = ic.safe_move(c,ser_ee,Pose=dict(iw.home_joints),Grip=demand_Grip,CMD=2)
# Set tool to iros_1
ic.socket_send(c,sCMD=201)
# Close Gripper
msg = ic.end_effector_move(ser_ee,demand_Grip)
msg = ic.safe_ur_move(c,Pose=dict(usb1_down),CMD=2, Speed = 0.2)
# Close Gripper
demand_Grip["servo"]=30
msg = ic.end_effector_move(ser_ee,demand_Grip)
time.sleep(0.5)
raw= raw_input("wait")
current_Pose = ic.get_ur_position(c,1)
demand_Pose = {"x":current_Pose[0],"y":current_Pose[1],"z":current_Pose[2],"rx":current_Pose[3],"ry":current_Pose[4],"rz":current_Pose[5]}
demand_Pose["z"] = demand_Pose["z"] + 30
msg = ic.safe_move(c,ser_ee,Pose=demand_Pose,Grip=demand_Grip,Speed=0.2,CMD=4)
time.sleep(2)
msg = ic.safe_ur_move(c,Pose=dict(usb1_down),CMD=2, Speed = 0.2)
raw= raw_input("wati")
# Open Gripper
demand_Grip["servo"]=120
msg = ic.end_effector_move(ser_ee,demand_Grip)
time.sleep(0.5)
#Pull out
demand_Pose["z"] = height_u + 50
msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4)
time.sleep(5)
#Push in and release
demand_Pose["z"] = height_u - 5
demand_Grip["servo"]=120
msg = ic.safe_move(c,ser_ee,Pose=demand_Pose,Speed=0.25,Grip=demand_Grip,CMD=4)
else:
# Home position
demand_Grip = dict(iw.ee_home)
demand_Grip["act"]=act_light
msg = ic.safe_move(c,ser_ee,Pose=dict(iw.home_joints),Grip=demand_Grip,CMD=2)
# Set tool to iros_1
ic.socket_send(c,sCMD=201)
# Goto position of light
current_Pose = ic.get_ur_position(c,1)
demand_Pose = {"x":pos_light[0], "y":pos_light[1], "z":current_Pose[2], "rx":current_Pose[3], "ry":current_Pose[4], "rz":current_Pose[5]}
msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4)
# Lower to light
demand_Pose["z"] = height_light
msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4)
# Close Gripper
demand_Grip["servo"]=30
msg = ic.end_effector_move(ser_ee,demand_Grip)
time.sleep(0.5)
# Pull out
demand_Pose["z"] = height_light + 50
msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4)
# Push in and release
demand_Pose["z"] = height_light - 5
demand_Grip["servo"]=120
msg = ic.safe_move(c,ser_ee,Pose=demand_Pose,Speed=0.25,Grip=demand_Grip,CMD=4)
# Raise
current_Pose = ic.get_ur_position(c,1)
demand_Pose = {"x":current_Pose[0], "y":current_Pose[1], "z":current_Pose[2]+80, "rx":current_Pose[3], "ry":current_Pose[4], "rz":current_Pose[5]}
msg = ic.safe_ur_move(c,Pose=demand_Pose,CMD=4)
# Set tool to iros_0
ic.socket_send(c,sCMD=200)
# Home position
msg = ic.safe_move(c,ser_ee,Pose=dict(iw.home_joints),CMD=2)
print ".....................Done......................"