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