def grow(self, goal, iterations=50, store=ts.PATH, max_tree_size=500): if goal in self: self[goal].retrace() if self.collision(goal): return None nodes1, new_nodes1 = list(take(randomize(self.nodes.values()), max_tree_size)), [] nodes2, new_nodes2 = [], [TreeNode(goal)] for _ in irange(iterations): if len(nodes1) + len(new_nodes1) > len(nodes2) + len(new_nodes2): nodes1, nodes2 = nodes2, nodes1 new_nodes1, new_nodes2 = new_nodes2, new_nodes1 s = self.sample() last1 = argmin(lambda n: self.distance(n.config, s), nodes1 + new_nodes1) for q in self.extend(last1.config, s): if self.collision(q): break last1 = TreeNode(q, parent=last1) new_nodes1.append(last1) last2 = argmin(lambda n: self.distance(n.config, last1.config), nodes2 + new_nodes2) for q in self.extend(last2.config, last1.config): if self.collision(q): break last2 = TreeNode(q, parent=last2) new_nodes2.append(last2) else: if len(nodes1) == 0: nodes1, nodes2 = nodes2, nodes1 new_nodes1, new_nodes2 = new_nodes2, new_nodes1 last1, last2 = last2, last1 path1, path2 = last1.retrace(), last2.retrace()[:-1][::-1] for p, n in pairs(path2): n.parent = p if len(path2) == 0: # TODO - still some kind of circular error for n in new_nodes2: if n.parent == last2: n.parent = path1[-1] else: path2[0].parent = path1[-1] path = path1 + path2 if store in [ts.ALL, ts.SUCCESS]: self.add(*(new_nodes1 + new_nodes2[:-1])) elif store == ts.PATH: new_nodes_set = set(new_nodes1 + new_nodes2[:-1]) self.add(*[n for n in path if n in new_nodes_set]) return path if store == ts.ALL: self.add(*new_nodes1) return None
def grow(self, goal_sample, iterations=50, goal_probability=.2, store=ts.PATH, max_tree_size=500): if not callable(goal_sample): goal_sample = lambda: goal_sample nodes, new_nodes = list(take(randomize(self.nodes.values()), max_tree_size)), [] for i in irange(iterations): goal = random() < goal_probability or i == 0 s = goal_sample() if goal else self.sample() last = argmin(lambda n: self.distance(n.config, s), nodes + new_nodes) for q in self.extend(last.config, s): if self.collision(q): break last = TreeNode(q, parent=last) new_nodes.append(last) else: if goal: path = last.retrace() if store in [ts.ALL, ts.SUCCESS]: self.add(*new_nodes) elif store == ts.PATH: new_nodes_set = set(new_nodes) self.add(*[n for n in path if n in new_nodes_set]) return path if store == ts.ALL: self.add(*new_nodes) return None