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)
def approach(self, pos, goal):
route = line(pos, goal)[1:]
for point in route:
if not self.configspace.clearance(point):
break
yield tuple(point)
