def connect_to_explored(a_tree, explored, region, obstacles):
connected = False
for e in explored:
conn_explored = rrt.connect(a_tree.node, np.array(e.nodes()),
region, obstacles)
if conn_explored is not None:
parent = a_tree.parent
ecopy = e.copy()
ecopy.find(conn_explored).add_child(a_tree)
a_tree.make_root()
a_tree.parent = parent
explored.remove(e)
connected = True
return connected
def connect_to_expansion(exp_region, region, obstacles, t, goal, dx, eps,
t_max, explored, iters):
for face, direction, box in faces(exp_region):
try:
obs = obstacles + \
[b.aspoly() for b in exp_region if b is not box]
a, b = drh_connect_pair(
face.aspoly(), extend(face, direction, eps).aspoly(),
region, obs, t_max, True)
util.plot_casestudy3(region, goal, obstacles, t, exp_region, np.vstack([a,b]))
last = rrt.connect(a, np.array(t.nodes()), region, obstacles)
a_tree = Tree(a)
if last is not None:
last = Tree(last)
a_tree.add_child(last)
else:
connect_to_explored(a_tree, explored, region, obstacles)
# a_tree's root is a, last is in t
a_tree, last = expand_tree(
region, obstacles, a_tree, np.vstack(t.nodes()), dx, eps, t_max,
[t] + explored, iters)
last.make_root()
t.find(last.node).add_children(last.children)
b_tree = Tree(b)
a_tree.add_child(b_tree)
return b_tree, last
except DRHNoModel:
pass
except DRMNotConnected as e:
b_tree = Tree(b)
a_tree.add_child(b_tree)
explored.insert(0, e.tree_progress)
raise DRMNotConnected(t)
