def rrt_connect(q1,
q2,
distance,
sample,
extend,
collision,
iterations=RRT_ITERATIONS):
if collision(q1) or collision(q2):
return None
root1, root2 = TreeNode(q1), TreeNode(q2)
nodes1, nodes2 = [root1], [root2]
for _ in irange(iterations):
if len(nodes1) > len(nodes2):
nodes1, nodes2 = nodes2, nodes1
s = sample()
last1 = argmin(lambda n: distance(n.config, s), nodes1)
for q in extend(last1.config, s):
if collision(q):
break
last1 = TreeNode(q, parent=last1)
nodes1.append(last1)
last2 = argmin(lambda n: distance(n.config, last1.config), nodes2)
for q in extend(last2.config, last1.config):
if collision(q):
break
last2 = TreeNode(q, parent=last2)
nodes2.append(last2)
else:
path1, path2 = last1.retrace(), last2.retrace()
if path1[0] != root1:
path1, path2 = path2, path1
return configs(path1[:-1] + path2[::-1])
return None
def rrt_connect_renamed(q1, q2, distance, sample, extend, collision,
iterations):
# check if q1 or q2 is in collision
if collision(q1) or collision(q2):
print 'collision in either initial or goal'
return None
# define two roots of the tree
root1, root2 = TreeNode(q1), TreeNode(q2)
# tree1_nodes grows from q1, tree2_nodes grows from q2
tree1_nodes, tree2_nodes = [root1], [root2]
# sample and extend iterations number of times
for ntry in range(iterations):
if len(tree1_nodes) > len(
tree2_nodes): # balances the sizes of the trees
tree1_nodes, tree2_nodes = tree2_nodes, tree1_nodes
# sample a configuration
new_config = sample()
# returns the node with the closest distance to s from a set of nodes tree1_nodes
tree1_node_closest_to_new_config = argmin(
lambda n: distance(n.config, new_config), tree1_nodes)
# extend from the closest config to s
extended_tree1_node = tree1_node_closest_to_new_config
for q in extend(tree1_node_closest_to_new_config.config, new_config):
# if there is a collision, extend upto that collision
if collision(q):
break
extended_tree1_node = TreeNode(
q, parent=tree1_node_closest_to_new_config)
tree1_nodes.append(extended_tree1_node)
# try to extend to the tree grown from the other side
tree2_node_closest_to_extended_tree1_node = argmin(
lambda n: distance(n.config, extended_tree1_node.config),
tree2_nodes
) # min dist to extended_tree1_node (extended to new config s)
extended_tree2_node = tree2_node_closest_to_extended_tree1_node
for q in extend(tree2_node_closest_to_extended_tree1_node.config,
extended_tree1_node.config):
if collision(q):
break
extended_tree2_node = TreeNode(
q, parent=tree2_node_closest_to_extended_tree1_node)
tree2_nodes.append(extended_tree2_node)
else: # where is the if for this else? if none of q gets into if-collision(q) stmt, then it will enter here
# two trees meet
path1, path2 = extended_tree1_node.retrace(
), extended_tree2_node.retrace()
if path1[0] != root1:
path1, path2 = path2, path1
return configs(path1[:-1] + path2[::-1])
return None
def grow(self, goal, iterations=50, store=ts.PATH, max_tree_size=500):
if goal in self:
return 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 rrt_connect(q1, q2, distance, sample, extend, collision, iterations):
# check if q1 or q2 is in collision
if collision(q1) or collision(q2):
print 'collision in either initial or goal'
return None
# define two roots of the tree
root1, root2 = TreeNode(q1), TreeNode(q2)
# nodes1 grows from q1, nodes2 grows from q2
nodes1, nodes2 = [root1], [root2]
# sample and extend iterations number of times
for ntry in range(iterations):
if len(nodes1) > len(nodes2): # ????
nodes1, nodes2 = nodes2, nodes1
# sample a configuration
s = sample()
# returns the node with the closest distance to s from a set of nodes nodes1
last1 = argmin(lambda n: distance(n.config, s), nodes1)
# extend from the closest config to s
for q in extend(last1.config,
s): # I think this is what is taking up all the time
# if there is a collision, extend upto that collision
if collision(q):
break
last1 = TreeNode(q, parent=last1)
nodes1.append(last1)
# try to extend to the tree grown from the other side
last2 = argmin(lambda n: distance(n.config, last1.config), nodes2)
for q in extend(last2.config, last1.config):
if collision(q):
break
last2 = TreeNode(q, parent=last2)
nodes2.append(last2)
else: # where is the if for this else?
# apparently, if none of q gets into
# if collision(q) stmt, then it will enter here
# two trees meet at the new configuration s
path1, path2 = last1.retrace(), last2.retrace()
if path1[0] != root1:
path1, path2 = path2, path1
return configs(path1[:-1] + path2[::-1])
return None
def rrt(start,
goal_sample,
distance,
sample,
extend,
collision,
goal_test=lambda q: False,
iterations=RRT_ITERATIONS,
goal_probability=.2):
if collision(start):
return None
if not callable(goal_sample):
g = goal_sample
goal_sample = lambda: g
nodes = [TreeNode(start)]
for i in irange(iterations):
goal = random() < goal_probability or i == 0
s = goal_sample() if goal else sample()
last = argmin(lambda n: distance(n.config, s), nodes)
for q in extend(last.config, s):
if collision(q):
break
last = TreeNode(q, parent=last)
nodes.append(last)
if goal_test(last.config):
return configs(last.retrace())
else:
if goal:
return configs(last.retrace())
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
def rrt_region(q1, region, distance, sample, extend, collision, iterations):
# check if q1 or q2 is in collision
if collision(q1):
print 'ignoring collision in initial'
# return None
# define two roots of the tree
root1 = TreeNode(q1)
# tree1_nodes grows from q1, tree2_nodes grows from q2
tree1_nodes = [root1]
# sample and extend iterations number of times
for ntry in range(iterations):
# sample a configuration
s = sample()
# returns the node with the closest distance to s from a set of nodes tree1_nodes
extended_tree1_node = argmin(lambda n: distance(n.config, s),
tree1_nodes)
# extend from the closest config to s
for q in extend(extended_tree1_node.config,
s): # I think this is what is taking up all the time
# if there is a collision, extend upto that collision
if collision(q):
break
extended_tree1_node = TreeNode(q, parent=extended_tree1_node)
tree1_nodes.append(extended_tree1_node)
import pdb
pdb.set_trace()
if region.contains_point(q):
path1 = extended_tree1_node.retrace()
return configs(path1)
return None
def rrt_star(start,
goal,
distance,
sample,
extend,
collision,
radius,
max_time=INF,
max_iterations=INF,
goal_probability=.2,
informed=True):
if collision(start) or collision(goal):
return None
nodes = [OptimalNode(start)]
goal_n = None
t0 = time()
it = 0
while (t0 - time()) < max_time and it < max_iterations:
do_goal = goal_n is None and (it == 0 or random() < goal_probability)
s = goal if do_goal else sample()
# Informed RRT*
if informed and goal_n is not None and distance(start, s) + distance(
s, goal) >= goal_n.cost:
continue
if it % 100 == 0:
print it, time() - t0, goal_n is not None, do_goal, (
goal_n.cost if goal_n is not None else INF)
it += 1
nearest = argmin(lambda n: distance(n.config, s), nodes)
path = safe_path(extend(nearest.config, s), collision)
if len(path) == 0:
continue
new = OptimalNode(path[-1],
parent=nearest,
d=distance(nearest.config, path[-1]),
path=path[:-1],
iteration=it)
# if safe and do_goal:
if do_goal and distance(new.config, goal) < 1e-6:
goal_n = new
goal_n.set_solution(True)
# TODO - k-nearest neighbor version
neighbors = filter(lambda n: distance(n.config, new.config) < radius,
nodes)
nodes.append(new)
for n in neighbors:
d = distance(n.config, new.config)
if n.cost + d < new.cost:
path = safe_path(extend(n.config, new.config), collision)
if len(path) != 0 and distance(new.config, path[-1]) < 1e-6:
new.rewire(n, d, path[:-1], iteration=it)
for n in neighbors: # TODO - avoid repeating work
d = distance(new.config, n.config)
if new.cost + d < n.cost:
path = safe_path(extend(new.config, n.config), collision)
if len(path) != 0 and distance(n.config, path[-1]) < 1e-6:
n.rewire(new, d, path[:-1], iteration=it)
if goal_n is None:
return None
return goal_n.retrace()
