-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathmain.py
269 lines (220 loc) · 10.8 KB
/
main.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
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
# 3D PICK AND PLACE SIMULATION
import numpy as np
import logging
from math import *
from matplotlib import pyplot as plt
from utils import init_fonts, get_nearest_object, add_obstacle, dump_graphics
from obstacles import Static_Obstacle
from objects import Object
from arm import Arm
from arm_state_machine import ArmStateMachine, ArmState
from velocity_control import *
from constants import *
import random
from numpy.linalg import norm
logger = logging.getLogger(__name__)
logging.basicConfig()
logger.setLevel(logging.INFO)
### PARAMETERS ###
show_RRT = False
### Objects ###
def generate_objects():
obj_list = []
obj_count = 0
# range: x E [-0.5, 2.0], y E [-2.5, -2.5], z = 0.33
while obj_count < 6:
obj_x = random.uniform(-0.5, 1.0)
obj_y = random.uniform(-1.5, 1.5)
obj_z = 0.33
pos = [obj_x, obj_y, obj_z]
print(pos)
# if object not within 0.3 dist of other obj, add to list
if np.any(obj_list):
valid = True
for obj in obj_list:
if euclidean_distance(obj.get_position(), pos) < OBJ_SPACING:
valid = False
if valid:
new_obj = Object(name="OBJ{}".format(obj_count+1), position=pos)
obj_list.append(new_obj)
obj_count += 1
else:
new_obj = Object(name="OBJ{}".format(obj_count+1), position=pos)
obj_list.append(new_obj)
obj_count += 1
for obj in obj_list: print("{}: {}".format(obj.get_name(), obj.get_position()))
# original sim:
# OBJ1 = Object(name="OBJ1", position=[0.0, -1.8, 0.33])
# OBJ2 = Object(name="OBJ2", position=[0.5, 0.0, 0.33])
# OBJ3 = Object(name="OBJ3", position=[1.5, 1.8, 0.33])
# OBJ4 = Object(name="OBJ4", position=[1.0, -2.0, 0.33])
# OBJ5 = Object(name="OBJ3", position=[1.8, 1.8, 0.33])
# OBJ6 = Object(name="OBJ4", position=[2.0, -2.0, 0.33])
# obj_list = [OBJ1, OBJ2, OBJ3, OBJ4, OBJ5, OBJ6]
return obj_list
def add_objects(ax, objects):
# bowl
ax.scatter3D(BOWL[0], BOWL[1], BOWL[2], color='red', s=600, alpha=1)
# home pos
ax.scatter3D(ARM1_HOME_POS[0], ARM1_HOME_POS[1], ARM1_HOME_POS[2], color='blue', s=100, alpha=1)
ax.scatter3D(ARM2_HOME_POS[0], ARM2_HOME_POS[1], ARM2_HOME_POS[2], color='green', s=100, alpha=1)
for obj in objects:
# set color
color = random.choices(COLORS, k=1)
COLORS.remove(color[0])
print(color[0])
pos = obj.get_position()
obj_plot = ax.scatter3D(pos[0], pos[1], pos[2], color=color[0], s=70, alpha=1)
obj.set_color(color[0])
obj.set_plot(obj_plot)
return objects
def main():
### SET UP ENV ###
init_fonts()
fig = plt.figure(figsize=(10,10))
ax = plt.axes(projection='3d')
ax.set_xlabel('X, [cm*10]')
ax.set_ylabel('Y, [cm*10]')
ax.set_zlabel('Z, [cm*10]')
ax.set_xlim([-3, 3])
ax.set_ylim([-3, 3])
ax.set_zlim([0.0, 4])
mngr = plt.get_current_fig_manager()
# win in the lower right corner:
mngr.window.wm_geometry("+500+0")
### SET UP STATIC OBSTACLES AND BOWL ###
temp_obstacles = []
obstacles = []
for pose, dim in zip(OBSTACLE_POSES, OBSTACLE_DIMS):
obstacles = add_obstacle(obstacles, pose, dim)
for obstacle in obstacles: obstacle.draw(ax)
obj_list = generate_objects()
objects = add_objects(ax, obj_list)
obj1, obj2 = None, None
arm1 = Arm(name="BLUE", home=ARM1_HOME_POS, position=ARM1_HOME_POS, destination=ARM1_HOME_POS, velocity=INIT_VEL, color='blue')
arm2 = Arm(name="GREEN", home=ARM2_HOME_POS, position=ARM2_HOME_POS, destination=ARM2_HOME_POS, velocity=INIT_VEL, color='green')
arm1_sm = ArmStateMachine(ax, obstacles, arm1, obj1, BOWL)
arm2_sm = ArmStateMachine(ax, obstacles, arm2, obj2, BOWL)
logger.info("Pick and Place Simulation Start")
# default
arm1_collision = np.empty(3)
arm2_collision = np.empty(3)
# plt.show()
#### MAIN LOOP ####
while (arm1_sm.state != ArmState.DONE) or (arm2_sm.state != ArmState.DONE):
# if arm done picking last obj (full cycle), set next obj
# get object nearest to each arm and set each arm sm
if arm1_sm.pick_ready:
obj1 = get_nearest_object(objects, arm1.get_position())
arm1_sm.set_object(obj1)
if obj1 != None:
objects.remove(obj1)
logger.info("ASSIGNING {} TO {}".format(arm1.get_name(), obj1.get_name()))
if arm2_sm.pick_ready:
obj2 = get_nearest_object(objects, arm2.get_position())
arm2_sm.set_object(obj2)
if obj2 != None:
objects.remove(obj2)
logger.info("ASSIGNING {} TO {}".format(arm2.get_name(), obj2.get_name()))
arm1_sm.run_once()
arm2_sm.run_once()
# if stop count > 5, possible deadlock, set temp obstacle and replan
if arm1_sm.stop_count > 5 or arm2_sm.stop_count > 5:
logger.info("OH NO, WE HAVE A DEADLOCK!! >:(")
logger.info("ARM POS: {} {}, {} {}".format(arm1.get_name(), arm1.get_position(), arm2.get_name(), arm2.get_position()))
logger.info("ARM DIST: {}".format(euclidean_distance(arm1.get_position(), arm2.get_position())))
# make fast arm replan path
if arm1.get_velocity() != INIT_VEL:
arm2_sm.state = ArmState.PLANNING # replan fast arm
arm2_sm.compute_path = True
slow_arm_pos = arm1.get_position()
elif arm2.get_velocity() != INIT_VEL:
arm1_sm.state = ArmState.PLANNING # replan fast arm
arm1_sm.compute_path = True
slow_arm_pos = arm2.get_position()
# create temp obstacle box around slow arm:
logger.info("Setting Temp Obstacle at: {}".format(slow_arm_pos))
obstacles = add_obstacle(obstacles, pose=slow_arm_pos, dim=TEMP_OBS)
temp_obstacles.append(obstacles[-1])
obstacles[-1].draw(ax)
# reset stop counts
arm1_sm.stop_count = 0
arm2_sm.stop_count = 0
path1 = arm1_sm.get_path() # make sure that paths are already generated
path2 = arm2_sm.get_path()
# critical stop if arms in eachother safety zone
# TODO: delete? not needed?
if euclidean_distance(arm1.get_position(), arm2.get_position()) <= 0.3:
# pause slow arm
if arm1.get_velocity() != INIT_VEL:
logger.info("{} CRITICAL STOP!!".format(arm1.get_name()))
arm1_sm.pause = True
else:
logger.info("{} CRITICAL STOP!!".format(arm2.get_name()))
arm2_sm.pause = True
else:
arm1_sm.pause = False
arm2_sm.pause = False
# check for intersection
if arm1_sm.check_collisions or arm2_sm.check_collisions:
logger.info("Checking Collisions...")
# if one arm's path not computed, simply use current pos to compare
if path1.shape[0] == 0: path1 = np.array([arm1.get_position()])
if path2.shape[0] == 0: path2 = np.array([arm2.get_position()])
#### CHECK COLLISIONS ####
intersect_pts1, intersect_pts2 = find_intersection(path1, path2)
# plot collision points
for col_pt in intersect_pts1:
point, = ax.plot(col_pt[0], col_pt[1], col_pt[2], 'o', color='cyan')
arm1_sm.set_temp_graphics(point)
for col_pt in intersect_pts2:
point, = ax.plot(col_pt[0], col_pt[1], col_pt[2], 'o', color='cyan')
arm2_sm.set_temp_graphics(point)
# if collision detected, adjust path velocities
if (intersect_pts1.shape[0] != 0) and (intersect_pts2.shape[0] != 0):
logger.info("COLLISION DETECTED!")
logger.debug("INTERSECTIONS 1: {}, SHAPE: {}".format(intersect_pts1, intersect_pts1.shape[0]))
logger.debug("INTERSECTIONS 2: {}, SHAPE: {}".format(intersect_pts2, intersect_pts2.shape[0]))
# set collision point as first or last collision point in intersection pts
if RESET_VELOCITY_AT == ResetPoint.LAST_POINT:
arm1_collision = intersect_pts1[-1,:]
arm2_collision = intersect_pts2[-1,:]
elif RESET_VELOCITY_AT == ResetPoint.FIRST_POINT:
arm1_collision = intersect_pts1[0,:]
arm2_collision = intersect_pts2[0,:]
logger.info("COLLISION POINTS: {}, {}".format(arm1_collision, arm2_collision))
# update current path ONLY to first OR last collision point, keep initial path post collision pt
path1_col_idx = np.where(path1 == arm1_collision)[0][0]
path2_col_idx = np.where(path2 == arm2_collision)[0][0]
# choose whether to speed up arm nearest or furthest to goal
# update paths such that speed is inc/dec until collision point only
new_path1, new_path2, = adjust_arm_velocity(path1, path2, path1_col_idx, path2_col_idx, arm1, arm2)
logger.info("ARM VEL: {}, {}".format(arm1.get_velocity(), arm2.get_velocity()))
else:
#### CHECK COMMON GOAL ####
if euclidean_distance(path1[-1], path2[-1]) <= COLLISION_RANGE:
logger.info("ARMS MOVING TOWARD COMMON GOAL {}, {}".format(path1[-1], path2[-1]))
point = ax.scatter3D(path1[-1,0], path1[-1,1], path1[-1,2], color='cyan', s=100)
arm1_sm.set_temp_graphics(point)
point = ax.scatter3D(path2[-1,0], path2[-1,1], path2[-1,2], color='cyan', s=100)
arm2_sm.set_temp_graphics(point)
new_path1, new_path2 = common_goal_collision(path1, path2, arm1, arm2)
else:
# no collision, or reset paths velocities if collision avoided
logger.info("NO COLLISION DETECTED!")
new_path1, new_path2 = update_velocity(path1, path2, vel=INIT_VEL)
arm1.set_velocity(INIT_VEL)
arm2.set_velocity(INIT_VEL)
arm1_collision, arm2_collision = np.empty(3), np.empty(3)
# if previously deadlocked, remove temp obstacles
for obstacle in temp_obstacles:
obstacles.remove(obstacle)
temp_obstacles = []
# set new path, collision point, and slow arm bool
arm1_sm.set_path(new_path1, arm1_collision)
arm2_sm.set_path(new_path2, arm2_collision)
plt.pause(PAUSE_TIME)
logger.info("Pick and Place Simulation End")
plt.show()
if __name__ == '__main__':
main()