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 __call__(self, q1, q2=None, iterations=50):
if q1 in self:
path1 = self[q1].retrace()
else:
path1 = self.grow(q1, iterations=iterations)
if q2 is None:
return configs(path1)
if q2 in self:
path2 = self[q2].retrace()
else:
path2 = self.grow(q2, iterations=iterations)
if path1 is None or path2 is None:
return None
for i in range(min(len(path1), len(path2))):
if path1[i] != path2[i]:
break
else:
i += 1
return configs(path1[:i - 1:-1] + path2[i - 1:])
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 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
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
