def _get_path(self, startpoint, food, others, walls, posion=[], guess_posion=[]): counts = 0 next = None for e in food: endpoint = e mapdata = [] for y in range(self.map['size'][1]): for x in range(self.map['size'][0]): rc = [x, y] if rc == self.head: mapdata.append(5) continue if rc == endpoint: mapdata.append(6) continue if rc in others or rc in walls or rc in posion or rc in guess_posion: mapdata.append(-1) continue mapdata.append(1) astar = AStar(SQ_MapHandler(mapdata, self.map['size'][0], self.map['size'][1])) start = SQ_Location(startpoint[0], startpoint[1]) end = SQ_Location(endpoint[0], endpoint[1]) p = astar.findPath(start, end) if not p:continue if len(p.nodes) < counts or next == None: counts = len(p.nodes) next = [p.nodes[0].location.x , p.nodes[0].location.y] return next
def main(): io_handler = IOHandler() input_values = io_handler.read_input_file() strategy = io_handler.get_strategy strategy_param = io_handler.get_strategy_param if strategy == 'bfs': bfs = BFS(strategy_param, input_values) bfs.solve_puzzle() io_handler.write_result_file(bfs.solution_length, bfs.solution_path) io_handler.write_stat_file(bfs.solution_length, bfs.number_of_visited_nodes, bfs.number_of_processed_nodes, \ bfs.recursion_depth, bfs.solution_time) elif strategy == 'dfs': dfs = DFS(strategy_param, input_values) dfs.solve_puzzle() if dfs.solution_length == -1: io_handler.write_wrong_result_file(dfs.solution_length) io_handler.write_wrong_stat_file(dfs.solution_length) else: io_handler.write_result_file(dfs.solution_length, dfs.solution_path) io_handler.write_stat_file(dfs.solution_length, dfs.number_of_visited_nodes, dfs.number_of_processed_nodes,\ dfs.recursion_depth, dfs.solution_time) elif strategy == 'astr': a_star = AStar(strategy_param, input_values) a_star.solve_puzzle() io_handler.write_result_file(a_star.solution_length, a_star.solution_path) io_handler.write_stat_file(a_star.solution_length, a_star.number_of_visited_nodes, \ a_star.number_of_processed_nodes, a_star.recursion_depth, a_star.solution_time) else: print('nieprawdiłowa nazwa strategii')
def use_lizard(self, action, game_map, player_info): max_ = speed_bracket[player_info.damage] if action == Commands.USE_LIZARD: if not itemize_command(action) in player_info.power_ups: return { "player_info": player_info, "game_map": game_map, "progression": False, "additional_score": -1000 } y = player_info.position.y x = player_info.position.x + player_info.speed progress = True # goes deeper into the game tree? if not x in game_map[y].keys(): x = [i for i in game_map[y].keys()][-1] progress = False a = AStar() path = a.forward(player_info.position, Position(y, x), game_map) p_speed = 0 # plaussible speeds if not player_info.boosting: if "BOOST" in player_info.power_ups: p_speed = max_ - player_info.speed else: n_speed = max_ if player_info.boosting else speeds[ player_info.speed].right p_speed = n_speed if n_speed else player_info.speed score = 0 if True: score += 4 player_info.power_ups.remove(itemize_command(action)) score -= abs((player_info.position.x + player_info.speed) - (player_info.position.x + p_speed)) score -= abs((player_info.position.x + player_info.speed) - [i for i in game_map[y].keys()][-1]) # walking the paths to collect damages/speed-deduction and points path_walk = self.walk_path(path, game_map, is_lizard=True) score += len( path_walk["powerups"] ) * 4 # <-- actually using of powerups will justify this more score += self.compute_wall_damages(path_walk["obstacles"], player_info) player_info.power_ups += path_walk["powerups"] if progress: player_info.position = Position(y, x) return { "player_info": player_info, "game_map": game_map, "progression": progress, "additional_score": score }
def call_AStar(output_file): """ AStar ~ Deschid fisierul ~ Verific datele de intrare ~ Creez obiectul de tip AStar ~ Apelez functia Solve ~ Afisez solutia """ g = open((output_folder + '/' + output_file), 'w') if not isInRange(matrix, robinet_x, robinet_y) or not isInRange( matrix, canal_x, canal_y): g.write('Datele de input sunt gresite. Problema nu are solutii') else: g.write('AStar\n') for heuristic in heuristics: g.write('\n\n\n' + heuristic + '\n\n') a = AStar(heuristic, deepcopy(matrix), robinet_x, robinet_y, canal_x, canal_y, timeout) paths = a.Solve(solutions_number) print_paths(paths, g) g.write( '\n\n\n\n ______________________________________________________ \n\n\n\n' ) g.close()
def _add_new_env(self): ''' Add a new environment to the environment list. ''' # Generate a random map and make sure that all non-obstacle # positions can reach the goal. while True: model_settings = { 'height': self.height, 'width': self.width, 'obs_count': self.num_obstacle, 'random_seed': self.seed, 'device': self.device } self.seed += 1 new_env = PathPlanningEnv(**model_settings) astar = AStar(new_env.grid[2, :, :], (new_env.goal_row, new_env.goal_col)) all_reachable = True for row in range(self.height): for col in range(self.width): if new_env.grid[2,row,col] == 0 \ and new_env.grid[1,row,col] != 1 \ and not astar.plan(row, col): all_reachable = False if all_reachable: break self.envs.append(new_env)
def __init__(self, side, field: np.ndarray, my_player, enemy): self.side = side self.me = my_player self.enemy: PublicPlayer = enemy self.field = field self.a_star = AStar() self.obj_dist = ObjectDistance(self.field, self.side) self.defence_line = self.get_defenceline()
def act(self, ant): # Create a path to the home node at 0,0 aStar = AStar(ant.tiles, ant.tiles[ant.x][ant.y], ant.tiles[0][0]) path = aStar.takeStep() if path != None: while aStar.done == False: path = aStar.takeStep() ant.path = path
def accelerate(self, action, game_map, player_info): max_ = speed_bracket[player_info.damage] if action == Commands.ACCELERATE: n_speed = None if not player_info.boosting: n_speed = speeds[player_info.speed].right if player_info.boosting or not n_speed or n_speed > max_: return { "player_info": player_info, "game_map": game_map, "progression": None, "additional_score": -1000 } player_info.speed = n_speed y = player_info.position.y x = player_info.position.x + n_speed progress = True # goes deeper into the game tree? if not x in game_map[y].keys(): x = [i for i in game_map[y].keys()][-1] progress = False a = AStar() path = a.forward(player_info.position, Position(y, x), game_map) p_speed = 0 # plaussible speeds if not player_info.boosting: if "BOOST" in player_info.power_ups: p_speed = max_ - player_info.speed else: p_speed = n_speed score = 0 score -= abs((player_info.position.x + n_speed) - (player_info.position.x + p_speed)) score -= abs((player_info.position.x + n_speed) - [i for i in game_map[y].keys()][-1]) # walking the paths to collect damages/speed-deduction and points path_walk = self.walk_path(path, game_map, is_lizard=False) score += len( path_walk["powerups"] ) * 4 # <-- actually using of powerups will justify this more score += self.compute_wall_damages(path_walk["obstacles"], player_info) player_info.power_ups += path_walk["powerups"] if progress: player_info.position = Position(y, x) return { "player_info": player_info, "game_map": game_map, "progression": progress, "additional_score": score }
def CalcPath(self, start, goal): astar = AStar(self.nodes) print('Calculating Shortest Path') coords = astar.GetPath(start, goal) if coords == None: post({'astar': False}, 'astar') coords = {} #print(coords) post({'coords': coords}, 'coords')
def calcAStar(gridMap): aStar = AStar() # shortest path algorithm class pathWaypoints = aStar.findPath(gridMap) if not pathWaypoints == None: for gridCell in pathWaypoints: if not gridCell == None: gridMap.setGridCell(gridCell.position.x, gridCell.position.y, CellConstants.PATH, CellConstants.NORMAL_CELL)
def openFile(self): """Funció per obrir el fitxer de transport per treballar""" fitxer = QtGui.QFileDialog.getOpenFileName( self, "Selecciona un fitxer de Transport", ".", "*.yaml") if not fitxer or fitxer == "": return self.trans = Transport.loadFile(str(fitxer)) self.loadStations() self.mF.setTrans(self.trans) self.a = AStar(self.trans) self.unLockForm()
def generate_dataset(self, dataset_size: int): generator = AStar(self.generator) X, y = [Board.ordered()], [[0]] while len(X) < dataset_size: boards = Board.scrambled(self.max_steps, True) path, pathLength, _, _ = generator.run(boards[-1]) for i, board in enumerate(path[:-1]): X.append(board) y.append([min(pathLength - 1 - i, self.max_steps)]) # to ensure consistent shape X = [self.network.transform(board) for board in X] return [nd.array(a, ctx=self.cfg.context) for a in (X, y)]
def act(self, ant): # Select a random water tile from the ones it knows about ran = random.randint(0, len(ant.knownWater) - 1) randomWater = ant.knownWater[ran] # Create a path to that tile. aStar = AStar(ant.tiles, ant.tiles[ant.x][ant.y], randomWater) path = aStar.takeStep() if path != None: while aStar.done == False: path = aStar.takeStep() ant.path = path
def turn_right(self, action, game_map, player_info): max_ = speed_bracket[player_info.damage] if action == Commands.TURN_RIGHT: y = player_info.position.y + 1 if not y in game_map.keys() or max_ == 0: return { "player_info": player_info, "game_map": game_map, "progression": False, "additional_score": -10000 } x = player_info.position.x + player_info.speed - 1 progress = True if not x in game_map[y].keys(): x = [i for i in game_map[y].keys()][-1] progress = False a = AStar() path = a.forward(player_info.position, Position(y, x), game_map) p_speed = 0 # plaussible speeds if not player_info.boosting: if "BOOST" in player_info.power_ups: p_speed = max_ - player_info.speed - 1 else: n_speed = max_ if player_info.boosting else speeds[ player_info.speed].right p_speed = n_speed - 1 if n_speed else player_info.speed - 1 score = 0 score -= abs((player_info.position.x + player_info.speed) - (player_info.position.x + p_speed)) score -= abs((player_info.position.x + player_info.speed) - [i for i in game_map[y].keys()][-1]) # walking the paths to collect damages/speed-deduction and points path_walk = self.walk_path(path, game_map, is_lizard=False) score += len( path_walk["powerups"] ) * 4 # <-- actually using of powerups will justify this more score += self.compute_wall_damages(path_walk["obstacles"], player_info) player_info.power_ups += path_walk["powerups"] if progress: player_info.position = Position(y, x) return { "player_info": player_info, "game_map": game_map, "progression": progress, "additional_score": score }
def act(self, ant): # Select a random food tile from the ones it knows about. ran = random.randint(0, len(ant.knownFood) - 1) randomFood = ant.knownFood[ran] # Build a path to that tile aStar = AStar(ant.tiles, ant.tiles[ant.x][ant.y], randomFood) path = aStar.takeStep() if path != None: while aStar.done == False: path = aStar.takeStep() ant.path = path
def __init__(self, board): self.visible = False self.limit = board.matrix_dimension[1] - 1 self.initial_position = self.random_position() self.position_x = self.initial_position[0] self.position_y = self.initial_position[1] self.star = AStar(board) # self.star = AStar2(board) self.smell_distance = 2 self.smell_positions = [] self.live = True self.smell_visible = False self.image = "../res/images/wumpus/wumpus.png" self.smell = "../res/images/wumpus/smell.png" self.srt = '({}, {})'
def newPath2(self, startTime): timeList = [] for queue in self.queueList: timeList.append(queue.endTime) newMap = Array2D.Array2D(self.width, self.height) for x in range(len(self.destList)): newAstar = AStar.AStar(newMap, self.startPoint, self.destList[x].position) newAstar.addAllObastacleArea(self.obstacleList) pathList = newAstar.start() if pathList is not None and len(pathList) > 0: print("路径重新规划") else: print("位置已在障碍物区域内部,无法重新规划") # user.inFlag = False distance = self.calPathLength(pathList) totalTime = round(distance / common_utils.calRandSpeed(), 3) * 1000 # 重新规划到达闸机时间 destTime = startTime + totalTime print("calculate destTime:", x, ":", destTime) # 将各闸机口给出的时间 存储到临时列表中,方便计算最早到闸机口时间 if destTime > timeList[x]: timeList[x] = destTime print("updated destTime:", timeList[x]) minTime = 0 for x in range(len(timeList)): if minTime == 0: minTime = timeList[x] if minTime > timeList[x]: minTime = timeList[x] index = x return self.destList[index].id
def start_activity(self): self.markov = False self.place_to_go = self.getPlaceToGo() self.movements = AStar(self, self.pos, self.place_to_go).process() time_in_state = self.model.getTimeInState(self)[list( self.positionByState.keys()).index(self.state)] self.time_activity = int(time_in_state * 60 * 100 / self.model.clock.timeByStep)
def solve_tile_puzzle(selected_algo, n, board): logic = TilePuzzleLogic(n) result = None if selected_algo == 1: idfs = IDFS(logic) result = idfs.search(State(board)) elif selected_algo == 2: bfs = BFS_Seaerch(logic) result = bfs.search(State(board)) elif selected_algo == 3: a_star = AStar(logic) result = a_star.search(HeuristicState(board)) else: raise Exception('Bad input! no such search algorithm') write_results(result)
def tournament(args): i, steps, (name_a, h_a), (name_b, h_b) = args boards = Board.scrambled(steps, True) # fixed length agents = [AStar(h) for h in [h_a, h_b]] results = [agent.run(boards[-1]) for agent in agents] print(f"Round {i}, {name_a}: {results[0][1:]}") print(f"Round {i}, {name_b}: {results[1][1:]}") return results
def dist_goal(self, score1, goal, character, world): """ Returns the world score based on if the character is at or next to the goal :param score1: value given that the character is at or next to the goal for a range of 2 :param world: the game state :param character: the player to evaluate :return: an integer score value """ if character is not None: m_x = character.x m_y = character.y else: return -999 if self.path is None: astar = AStar([m_x, m_y], goal, world, False) self.path = astar.a_star() position_val = 0 multiplier = len(self.path) minimum_dist = 1 blocked_positions = [] for x in range(-1, 2): for y in range(-1, 2): if [x, y] != [0, 0]: nx = m_x + x ny = m_y + y if 0 < nx < world.width() and 0 < ny < world.height( ) and world.wall_at(nx, ny): blocked_positions.append([m_x + 2 * x, m_y + 2 * y]) blocked_positions.append([m_x + 3 * x, m_y + 3 * y]) while position_val == 0: for point in self.path: if not blocked_positions.__contains__(point): x_diff = point[0] - m_x y_diff = point[1] - m_y sq_dist = x_diff * x_diff + y_diff * y_diff lin_dist = sq_dist**.5 if 0 < lin_dist <= minimum_dist: position_val += (1.0 / lin_dist) * multiplier multiplier -= 1 minimum_dist += 1 val = position_val * score1 print("val at " + str(m_x) + ", " + str(m_y) + " is " + str(val)) return (1 / position_val) * score1
def openFile(self): """Funció per obrir el fitxer de transport per treballar""" fitxer = QtGui.QFileDialog.getOpenFileName(self, "Selecciona un fitxer de Transport", ".", "*.yaml") if not fitxer or fitxer == "": return self.trans = Transport.loadFile(str(fitxer)) self.loadStations() self.mF.setTrans(self.trans) self.a = AStar(self.trans) self.unLockForm()
def AStarAlgo(state, heuristic): a = AStar() a.init() start = timeit.default_timer() aAlgo = a.doAStar(state, "1,2,5,3,4,0,6,7,8", heuristic) path = aAlgo[0][0] depth = aAlgo[0][1] explored = aAlgo[1] stop = timeit.default_timer() print('============= Time =============') print(stop - start) print('============= Depth =============') print(depth) print('============= Explored =============') print(explored) print('============= Path =============') for i in path: printBoard(i.split(','))
class Wumpus(object): def __init__(self, board): self.visible = False self.limit = board.matrix_dimension[1] - 1 self.initial_position = self.random_position() self.position_x = self.initial_position[0] self.position_y = self.initial_position[1] self.star = AStar(board) # self.star = AStar2(board) self.smell_distance = 2 self.smell_positions = [] self.live = True self.smell_visible = False self.image = "../res/images/wumpus/wumpus.png" self.smell = "../res/images/wumpus/smell.png" self.srt = '({}, {})' def random_position(self): x = 0 y = 0 while (x <= y): x = randint(0, self.limit) y = randint(0, self.limit) return (x, y) def position(self): return self.position_x, self.position_y def increment_x(self): self.position_x += 1 def decrement_x(self): self.position_x -= 1 def increment_y(self): self.position_y += 1 def decrement_y(self): self.position_y -= 1 def set_smell_positions(self, positions): self.smell_positions = positions def clean_smell_positions(self): self.smell_positions = [] def __str__(self): return self.srt.format(self.position_x, self.position_y) def __repr__(self): return self.__str__() def move_a_star(self, goal): return self.star.move(self, goal)
def remove_isolated_paths(self): for path_coords in [(i, j) for i in range(len(self.grid)) for j in range(len(self.grid[i])) if self.grid[i, j] == self.path_value]: if path_coords not in [self.entry_coords, self.exit_coords]: if AStar(self.grid, path_coords, self.entry_coords, use_euclidean_distance=False).get_shortest_path( ) is None: self.grid[path_coords] = self.obstacle_value
def _compute_all_distances(self, silent=True): assert self.grid_initial.shape[0] != 0, 'Error: expect a non-empty grid' planner = AStar(self.grid_initial[2, :, :], (self.goal_row, self.goal_col), False) if not silent: print(self.grid_initial[2, :, :]) self.distances = torch.empty(size=(self.height, self.width)) for i in range(self.height): for j in range(self.width): if not silent: print("Evaluate distance from ({}, {}) to ({}, {})".format( i, j, self.goal_row, self.goal_col)) distance = 100 if (self.grid_initial[2, i, j] == 0): path = planner.plan(i, j) distance = len(path) if not silent: print(" Distance {}".format(distance)) self.distances[i, j] = distance
def main(): rospy.init_node('location_monitor') cmd_publish = rospy.Publisher('/cmd_vel', Twist, queue_size=10) rate = rospy.Rate(1) # 10hz # to induce the wandering behaviour stepSize = [int(sys.argv[7]), int(sys.argv[8])] clearance = 5 aStar = AStar([ int((float(sys.argv[1]) + 5) * 10), int((float(sys.argv[2]) + 5) * 10), int(degrees(float(sys.argv[3]))) ], [ int((float(sys.argv[4]) + 5) * 10), int((float(sys.argv[5]) + 5) * 10) ], clearance + int(sys.argv[6]), stepSize) solution = aStar.solve() print(len(solution)) temp = solution[0] for i in range(1, len(temp)): command = Twist() # to induce the linear motion #command.link_name = 'turtlebot3_burger::wheel_left_link' #command.angular.z = 0.6 r = 0.0033 l = 0.9 action = temp[i].action rpm1, rpm2 = getRPM(action, stepSize) command.linear.x = r / 2 * (rpm1 + rpm2) command.angular.z = r / l * (rpm1 - rpm2) print(command.linear.x, command.angular.z) #command.twist = twist #command.reference_frame = "map" # to induce the angular motion cmd_publish.publish(command) sleep(10) command = Twist() command.linear.x = 0 command.angular.z = 0 cmd_publish.publish(command)
def __init__(self, X, Y, height=100.0, metric='euclidean', p=2, smoothing=0.5, maxdrift=100, silent=True): if not silent: print("[DTW] Calculating Distance Matrix", len(X), len(Y)) cost = sp_spatial.distance.cdist(X, Y, metric=metric, p=p) im, jm = cost.shape[0], cost.shape[1] def neighbor_func(n): ns = [] r = n[0] != im - 1 and ((n[0] / im) - (n[1] / jm)) * ((im + jm) / 2) < maxdrift d = n[1] != jm - 1 and ((n[1] / jm) - (n[0] / im)) * ((im + jm) / 2) < maxdrift if r: ns.append((n[0] + 1, n[1])) if d: ns.append((n[0], n[1] + 1)) if r and d: ns.append((n[0] + 1, n[1] + 1)) return ns def dist_func(n, m): return np.sqrt(np.sum( (np.array(n) - np.array(m))**2.0)) + height * cost[m[0], m[1]] if not silent: print("[DTW] Performing Path Search", len(X)) astar = AStar(neighbor_func, dist_func, dist_func, bias=0.0, silent=silent) path = np.array(astar((0, 0), (im - 1, jm - 1))).astype(np.float) path[0] = ((1 - smoothing) * path[0] + (smoothing) * path[1]) path[1:-1] = (0.5 * (1 - smoothing) * path[:-2] + (smoothing) * path[1:-1] + 0.5 * (1 - smoothing) * path[2:]) path[-1] = ((1 - smoothing) * path[-1] + (smoothing) * path[-2]) for i in range(1, len(path)): if path[i, 1] <= path[i - 1, 1]: path[i, 1] = path[i - 1, 1] + 1e-5 self.path = path
def initializeMethod(method_type): if (method_type == UCS_type): return UCSearch(start, goal) elif (method_type == BFS_type): return BFSearch(start, goal) elif (method_type == Astar_type): return AStar(start, goal, OctileDist(), 4, 4) elif (method_type == IDS_type): return IDSearch(start, goal, 50) else: print("Method not recognized...") exit()
def run(self): self.listenConfiguration() algorithm = self.algorithmType.get() if (algorithm == "A*"): AStarAlgorithm = AStar(self.map,self.startGrid,self.targetGrid,self.isPathVisible,self.screen) return AStarAlgorithm.run() elif (algorithm == "BFS"): BFSAlgorithm = BFS(self.map,self.startGrid,self.targetGrid,self.isPathVisible,self.screen) return BFSAlgorithm.run() elif (algorithm == "DFS"): DFSAlgorithm = DFS(self.map, self.startGrid, self.targetGrid, self.isPathVisible,self.screen) return DFSAlgorithm.run() elif (algorithm == "Dijkstra"): DijkstraAlgorithm = Dijkstra(self.map, self.startGrid, self.targetGrid, self.isPathVisible,self.screen) return DijkstraAlgorithm.run() else: return
def main(): # Task 1 map = Map_Obj(1) start_state = (map, map.get_start_pos()[0], map.get_start_pos()[1]) a_star = AStar(start_state, walking_distance, generate_adjacent_states, goal_evaluate) node_path = a_star.run() pos_path = [] for node in node_path: pos_path.append([node.state[1], node.state[2]]) print('Positions in the path of task 1:') print(pos_path) visualise_path_map(map, pos_path) # Task 2 map = Map_Obj(2) start_state = (map, map.get_start_pos()[0], map.get_start_pos()[1]) a_star = AStar(start_state, walking_distance, generate_adjacent_states, goal_evaluate) node_path = a_star.run() pos_path = [] for node in node_path: pos_path.append([node.state[1], node.state[2]]) print('Positions in the path of task 2:') print(pos_path) visualise_path_map(map, pos_path)
def dumb_solution(self, wrld): exit = self.find_exit(wrld) if exit is None: self.move(0, 0) else: print("current pos: " + str(self.x) + ", " + str(self.y)) astar = AStar([self.x, self.y], exit, wrld, False) path = astar.a_star() bomb_needed = False for point in path: if wrld.wall_at(point[0], point[1]): self.place_bomb() bomb_needed = True if path[1][0] == exit[0] and path[1][1] == exit[1]: next_point = path[1] dx = next_point[0] - self.x dy = next_point[1] - self.y self.move(dx, dy) return (new_world, events) = wrld.next() (new_world2, events) = new_world.next() [in_danger, move_plan] = self.in_danger(new_world2, new_world, wrld) if bomb_needed is False or in_danger is False: if len(path) > 0: next_point = path[1] dx = next_point[0] - self.x dy = next_point[1] - self.y self.move(dx, dy) if self.debug: print("moving: " + str( [next_point[0] - self.x, next_point[1] - self.y])) (n_world, events) = wrld.next() if n_world.explosion_at(dx + self.x, dy + self.y): self.move(0, 0) else: self.move(0, 0) else: self.move(move_plan[0], move_plan[1])
def solvePuzzle(self): self.lblSteps['text']='' self.lblSteps.update() self.lblExpandedNodes['text']='' self.lblExpandedNodes.update() self.lblrunningTime['text']='' self.lblrunningTime.update() self.drawBoard(self.rushhour.get_board()) heuristic = ZeroHeuristic() if self.heuristicChoice.get() == 2: heuristic = DistanceFromTargetToExit() elif self.heuristicChoice.get() == 3: heuristic = BlockingExitHeuristic() elif self.heuristicChoice.get() == 4: heuristic = BlockingLowerBoundEstimation() aStar = AStar(heuristic) elapsedTime = time.time() sol = aStar.aStar(self.rushhour) runningTime = round((time.time()-elapsedTime)*1000) lbl = Label(self.bottomRightPanel, text = 'Steps: ') lbl.grid(row = 7, column = 0,sticky=W) steps = 0 for board in sol['Solution']: self.drawBoard(board.get_board()) self.lblSteps['text'] = str(steps) self.lblSteps.update() steps += 1 time.sleep(.5) lbl = Label(self.bottomRightPanel, text = 'Expanded Nodes: ') lbl.grid(row = 8, column = 0,sticky=W) lbl = Label(self.bottomRightPanel, text = 'Running Time: ') lbl.grid(row = 9, column = 0,sticky=W) self.lblExpandedNodes['text'] = str(sol['Expanded Nodes']) self.lblrunningTime['text'] = str(runningTime)+' ms'
def run(self): repeat = False speed_signal = 1360 turn_signal = 1200 map_width = 120*12*2.54 # cm map_height = 220*12*2.54 # cm vehicle_width = 35*2.54 # Width of agent (cm) buffer_distance = 12*2.54 # Space to keep between agent and obstacles (cm) buff_width = vehicle_width + buffer_distance MAX_R = int(10*12*2.54) # Maximum distance to ever consider (cm) R = MAX_R # Maximum distance to consider for a iteration move_scale = 0.25 # Percentage of R to actually move # Create initial map of environment map = AStar(map_width, map_height, vehicle_width) # GPS calculations """ phi latitude longitude 0 110.574 111.32 15 110.649 107.55 30 110.852 96.486 45 111.132 78.847 60 111.412 55.8 75 111.618 28.902 90 111.694 0 """ # Linear interpolation using known values deg_lat = (110.852 - 111.132)/(30 - 45)*(self.initial_position[0] - 45) + 111.132 deg_long = (96.486 - 78.847)/(30 - 45)*(self.initial_position[0] - 45) + 78.847 deg_lat *= 1000*100 # 1 degree latitude = deg_lat cm deg_long *= 1000*100 # 1 degree longitude = deg_long cm angle_to_00 = self.initial_direction - math.atan2(self.normal_location[1], self.normal_location[0]) distance_to_00 = math.sqrt(self.normal_location[0]**2 + self.normal_location[1]**2) zero_zero = ( initial_position[0] - distance_to_00*math.cos(angle_to_00)*deg_lat, initial_position[1] - distance_to_00*math.sin(angle_to_00)*deg_long ) xscale = (zero_zero[0]-self.initial_position[0])/(0-self.normal_location[0]) # Slope yscale = (zero_zero[1]-self.initial_position[1])/(0-self.normal_location[1]) # Slope xshift = -xscale*self.normal_location[0] + self.initial_position[0] # "y" intercept yshift = -yscale*self.normal_location[1] + self.initial_position[1] # "y" intercept theta_shift = self.normal_direction - self.initial_direction # Clear the data lists self.sensors.clear_all() while self.queue.empty(): if not repeat: data = 500000*np.ones(360) # Get LiDAR data data[0:181] = self.sensors.lidar_data() # Get direction from compass and normalize to programmatic world self.direction = self.sensors.compass_data() self.direction = (self.direction - (self.initial_direction-self.normal_direction)) % 360 # Rotate, scale, and translate GPS coordinates to programmatic world self.location = np.array(self.sensors.gps_data()) transform = np.array([ [np.cos(theta_shift), -np.sin(theta_shift)], [np.sin(theta_shift), np.cos(theta_shift)] ]) self.location = np.dot(transform, self.location) # Unrotate GPS coordinates self.location = np.append(self.location, [1]) # Add bias term to allow translation transform = np.array([ [xscale, 0, xshift], [0, yscale, yshift] ]) self.location = np.dot(transform, self.location) # Scale and translate to correct range self.location = tuple(self.location[:2]) # Throw out bias term # Add in past data set from LiDAR for sample in map.last_data_set: r = math.sqrt((self.location[0]-sample[0])**2 + (self.location[1]-sample[1])**2) theta = math.atan2(self.location[1]-sample[1], self.location[0]-sample[0]) theta = np.pi - theta # Needed because y coordinates are upside down theta = (theta*180/np.pi - (self.direction-90)) # Find relative angle from absolute theta = theta % 360 # Only modify unknown region around agent if theta > 180: angle = int(theta) % 360 if r < data[angle]: data[angle] = r # Pick a direction to move viable_angles = [] for i in range(45, 136): theta = (self.direction - 90 + i) % 360 viable = True for rgn in range(math.ceil(buff_width/2), R+10, 5): # Calculate theta range and limits to search for obstacles in range rgn theta_range_needed = 180/np.pi*2*math.asin(buff_width/(2*rgn)) low_lim = math.floor(i-theta_range_needed/2) % 360 high_lim = math.ceil(i+theta_range_needed/2) % 360 # Check if the path around theta is clear if low_lim > high_lim: # Handle the roll over case made possible by "% 360" if np.any(np.concatenate((data[low_lim:], data[0:high_lim])) < rgn): viable = False break else: if (np.any(data[low_lim:high_lim] < rgn)): viable = False break # Save viable angles for further inspection later if viable: viable_angles.append(theta) # If there are no viable angles, try decreasing R if len(viable_angles) == 0: if R > buff_width/2: print("No viable angles, decreasing R") R = int(R*0.9) self.motors.drive(1360) repeat = True continue else: # Stuck situation # TODO: Use A* and Map to find a path out motors.stop() print("Help, I'm stuck and too stupid to get out!") break else: R = MAX_R repeat = False # Determine which viable angle will move you towards goal the fastest min_index = 0 min_distance = 10000000 for i, angle in enumerate(viable_angles): # Simulate moving forward R movement at the current angle and calculate distance to goal x = self.location[0] + move_scale*R*np.cos(angle*np.pi/180) y = self.location[1] - move_scale*R*np.sin(angle*np.pi/180) sim_distance = math.sqrt((x-self.normal_goal[0])**2 + (y-self.normal_goal[1])**2) # Keep the minimum distance to find the most efficient angle if sim_distance < min_distance: min_distance = sim_distance min_index = i min_location = (int(x), int(y)) # Record the resulting data map.record_data(data, self.location, self.direction) # Calculate speed and turning based on amount of turn desired angle_change = self.direction - viable_angles[min_index] speed_signal = int(20*math.fabs(angle_change)/3 + 1000) turn_signal = int(-80*angle_change/9 + 1200) print("Speed: {0}".format(speed_signal)) print("Turn to: {0}".format(angle_change)) print("Turn signal: {0}".format(turn_signal)) self.motors.drive(speed_signal) self.motors.turn(turn_signal)
def get_move(self, gameboard, player, opponent): if not self.init_with_data: self.turrets = {"x": {}, "y": {}} for turret in gameboard.turrets: bounds = [turret.x-4, turret.x+4] for coeff in [-1, 1]: for j in range(1, 5): if gameboard.wall_at_tile[(turret.x+j*coeff) % gameboard.width][turret.y]: bounds[int((coeff+1)/2)] = turret.x+(j-1)*coeff break if(turret.y not in self.turrets['x']): if bounds[1] > gameboard.height: self.turrets['x'][turret.y] = [(0, bounds[1] % gameboard.width, turret), (bounds[0], gameboard.width-1, turret)] elif bounds[0] < 0: self.turrets['x'][turret.y] = [(bounds[0] % gameboard.width, gameboard.width-1, turret), (0, bounds[1], turret)] else: self.turrets['x'][turret.y] = [(bounds[0], bounds[1], turret)] else: if bounds[1] > gameboard.height: self.turrets['x'][turret.y] += [(0, bounds[1] % gameboard.width, turret), (bounds[0], gameboard.width-1, turret)] elif bounds[0] < 0: self.turrets['x'][turret.y] += [(bounds[0] % gameboard.width, gameboard.width-1, turret), (0, bounds[1], turret)] else: self.turrets['x'][turret.y] += [(bounds[0], bounds[1], turret)] for turret in gameboard.turrets: bounds = [turret.y-4, turret.y+4] for coeff in [-1, 1]: for j in range(1, 5): if gameboard.wall_at_tile[turret.x][(turret.y+j*coeff) % gameboard.width]: bounds[int((coeff+1)/2)] = turret.y+(j-1)*coeff break print(bounds) if(turret.x not in self.turrets['y']): if bounds[1] >= gameboard.width: self.turrets['y'][turret.x] = [(0, bounds[1] % gameboard.height, turret), (bounds[0], gameboard.height-1, turret)] elif bounds[0] < 0: self.turrets['y'][turret.x] = [(bounds[0] % gameboard.height, gameboard.height-1, turret), (0, bounds[1], turret)] else: self.turrets['y'][turret.x] = [(bounds[0], bounds[1], turret)] else: if bounds[1] >= gameboard.width: self.turrets['y'][turret.x] += [(0, bounds[1] % gameboard.height, turret), (bounds[0], gameboard.height-1, turret)] elif bounds[0] < 0: self.turrets['y'][turret.x] += [(bounds[0] % gameboard.height, gameboard.height-1, turret), (0, bounds[1], turret)] else: self.turrets['y'][turret.x] += [(bounds[0], bounds[1], turret)] grid = Grid(gameboard) astar = AStar(grid, player, gameboard, self.turrets) objectives = astar.process() self.objectives = objectives; #print(objectives) self.init_with_data = True #CHOOSE THE PATH Using indexMax #If turn is 1: #decrement all the turns and pop and return foward #Else: #decrement all the turns and change the face for turret in gameboard.turrets: print("~~~", (turret.x, turret.y), turret.is_dead, "~~~") if len(self.force_stack) > 0: print("SHIELDING") return self.force_stack.pop() else: if self.last_index != None and not self.objectives[self.last_index]['path']: grid = Grid(gameboard) astar = AStar(grid, player, gameboard, self.turrets) objectives = astar.process() self.objectives = objectives; print("\n") new_list = [] x = -1 for pair in self.objectives: x += 1 if(pair['path'] == None or pair['path'][0]['turns'] == 0): del(self.objectives[x]) continue distance = pair['path'][0]['turns']; new_list.append(distance) index = new_list.index(min(new_list)); #index = index_max(objectives, gameboard.get_turns_remaining()); Path = self.objectives[index]; self.last_index = index print(Path) for step in Path['path']: step['turns'] -= 1 if (Path['path'][-1]['turns'] == 0): if self.wait_move(gameboard, player, opponent, Path['path'][-1]['coord']): return Move.NONE; if self.away(opponent, player, gameboard, 1): if opponent.shield_active: step['turns'] += 1; return Move.SHOOT; if self.away(opponent, player, gameboard, 2): if (opponent.shield_count == 0): step['turns'] += 1; return Move.SHOOT; if len(Path['path']) == 1 and isinstance(Path['objective'], Turret) and "_need_powerup" in Path['path'][-1] and Path['path'][-1]['_need_powerup']: #this is the last step of a turret instruction, and a powerup is needed if player.laser_count > 0: self.force_stack.append(Move.LASER) elif player.shield_count > 0: self.force_stack.append(Move.SHIELD) else: step['turns'] += 1; Path['path'].pop() return Move.NONE # else: # # we can shoot the turret! # dist = abs(Path['objective'].x - Path['path'][-1]['coord'][0]) + abs(Path['objective'].y - Path['path'][-1]['coord'][1]) # # we have to push onto stack backwards # self.force_stack.append( Move.FORWARD ) # self.force_stack.append( player.direction ) # self.force_stack.append( Move.SHOOT ) # self.force_stack.append(\ # self.normalized_directions[(ceil((Path['objective'].x - Path['path'][-1]['coord'][0])/dist), ceil((Path['objective'].y - Path['path'][-1]['coord'][1])/dist))][1] \ # ) motion = (Move.FORWARD if Path['path'][-1]['move'] == self.direction_to_move[player.direction] else Path['path'][-1]['move']) Path['path'].pop(); # print("~~~~", motion, "~~~~") return motion else: # print(Path['path'][-1]) return Path['path'][-1]['move']
randomVehicleID = TRUCK_ID[truckIDIndex] else : randomVehicleID = CAR_ID[carIDIndex] randomSize = 2 x = random.randint(0,5) y = random.randint(0,5) while not self.__onBoundries(x, y, randomSize, randomVType) : x = random.randint(0,5) y = random.randint(0,5) return Vehicle(randomVehicleID,x,y,randomVType) ########################## boardConfigsFolder = "C:/Users/Abbas/PycharmProjects/Rush Hour Puzzle Game/New folder" if __name__ == '__main__': aStar = AStar(BlockingExitHeuristic()) rushHourPuzzles = [] generator = Generator(12,12) i = 0 while i < 25: puzzle = generator.generate() if puzzle: sol = aStar.aStar(puzzle) if sol['Steps'] > 10 : while True: alreadyExist = False for p in rushHourPuzzles: if p.isSamePuzzle(puzzle): alreadyExist = True break if not alreadyExist:
import pygame from AStar import AStar # Define colors black = ( 0, 0, 0) white = ( 255, 255, 255) green = ( 0, 255, 0) red = ( 255, 0, 0) boardsize = 100 size=[500,500] search = AStar(boardsize,(4,1),(40,70)) board = search.getBoard() pygame.init() # Setup screen screen=pygame.display.set_mode(size) pygame.display.set_caption("My Game") #Loop until the user clicks the close button. done=False clock=pygame.time.Clock() while done==False: for event in pygame.event.get(): if event.type == pygame.QUIT: done=True #Exit main loop screen.fill(white)
def avoid(self): map_width = 100*12*2.54 # cm map_height = 200*12*2.54 # cm location = (600, 580) # Initial position (bottom center) goal = (600, 80) # Goal direction = 180 # Initial direction (degrees) vehicle_width = 35*2.54 # Width of agent (cm) buffer_distance = 12*2.54 # Space to keep between agent and obstacles (cm) buff_width = vehicle_width + buffer_distance MAX_R = int(10*12*2.54) # Maximum distance to consider (cm) R = MAX_R # Maximum distance to consider for a iteration move_scale = 0.25 # Percentage of R to actually move # Create initial map of environment map = AStar(map_width, map_height, vehicle_width) while True: data = 500000*np.ones(360) # TODO: Update location based on GPS and direction based on Compass data[0:181] = self.sensors.lidar_data() #vis.setLocation(location) #vis.setDirection(direction) # Add in past 2 data sets from sensors for sample in map.last_data_set: r = math.sqrt(math.pow(location[0]-sample[0], 2) + math.pow(location[1]-sample[1], 2)) theta = math.atan2(location[1]-sample[1], location[0]-sample[0]) theta = np.pi - theta # Needed because y coordinates are upside down theta = (theta*180/np.pi - (direction-90)) # Find relative angle from absolute theta = theta % 360 # Only modify unknown region around agent if theta > 180: angle = int(theta) % 360 if r < data[angle]: data[angle] = r # Pick a direction to move viable_angles = [] for i in range(45, 136): theta = (direction - 90 + i) % 360 viable = True for rgn in range(math.ceil(buff_width/2), R+10, 5): # Calculate theta range and limits to search for obstacles in range rgn theta_range_needed = 180/np.pi*2*math.asin(buff_width/(2*rgn)) low_lim = math.floor(i-theta_range_needed/2) % 360 high_lim = math.ceil(i+theta_range_needed/2) % 360 # Check if the path around theta is clear if low_lim > high_lim: # Handle the roll over case made possible by "% 360" if np.any(np.concatenate((data[low_lim:], data[0:high_lim])) < rgn): viable = False break else: if (np.any(data[low_lim:high_lim] < rgn)): viable = False break # Save viable angles for further inspection later if viable: viable_angles.append(theta) # If there are no viable angles, try adjusting parameters if len(viable_angles) == 0: if R > buff_width/2: # Try decreasing R print("No viable angles, decreasing R") R = int(R*0.9) continue else: # Stuck situation # TODO: Use A* and Map to find a path out print("Help, I'm stuck and too stupid to get out!") break else: R = MAX_R # Determine which viable angle will move you towards goal the fastest min_index = 0 min_distance = 10000000 for i, angle in enumerate(viable_angles): # Simulate moving forward R movement at the current angle and calculate distance to goal x = location[0] + move_scale*R*np.cos(angle*np.pi/180) y = location[1] - move_scale*R*np.sin(angle*np.pi/180) sim_distance = math.sqrt(math.pow(x-goal[0], 2) + math.pow(y-goal[1], 2)) # Keep the minimum distance to find the most efficient angle if sim_distance < min_distance: min_distance = sim_distance min_index = i min_location = (int(x), int(y)) # Record the resulting data map.record_data(data, location, direction) # TODO: send instructions to motor control #location = min_location #direction = viable_angles[min_index]
astar.setMode(1) end = astar.run() print("Nodes expanded: {}\nFinal path lenghth: {}\n".format(astar.expanded_nodes, end.g)) astar.init(start) astar.setMode(2) end = astar.run() print("Nodes expanded: {}\nFinal path lenghth: {}\n".format(astar.expanded_nodes, end.g)) if (__name__ == "__main__"): running = True pygame.init() display = pygame.display.set_mode((WIDTH, HEIGHT), pygame.DOUBLEBUF | pygame.HWSURFACE) pygame.display.set_caption("AStar testing") renderer = ManhattenRenderer(display) astar = AStar(renderer) #read args from commandline args = sys.argv start, goal, map = parse_file(sys.argv[1]) goal.hash = goal.generateHash() start.goal = goal astar.init(start) astar.render(start) fpsClock = pygame.time.Clock() while running: for event in pygame.event.get():
sourceNeighbors = [] for i in range(1, len(self.vertex)): (tmpIndex, (tempX, tempY), NotImportant) = self.vertex[i] if self.isWayBlocked((tempX, tempY), (x1, y1)): pass else: sourceNeighbors.append((i, (tempX, tempY))) if self.isWayBlocked((tempX, tempY), (x2, y2)): pass else: self.graph[i].append((len(self.vertex), (x2, y2))) self.vertex.append((len(self.vertex), (x2, y2), 255)) self.graph[0] = sourceNeighbors AstarInstance = AStar() resultRoad = AstarInstance.run(self.graph, (0, (x1, y1)), (len(self.vertex) - 1, (x2, y2)), "temp.txt") resultRoad.insert(0, (0, (x1, y1))) self.vertex[0] = None self.vertex.pop() self.graph[0] = None return resultRoad def draw(self, result): test = cv2.imread("test.jpg") for (index, (x, y)) in result: cv2.circle(test, (x, y), 3, (0, 0, 255)) cv2.imshow("images", test) cv2.waitKey(0)
class TransPublic(QtGui.QMainWindow): """ Interficie grafica per l'algorisme """ SELECT_DEFECTE = u"Escull una estació" def __init__(self): QtGui.QMainWindow.__init__(self) self.trans = None # Per mantenir l'estructura del fitxer de transport en memoria self.stOrigen = None self.stDesti = None self.a = None self.cami = None self.ui = uic.loadUi('transPublic.ui') #Preparem el Frame del mapa i el posem al form self.mF = Map(self,None) lay = QtGui.QGridLayout() lay.addWidget(self.mF) self.ui.mapGruopBox.setLayout(lay) self.connect(self.ui.menuOpen, QtCore.SIGNAL("activated()"), self.openFile) self.connect(self.ui.cbOrigen, QtCore.SIGNAL("activated(QString)"), self.cbOrigen_changed) self.connect(self.ui.cbDesti, QtCore.SIGNAL("activated(QString)"), self.cbDesti_changed) self.connect(self.ui.pbCalcula, QtCore.SIGNAL("clicked()"), self.calcula) self.ui.show() def setOrigen(self,st): self.stOrigen = st def setDesti(self,st): self.stDesti = st def unLockForm(self): self.ui.centralwidget.setEnabled(True) def openFile(self): """Funció per obrir el fitxer de transport per treballar""" fitxer = QtGui.QFileDialog.getOpenFileName(self, "Selecciona un fitxer de Transport", ".", "*.yaml") if not fitxer or fitxer == "": return self.trans = Transport.loadFile(str(fitxer)) self.loadStations() self.mF.setTrans(self.trans) self.a = AStar(self.trans) self.unLockForm() def loadStations(self): cbOrigen = self.ui.cbOrigen cbOrigen.clear() cbOrigen.addItem(self.SELECT_DEFECTE) cbDesti = self.ui.cbDesti cbDesti.clear() cbDesti.addItem(self.SELECT_DEFECTE) for st in self.trans.stations: text = str(st.id)+" "+st.name cbOrigen.addItem(text) cbDesti.addItem(text) def seleccionaEstacio(self,t,funcio): # Permet seleccionar tant l'origen com la desti, per no repetir codi self.cami = None # reiniciem cami! self.ui.tResultat.setText("") # reiniciem text resultat if not t == self.SELECT_DEFECTE: text = str(t) ident = int(text.split(" ")[0]) # Separem per espais i agafem la ID funcio(self.trans.getStationByID(ident)) # marquem l'estació com seleccionada else: funcio(None) # marquem l'estació com seleccionada self.mF.repaint() # repintem def cbOrigen_changed(self,text): self.seleccionaEstacio(text,self.setOrigen) def cbDesti_changed(self,text): self.seleccionaEstacio(text,self.setDesti) def calcula(self): if self.stOrigen and self.stDesti: # Agafem valors de spinbox (si son diferents a 0, sino None) max_dist = None if int(self.ui.spDistancia.value()) == -1 else int(self.ui.spDistancia.value()) max_transbords = None if int(self.ui.spTransbords.value()) == -1 else int(self.ui.spTransbords.value()) max_parades = None if int(self.ui.spParades.value()) == -1 else int(self.ui.spParades.value()) max_caminant = int(self.ui.spCaminant.value()) # sempre hi ha un maxim caminant! # Executem algorisme a estrella camins = self.a.doAStarSearch(self.stOrigen,self.stDesti,max_dist,max_transbords,max_parades,max_caminant) if len(camins) > 0: self.cami = camins[0] textHR = self.a.transformPathToHumanReadable(self.cami) # text Human Readable textHR = textHR.replace("\n","<br />") # fiquem els salts de linea en format HTML textQT = QtCore.QString.fromUtf8(textHR) # el passem a format QT ara es veuran bé els accents al QTextEdit self.ui.tResultat.setHtml(textQT+"<br /><br />Cost (temps): <b>"+str(self.cami.cost)+"</b>\ <br />Distancia: <b>"+str(self.cami.distancia)+"</b>\ <br />Transbords: <b>"+str(self.cami.transbords)+"</b>\ <br />Parades: <b>"+str(self.cami.parades)+"</b>\ <br />Temps caminant: <b>"+str(self.cami.caminant)+"</b>") else: # No hi ha solucio self.ui.tResultat.setHtml(QtCore.QString.fromUtf8("No hi ha un camí possible amb aquests parametres")) self.mF.repaint() # repintem def selectStationByCoords(self,x,y,button): st = self.trans.getStationByCoords(x,y) # pot ser que no la trobi per click en coordenades on no hi ha res if st: # Boto esquerre: Origen. Boto dret: Desti if button == QtCore.Qt.LeftButton: cb = self.ui.cbOrigen funcio = self.setOrigen elif button == QtCore.Qt.RightButton: cb = self.ui.cbDesti funcio = self.setDesti else: return # No fem res for i in range(1,cb.count()): # A la 0 hi ha el Escull... text = str(cb.itemText(i)) ident = int(text.split(" ")[0]) if (st.id == ident): # La marquem com seleccionada cb.setCurrentIndex(i) self.seleccionaEstacio(cb.itemText(i),funcio)
renderer = Renderer(graphic_size,map_type) # Get the size of the map in pixels for setting the # display size map_size_pixels = renderer.get_map_size_pixels(map_size) screen = pygame.display.set_mode(map_size_pixels,pygame.RESIZABLE) screen.fill(background_color) # Create a randomly generated Node_Map map node_map = Node_Map(map_size, start, end, barrier_percentage, map_type) astar = AStar() # Main Loop ================================================ # render map up-front and only re-render # when something changes path = [] path = astar.find_path(node_map.get_node_dict(), node_map.start_pos, node_map.end_pos, node_map.adjacency_function) renderer.render(node_map,path,screen) while True: for event in pygame.event.get():
# -*- coding: UTF-8 -*- import time from ManhattanDistance import ManhattanDistance from AStar import AStar from Node import Node from pprint import pprint heuristic = ManhattanDistance() astar = AStar(heuristic) start = [ [ 1, 6, 7, 5], [ 9, 3, 10, 2], [13, 8, 4, 12], [14, 11, 15, 0] ] startTime = time.time() startComplexity = heuristic.compute( Node(start, [], None) ) result = astar.solve(start) if result is None: print('No solution found') else: pprint(result) print('Heuristic said at least %d moves were needed.' % startComplexity) print('Actually solution is %d moves away. Best solution found guaranteed!' % len(result)) print('Solved in %d seconds.' % (time.time() - startTime))