def evade(self, pos, goal, visited): print "Started evasion..." for candpos in self.dfs(pos, visited): if not self.configspace.clearance(candpos): continue yield candpos try: nextpoint = tuple(line(candpos, goal)[1]) except IndexError: # We are right before the goal. break if self.configspace.clearance(nextpoint) and nextpoint not in visited: break else: raise Exception("I got stuck.") print "evaded."
def dfs(self, pos, visited): """Return a depth first traversal from the current position. Steps in the direction of the goal are favored (around the clock).""" shift = lambda l, i: l[i:] + l[:i] to_visit = list(self.neighbours[pos]) while to_visit: try: direc = line(pos, self.configspace.goal)[1] - pos except IndexError: # We are right before the goal. yield self.configspace.goal ind = self.evasions.index(tuple(direc)) pos = to_visit.pop() if pos in visited or not self.configspace.clearance(pos): continue visited.add(pos) yield pos to_visit += shift([i for i in self.neighbours[pos] if i not in visited], ind)
def search(self): # Phase for building up roadmap. while True: point = self.sample() yield point if not self.configspace.clearance(point): continue # Search for a node that we can connect with. for node in self.graph.nodes(): if self.configspace.link(point, node): self.graph.add_edge(point, node, sp.dot(sp.asarray(point), sp.asarray(node))) path = nx.shortest_path(self.graph, self.configspace.start, self.configspace.goal) if path: break # Build up path. self.path = [] for a, b in zip(path, path[1:]): self.path += line(a, b)
def dfs(self, pos, visited): """Return a depth first traversal from the current position. Steps in the direction of the goal are favored (around the clock).""" shift = lambda l, i: l[i:] + l[:i] to_visit = list(self.neighbours[pos]) while to_visit: try: direc = line(pos, self.configspace.goal)[1] - pos except IndexError: # We are right before the goal. yield self.configspace.goal ind = self.evasions.index(tuple(direc)) pos = to_visit.pop() if pos in visited or not self.configspace.clearance(pos): continue visited.add(pos) yield pos to_visit += shift( [i for i in self.neighbours[pos] if i not in visited], ind)
def search(self): # Phase for building up roadmap. while True: point = self.sample() yield point if not self.configspace.clearance(point): continue # Search for a node that we can connect with. for node in self.graph.nodes(): if self.configspace.link(point, node): self.graph.add_edge( point, node, sp.dot(sp.asarray(point), sp.asarray(node))) path = nx.shortest_path(self.graph, self.configspace.start, self.configspace.goal) if path: break # Build up path. self.path = [] for a, b in zip(path, path[1:]): self.path += line(a, b)
def approach(self, pos, goal): route = line(pos, goal)[1:] for point in route: if not self.configspace.clearance(point): break yield tuple(point)