def sparsify_path(robot, path): diff = [robot.SubtractActiveDOFValues(q2, q1) for q1, q2 in pairs(path)] sparse_path = [] config = path[0] i = 0 while i < len(diff) - 1: sparse_path.append(config.copy()) config += diff[i] for j in range(i+1, len(diff)): if not np.allclose(diff[i], diff[j]): break config += diff[j] i = j return sparse_path + [config.copy()]
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 draw(self, env, color=(1, 0, 0, .5)): if self._path is None: return #self._handle = draw_edge(env, self.v1.q, self.v2.q, color=color) for q1, q2 in pairs(self.configs()): self._handles.append(draw_edge(env, q1, q2, color=color))