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 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