def run(obstacles, start, goal, step_size, max_size, plotter):
circ_rad = min(step_size/5, 5)
final_pos = [np.array(goal), np.array(start)]
# Pre-compute for generating new random points in Q_free
minp, maxp = obstacles.vertices.min(0), obstacles.vertices.max(0)
span, offset = (maxp-minp)*1.1, minp-(maxp-minp)*0.05
def gen_valid_rand(valid_function):
""" Generates a valid q_rand in Q_free given a validity function """
tmp = np.random.random(2) * span + offset
while not valid_function(*tmp):
tmp = np.random.random(2) * span + offset
return tmp
KD = [KDTree(start), KDTree(goal)]
RRT = [PathTree(start), PathTree(goal)]
n = 1
rnd_display = False
obstacles = Obstacles(obstacles.to_polygons())
"""
• Expand tree T_1 randomly, add node q_new
• Expand T_2 towards q_new
• If tree T_2 connects to q_new, path formed else add a q_new for tree T_2
• Now expand T_1 to q_new in tree T_2
• Keep swapping T_1 and T_2 for expansion towards the
other tree until they meet
"""
trials = 0
q_new, last_expanded = None, -1
while KD[0].length + KD[1].length < 1000:
trials += 1
if rnd_display: circ1.remove(); rnd_display = False
n = 1 - n
# If the last expanded node was in the other tree, try expanding towards q_new
if last_expanded != n and q_new is not None:
q_near, dist = KD[n].nearestNode(q_new)
q_next = gen_next(q_near, q_new, step_size) if dist>step_size else q_new
# Expansion towards q_new is possible. Add to path and goal check
if obstacles.point_is_valid(*q_next) and \
not obstacles.check_collisions((q_near, q_next)):
RRT[n].addPath(q_near, q_next)
KD[n].addNode(q_next)
plotter.draw_circle(q_next, circ_rad, edgecolor='k', facecolor='w', zorder=1)
plotter.draw_line(q_near, q_next, color='kb'[n], zorder=1)
if q_next == q_new: break # Path found
q_new, last_expanded, trials = q_next, n, 0 # Update for next iteration
continue
# If last expanded node was not in the other tree or expansion to q_new not possible
# Try to expand to q_rand if possible
q_rand = gen_valid_rand(obstacles.point_is_valid) if np.random.randint(0,100)>5 else final_pos[n]
rnd_display, circ1 = True, plotter.draw_circle(q_rand, 5, zorder=5)
q_near, dist = KD[n].nearestNode(q_rand)
if dist < step_size:
if trials < 10: continue
q_next = tuple(q_rand)
else:
q_next = gen_next(q_near, q_rand, step_size)
if not obstacles.point_is_valid(*q_next): continue
if obstacles.check_collisions((q_near, q_next)): continue
KD[n].addNode(q_next)
RRT[n].addPath(q_near, q_next)
plotter.draw_line(q_near, q_next, color='kb'[n], zorder=1)
plotter.draw_circle(q_next, circ_rad, edgecolor='k', facecolor='w', zorder=1)
q_new, last_expanded, near_count = q_next, n, 0
print("n =", KD[0].length + KD[1].length, "(%d, %d)"%(KD[0].length, KD[1].length))
# Plot out goal path
cur = RRT[0][tuple(q_next)]
while cur.parent:
plotter.draw_line(cur, cur.parent, update=False, color='y', zorder=3)
plotter.draw_circle(cur, circ_rad*1.5, update=False, facecolor='xkcd:green', edgecolor='k', zorder=4)
cur = cur.parent
cur = RRT[1][tuple(q_next)]
while cur.parent:
plotter.draw_line(cur, cur.parent, update=False, color='y', zorder=3)
plotter.draw_circle(cur, circ_rad*1.5, update=False, facecolor='xkcd:green', edgecolor='k', zorder=4)
cur = cur.parent
plotter.update()
""" Unit test for line in obstacle """
minp, maxp = path.vertices.min(0), path.vertices.max(0)
step_size = 50
starts = gen_rand(minp, maxp, num_tests)
rands = gen_rand(minp, maxp, num_tests)
# starts, rands = starts.astype(int), rands.astype(int)
dirs = starts - rands
ends = starts + dirs / np.hypot(dirs[:,0], dirs[:,1])[:,None] * step_size
paths = [Path([starts[i], ends[i]], [Path.MOVETO, Path.LINETO]) for i in range(num_tests)]
# Shapely answer
my_ans = [obstacles.check_collisions(paths[i].vertices) for i in range(num_tests)]
ex_ans = [shapely_intersect(paths[i].vertices) for i in range(num_tests)]
correct_positives, correct_negatives = [], []
false_positives, false_negatives = [], []
for i in range(num_tests):
if my_ans[i] and ex_ans[i]: correct_positives.append(i)
elif not my_ans[i] and not ex_ans[i]: correct_negatives.append(i)
elif ex_ans[i] and not my_ans[i]: false_negatives.append(i)
elif my_ans[i] and not ex_ans[i]: false_positives.append(i)
print("Correct : ", len(correct_positives), len(correct_negatives))
print("False : ", len(false_positives), len(false_negatives))
for idx in false_negatives:
print(idx, Line(Point(*paths[idx].vertices[0]), Point(*paths[idx].vertices[1])), my_ans[idx], ex_ans[idx])
def run(obstacles, start, goal, step_size, max_size, plotter):
circ_rad = min(step_size / 5, 5)
final_pos = np.array(goal[:2])
# Pre-compute for generating new random points in Q_free
minp, maxp = obstacles.vertices.min(0), obstacles.vertices.max(0)
span, offset = (maxp - minp) * 1.1, minp - (maxp - minp) * 0.05
def gen_valid_rand(valid_function):
""" Generates a valid q_rand in Q_free given a validity function """
tmp = np.random.random(2) * span + offset
while not valid_function(*tmp):
tmp = np.random.random(2) * span + offset
return tmp
KD = KDTree(start)
RRT = PathTree(start)
circ1 = plotter.draw_circle(start, 1, time=1, zorder=5)
obstacles = Obstacles(obstacles.to_polygons())
trials = 0
while KD.length < max_size:
trials += 1
circ1.remove()
# Select a random point q_rand \in Q_free
q_rand = gen_valid_rand(obstacles.point_is_valid) if np.random.randint(
0, 100) > 5 else final_pos
circ1 = plotter.draw_circle(q_rand, 5, time=0.01, zorder=5)
# Find the nearest node and distance to it
q_near, dist = KD.nearestNode(q_rand)
# Generate the next node in the direction of q_rand
if dist < step_size:
if trials < 10: continue # Prevents step_size too big bug
q_next = tuple(q_rand)
else:
q_next = gen_next(q_near, q_rand, step_size)
if not obstacles.point_is_valid(*q_next): continue
# Check validity and update tree
if obstacles.check_collisions((q_near, q_next)): continue
KD.addNode(q_next)
RRT.addPath(q_near, q_next)
plotter.draw_line(q_near, q_next, color='k', zorder=1, update=False)
plotter.draw_circle(q_next,
circ_rad,
edgecolor='k',
facecolor='w',
zorder=2)
if not obstacles.check_collisions((q_next, goal)):
# IF there is a direct line to the goal, then TAKE IT
goal_distance = math.hypot(q_next[0] - goal[0],
q_next[1] - goal[1])
while goal_distance > 0:
q_new = gen_next(q_next, goal, min(goal_distance, step_size))
RRT.addPath(q_next, q_new)
plotter.draw_line(q_next,
q_new,
color='k',
zorder=1,
update=False)
plotter.draw_circle(q_new,
circ_rad,
edgecolor='k',
facecolor='w',
zorder=2)
q_next = q_new
goal_distance -= step_size
break
trials = 0
print("n =", KD.length)
cur = RRT[goal]
while cur.parent:
plotter.draw_line(cur, cur.parent, update=False, color='b', zorder=3)
plotter.draw_circle(cur,
circ_rad * 1.5,
update=False,
facecolor='xkcd:green',
edgecolor='k',
zorder=4)
cur = cur.parent
plotter.update()
