def __init__(self, name, speed=0.8): rospy.init_node(name) self.name = name self.speed = speed self.caught = False self.my_pose = GroundtruthPose(name) self.cop_poses = [ GroundtruthPose(c) for c in ['cop_0', 'cop_1', 'cop_2'] ] self.rate_limit = rospy.Rate(100) # 200ms self.res = 0.2 self.occupancy_grid = OccupancyGrid(self.res, 0.5) self.path_finder = AStar('euclidean', self.occupancy_grid, resolution=self.res) self.map = list(self.path_finder.occupancy_grid.map) self.velocity_publisher = rospy.Publisher('/{}/cmd_vel'.format(name), Twist, queue_size=5) rospy.Subscriber('/caught', String, self.catch_listener) # file for pose history self.pose_history = [] self.pose_history_fn = '/tmp/gazebo_{}.txt'.format(name) with open(self.pose_history_fn, 'w'): pass # wait for ground truth poses node to init while not self.my_pose.ready or not all(c.ready for c in self.cop_poses): self.rate_limit.sleep() continue self.target_position, self.path = self.get_random_target_position_and_path( )
def main(): global window, origin, astar, objective window = pygame.display.set_mode(WINDOW_SIZE) pygame.display.set_caption("A* Pathfinding") gen_rects() origin = grid_list[3][5] objective = grid_list[12][28] astar = AStar(origin, objective) gen_objective_distance() gen_adjacents() loop = True start = False vel = 30 while loop: window.fill(WHITE) #events for event in pygame.event.get(): if event.type == pygame.QUIT: pygame.quit() sys.exit() if event.type == pygame.KEYDOWN: if event.key == pygame.K_RETURN: start = True if event.type == pygame.MOUSEBUTTONUP: if pygame.mouse.get_pressed(3)[0]: click_wall(pygame.mouse.get_pos(), True) if pygame.mouse.get_pressed(3)[2]: click_wall(pygame.mouse.get_pos(), False) if event.type == pygame.MOUSEMOTION: if pygame.mouse.get_pressed(3)[0]: click_wall(pygame.mouse.get_pos(), True) if pygame.mouse.get_pressed(3)[2]: click_wall(pygame.mouse.get_pos(), False) if start: vel = 5 if not astar.found: astar.seach() pygame.draw.rect(window, RED, objective.rect) pygame.draw.rect(window, GREEN, origin.rect) draw_grid() # if astar.found: # back(vel,astar) # astar.seach_back() # for sqr in astar.path: # pygame.draw.rect(window,BLUE,sqr.rect) pygame.display.update() fps.tick(vel)
def test_map_with_40_nodes_start_5_end_34(self): map_40 = Map(Map40.intersections, Map40.roads) start_node_id = 5 dest_node_id = 34 astar = AStar(map_40) self.assertEqual(astar.search(start_node_id, dest_node_id), [5, 16, 37, 12, 34])
def test_map_with_40_nodes_start_8_end_24(self): map_40 = Map(Map40.intersections, Map40.roads) start_node_id = 8 dest_node_id = 24 astar = AStar(map_40, start_node_id, dest_node_id) self.assertEqual(astar.search(start_node_id, dest_node_id), [8, 14, 16, 37, 12, 17, 10, 24])
def get_move(self, grid_data, data): self.height = data.get("board").get("height") self.width = data.get("board").get("width") self.head_x = data.get("you").get("body")[0].get("x") self.head_y = data.get("you").get("body")[0].get("y") self.my_snake_health = data.get("you").get("health") self.my_snake_length = len(data.get("you").get("body")) self.pathfinder = AStar((self.head_x, self.head_y), grid_data[0], self.width, self.height) self.grid_data = grid_data self.data = data move = self.move_to_food() #NOTE FIND TAIL MODE if self.my_snake_length > 3 and self.my_snake_health > 65 or move == None: move = self.chase_tail(self.my_snake_health == 100) if move: return move else: backup_moves = self.helper.get_backup_move((self.head_x, self.head_y), self.grid_data[0], self.height, self.width) if backup_moves: return self.helper.get_move_letter((self.head_x, self.head_y), backup_moves[0]) else: return 'up'
def get_move(self, grid_data, data): self.height = data.get("board").get("height") self.width = data.get("board").get("width") self.head_x = data.get("you").get("body")[0].get("x") self.head_y = data.get("you").get("body")[0].get("y") self.my_snake_health = data.get("you").get("health") self.my_snake_length = len(data.get("you").get("body")) self.pathfinder = AStar((self.head_x, self.head_y), grid_data[0], self.width, self.height) self.grid_data = grid_data self.data = data snakes = data.get("board").get("snakes") head = (self.head_x, self.head_y) # find closest snake ID target_snake_id = self.find_closest_snake_head(snakes) # Guard to make sure our snake doesn't just starve if self.my_snake_health < 40: return self.default_behaviour() if target_snake_id: return self.attack_snake_head(snakes, target_snake_id) else: return self.default_behaviour()
def __init__(self): self._cursor = (1, 1) self._start = (1, 1) self._goal = (33, 8) self._console = Console() self._map = Map() self._astar = AStar(self._map.cost)
def compute_route(self, node_source, source_ori, node_target, target_ori): self._previous_node = node_source a_star = AStar() a_star.init_grid( self._map.get_graph_resolution()[0], self._map.get_graph_resolution()[1], self._map.get_walls_directed(node_source, source_ori, node_target, target_ori), node_source, node_target) route = a_star.solve() # JuSt a Corner Case # Clean this to avoid having to use this function if route is None: a_star = AStar() a_star.init_grid( self._map.get_graph_resolution()[0], self._map.get_graph_resolution()[1], self._map.get_walls_directed(node_source, source_ori, node_target, target_ori, both_walls=False), node_source, node_target) route = a_star.solve() self._route = route return route
def move(): data = bottle.request.json board = data.get('board') board_height = board.get('height') board_width = board.get('width') # print(board) GRID = buildGrid(board, board_height, board_width) # print(GRID) # TODO: Do things with data # get the snake VINCE = data['you'] xhead = VINCE['body'][0]['x'] yhead = VINCE['body'][0]['y'] head_tuple = (xhead, yhead) # first food bit first_food_bit = (board['food'][0]['x'], board['food'][0]['y']) AS = AStar() path_to_food = AS.find_path(GRID, head_tuple, first_food_bit) print(path_to_food) directions = ['up', 'down', 'left', 'right'] direction = random.choice(directions) direction = get_dir(head_tuple, path_to_food[1]) print(direction) return {'move': direction, 'taunt': 'battlesnake-python!'}
def create_astar(self): self.bfs = AStar(self.bfs_grid, 'BFS') self.dfs = AStar(self.dfs_grid, 'DFS') self.astar = AStar(self.astar_grid, 'AStar') self.active_search = self.bfs self.active_grid = self.bfs_grid
def __init__(self): self.astar=AStar(MAP_COLS,MAP_ROWS) self.map=self.astar.map self.astar.print_map() self.game_over=False self.score=0 self.done=False
def create(self, method_name): if method_name == "dijkstra": return Dijkstra() elif method_name == "astar": return AStar() else: return AStar()
def main(): parser = argparse.ArgumentParser( description="Compute the optimal path using A*" ) group = parser.add_mutually_exclusive_group() group.add_argument( "-w", "--world", help="The filename of the world to load", ) group.add_argument( "-s", "--size", type=int, help="Size of the random board to be generated" ) parser.add_argument( "-f", "--file", help="The file to output the world to" ) parser.add_argument( "heuristic", type=int, help="The heuristic number to run (in range(1, 7))" ) args = parser.parse_args() if args.world: newWorld = World(filepath=args.world) elif args.size: newWorld = World(height=args.size, width=args.size) else: newWorld = World(height=10, width=10) if args.file: newWorld.write_world(args.file) astar = AStar(newWorld, args.heuristic) astar.start()
class AStarApplication(QWidget): def __init__(self): super(AStarApplication, self).__init__() self.close = self.cleanup self.layout = QVBoxLayout(self) self.setLayout(self.layout) self.canvas = QGraphicsView() self.canvas.setViewportUpdateMode(QGraphicsView.FullViewportUpdate) self.grid = Grid(24, 20) self.tiles = [] for x in range(self.grid.w): for y in range(self.grid.h): tile = Tile(self.grid.get_node(x, y), self) self.tiles.append(tile) self.scene = QGraphicsScene() self.scene.setBackgroundBrush(QBrush(Qt.lightGray, Qt.SolidPattern)) for t in self.tiles: self.scene.addItem(t) self.canvas.setScene(self.scene) self.layout.addWidget(self.canvas) self.refresh_timer = QTimer() self.refresh_timer.setInterval(1000 / 60) self.refresh_timer.timeout.connect(self.refresh) self.refresh_timer.start() self.AStar = AStar(self.grid) self.focusPolicy = Qt.StrongFocus self.keyPressEvent = self.kbdHandler # Avoid the segfault caused by some obscure GC stuff. @pyqtSlot() def cleanup(self, obj): pass @pyqtSlot() def refresh(self): self.scene.invalidate() @pyqtSlot() def kbdHandler(self, e): if e.key() == Qt.Key_Space: self.AStar.step() # Called when a tile changes type, so all tiles can be updated def update_tiles(self, caller): for t in self.tiles: if self.grid.target is not None: t.subtext = t.node.heuristic(self.grid.target) else: t.subtext = '' if t != caller and t.type != 'clear' and t.type != 'wall' and t.type == caller.type: t.set_type('clear') self.AStar.reset()
def __init__(self): super(AStarApplication, self).__init__() self.close = self.cleanup self.layout = QVBoxLayout(self) self.setLayout(self.layout) self.canvas = QGraphicsView() self.canvas.setViewportUpdateMode(QGraphicsView.FullViewportUpdate) self.grid = Grid(24, 20) self.tiles = [] for x in range(self.grid.w): for y in range(self.grid.h): tile = Tile(self.grid.get_node(x, y), self) self.tiles.append(tile) self.scene = QGraphicsScene() self.scene.setBackgroundBrush(QBrush(Qt.lightGray, Qt.SolidPattern)) for t in self.tiles: self.scene.addItem(t) self.canvas.setScene(self.scene) self.layout.addWidget(self.canvas) self.refresh_timer = QTimer() self.refresh_timer.setInterval(1000 / 60) self.refresh_timer.timeout.connect(self.refresh) self.refresh_timer.start() self.AStar = AStar(self.grid) self.focusPolicy = Qt.StrongFocus self.keyPressEvent = self.kbdHandler
def send_pose_sp(self): if self.occupancy and self.has_robot_location and self.nav_sp: state_min = (-int(round(self.plan_horizon)), -int(round(self.plan_horizon))) state_max = (int(round(self.plan_horizon)), int(round(self.plan_horizon))) # Round initial, goal positions to grid resolution x_init = round_pt_to_grid(self.robot_translation[:2], self.plan_resolution) x_goal = round_pt_to_grid(self.nav_sp[:2], self.plan_resolution) astar = AStar(state_min, state_max, x_init, x_goal, self.occupancy, self.plan_resolution) # uncomment to add buffering to obstacles bufferRadius = 4 astar.bufferOccupancy(bufferRadius) rospy.loginfo("Computing new navigation plan") if astar.solve(): # If initial state == goal, path len == 1 # Handle case where A* solves, but no 2nd element -> don't send msg if len(astar.path) < self.short_path_len: rospy.loginfo("Path goal matches current state") # Typical use case else: wp_x, wp_y, wp_th = self.next_waypoint(astar) self.publish_path(astar, wp_x, wp_y, wp_th) else: rospy.logwarn("Could not find path")
def __init__(self, name, speed=0.6): rospy.init_node(name) self.name = name self.speed = speed self.my_pose = GroundtruthPose(name) self.occupancy_grid = OccupancyGrid(0.2, 0.5) # assign cop to unique robber self.target = 'robber_' + name[-1] self.target_pose = GroundtruthPose(self.target) self.rate_limit = rospy.Rate(100) # 200ms self.path_finder = AStar('euclidean', self.occupancy_grid, resolution=0.2) self.velocity_publisher = rospy.Publisher('/{}/cmd_vel'.format(name), Twist, queue_size=5) self.catch_publisher = rospy.Publisher('/caught', String, queue_size=1) rospy.Subscriber('/caught', String, self.catch_listener) # file for pose history self.pose_history = [] self.pose_history_fn = '/tmp/gazebo_{}.txt'.format(name) with open(self.pose_history_fn, 'w'): pass # wait for ground truth poses node to init while not self.my_pose.ready and not self.target_pose.ready: self.rate_limit.sleep() continue
def __init__(self): self.list = [] count = 0 # 依次添加行人至self.list for i in range(0, 15): self.list.append(People("o" + str(count), 60, 60 + i * 40)) count = count + 1 self.list.append(People("o" + str(count), 100, 60 + i * 40)) count = count + 1 self.list.append(People("o" + str(count), 140, 60 + i * 40)) count = count + 1 # 在一开始就计算好各个位置的下一步方向向量,并存储到矩阵中,以节省算力 k = 5 self.matrix = [[0 for i in range(17)] for i in range(27)] for i in range(0, 27): for j in range(0, 17): if i == 0: # 将障碍物的位置也设置对应的方向向量,以便在特殊情况撞墙后能及时调整回来 self.matrix[i][j] = (k, 0) elif i == 26: self.matrix[i][j] = (-1 * k, 0) elif j == 0: self.matrix[i][j] = (0, k) elif j == 16: self.matrix[i][j] = (0, -1 * k) elif i == 10: if (j == 4) or (j == 10): self.matrix[i][j] = (-1 * k, -1 * k) elif (j == 5) or (j == 11): self.matrix[i][j] = (-1 * k, 0) elif (j == 6) or (j == 12): self.matrix[i][j] = (-1 * k, k) else: self.matrix[i][j] = AStar.next_loc(i, j) elif i == 17: if (j == 4) or (j == 10): self.matrix[i][j] = (k, -1 * k) elif (j == 5) or (j == 11): self.matrix[i][j] = (k, 0) elif (j == 6) or (j == 12): self.matrix[i][j] = (k, k) else: self.matrix[i][j] = AStar.next_loc(i, j) elif (j == 4) or (j == 10): if (i > 10) and (i < 17): self.matrix[i][j] = (0, -1 * k) else: self.matrix[i][j] = AStar.next_loc(i, j) elif (j == 6) or (j == 12): if (i > 10) and (i < 17): self.matrix[i][j] = (0, k) else: self.matrix[i][j] = AStar.next_loc(i, j) elif (i > 10) and (i < 17) and ((j == 5) or (j == 11)): self.matrix[i][j] = (0, k) else: self.matrix[i][j] = AStar.next_loc(i, j) self.matrix[26][4] = (1, 0)
def calculatePath(self, start, goal, clusterIds): # Only length starSolver = AStar(self) t = starSolver.solveBetweenNodes(clusterIds, start, goal) if t > 0: self.graph.addEdge(start, goal, t) return True else: return False
def main(argv): # queens = createBoard(argv[0]) queens = getDemoBoard(argv[0]) heur = 0 if argv[2] == "H1" else 1 if int(argv[1]) == 1: # A* Algorithm astar_solver = AStar(queens, heur) print(astar_solver.Solve()) elif int(argv[1]) == 2: # Hill Climb hill_solver = HillClimb(queens, heur) hill_solver.solve()
def startGame(): mystdout = StdOutWrapper() sys.stdout = mystdout sys.stderr = mystdout curses.initscr() curses.beep() curses.beep() window = curses.newwin(HEIGHT, WIDTH, 0, 0) window.timeout(TIMEOUT) window.keypad(1) curses.noecho() curses.curs_set(0) window.border(0) snake = Snake(SNAKE_X, SNAKE_Y, window) food = Food(window, snake) astar = AStar() while True: window.clear() window.border(0) # rendering the objects snake.render() food.render() window.addstr(0, 5, snake.getScore) event = window.getch() if event == 27: break if snake.head.x == food.x and snake.head.y == food.y: snake.eatFood(food) curses.beep() event = astar.getKey(food, snake) # print(event) if event in (KEY_UP, KEY_DOWN, KEY_LEFT, KEY_RIGHT): snake.makeMove(event) snake.update() if snake.collided(): break curses.endwin() print(f"High score :: {snake.score}") sys.stdout = sys.__stdout__ sys.stderr = sys.__stderr__ sys.stdout.write(mystdout.get_text())
def __call__(self, matrix, snake, candy_pos): if self.path: return self.get_direction(snake[0], self.path.pop(0)) head2candy, _ = AStar(matrix, snake[0], candy_pos, walls="qzpmublrto") if head2candy: sim_matrix, sim_tail = self.simulate(matrix, snake, head2candy) candy2tail, _ = AStar(sim_matrix, candy_pos, sim_tail, walls="qzpmublro") if candy2tail: self.path = head2candy[2:] return self.get_direction(snake[0], head2candy[1]) head2tail, _ = AStar(matrix, snake[0], snake[-1], walls="qzpmublro") return self.get_direction(snake[0], head2tail[1])
def find_alternative_path(self, start, blackout, goal): if start == goal: return True if (start, blackout, goal) in self.alternative_paths: return True backup_val = self.grid[blackout] self.grid[blackout] = inf pather = AStar(self.grid, start, goal) success = pather.findpath() if success: self.alternative_paths[(start, blackout, goal)] = pather.path self.alternative_paths[(goal, blackout, start)] = deque(reversed(pather.path)) self.grid[blackout] = backup_val return success
def find_closest(x_init, x_goals, statespace_lo, statespace_hi, occupancy, resolution): min_dist = np.float('inf') closest = None for x_goal in x_goals: astar = AStar(statespace_lo, statespace_hi, x_init, x_goal, occupancy, resolution) if astar.solve() and len(astar.path) < min_dist: min_dist = len(astar.path) closest = x_goal return closest, min_dist
def send_pose_sp(self): try: (robot_translation, robot_rotation) = self.trans_listener.lookupTransform( "/map", "/base_footprint", rospy.Time(0)) self.has_robot_location = True except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): robot_translation = (0, 0, 0) robot_rotation = (0, 0, 0, 1) self.has_robot_location = False if self.occupancy and self.has_robot_location and self.nav_sp: state_min = (-int(round(self.plan_horizon)), -int(round(self.plan_horizon))) state_max = (int(round(self.plan_horizon)), int(round(self.plan_horizon))) x_init = (round(robot_translation[0] / self.plan_resolution) * self.plan_resolution, round(robot_translation[1] / self.plan_resolution) * self.plan_resolution) x_goal = ((round(self.nav_sp[0] / self.plan_resolution)) * self.plan_resolution, round(self.nav_sp[1] / self.plan_resolution) * self.plan_resolution) rospy.logwarn("x_init (after rounding): (%6.4f,%6.4f)", x_init[0], x_init[1]) rospy.logwarn("x_goal (after rounding): (%6.4f,%6.4f)", x_goal[0], x_goal[1]) astar = AStar(state_min, state_max, x_init, x_goal, self.occupancy, self.plan_resolution) rospy.loginfo("Computing Navigation Plan") if astar.solve(): rospy.loginfo( "Navigation Success. Found %d waypoint path to (%6.3f, %6.3f)", len(astar.path), astar.path[-1][0], astar.path[-1][1]) # a naive path follower we could use # pose_sp = (astar.path[1][0],astar.path[1][1],self.nav_sp[2]) # msg = Float32MultiArray() # msg.data = pose_sp # self.pose_sp_pub.publish(msg) # astar.plot_path() path_msg = self.buildPath(astar.path) self.nav_path_pub.publish(path_msg) else: rospy.logwarn("Navigation Failure")
async def step_4(robot1, robot2, currentPos1, currentPos2, destPos): # Navigation procedure as in Step 2. # ROBOT 1 a = AStar(robot1, currentPos1) print('Navigating from:', (4, 6), 'to', (8, 4)) print('') path = a.initLegoWorld((4, 6), (8, 4)) for i in range(len(path)): await a.move(path[i]) await a.face("north") currentPos1 = a.getPos() # ROBOT 2 a = AStar(robot2, currentPos2) print('Navigating from:', (2, 6), 'to', (6, 4)) print('') dest = (destPos[0], destPos[1]) path = a.initLegoWorld((2, 6), dest) for i in range(len(path)): await a.move(path[i]) await a.face("east") currentPos1 = a.getPos() return True, currentPos1, currentPos2
def run(): # Initialize a 10x10 stage. No other stage sizes are # coded yet, but these constructor arguments exist in # case we want to later on board = Board() # Initialize the A-Star algorithm class with our # current graph and reference to the board because # I'm lazy. It shouldn't have a dependency on a board, # but it's been 8 hours already and I'm tired. astar = AStar(board.graph, board) # Render the board to show where we stand board.render() inp = get_input() while inp[0].lower() not in "qe": if inp[0].lower() in "h": # Players is babies and wants helps! with open(os.path.join(resource_root, "maze", "help.txt"), "rb") as help_file: print(help_file.read()) board.render() inp = raw_input("Next move? (enter h or help for help)").strip() if inp == "": inp = " " continue # Try to move the player. Run, Forrest! Run! if inp[0].lower() == "u": (x, y) = board.player board.move_entity("player", x, y - 1) elif inp[0].lower() == "d": (x, y) = board.player board.move_entity("player", x, y + 1) elif inp[0].lower() == "l": (x, y) = board.player board.move_entity("player", x - 1, y) elif inp[0].lower() == "r": (x, y) = board.player board.move_entity("player", x + 1, y) # The enemy is a greedy bastard and moves every chance he can path = astar.get_path(astar.search(board.enemy, board.player)[0], board.enemy, board.player) board.move_entity("enemy", *path[1]) board.render(path=path[2:-1]) if board.enemy == board.player: print("You lost!") exit() if board.player == board.goal: print("You escaped death! Huzzah!") exit() inp = get_input() print("Scaredy-cat!")
async def step_6(robot1, robot2, currentPos1, currentPos2): # Navigation procedure as in Step 2. # ROBOT 2 a = AStar(robot2, currentPos2) print('Navigating from:', (6, 4), 'to', (4, 0)) print('') path = a.initLegoWorld((6, 4), (4, 0)) for i in range(len(path)): await a.move(path[i]) await a.face("south") currentPos2 = a.getPos() # ROBOT 1 a = AStar(robot1, currentPos1) print('Navigating from:', (8, 4), 'to', (4, 2)) print('') path = a.initLegoWorld((8, 4), (4, 2)) for i in range(len(path)): await a.move(path[i]) currentPos1 = a.getPos() await robot1.drive_straight(distance_mm(150), speed_mmps(50)).wait_for_completed() await robot1.turn_in_place(degrees(70)).wait_for_completed() return True, currentPos1, currentPos2
async def step_8(robot1, robot2, currentPos1, currentPos2): # Undo the movements made by the robots when trying to 'look natural' in Step 6. await robot1.turn_in_place(degrees(-70)).wait_for_completed() await robot1.drive_straight(distance_mm(-150), speed_mmps(50)).wait_for_completed() # Navigation procedure as in Step 2. # ROBOT 1 a = AStar(robot1, currentPos1) print('Navigating from:', (4, 2), 'to', (0, 2)) print('') path = a.initLegoWorld((4, 2), (0, 2)) for i in range(len(path)): await a.move(path[i]) currentPos1 = a.getPos() # ROBOT 2 a = AStar(robot2, currentPos2) print('Navigating from:', (4, 0), 'to', (0, 6)) print('') path = a.initLegoWorld((4, 0), (0, 6)) for i in range(len(path)): await a.move(path[i]) await a.face("south") currentPos2 = a.getPos() return True, currentPos1, currentPos2
def main(self): clock = pygame.time.Clock() graph = grid2graph(self.world) # Convert grid into graph astar = AStar() # Initialise Astar res = 25 # Resolution = cm per grid square while not self.quit: source = self.node_nb(self.buggy_pos, res) # Source = buggy pos sink = self.node_nb(self.cur_pos, res * self.scale) # Sink = cursor pos [node.reset() for node in graph] # Reset every node path = astar.solve(graph, source, sink) # Solve graph self.event_handler( ) # Put all keypress events into key_status dictionary self.update_window(path, res) # Update display clock.tick(self.fps) # Restrict rate
def callAstar(self): occupancy = DetOccupancyGrid2D(self.map_width, self.map_height, self.obstacles) self.astar = AStar((0, 0), (self.map_width, self.map_height), self.start_pos, self.end_pos, occupancy) if not self.astar.solve(): exit(0) self.trajectory = Trajectory() self.trajectory.x.data = (self.astar.path * self.astar.resolution)[:, 0] self.trajectory.y.data = (self.astar.path * self.astar.resolution)[:, 1] self.trajectory.theta.data = np.zeros((len(self.trajectory.x.data), 1))
def find_solver(self, algorithm): solver = None while solver is None: if algorithm == 'depth_first' or algorithm == 'dfs': solver = Depth_first(self.map) elif algorithm == 'breadth_first' or algorithm == 'bf' or algorithm == 'bfs': solver = Breadth_first(self.map) elif algorithm == 'astar' or algorithm == 'a*': solver = AStar(self.map) elif algorithm == 'greedy_best_first' or algorithm == 'gbf' or algorithm == 'gbfs': solver = Greedy_best_first(self.map) elif algorithm == 'iterative_deepening' or algorithm == 'id' or algorithm == 'ids': solver = Iterative_deepening(self.map) elif algorithm == 'greedy' or algorithm == 'g' or algorithm == 'gs': solver = Greedy(self.map) #Iterative astar works, but it does not work well. #I did not research it much before building it, and I got a few key details wrong. #I decided to scrap it and use a different informed custom search (greedy), as it would have taken me far too long to redo it. #But I figured it would still be a valuable thing to include just to show my progress. elif algorithm == 'iterative_astar' or algorithm == 'ia*': solver = Iterative_astar(self.map) else: print( 'Valid searches are: dfs (depth first), bfs (breadth first), A*, greedy, ids (iterative deepening) or gbfs (greedy best first)' ) print('Please enter an algorithm:') algorithm = input() return solver
def update(cls, screen, survivor): del_zmbs = set() for zmb in cls.instances: if zmb.health <= 0.: Drop.spawn(zmb.pos) del_zmbs.add(zmb) stats["Zombies Killed"] += 1 continue screen.blit(zmb.img, zmb.pos) zmb.health_bar(surface=screen) # Health bar with rounded edges if Options.debug: for tile in zmb.path: pygame.draw.circle(screen, zmb.path_colour, tile.get_centre(), Tile.length // 3) zmb_to_survivor_dist = (survivor.pos - zmb.pos).magnitude() if zmb_to_survivor_dist <= cls.attack_range: survivor.health -= 0.4 if zmb.to is not None: # if the zombie is not next to the player angle = angle_between(*zmb.pos, *(zmb.to + zmb.vel)) if zmb.pos == zmb.to: # If the zombie is directly on a tile zmb.to = None # Trigger A-Star, not run between tiles for performance else: if "freeze" not in Drop.actives: zmb.pos += zmb.vel if zmb.direction != angle: # New direction, frame after a turn zmb.rotate(angle) if zmb.to is None and zmb_to_survivor_dist > Tile.length: AStar(zmb, survivor).solve() cls.instances -= del_zmbs
def test_heuristic_gen(self): root = self.gen_tree() pvar = pred_variance(root) max_var = 1.5 * max(pvar.values()) pvar1 = dict((p, max_var - v) for p, v in pvar.items()) pvar1s = sum(pvar1.values()) weights = dict((p, root.num_occurrences(p) * v / pvar1s) for p, v in pvar1.items()) for p, w in weights.items(): print '%s: %f' % (p, w) preds = list(root.get_all_predicates()) preds.sort() weights = fmincon.run_fmincon(root) h = heuristic.Heuristic(dict(zip(preds, weights))) root.make_graphviz(open('temp.dot', 'w')) search = AStar(root, lambda x: h(x.preds)) iter = search.gen iterations = 0 try: while not iter.next(): iterations += 1 except StopIteration: print iterations print[''.join(s.preds) for s in search.solution]
class AStarGAC: def __init__(self): # Keep track of all the nodes in the system self.nodes = [] # Set node type for AStar AStar.State = GACState # Reference to AStar and CSP self.astar = AStar() self.gac_state = None # Set behavior self.astar.set_behavior(BestFirst) def start(self): # Run CSP-initialize and CSP-domain-filtering-loop self.gac_state.gac.initialize() self.gac_state.gac.domain_filtering() # Assign the current state to AStar and begin to solve self.astar.states[self.gac_state.get_hash()] = self.gac_state self.astar.behavior.add(self.gac_state, self.astar.open) # Check if we are finished or if the current state is invalid if self.gac_state.gac.check_finished() or self.gac_state.gac.check_contradictory_state(): if self.gac_state.gac.check_finished(): # We found the solution without actually running the agenda loop, make sure we save goal state self.gac_state.type = self.astar.State.GOAL return True return False def run(self): if self.gac_state.gac.check_finished(): return True # Do one step in the agenda loop finished = self.astar.agenda_loop() # Update the CSP state self.gac_state = self.astar.closed[-1] # Return the result from the agenda loop return finished
def __init__(self): # Keep track of all the nodes in the system self.nodes = [] # Set node type for AStar AStar.State = GACState # Reference to AStar and CSP self.astar = AStar() self.gac_state = None # Set behavior self.astar.set_behavior(BestFirst)
def step(self, execute): if self.mmap.isLiftOpen(): self.tar = self.mmap.getLift() aStar = AStar(self.mmap.copy(), self.tar) self.path = aStar.process().path() else: self.tars = [] for l in self.mmap.getLambdas(): aStar = AStar(self.mmap.copy(), l) self.path = aStar.process().path() self.tars.append((l, self.path)) minmin = reduce(lambda a, b: min(a, b), map(lambda t: len(t[1]), self.tars)) self.tar, self.path = filter(lambda t: len(t[1]) == minmin, self.tars)[0] if (not self.wouldGetKilledFor(self.path) and self.mmap.getLeftCommands() > len(self.path) and len(self.path) > 0): execute(self.path) self.skip = [] else: self.skip.append(self.tar) return self
def move(self): if self.next_step_x == None and self.next_step_y == None: if self.player.x != self.player_last_position[0] or self.player.y != self.player_last_position[1]: self.player_last_position = (self.player.x, self.player.y) if self.player.next_step_x == None and self.player.next_step_y == None: self.path = AStar.get_path(self) if self.next_step_x == None and self.next_step_y == None: if len(self.path) > 0: l = self.path.pop() self.next_step_x = l.x self.next_step_y = l.y else: if self.x > self.next_step_x: self.x -= 4 self.direction = "W" elif self.x < self.next_step_x: self.x += 4 self.direction = "E" if self.y > self.next_step_y: self.y -= 4 self.direction = "N" elif self.y < self.next_step_y: self.y += 4 self.direction = "S" if self.x == self.next_step_x and self.y == self.next_step_y: self.next_step_x = None self.next_step_y = None
class Astar_GAC(Graph): """Astar_GAC integrates Astar and GAC""" #both def __init__(self, variables, domains, expressions): super(Astar_GAC, self).__init__() self.cnet = CNET(variables, domains, expressions) self.currentState = self.initializeState(self.cnet) self.gac = GAC(self.currentState) self.Astar = AStar(self) @abstractmethod def createNewState( self, variables, constraints): pass def initializeState(self, cnet): """in initState each variable has its full domain. It will be set as root node initilizes cnet""" s = self.createNewState( cnet.variables, cnet.constraints ) s.update('start') self.startNode = s self.stateCounter = 0 self.nofAssumption = 0 self.nofExpanded = 0 return s def search(self): self.currentState = self.gac.domainFiltering(self.currentState) self.stateCounter += 1 if self.currentState.isSolution(): self.printStatistics(self.currentState) return self.currentState return self.iterateSearch() def iterateSearch(self): prev = self.currentState if prev.isSolution(): return prev self.currentState = self.Astar.iterateAStar() # self.currentState.updateColors() self.stateCounter += 1 self.currentState.parent = prev #used for backtracking to find 'shortest path' for statistics self.nofExpanded = self.Astar.nofExpandedNodes if self.currentState.isSolution(): self.printStatistics(self.currentState) return self.currentState self.currentState = self.gac.domainFiltering(self.currentState) return self.currentState def makeAssumption(self, newVI, parentState): """Generate one successor, and make sure all pointers are correct""" newVertices = {} newVIList = [] for vi in parentState.viList: viID = vi.getID() tmpVI = VI(viID, vi.domain.copy()) newVertices[viID] = tmpVI newVIList.append( tmpVI ) for vi in parentState.viList: viID = vi.getID() for neighbor in vi.neighbors: n = newVertices[ neighbor.getID() ] newVertices[viID].add_neighbor( n ) for vi in newVIList: if vi.getID() == newVI.getID(): newVIList.remove(vi) newVIList.append(newVI) else: for vi_n in vi.neighbors: if vi_n.getID() == newVI.getID(): vi.neighbors.remove(vi_n) vi.neighbors.append(newVI) succ = self.createNewState(newVIList, parentState.constraintList) succ.parent = parentState succ.updateUndecided() # maybe not needed succ.ciList = [] constraints = self.cnet.getConstraints() for v in succ.undecidedVariables: for n in v.neighbors: for c in constraints: succ.ciList.append( CI(c,[v,n]) ) return succ def generateSucc(self, state): """ make a guess. start gussing value for variables with min. domain length""" succStates = [] finishedVIs = [] varsCopy = state.undecidedVariables.copy() if not len(varsCopy): return [] otherVIs = sorted(varsCopy, key=lambda v: len(v.domain), reverse=True) betterVI = otherVIs.pop() if betterVI.domain: initID = betterVI.getID() for d in betterVI.domain: newVI = VI( initID, [d]) newVI.neighbors = betterVI.neighbors.copy() successor = self.makeAssumption(newVI, state) succStates.append( self.gac.rerun(successor) ) return succStates else: return [] # Not complete : TODO def printStatistics(self, state): print ( 'The number of unsatisfied constraints = ', self.countUnsatisfiedConstraints(state), '\n' ) print ( 'The total number of verticies without color assignment = ', self.countColorLess(state), '\n' ) print ( 'The total number of nodes in search tree = ', self.stateCounter, '\n' ) print ( 'The total number of nodes poped from agenda and expanded = ', self.nofExpanded, '\n' ) print ( 'The length of the path = ', self.nofAssumption ,'\n') def countColorLess(self, state): nofColorLess = 0 for vi in state.viList: if len(vi.domain) != 1: nofColorLess += 1 return nofColorLess # def countUnsatisfiedConstraints(self, state): # unsatisfied = 0 # varList = state.viList # for c in state.ciList: # for var in varList: # if var in c.variables: # if self.countInconsistentDomainValues(var, c) or not len(var.domain): # unsatisfied += 1 # return unsatisfied def countUnsatisfiedConstraints(self, state): unsatisfied = 0 varList = state.viList for var in varList: if len(var.domain) != 1: unsatisfied += 1 return unsatisfied # Needed for Graph def getGoal(self): return None def countInconsistentDomainValues(self, x, c): pairs = [] nofInconsistency = 0 for k in c.variables: for value in x.domain: pairs.extend( list(itertools.product([value], k.domain)) ) for p in pairs : if not c.constraint( p[0], p[1] ): nofInconsistency += 1 return nofInconsistency
def __init__(self, variables, domains, expressions): super(Astar_GAC, self).__init__() self.cnet = CNET(variables, domains, expressions) self.currentState = self.initializeState(self.cnet) self.gac = GAC(self.currentState) self.Astar = AStar(self)
def A(m, h=0): a = AStar(m, h) for i in a.step(): pass return None if not a.path else True
def write_file(self, name, closed_set): """ Escrita de um ficheiro .txt para o armazenamento dos pontos resultantes do algoritmo @param name nome do ficheiro .txt onde serao armazenados os dados @closed_set lista de pontos resultantes do algoritmo """ data = open(name, "w") for x in closed_set: data.writelines((str(x[0]), " , ", str(x[1]), "\n")) data.close() if __name__ == "__main__": open_image = OpenImage("peppersgrad.pgm") a_star = AStar() open_image.open_image(a_star.START_POINT, a_star.END_POINT) start = time.time() a_star.a_star(open_image.img) finish = time.time() - start for point in a_star.closed_set: open_image.draw_point(point) print "Tempo de execucao: %.15f" % finish print "Iteracoes:", a_star.count print "Media por cada iteracao: %.15f" % (finish / a_star.count) open_image.show_image() open_image.save_image("result.png") write_file = WriteFile() write_file.write_file("data.txt", a_star.closed_set)
class Window: def create_grid(self, rows, columns): self.bfs_grid = Grid(self.width, self.height*2//3, rows, columns, self.screen) self.dfs_grid = Grid(self.width, self.height*2//3, rows, columns, self.screen) self.astar_grid = Grid(self.width, self.height*2//3, rows, columns, self.screen) def update_cell(self, row, column, state): self.bfs_grid.update_cell(row, column, state) self.dfs_grid.update_cell(row, column, state) self.astar_grid.update_cell(row, column, state) def create_astar(self): self.bfs = AStar(self.bfs_grid, 'BFS') self.dfs = AStar(self.dfs_grid, 'DFS') self.astar = AStar(self.astar_grid, 'AStar') self.active_search = self.bfs self.active_grid = self.bfs_grid def create_buttons(self): l = 10 h = 25 t = self.height*2//3 + 10 w = (self.width-l*4)//3 p = [(l,t,w,h),(l*2 + w,t, w,h),(l*3 + w*2, t,w, h)] self.buttons = [pygbutton.PygButton(p[0], "BFS"), pygbutton.PygButton(p[1], "DFS"), pygbutton.PygButton(p[2], "A*")] def __init__(self, width=500,height=750): pygame.init() self.width = width self.height = height self.screen = pygame.display.set_mode((self.width, self.height)) self.create_buttons() # self.font = pygame.font.SysFont('Arial', 25) self.font = pygame.font.Font('freesansbold.ttf', 16) self.WHITE = (255, 255, 255) self.BLACK = (0, 0, 0) def show_text(self): x = 10 y = self.height*2//3 + 50 text = self.bfs.getStats() self.screen.blit(self.font.render(text, True, self.BLACK), (x, y)) y += 25 text = self.dfs.getStats() self.screen.blit(self.font.render(text, True, self.BLACK), (x, y)) y += 25 text = self.astar.getStats() self.screen.blit(self.font.render(text, True, self.BLACK), (x, y)) def drawPath(self): self.active_search.backtrackPath() pathNodes = self.active_search.bestPath for node in pathNodes[:-1]: ((x,y),s) = node.getID() if node.state != 'goal': self.active_grid.drawPath(x, y) def loop(self): clock = pygame.time.Clock() results = [None, None, None] active_result_index = 0 active_result = results[active_result_index] while True: pygame.event.pump() active_result = results[active_result_index] if active_result == None or active_result.state != 'goal': active_result = self.active_search.iterateAStar() results[active_result_index] = active_result self.screen.fill(self.WHITE) self.active_grid.draw() self.drawPath() self.show_text() for event in pygame.event.get(): if 'click' in self.buttons[0].handleEvent(event): self.active_search = self.bfs self.active_grid = self.bfs_grid active_result_index = 0 if 'click' in self.buttons[1].handleEvent(event): self.active_search = self.dfs self.active_grid = self.dfs_grid active_result_index = 1 if 'click' in self.buttons[2].handleEvent(event): self.active_search = self.astar self.active_grid = self.astar_grid active_result_index = 2 if event.type == pygame.QUIT: sys.exit() for button in self.buttons: button.draw(self.screen) # pygame.display.update(changed_rectangles) is faster pygame.display.flip() # --- Limit to 60 frames per second clock.tick(60)
''' grid_mdp = GridWorldMDP(map_struct['seed_map'], map_struct['goal'], map_struct['start'], map_struct['bridge_probabilities'], map_struct['bridge_locations']) init_value = {} for s in grid_mdp.states: init_value[s.tostring()] = np.linalg.norm(s - grid_mdp.goal_state) mdp = MDP(grid_mdp.states, grid_mdp.valid_actions_function, grid_mdp.cost_function) #value_fcn = mdp.value_iteration(value = value_fcn, plot=True, world_size = 50) value_fcn = mdp.value_iteration(value = init_value, plot=True, world_size = 50) #set up dubins astar dub = dubins_astar(world_points, value_fcn) astar = AStar(motion_primitives, dub.cost_function, dub.heuristic, dub.valid_edge, dub.state_equality, plot = False) astar_state = np.array([state['x'],state['y'],state['theta']]) else: ''' following_dist = 0.0 temp_idx = dub.last_idx while following_dist < dub.look_ahead_dist temp_idx -= 1 path_diff = numpy.array([,]) following_dist += np.linalg.norm(path_diff) index = temp_idx ''' #ind_diff = np.abs(np.array(indices) - carrot_idx) #index = indices[np.argmin(ind_diff)] astar_state = np.array([state['x'],state['y'],state['theta']]) #path_states[index,:]
def move(self): self.step_num += 1 # if bomb in the neigbourhood if self.parent.target in neighbours((self.x, self.y), True): # The agent found the bomb self.bomb_location = self.parent.target if self.is_scout: # bomb deactivated self.parent.bomb_found = True return True else: self.color = QColor(200, 96, 109, 255) # the path is being reinitialized in # order to be show to the scout self.bomb_location = self.parent.target # if agent is scout and scout knows the bomb if self.bomb_location and self.is_scout: if self.path_to_bomb: (self.x, self.y) = self.path_to_bomb.pop() return True # create a shortcut target = self.bomb_location known_region = self.path start = (self.x, self.y) print 'calling astar' shortest_path = AStar() print 'will find the shortest path' self.path_to_bomb = shortest_path.find_path(start, target, known_region) return True # select to move randomly avoiding # to go to the last position order_list = ['u', 'r', 'd', 'l'] random.shuffle(order_list) # get the available moves moves = [] for order in order_list: if order == 'u': moves = self.up(moves) elif order == 'r': moves = self.right(moves) elif order == 'd': moves = self.down(moves) elif order == 'l': moves = self.left(moves) # if moves available if len(moves) != 0: # check if agents meet each other for agent in self.parent.agents: if (agent.x, agent.y) in neighbours((self.x, self.y)): self.exchanges += 1 agent.path += self.path self.path += agent.path self.path = list(set(self.path)) agent.path = list(set(agent.path)) if (agent.is_scout) and (self.bomb_location) and (not agent.bomb_location): agent.bomb_location = self.bomb_location return True else: if self.bomb_location and not agent.bomb_location: agent.bomb_location = self.bomb_location agent.color = QColor(200, 96, 109, 255) for m in moves: # The agent prefers to go to a # square he has not visited if m in self.path: #we put this move at the end moves.remove(m) moves.append(m) (self.x, self.y) = moves[0] if moves[0] in self.path: self.path.remove((self.x, self.y)) self.path.append((self.x, self.y)) else: print 'cannot move' return False return True
self.delta_state = delta_state def get_end_state(self, state): return state + self.delta_state delta_states = [np.array([ 1, 0]), np.array([ 1, 1]), np.array([ 0, 1]), np.array([-1, 1]), np.array([-1, 0]), np.array([-1, -1]), np.array([ 0, -1]), np.array([ 1, -1])] motion_primitives = [motion_primitive(delta_state) for delta_state in delta_states] def cost_function(state, motion_primitive): return np.linalg.norm(motion_primitive.delta_state) astar = AStar(motion_primitives, cost_function, heuristic, valid_edge_function, state_equality) plan = astar.plan(start_state, goal_state) pathx = [state[0] for state in plan] pathy = [state[1] for state in plan] plt.plot(pathx,pathy) plt.plot([0, 7],[3, 3]) plt.plot([3, 10],[6, 6]) plt.show()
def A(m, h=0): a = AStar(m, h) for i in a.step(): pass if not a.path: raise Exception("\a[!] A* PATH NOT FOUND!")
def get_path(self, flags, start, end): astar = AStar(flags, start, end) astar.process() return astar.path()
def amiCoSimulator(): """simple AmiCo env interaction""" print "Py: AmiCo Simulation Started:" print "library found: " #add syspath new_path = os.path.abspath("./").lower() java_path = os.path.abspath("C:\\Program Files (x86)\\Java\\jre6\\bin").lower() java_path2 = os.path.abspath("C:\\Program Files (x86)\\Java\\jre6\\bin\\client").lower() sys.path.append(new_path) sys.path.append(java_path) sys.path.append(java_path2) if (sys.platform == 'linux2'): ########################################## # find_library on Linux could only be used if your libAmiCoPyJava.so is # on system search path or path to the library is added in to LD_LIBRARY_PATH # # name = 'AmiCoPyJava' # loadName = find_library(name) ########################################## loadName = './libAmiCoPyJava.so' libamico = ctypes.CDLL(loadName) print libamico else: #else if OS is a Mac OS X (libAmiCo.dylib is searched for) or Windows (AmiCo.dll) name = 'AmiCoPyJava.DLL' loadName = find_library(name) print loadName libamico = ctypes.CDLL(name) print libamico javaClass = "ch/idsia/benchmark/mario/environments/MarioEnvironment" libamico.amicoInitialize(1, "-Djava.class.path=." + os.pathsep +":../lib/jdom.jar") libamico.createMarioEnvironment(javaClass) reset = cfunc('reset', libamico, None, ('list', ListPOINTER(c_int), 1)) getEntireObservation = cfunc('getEntireObservation', libamico, py_object, ('list', c_int, 1), ('zEnemies', c_int, 1)) performAction = cfunc('performAction', libamico, None, ('list', ListPOINTER(c_int), 1)) getEvaluationInfo = cfunc('getEvaluationInfo', libamico, py_object) getObservationDetails = cfunc('getObservationDetails', libamico, py_object) agent =AStar() options = "" if len(sys.argv) > 1: options = sys.argv[1] print "options: ", options k = 1 seed =0 print "Py: ======Evaluation STARTED======" totalIterations = 0 for i in range(k, k+100): options1 = options + "-ls " + str(seed)+" -fps 30 -ld 0" reset(options1) obsDetails = getObservationDetails() agent.setObservationDetails(obsDetails[0], obsDetails[1], obsDetails[2], obsDetails[3]) while (not libamico.isLevelFinished()): totalIterations +=1 libamico.tick(); obs = getEntireObservation(1, 0) agent.integrateObservation(obs[0], obs[1], obs[2], obs[3], obs[4]); action = agent.getAction() performAction(action); evaluationInfo = getEvaluationInfo() print evaluationInfo seed += 1