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)