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_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 __init__(self, start, distance, sample, extend, collision):
self.nodes = {}
self.distance = distance
self.sample = sample
self.extend = extend
self.collision = collision
self.add(TreeNode(start))
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_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 hill_climbing_rrt(start,
goal,
distance,
sample,
extend,
collision,
iterations=RRT_ITERATIONS):
if collision(start): return None
best_h = distance(start, goal)
best_node = TreeNode(start)
nodes = [best_node]
while True:
for q in extend(best_node.config, goal):
if collision(q):
break
best_node = TreeNode(q, parent=best_node)
nodes.append(best_node)
else:
return configs(best_node.retrace())
for _ in irange(iterations):
s = sample()
last = argmin(lambda n: distance(n.config, s), nodes)
success = False
for q in extend(last.config, s):
if collision(q):
break
last = TreeNode(q, parent=last)
nodes.append(last)
h = distance(q, goal)
if h < best_h:
best_h = h
best_node = last
success = True
break
if success:
break
return None