def extend_graph(vertex_rand, robot_root, obstacles, goal_set, pywindow, color_, k, x, goal): # add vertex and edges to sets for robot i
if k == 1:
add_to_kd_tree(goal, robot_root, x)
new_paths = False # initialize
if k % 2 == 0 or k > 100: # nearest is from tree1 from root
tree_num = 1
first = robot_root
else: # nearest is from tree2 from goal (two trees growing towards each other for increased path variety)
tree_num = 2
first = goal
vertex_nearest2 = nearest2(vertex_rand, robot_root, robot_root, first, x, tree_num)
vertex_new2 = steer4(vertex_nearest2, vertex_rand, goal_set, tree_num)
collision_, intersect = collisions(vertex_nearest2, vertex_new2, obstacles, 'obstacles')
if collision_ == 0:
if tree_num == 1:
add_edge(vertex_new2, vertex_nearest2, color_, 1, pywindow) # edge direction depends on which tree nearest vertex is in
else:
add_edge(vertex_nearest2, vertex_new2, Colors.dark_red, 1, pywindow) # edge direction depends on which tree nearest vertex is in
vertices_near2, radius = near_vertices2(vertex_new2, robot_root, robot_root, k, [], x, tree_num) # computationally efficient function
for i in range(len(vertices_near2)): # for all near vertices
if vertices_near2[i] != vertex_nearest2: # since already added nearest
collision_, intersect = collisions(vertices_near2[i], vertex_new2, obstacles, 'obstacles')
if collision_ == 0: # if no collision
if tree_num == 1:
add_edge(vertex_new2, vertices_near2[i], color_, 1, pywindow) # edge direction depends on which tree near vertex is in
else:
add_edge(vertices_near2[i], vertex_new2, Colors.dark_red, 1, pywindow) # edge direction depends on which tree near vertex is in
add_to_kd_tree(vertex_new2, robot_root, x) # kd tree for spatial sorting and faster nearest/near searching
if tree_num == 1:
tree_num_other = 2
first = goal
else:
tree_num_other = 1
first = robot_root
vertex_nearest_opposite_tree = nearest2(vertex_new2, robot_root, robot_root, first, x, tree_num_other)
if norm(vertex_new2, vertex_nearest_opposite_tree) <= radius:
new_paths = True
if tree_num == 1: # if new point is in tree 1
add_edge(vertex_nearest_opposite_tree, vertex_new2, color_, 1, pywindow) # new point is parent in edge between trees
else: # if new point is in tree 2
add_edge(vertex_new2, vertex_nearest_opposite_tree, Colors.dark_red, 1, pywindow) # new point is child in edge between trees
else:
del vertex_new2 # save storage if point not used
vertex_new2 = None
return vertex_new2, new_paths
def extend_graph(vertex_rand, robot_root, obstacles, goal_set, pywindow,
color_, k, x): # add vertex and edges to sets for robot i
# vertex_nearest = nearest(vertex_rand, robot_root, robot_root, k) # find nearest vertex in robot i's graph, starting with root
vertex_nearest2 = nearest2(vertex_rand, robot_root, robot_root, robot_root,
x) # computationally efficient function
# vertex_new = steer(vertex_nearest, vertex_rand, goal_set) # steer that vertex towards x_rand within some radius
vertex_new2 = steer4(vertex_nearest2, vertex_rand, goal_set)
collision_, intersect = collisions(vertex_nearest2, vertex_new2, obstacles,
'obstacles')
if collision_ == 0:
# if vertex_new2.at_goal_set:
# print 'no obstacle collision for goal set vertex, added to kd tree'
# vertices_near = near_vertices(vertex_new2, robot_root, k, [])
vertices_near2 = near_vertices2(
vertex_new2, robot_root, robot_root, k, [],
x) # computationally efficient function
# if vertex_new2.at_goal_set:
# print 'near verts found:', len(vertices_near2)
edges_added = 0
for i in range(len(vertices_near2)): # for all near vertices
collision_, intersect = collisions(vertices_near2[i], vertex_new2,
obstacles, 'obstacles')
if collision_ == 0: # if no collision
# if vertex_new2.at_goal_set:
# print 'no collision for near vertex, adding edge'
add_edge(vertex_new2, vertices_near2[i], color_, 1,
pywindow) # connect the dots
edges_added = edges_added + 1
else:
pass # print 'collision with near vertex'
if edges_added == 0:
# print 'no edge had been added'
del vertex_new2
return None
else:
add_to_kd_tree(
vertex_new2, robot_root, x
) # kd tree for spatial sorting and faster nearest/near searching
else:
# print 'collision, extend procedure returning None'
del vertex_new2 # save storage if point not used
return None
return vertex_new2