def build_tree(self, x_init, goal, t_init=None, iters=-1):
if t_init is None:
t = Tree(x_init)
else:
t = t_init
cur = t
while not contains(goal, cur.node) and iters != 0:
x_random = self.pick_random()
x_near_tree = nearest(t, x_random)
x_near = x_near_tree.node
v = x_random - x_near
vnorm = np.linalg.norm(v)
if vnorm > self.dx:
x_new = x_near + self.dx * v / vnorm
else:
x_new = x_random
if isvalid(x_new, x_near, self.constraints, self.obstacles):
cur = Tree(x_new)
x_near_tree.add_child(cur)
g = connect(x_new, goal, self.constraints, self.obstacles)
if g is not None:
g_tree = Tree(g)
cur.add_child(g_tree)
cur = g_tree
iters -= 1
return t, cur
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)
