def form_trees_and_paths(): # this is the high-level of the algorithm as described at the source linked above
pywindow, obstacles, axis, buttons = init_pywindow(
'i-Nash Policy 1') # set up pygame window, dimensions and obstacles
start, goal_set, num_robots, robo_colors, sign = user_prompt(pywindow) # prompt for num bots, start, end positions
all_bots, active_bots, inactive_bots, paths, costs, paths_prev, goal_pts, path_num = init_arrays(num_robots)
k = 1; k_ = 4 * 75000 # total attempted vertices for each robot / 4
samp_bias = 0 # for biased sampling. See sample_free()
while k < k_: # main loop
new_vertices = [None] * num_robots # get list of new vertices each k iteration
for i in all_bots: # for all robots
vert_rand, samp_bias = sample_free(paths[i], goal_set[i], samp_bias,
sign[i]) # get random Vertex in pywindow
vert_new = extend_graph(vert_rand, start[i], obstacles, goal_set[i], pywindow, robo_colors[i], k, axis)
goal_pts, new_vertices = update_pt_lists(vert_new, goal_pts, new_vertices, i)
k = iterate_or_stop(pywindow, buttons, k, k_) # call this now as scrappy way of seeming more asynchronous
for i in inactive_bots:
check_active(new_vertices, i, active_bots, inactive_bots)
for i in active_bots:
paths_prev[i] = list(paths[i]) # save previous path list before updating list
k = iterate_or_stop(pywindow, buttons, k, k_) # call this now as scrappy way of seeming more asynchronous
for i in active_bots:
perform_better_response(i, active_bots, paths_prev, paths, costs, pywindow, new_vertices, goal_pts,
robo_colors)
k = iterate_or_stop(pywindow, buttons, k, k_) # call this now as scrappy way of seeming more asynchronous
# time.sleep(.05)
return pywindow, buttons, active_bots, paths, costs, robo_colors
def main():
pywindow, obstacles, axis = init_pywindow(
'i-Nash Trajectory Final 3'
) # set up pygame window, dimensions and obstacles
start, goal_set, num_robots, robo_colors = user_prompt(
pywindow) # prompt for num bots, start, end positions
all_bots, active_bots, inactive_bots, paths, costs, paths_prev, goal_pts, path_num = init_arrays(
num_robots)
k = 1
k_ = 75000
while k < k_: # main loop
new_vertices = [
None
] * num_robots # get list of new vertices each k iteration
for i in all_bots: # for all robots
vertex_rand = sample_free(
paths[i], goal_set[i]) # get random Vertex in pywindow
vertex_new = extend_graph(vertex_rand, start[i], obstacles,
goal_set[i], pywindow, robo_colors[i], k,
axis)
goal_pts, new_vertices = update_pt_lists(vertex_new, goal_pts,
new_vertices, i)
for i in inactive_bots:
if new_vertices[i] is not None:
if new_vertices[
i].at_goal_set: # if previously inactive bot is now active
update_active(
active_bots, inactive_bots,
i) # then update the inactive and active lists
print 'robot', i, 'became active'
for i in active_bots:
paths_prev[i] = list(
paths[i]) # save previous path list before updating list
for q in range(len(
active_bots)): # use q for easier j < i and j2 > i calculation
perform_better_response(q, active_bots, paths_prev, paths, costs,
pywindow, new_vertices, goal_pts,
robo_colors)
#time.sleep(.05)
k = k + 1
print 'main loop exited'
"""repeat = False
for i in range(num_robots): # for all bots
for j in range(len(goal_pts[i])): # for all goal pts for that bot
for P in range(len(goal_pts[i][j].paths)): # for all points for that goal pt
string = []
string.append(goal_pts[i][j].costs[P])
for F in range(len(goal_pts[i][j].paths[P])):
string.append(floor(goal_pts[i][j].paths[P][F].x))
string.append(floor(goal_pts[i][j].paths[P][F].y))
#print 'vertex.paths for all vertices in path', P, ':', goal_pts[i][j].paths[P][F].paths
for F2 in range(len(goal_pts[i][j].paths[P])):
if goal_pts[i][j].paths[P][F] == goal_pts[i][j].paths[P][F2]:
repeat = True
#print 'robot', i, ', goalpt', j, ',path', P, 'cost', string
display_path(goal_pts[i][j].paths[P], pywindow, Colors.dark_green) # display
#print 'repeat is ', repeat"""
return
def form_trees_and_paths(): # this is the high-level of the algorithm as described at the source linked above
pywindow, obstacles, axis, buttons = init_pywindow('i-Nash Policy 1') # set up pygame window, dimensions and obstacles
start, goal_set, num_robots, robo_colors, sign, goal = user_prompt(pywindow) # prompt for num bots, start, end positions
print 'start', start[0].x, start[0].y, 'goal', goal[0].x, goal[0].y
all_bots, active_bots, inactive_bots, paths, costs, paths_prev, goal_pts, path_num, new_paths, time_to_path = init_arrays(num_robots)
k = 1; k_ = 100000 # total attempted vertices for each robot (if user never clicks end planning)
samp_bias = 0 # for biased sampling. See sample_free()
start_time = time.time()
while k < k_: # main loop
#if k == 100 or k == 500 or k == 750 or k == 1000 or k == 1200 or k == 1500:
print k
k_end = k
new_vertices = [None] * num_robots # get list of new vertices each k iteration
for i in all_bots: # for all robots
vert_rand, samp_bias = sample_free(paths[i], goal_set[i], samp_bias, sign[i]) # get random Vertex in pywindow
vert_new, new_paths_ = extend_graph(vert_rand, start[i], obstacles, # use procedure similar to RRG, with dual-tree
goal_set[i], pywindow, robo_colors[i],
k, axis, goal[i])
goal_pts, new_vertices, new_paths = update_pt_lists(vert_new, goal_pts,
new_vertices, i, new_paths_,
new_paths)
k = iterate_or_stop(pywindow, buttons, k, k_, False) # call this now as scrappy way of seeming more asynchronous
for i in inactive_bots:
check_active(new_vertices, i, active_bots, inactive_bots, new_paths) # see if any inactive bots have become active
for i in active_bots:
paths_prev[i] = list(paths[i]) # save previous path list before updating list
k = iterate_or_stop(pywindow, buttons, k, k_, False) # call this again as scrappy way of seeming more asynchronous
for i in active_bots:
perform_better_response(i, active_bots, paths_prev, paths, costs, pywindow, # check inter-robot collisions, choose low cost paths
new_vertices, goal_pts, robo_colors, new_paths[i],
goal[i], start[i], time_to_path)
k = iterate_or_stop(pywindow, buttons, k, k_, False) # call this again as scrappy way of seeming more asynchronous
k = iterate_or_stop(pywindow, buttons, k, k_, True)
draw_obstacles(pywindow, obstacles, Colors.dark_blue)
pygame.display.flip()
#time.sleep(.05) # slows overall tree growth, can be useful for some debugging
time_waited = calculate_times(time_to_path, start_time)
#display_paths(goal[0].paths, pywindow, Colors.black)
#for i in range(len(goal_pts[0])):
#display_paths(goal_pts[0][i].paths, pywindow, Colors.black)
return pywindow, buttons, active_bots, paths, costs, robo_colors, k_end, time_to_path, time_waited, obstacles
def form_trees_and_paths(
pywindow, obstacles, axis, buttons, start, goal_set, num_robots,
robo_colors, sign, goal, all_bots, active_bots, inactive_bots, paths,
costs, paths_prev, goal_pts, path_num, new_paths, time_to_path, changed
): # this is the high-level of the algorithm as described at the source linked above
print 'in form trees and paths. printing all input things:', start, goal_set, num_robots,\
goal, all_bots, active_bots, inactive_bots, paths, costs, paths_prev,\
goal_pts, path_num, new_paths, time_to_path, changed
k = 1
k_ = 200000 # total attempted vertices for each robot (if user never clicks end planning)
samp_bias = 0 # for biased sampling. See sample_free()
start_time = time.time()
nash_success = True
while k < k_: # main loop
k_end = k
new_vertices = [
None
] * num_robots # get list of new vertices each k iteration
for i in all_bots: # for all robots
vert_rand, samp_bias = sample_free(
paths[i], goal_set[i], samp_bias,
sign[i]) # get random Vertex in pywindow
vert_new, new_paths_ = extend_graph(
vert_rand,
start[i],
obstacles, # use procedure similar to RRG, with dual-tree Connect
goal_set[i],
pywindow,
robo_colors[i],
k,
axis,
goal[i])
goal_pts, new_vertices, new_paths = update_pt_lists(
vert_new, goal_pts, new_vertices, i, new_paths_, new_paths)
k = iterate_or_stop(
pywindow, buttons, k, k_, False, time_to_path
) # call this now as scrappy way of seeming more asynchronous
for i in inactive_bots:
check_active(
new_vertices, i, active_bots, inactive_bots,
new_paths) # see if any inactive bots have become active
for i in active_bots:
paths_prev[i] = list(
paths[i]) # save previous path list before updating list
k = iterate_or_stop(
pywindow, buttons, k, k_, False, time_to_path
) # call this again as scrappy way of seeming more asynchronous
for i in active_bots:
perform_better_response(
i,
active_bots,
paths_prev,
paths,
changed,
costs,
pywindow, # check inter-robot collisions, choose low cost paths
new_vertices,
goal_pts,
robo_colors,
new_paths[i],
goal[i],
start[i],
time_to_path)
k = iterate_or_stop(
pywindow, buttons, k, k_, False, time_to_path
) # call this again as scrappy way of seeming more asynchronous
k = iterate_or_stop(pywindow, buttons, k, k_, True, time_to_path)
if time.time() - start_time > 850:
k = k_
nash_success = False
time_waited = calculate_times(time_to_path, start_time)
#display_paths(goal[0].paths, pywindow, Colors.black)
num_paths = [None] * num_robots
for j in range(len(goal)):
num_paths[j] = len(goal[j].paths)
for j in range(len(goal_pts)):
for i in range(len(goal_pts[j])):
num_paths[j] = num_paths[j] + len(goal_pts[j][i].paths)
print 'num paths for all bots', num_paths
return pywindow, buttons, active_bots, paths, costs, robo_colors, k_end, time_to_path, time_waited, num_robots, nash_success