def decide_left_or_right(parsed_frame): ''' based on the position of the cursor and walls, decide whether to 1) press left (return 'left') 2) press right (return 'right') 3) do nothing (return None) ''' action = None start = get_start(parsed_frame) path = find_path(start, 40, parsed_frame.rot_arr) if path and len(path) > 8: path = smooth_path(path) start_x = start[0] next_x, next_y = path[1] min_x_jump = 0 min_y_jump = 0 print '-------------------' print 'start: (%d, %d)' % start print 'next: (%d, %d)' % (next_x, next_y) if next_y - start[1] >= min_y_jump: if start_x == 0 and next_x > 50 and next_x <= 62 - min_x_jump: # wrap around action = 'left' elif start_x == 61 and next_x >= min_x_jump and next_x < 10: # wrap around action = 'right' elif next_x > start_x + min_x_jump: action = 'right' elif next_x < start_x - min_x_jump: action = 'left' return action
def find_path(self, unit, loc): #FIXME: remember to invalidate paths path_id = (unit, loc) if not self.paths.has_key(path_id): self.paths[path_id] = a_star.find_path(self, unit, loc) return self.paths[path_id]
def solve(filename, start=(0,0), end=(0,0)): """solve one problem and save the solution as an image.""" input_filename = os.path.join('problems', filename) output_filename = os.path.join('solutions', filename) problem = GridProblem(input_filename) path = a_star.find_path(problem, start, end) save_solution(input_filename, path, output_filename)
# Converts the image into a numpy array world_map = np.abs(np.array(world_map) - 1) # Creates an RGB Version of the world map world_map_rgb = line_plotter.concat_channels(world_map, world_map, world_map) # ---------- Plot Layout ---------- # Specify that we require subplots along two rows and two columns figure, axis = plot.subplots(1, 2) # Specifying the primary plot title figure.suptitle('Results for map: ' + maps_to_load) #print("before A-star") # ---------- A* Algorithm ---------- # Uses the A_star algorithm to find the shortest route to the goal path, path_length, computation_time, coordinates_expanded = a_star.find_path( world_map, start_coordinate, end_coordinate) # We initialize the visualization object with the number of rovers and the grid size (width x height) visualizer = RoverDomainVisualizer(1, (150, 150)) path = path[::-1] for pos in range(len(path) + 10): if pos >= len(path) - 1: pos = len(path) - 1 # We update the visualizer visualizer.update_visualizer([(path[pos][1], path[pos][0])], [end_coordinate], [True], False, 0.05) else: # We update the visualizer visualizer.update_visualizer([(path[pos][1], path[pos][0])], [end_coordinate], [False], False,
if i != j and stacks.depth(i) > 0: neighbor = stacks.move(i, j) if neighbor.is_legal(): neighbors.append(neighbor) return neighbors def heuristic(self, position, goal): # number of blocks successfully moved to the last peg return len(position.stacks[-1]) if __name__ == '__main__': import sys if len(sys.args) == 1: number_of_pegs = 3 else: number_of_pegs = int(sys.args[1]) hanoi = HanoiProblem(number_of_pegs) # the "points" in the HanoiProblem are of type "Stacks", # so the we need to instantiate Stacks for the start and ends points. start = Stacks.make_start(number_of_pegs) goal = Stacks.make_goal(number_of_pegs) # then a miracle occurs... solution = a_star.find_path(hanoi, start, goal) for position in solution: print position
xrange_ = [ Point32(x=(v[0] - 50) / 50.0 * 2.5, y=(v[1] - 50) / 50.0 * 2.5) for v in valid_edges ] debug.publish( PointCloud(header=Header(frame_id=topics.MAP_FRAME), points=xrange_)) print 'ideal %s' % (ideal, ) def is_goal(p): return p in valid_edges def weight((x, y)): if costmap[y][x] < 127: # can't traverse return float('inf') if is_goal((x, y)): return abs(ideal[0] - x) + abs(ideal[1] - y) return 1 center = MAP_SIZE_PIXELS / 2 path = find_path(start=(x, y), reached_goal=is_goal, neighbors=grid_neighbors(costmap, jump_size=SPACING), weight=weight, heuristic=lambda v: manhattan(v, ideal)) return path
doodl.marketmap.draw(frame) moving = 0 for element in doodl.customers: # print(element.id) if element.goal != [0, 0]: # print(MARKET2) moving = 1 # if MARKET2[element.goal[1], element.goal[0]] == 1: # element.goal = [0,0] # if destination tile is occuppied skip this round # element.location = 'stalled' # if destination tile is occuppied skip this round # else: MARKET2[element.y, element.x] = 0 start1 = (element.y, element.x) goal1 = (element.goal[1], element.goal[0]) path = find_path( MARKET2, start1, goal1, possible_moves) # will return 0 if no route found if (path == 0): # no route found if element.oldlocation == 'stalled': # if already stalled in the previous round, give up the search element.goal = [0, 0] element.oldlocation = 'stalled' print("OOPS! Couldn't calculate route.") print( f"Customer {element.id} cannot find the route from {element.oldlocation} {start1} to {element.location} {goal1}!" ) print(MARKET2) else: newx = path[1][1] newy = path[1][0] if MARKET2[ newy,
def main(): teststart = node(-6, -6) testgoal = node(-6, -4) find_path(teststart, testgoal) print(testgoal.parent.x) #Add Lines for robot path lines = [] iterator = testgoal while iterator.parent != None: print("(%d,%d),(%d,%d)" % (iterator.x, iterator.y, iterator.parent.x, iterator.parent.y)) lines.append([(iterator.x, iterator.y), (iterator.parent.x, iterator.parent.y)]) iterator = iterator.parent # for n in neighbors: # print("%d, %d" % (n.x,n.y)) # lines.append([(n.x,n.y),(test.x,test.y)]) #start grid fig, ax = plt.subplots() #Add Obstacles patches = [] patches.append(Polygon([[-1, -1], [-5, -1], [-5, 2], [-3, 2]])) patches.append(Polygon([[-2, 2], [2, 2], [2, -4]])) patches.append( Polygon([[-1, -2.5], [-5, -2.5], [-5, -5], [1.2, -5], [1.2, -4]])) for poly in patches: ax.add_patch(poly) #Set boundaries ax.set_xlim([-7, 4]) ax.set_ylim([-7, 4]) #Set size of plot fig_size = plt.rcParams["figure.figsize"] fig_size[0] = 12 fig_size[1] = 9 plt.rcParams["figure.figsize"] = fig_size #draw lines to the plot lc = mc.LineCollection(lines, linewidths=2) ax.add_collection(lc) #Set up Axis ax.minorticks_on() ax.xaxis.set_major_locator(plt.MultipleLocator(1)) ax.yaxis.set_major_locator(plt.MultipleLocator(1)) ax.xaxis.set_minor_locator(plt.MultipleLocator(0.25)) ax.yaxis.set_minor_locator(plt.MultipleLocator(0.25)) ax.grid(which='major', linestyle='-', linewidth='0.5', color='black') ax.grid(which='minor', linestyle='-', linewidth='0.2', color='black') plt.show() # '''plt.axis([-7,4,-7,4])
for idx, space in enumerate(position.spaces): if idx <= math.floor(number_of_spaces/2) and space == "F": score += 2 elif idx == math.floor(number_of_spaces/2) and space != "_": score += 1 elif idx > math.floor(number_of_spaces/2) and space == "T": score += 2 return score ''' __main__ runs when we do `python frogs_toads.py` ''' if __name__ == '__main__': ''' Works for any number of frogs and toads, although anything greater than 5 takes a bit to complete. ''' number_of_frogs_and_toads = 9 frogs_and_toads = FrogsAndToadsProblem(number_of_frogs_and_toads) # the "points" in the FrogsAndToadsProblem are of type "Spaces", # so the we need to instantiate Spaces for the start and ends points. start = Spaces.make_start(number_of_frogs_and_toads) goal = Spaces.make_goal(number_of_frogs_and_toads) # Find the path using our a_star method. solution = a_star.find_path(frogs_and_toads, start, goal) for position in solution: print position