def direction(self, loc1, loc2): 'determine the direction of the next step to take using A*' row1, col1 = loc1 path = heuristic_search(self.graph, loc1, loc2, scott()) try: row2, col2 = path[1] except IndexError: return False # translate into a direction SNEWSNEW! if row1 - row2 == -1: return 's' elif row1 - row2 == 1: return 'n' elif col1 - col2 == -1: return 'e' elif col1 - col2 == 1: return 'w' elif (row1 - row2) > 0: return 's' elif (row1 - row2) < 0: return 'n' elif (col1 - col2) > 0: return 'e' elif (col1 - col2) < 0: return 'w' else: return False
def _risk(self, _heuristic, board_with_heads, path, snakes): score = 0 for node in path: for snake in snakes: opponent_path = heuristic_search( board_with_heads, tuple(snake["coords"][0]), node, _heuristic) # distance between the opponent's head and path point score += len(opponent_path) return score
def way_between_areas(self,start,dest): way = [] try : way = heuristic_search(self.areas_graph, start, dest, self.heuristic) except pygraph.classes.exceptions.NodeUnreachable : pass except KeyError : print 'E:way_between_areas: Key error', start, dest return way
def search_A_star(self, sentence): tokens = nltk.word_tokenize(sentence) #if a node does not exist, it shall be added to the graph and connected to the previous node for i, token in enumerate(tokens): if not self.net.has_node(token): self.net.add_node(str(token)) #print "adding node", token #if the edge does not exist between the current node and the previous one, an edge shall be added if i != 0: edge = (tokens[i - 1], token) if not self.net.has_edge(edge): self.net.add_edge(edge, wt=self.EDGE_START_WEIGHT, label=self.START_OCCURRENCES_NUM) h = chow( tokens[0] ) #this optimization shall be performed chooosing a term which is connected: the choice of this term shall be performed accoding to some rule h.optimize(self.net) path = '' for k, token in enumerate(tokens): if k != 0: try: intermediate_node_path = heuristic_search( self.net, tokens[k - 1], token, h) #the path between each couple except NodeUnreachable, e: print str(e) print "NODE UREACHABLE" for j, node in enumerate( intermediate_node_path ): #increase the weight of the edges in the path if j != 0: edge = (intermediate_node_path[j - 1], node) if not self.net.has_edge(edge): raise Exception('edge not found') else: #if the edge exists, its weight is divided by the number of occurrences stored in the label number_of_occurrences = self.net.edge_label(edge) new_number_of_occurrences = number_of_occurrences + 1 self.net.set_edge_label(edge, new_number_of_occurrences) self.net.set_edge_weight(edge, wt=1.0 / new_number_of_occurrences) if k < len(tokens) - 1: #we remove the last element to prevent repetitions of head and tail in the subsequences of the final path path = path + ' ' + ' '.join(intermediate_node_path[0:-1]) else: path = path + ' ' + ' '.join(intermediate_node_path)
def pathgen(graph): """Calculates the shortest path from each node to each other node.""" # grnodes = len(graph.nodes()) h = chow(1) # Needed for heuristic_search() h.optimize(graph) paths = [[] for x in range(len(graph.nodes()))] for j in graph.nodes(): for i in graph.nodes(): paths[j].append(heuristic_search(graph, j, i, h)) return paths
def run_benchmark(self): G = self.fn_create() self.generate(G, self.GRAPH_ORDER, self.GRAPH_EDGES) reference_nodes = sample( G.nodes(), self.CHOW_REFERENCE_POINTS ) heuristic = chow( *reference_nodes ) heuristic.optimize(G) source_nodes = sample( G.nodes(), self.NUMBER_OF_SEARCHES ) dest_nodes = sample( G.nodes(), self.NUMBER_OF_SEARCHES ) for s,d in zip( source_nodes, dest_nodes ): log.debug("Searching from %s to %s" % (s,d) ) result = heuristic_search( G, s, d, heuristic )
def a_star(self, targets): paths = {} distances = {} heuristic = chow(*targets) heuristic.optimize(self.board) for target in targets: try: paths[target] = heuristic_search(self.board, tuple(self.snake["coords"][0]), target, heuristic) paths[target] = paths[target][1:] # remove first (current position) distances[target] = len(paths[target]) except Exception as e: distances[target] = sys.maxint print("ERROR %s" % e) pass return paths, distances
def search_A_star(self, sentence): tokens = nltk.word_tokenize(sentence) #if a node does not exist, it shall be added to the graph and connected to the previous node for i, token in enumerate(tokens): if not self.net.has_node(token): self.net.add_node(str(token)) #print "adding node", token #if the edge does not exist between the current node and the previous one, an edge shall be added if i!=0: edge = (tokens[i-1], token) if not self.net.has_edge(edge): self.net.add_edge(edge, wt=self.EDGE_START_WEIGHT, label = self.START_OCCURRENCES_NUM) h = chow(tokens[0]) #this optimization shall be performed chooosing a term which is connected: the choice of this term shall be performed accoding to some rule h.optimize(self.net) path = '' for k, token in enumerate(tokens): if k!=0: try: intermediate_node_path = heuristic_search(self.net, tokens[k-1], token, h) #the path between each couple except NodeUnreachable, e: print str(e) print "NODE UREACHABLE" for j, node in enumerate(intermediate_node_path): #increase the weight of the edges in the path if j != 0: edge = (intermediate_node_path[j-1], node) if not self.net.has_edge(edge): raise Exception('edge not found') else: #if the edge exists, its weight is divided by the number of occurrences stored in the label number_of_occurrences = self.net.edge_label(edge) new_number_of_occurrences = number_of_occurrences + 1 self.net.set_edge_label(edge, new_number_of_occurrences) self.net.set_edge_weight(edge, wt = 1.0/new_number_of_occurrences) if k < len(tokens)-1: #we remove the last element to prevent repetitions of head and tail in the subsequences of the final path path = path + ' ' + ' '.join(intermediate_node_path[0:-1]) else: path = path + ' ' + ' '.join(intermediate_node_path)
def findpath(self, nodeone, nodetwo): return heuristic_search(self.graph, nodeone, nodetwo, self.heuristic)[1:-1]
def plan(self, point1, point2): """ Plan a path which connects the two given 2D points. The points are represented by tuples of two numbers (x, y). Return a list of tuples where each tuple represents a point in the planned path, the first point is the start point, and the last point is the end point. If the planning algorithm failed the returned list should be empty. """ #reset number of trials self.number_of_trials = 50 self.search_result = False self.output_path = list() start_node_id = self.next_id() end_node_id = self.next_id() if not self.is_valid_points(point1, point2): rospy.logwarn(self.__class__.__name__ + ": invalid points") return self.output_path #return empty list else: # add point1, point2 to the graph self.graph.add_node(start_node_id, attrs=[('position', point1)]) self.graph.add_node(end_node_id, attrs=[('position', point2)]) # connect point1, point2 if self.line_free_cb(point1, point2): edge_weight = sqrt(pow(point1[0] - point2[0], 2) + pow(point1[1] - point2[1], 2)) self.graph.add_edge((start_node_id, end_node_id), wt = edge_weight) # add more edges between point1, point2 and the rest of graph for temp_id, attr in self.graph.node_attr.iteritems(): # 'temp_id' contains node temp_id, 'attr' contains array of attributes temp_position = attr[0][1] if point1 != temp_position and self.line_free_cb(point1, temp_position): edge_weight = sqrt(pow(point1[0] - temp_position[0], 2) + pow(point1[1] - temp_position[1], 2)) self.graph.add_edge((start_node_id, temp_id), wt = edge_weight) if point2 != temp_position and self.line_free_cb(point2, temp_position): edge_weight = sqrt(pow(point2[0] - temp_position[0], 2) + pow(point2[1] - temp_position[1], 2)) self.graph.add_edge((end_node_id, temp_id), wt = edge_weight) while not self.search_result and self.number_of_trials > 0: try: #find a path (sequence of node ids) between start and end self.search_algorithm.optimize(self.graph) node_ids = heuristic_search(self.graph, start_node_id, end_node_id, self.search_algorithm) for temp_id in node_ids: node_position = self.graph.node_attributes(temp_id) self.output_path.append(node_position[0][1]) self.search_result = True except NodeUnreachable: # create a random point if there is no path x = uniform(self.dimensions[0][0] , self.dimensions[0][1]) y = uniform(self.dimensions[1][0] , self.dimensions[1][1]) new_point_position = (x , y) # add the new point to graph if self.point_free_cb(new_point_position): new_point_id = self.next_id() self.graph.add_node(new_point_id, attrs=[('position', new_point_position)]) # connect the new_point with older nodes for temp_id, attr in self.graph.node_attr.iteritems(): temp_position = attr[0][1] if new_point_position != temp_position and self.line_free_cb(new_point_position, temp_position): edge_weight = sqrt(pow(new_point_position[0] - temp_position[0], 2) + pow(new_point_position[1] - temp_position[1], 2)) self.graph.add_edge((new_point_id, temp_id), wt = edge_weight) # decrease number of trials self.number_of_trials = self.number_of_trials-1 return self.output_path
def plan(self, point1, point2): """ Plan a path which connects the two given 2D points. The points are represented by tuples of two numbers (x, y). Return a list of tuples where each tuple represents a point in the planned path, the first point is the start point, and the last point is the end point. If the planning algorithm failed the returned list should be empty. """ path = [] #Add start and end self.node_counter += 1 start = self.node_counter self.graph.add_node(self.node_counter, attrs=[('position', (point1[0], point1[1]))]) self.node_counter += 1 end = self.node_counter self.graph.add_node(self.node_counter, attrs=[('position', (point2[0], point2[1]))]) new_node_counter = 0 #Create nodes until a solution is found, limit = self.node_limit while new_node_counter < self.node_limit: #Add 5 new random nodes each iteration for i in range(self.number_nodes): position_x = random.uniform(self.dimensions_x[0], self.dimensions_x[1]) position_y = random.uniform(self.dimensions_y[0], self.dimensions_y[1]) if self.point_free_cb((position_x, position_y)): self.node_counter += 1 new_node_counter += 1 self.graph.add_node(self.node_counter, attrs=[('position', (position_x, position_y))]) #Add edges for nid1, attr in self.graph.node_attr.iteritems(): position1 = attr[0][1] for nid2, attr2 in self.graph.node_attr.iteritems(): position2 = attr2[0][1] distance = self.distance(position1, position2) #Avoid self edges and repetitivity if distance > 0.0 and self.graph.has_edge( (nid1, nid2)) == False: if self.line_free_cb(position1, position2): self.graph.add_edge((nid1, nid2), wt=distance) #A* search try: heuristic = euclidean() heuristic.optimize(self.graph) id_path = heuristic_search(self.graph, start, end, heuristic) for member in id_path: attributes = self.graph.node_attributes(member) path.append(attributes[0][1]) except NodeUnreachable: rospy.logwarn("Path unreachable, not enough nodes") else: return path #If node limit is passed, then no path is found rospy.logwarn("Node limit reached, path is not connected") return path
def plan(self, point1, point2): """ Plan a path which connects the two given 2D points. The points are represented by tuples of two numbers (x, y). Return a list of tuples where each tuple represents a point in the planned path, the first point is the start point, and the last point is the end point. If the planning algorithm failed the returned list should be empty. """ #reset number of trials self.number_of_trials = 50 self.search_result = False self.output_path = list() start_node_id = self.next_id() end_node_id = self.next_id() if not self.is_valid_points(point1, point2): rospy.logwarn(self.__class__.__name__ + ": invalid points") return self.output_path #return empty list else: # add point1, point2 to the graph self.graph.add_node(start_node_id, attrs=[('position', point1)]) self.graph.add_node(end_node_id, attrs=[('position', point2)]) # connect point1, point2 if self.line_free_cb(point1, point2): edge_weight = sqrt( pow(point1[0] - point2[0], 2) + pow(point1[1] - point2[1], 2)) self.graph.add_edge((start_node_id, end_node_id), wt=edge_weight) # add more edges between point1, point2 and the rest of graph for temp_id, attr in self.graph.node_attr.iteritems(): # 'temp_id' contains node temp_id, 'attr' contains array of attributes temp_position = attr[0][1] if point1 != temp_position and self.line_free_cb( point1, temp_position): edge_weight = sqrt( pow(point1[0] - temp_position[0], 2) + pow(point1[1] - temp_position[1], 2)) self.graph.add_edge((start_node_id, temp_id), wt=edge_weight) if point2 != temp_position and self.line_free_cb( point2, temp_position): edge_weight = sqrt( pow(point2[0] - temp_position[0], 2) + pow(point2[1] - temp_position[1], 2)) self.graph.add_edge((end_node_id, temp_id), wt=edge_weight) while not self.search_result and self.number_of_trials > 0: try: #find a path (sequence of node ids) between start and end self.search_algorithm.optimize(self.graph) node_ids = heuristic_search(self.graph, start_node_id, end_node_id, self.search_algorithm) for temp_id in node_ids: node_position = self.graph.node_attributes(temp_id) self.output_path.append(node_position[0][1]) self.search_result = True except NodeUnreachable: # create a random point if there is no path x = uniform(self.dimensions[0][0], self.dimensions[0][1]) y = uniform(self.dimensions[1][0], self.dimensions[1][1]) new_point_position = (x, y) # add the new point to graph if self.point_free_cb(new_point_position): new_point_id = self.next_id() self.graph.add_node(new_point_id, attrs=[('position', new_point_position) ]) # connect the new_point with older nodes for temp_id, attr in self.graph.node_attr.iteritems(): temp_position = attr[0][1] if new_point_position != temp_position and self.line_free_cb( new_point_position, temp_position): edge_weight = sqrt( pow(new_point_position[0] - temp_position[0], 2) + pow(new_point_position[1] - temp_position[1], 2)) self.graph.add_edge((new_point_id, temp_id), wt=edge_weight) # decrease number of trials self.number_of_trials = self.number_of_trials - 1 return self.output_path
def shortestPathFromDataState(graph, node): def zeroHeuristic(neighbor, goal): return 0 return heuristic_search(graph, "dataState", node, zeroHeuristic)
def plan(self, point1, point2): a = self.dimensions[0] rospy.logwarn(np.random.uniform(a[0], a[1], 30)) rospy.logwarn(np.random.uniform(a[0], a[1], 30)) self.node_list = [] result_node_id = [] result_path = [] #Adding start node to the graph and local node list start_node_id = self.generate_id() self.graph.add_node(start_node_id, attrs=[('position', point1)]) self.node_list.append(point1) #Adding goal node to the graph and local node list goal_node_id = self.generate_id() self.graph.add_node(goal_node_id, attrs=[('position', point2)]) self.node_list.append(point2) #Checking is there is any direct path between the start and goal node if self.line_free_cb(point1, point2): rospy.loginfo('Direct path found between start and goal node') return self.node_list #Generating 30 random nodes and adding it to graph while (len(self.node_list) < 30): self.addNode() rospy.loginfo('Creating possible edges process started...') #Generating possible edges between the generated nodes for nid, attr in self.graph.node_attr.iteritems(): temp_id = nid temp_position = attr[0][1] for nid_1, attr_1 in self.graph.node_attr.iteritems(): if (nid_1 != temp_id): #Checking the line free between two nodes if self.line_free_cb(temp_position, attr_1[0][1]): if not self.graph.has_edge((temp_id, nid_1)): #Adding the edge between two nodes with euclidean distance self.graph.add_edge((temp_id, nid_1), self.calculateEuclidean( temp_position, attr_1[0][1])) rospy.loginfo('Creating possible edges process finished...') #Optimizing the graph self.heuristic.optimize(self.graph) rospy.loginfo('Optimizing graph finished...') try: #Performing A* search to find the shortest path between the start node and goal node rospy.loginfo('Finding shorted path using A* algorithm started') result_node_id = heuristic_search(self.graph, start_node_id, goal_node_id, self.heuristic) rospy.loginfo('Shorteset path found...') except: rospy.loginfo('No Shorteset path found...') #Creating the list for the shortest path between the start and goal node for name in result_node_id: result_path.append(self.get_node(name)) print result_path rospy.loginfo('Planning completed...') return result_path """
def distance(self, loc1, loc2): return len(heuristic_search(self.graph, loc1, loc2, scott()))
def plan(self, point1, point2): """ Plan a path which connects the two given 2D points. The points are represented by tuples of two numbers (x, y). Return a list of tuples where each tuple represents a point in the planned path, the first point is the start point, and the last point is the end point. If the planning algorithm failed the returned list should be empty. """ print "PLANNING METHOD CALLED" if not self.point_free_cb(point1): rospy.logwarn("START POINT UNAVAILIBLE") return [] if not self.point_free_cb(point2): rospy.logwarn("END POINT UNAVAILIBLE") return [] if self.line_free_cb(point1, point2): print "STRAIGHT LINE ROUTE FOUND" return [point1, point2] else: print "EXPANDING MAP" start_id = self.add_node_to_graph(point1, force=True) finish_id = self.add_node_to_graph(point2, force=True) h = euclidean() while True: # No DO..UNTIL loops in python. Condition check below path = [] try: h.optimize(self.graph) path = heuristic_search(self.graph, start_id, finish_id, h) except NodeUnreachable: # add another node new_node_added = False while not new_node_added: randomPoint = (uniform(*self.dimensions[0]), uniform(*self.dimensions[1])) if self.point_free_cb(randomPoint): if self.add_node_to_graph(randomPoint): new_node_added = True # DO..UNTIL emulation this way if len(path) > 0 or self.nodecount >= RandomizedRoadmapPlanner.MAX_NODES_NUM: print "Break loop" break # print path if len(path) > 0: found_path = [] rospy.loginfo("PATH FOUND") for n in path: found_path.append(self.graph.node_attr.get(n)[0][1]) return found_path elif self.nodecount >= RandomizedRoadmapPlanner.MAX_NODES_NUM: rospy.logwarn("MAX NUM NODES ALLOWED EXCEEDED!") rospy.logwarn("PATH NOT FOUND!") return []