def main(args): # Astar Module if args.model == "astar": model = Astar() # Reinforcement Learning module elif args.model == "rl": model = Rl() # Dummy test elif args.model == "dummy": model = Dummy() # Create an environment env = gym.make('voilier-v2').unwrapped state = env.reset() # TODO Get range from argument for step in range(0, 200, 1): action = model.step(state) state, _, _, _ = env.step(action) env.render()
def __init__(self, n_features, n_actions): self.n_features = n_features self.n_actions = n_actions # env, simulate observations env = open('env_hole.pkl', 'rb') self.observe_env = pickle.load(env) env.close() env = open('env_hole_vol.pkl', 'rb') self.observe_vol = pickle.load(env) env.close() # current position self.pos_x = None self.pos_y = 1.0 self.pos_z = None # 8 action dim self.action_labels = [ '0', '45', '90', '135', '180', '225', '270', '315' ] self.actor = Actor(self.n_features, self.n_actions, lr=0.004) self.actor.load_trained_model("save/multiple/hole/save100.ckpt") # fixme, first define critic before load : will report bug for not found in checkpoint self.critic = Critic(self.n_features, self.n_actions, lr=0.003, gamma=0.95) # fixme, use trained model to predict self.bin_graph = tf.Graph() with self.bin_graph.as_default(): self.bin_classfic = BinSupervisor(366, 2) # fixme, avoid obstacles, env defined in A star self.astar = Astar()
def test_astar2(self): nodes2 = { "A": [("B", 2), ("E", 2)], "B": [("A", 2), ("C", 2)], "C": [("B", 2), ("D", 2)], "D": [("C", 2), ("G", 2)], "E": [("A", 2), ("J", 3)], "F": [("B", 2), ("K", 2), ("G", 2)], "G": [("F", 2), ("D", 2), ("L", 4), ("H", 3)], "H": [("G", 3), ("I", 2)], "I": [("H", 2), ("M", 3)], "J": [("E", 3), ("O", 4)], "K": [("F", 2), ("O", 2)], "L": [("G", 4), ("M", 1), ("P", 5)], "M": [("I", 3), ("L", 1)], "O": [("K", 2), ("J", 4)], "P": [("L", 5)] } astar2 = Astar(nodes2, "A", "P") result2 = astar2.search() self.assertEqual(result2, ['A', 'B', 'C', 'D', 'G', 'L', 'P'])
def run(self): while not self.exit: self.screen.fill((255, 255, 255)) self.draw_grid(self.screen, self.grid, self.grid_size, self.margin) algorithm = Astar(self.grid, self.start, self.end) path = algorithm.astar() for event in pygame.event.get(): if event.type == pygame.QUIT: self.exit = True if path == 'failure': print('failure') else: for path_node in path: self.show_node(self.screen, (52, 158, 235), path_node.x, path_node.y, self.margin) pygame.display.flip() self.clock.tick(self.ticks) pygame.quit()
numlist = line.split(' ') if len(numlist) < 15: return problems.append(Puzzle([int(x) for x in numlist[1:]])) print('%5s%10s%10s%10s%10s' % ('#prob', '#exp', '#gen', '|sol|', 'tiempo')) problems = [] load_problems(problems) total_time = 0 total_cost = 0 total_expansions = 0 total_problems = len(problems) # cambiar si quieres menos problemas for prob in range(0, total_problems): init = problems[prob] # problema aleatorio s = Astar(init, heuristic, 1) result = s.search() print('%5d%10d%10d%10d%10.2f' % (prob + 1, s.expansions, len( s.generated), result.g, s.end_time - s.start_time)) total_time += s.end_time - s.start_time total_expansions += s.expansions total_cost += result.g if show_solutions: print(result.trace()) print('Total time: %.3f' % (total_time)) print('Expansiones totales: %d' % (total_expansions)) print('Total cost: %.3d' % (total_cost))
def setUp(self): self.searcher1 = Astar((0, 0), (10, 10), 10) self.searcher2 = Astar((-5, -5), (5, 5), 10) self.searcher3 = Astar((0, 0), (10, 10), 100)
def main(): global corner1, corner2, numCells global searcher, newPath rospy.init_node('astar_alpha_main') # set the parameters specified within the launch files # corner 1 and corner 2 specify the area in the map # that astar will consider # Corner1 if rospy.has_param('corner1x'): corner1x = rospy.get_param('corner1x') else: corner1x = -6.25 if rospy.has_param('corner1y'): corner1y = rospy.get_param('corner1y') else: corner1y = 8.2 # Corner2 if rospy.has_param('corner2x'): corner2x = rospy.get_param('corner2x') else: corner2x = 15.75 if rospy.has_param('corner2y'): corner2y = rospy.get_param('corner2y') else: corner2y = 28.2 corner1 = (corner1x, corner1y) corner2 = (corner2x, corner2y) # numCells specifies the number of cells to divide the # specified astar search space into if rospy.has_param('numCells'): numCells = rospy.get_param('numCells') else: numCells = 100 # topic that the node looks for the closed points on if rospy.has_param('inflatedTopic'): inflatedTopic = rospy.get_param('inflatedTopic') else: inflatedTopic = '/costmap_local_alpha/costmap_local/inflated_obstacles' # topic that the node looks for goal messages on if rospy.has_param('goalTopic'): goalTopic = rospy.get_param('goalTopic') else: goalTopic = 'goal_point' # initialize an instance of the Astar class searcher = Astar(corner1, corner2, numCells) naptime = rospy.Rate(RATE) print "corner1: " print corner1 print "" print "corner2: " print corner2 print "" print "numCells: %i" % numCells print "" print "goal topics: %s" % goalTopic print "" print "inflatedTopic: %s" % inflatedTopic rospy.Subscriber(goalTopic, GoalMsg, goalCallback) rospy.Subscriber(inflatedTopic, GridCellsMsg, inflatedObstaclesCallback) rospy.Subscriber('map_pos', PoseStampedMsg, poseCallback) pathPointPub = rospy.Publisher('point_list', PointListMsg) first_run = True pointList = PointListMsg() while not rospy.is_shutdown(): pointList.new = newPath print "searcher.start" print searcher.start print "" print "searcher.goal" print searcher.goal print "" print "newPath" print newPath print "" print "searcher.path" print searcher.path print "" pointList.points = [] for point in searcher.path: pathPoint = PointMsg() pathPoint.x = point[0] pathPoint.y = point[1] pointList.points.append(pathPoint) pathPointPub.publish(pointList) if newPath: newPath = False naptime.sleep()
altitude, safe_distance = 5, 3 self.grid = create_grid(data, altitude, safe_distance) def get_grid(self): return self.grid def show(self, x_values, y_values): skeleton = medial_axis(invert(self.grid)) plt.imshow(self.grid, origin='lower') plt.imshow(skeleton, cmap='Greys', origin='lower', alpha=0.7) plt.xlabel('EAST') plt.ylabel('NORTH') plt.plot(x_values, y_values, 'g') plt.show() start_ne = (25, 100) goal_ne = (750., 370.) medial = Medial('colliders.csv') grid = medial.get_grid() astar = Astar(grid) found, paths = astar.travel(start_ne, goal_ne) path = astar.trace_back(paths) if found else exit("Couldn't find a path") xpoints, ypoints = astar.axis_points(path) medial.show(xpoints, ypoints)
Puzzle.initialize_pdb(3) # heuristica basda en distancia manhattan heuristic = Puzzle.pdb_best # una heurística basada en pattern databases que debes implementar # inicialización de la heurística basada en PDBs print('%5s%10s%10s%10s%10s%10s' % ('#prob', '#exp', '#gen', '|sol|', 'tiempo', 'maxsubopt')) problems = [] load_problems(problems) total_time = 0 total_cost = 0 total_expansions = 0 #num_problems = len(problems) # cambiar si quieres ejecutar sobre todos los problemas num_problems = 10 # solo ejecutamos los primeros 10 problemas for prob in range(0, num_problems): init = problems[prob] s = Astar( init, heuristic, 2 ) # agregar un tercer parámetro una vez que lo hayas transformado en Weighted A* result = s.search() print('%5d%10d%10d%10d%10.2f%10.2f' % (prob + 1, s.expansions, len(s.generated), result.g, s.end_time - s.start_time, s.estimate_suboptimality())) total_time += s.end_time - s.start_time total_cost += result.g total_expansions += s.expansions if show_solutions: print(result.trace()) print('Tiempo total: %.2f' % (total_time)) print('Expansiones totales: %d' % (total_expansions)) print('Costo total: %d' % (total_cost))
def run_astar(episode): MAX_RUNS = 2000000 TURN_ANGLE = 30 FORWARD_STEP_SIZE = 0.25 AGENT_HEIGHT = 0.88 AGENT_RADIUS = 0.18 house_floor_threshold = { '2azQ1b91cZZ': [1.5], '8194nk5LbLH': [1.2], 'EU6Fwq7SyZv': [-1.3, 2.0], 'oLBMNvg9in8': [-0.8, 1.8, 4.6], 'pLe4wQe7qrG': [], 'QUCTc6BB5sX': [-1.5], 'TbHJrupSAjP': [-1.5, 1.7], 'X7HyMhZNoso': [1.8], 'x8F5xyUWy9e': [], 'Z6MFQCViBuw': [], 'zsNo4HB9uLZ': [], } # -- folders data_dir = '../data/ObjectNav/objectnav_mp3d_v1/val/' folder_pred = '../data/ObjectNav/semmap/' floormap_dir = '../data/ObjectNav/freespace_map/' info = json.load(open('../data/ObjectNav/semmap_objnav_info.json', 'r')) # -- setup naming bindings read_tsv = csv.reader(open("mpcat40.tsv"), delimiter="\t") mpcat40 = {line[0]: line[1] for line in read_tsv} jsonfile = json.load(open(os.path.join(data_dir, 'val.json'), 'r')) category_to_mp3d_category_id = jsonfile['category_to_mp3d_category_id'] house = episode['scene_id'].split('/')[1] # -- sample object category target obj_semantic_id = category_to_mp3d_category_id[episode['object_category']] obj_semantic_name = mpcat40[str(obj_semantic_id)] if obj_semantic_name in object_whitelist: sid = object_whitelist.index(obj_semantic_name) + 1 else: return (episode['episode_id'], [], []) # -- select floor -> env if house_floor_threshold[house]: level = bisect.bisect(house_floor_threshold[house], episode['start_position'][1]) else: level = 0 env = house + '_' + str(level) map_world_shift = np.array(info[env]['map_world_shift']) # -- load maps floormap = imread(os.path.join(floormap_dir, env + '.png')) floormap = floormap.astype(np.float) floormap /= 255 floormap = floormap.astype(np.bool) if not os.path.isfile(os.path.join(folder_pred, env + '.h5')): return (episode['episode_id'], [], []) h5file = h5py.File(os.path.join(folder_pred, env + '.h5'), 'r') map_semantic = np.array(h5file['semmap']) observed_map = np.array(h5file['observed_map']) observed_map = observed_map.astype(np.float) h5file.close() map_semantic = np.multiply(map_semantic, observed_map) map_semantic = map_semantic.astype(np.int) #goals = None goals = all_goals[house][episode['episode_id']] goals = np.array(goals) goals -= map_world_shift goals = goals[:, [0, 2]] # -- get init position start_pos = episode['start_position'] start_pos -= map_world_shift start_x = start_pos[0] start_y = start_pos[2] start_row = int(np.round(start_y / resolution)) start_col = int(np.round(start_x / resolution)) start_rot = np.array(episode['start_rotation']) r = R.from_quat(start_rot) _, start_heading, _ = r.as_rotvec() start_heading = (start_heading * 180 / np.pi) start_heading = -(start_heading + 90) start = Node(x=start_x, y=start_y, heading=start_heading, row=start_row, col=start_col) # -- get maps for Astar goal_mask = map_semantic == sid goal_mask = binary_opening(goal_mask.astype(int), structure=np.ones((3, 3))).astype(np.bool) floormap = binary_closing(floormap.astype(int), structure=np.ones((10, 10))).astype(np.bool) navmap = floormap & (map_semantic == 0) # compute Heuristic # -- Euclidean distance non_object_mask = ~goal_mask non_object_mask = non_object_mask.astype(np.uint8) distance_map = cv2.distanceTransform(non_object_mask.copy(), cv2.DIST_L2, 3) # -- Geodesic distance #os.system('pip install scikit-fmm') #import skfmm #import numpy.ma as ma #mask = ~navmap&~goal_mask #mask = ~mask #mask = binary_dilation(mask.astype(int), structure=np.ones((10,10))).astype(np.bool) #mask = ~mask #map = np.ones(navmap.shape) #map = map - 2*goal_mask.astype(np.float) #map = ma.masked_array(map, mask) #distance_map = skfmm.distance(map) pathfinder = Astar(navmap, observed_map, heuristic=distance_map, init_heading=start_heading, goals=goals) path, runs = pathfinder.run(start, goal_mask, max_runs=MAX_RUNS) if len(path) == 0: actions = [] else: path = path[::-1] # -- convert path to actions actions = [] prev_p = path[0] for p in path[1:]: # -- rotate pre_h = prev_p[2] new_h = p[2] delta_h = (new_h - pre_h + 360) % 360 if delta_h == 0: pass elif delta_h <= 180: #trun right num_rotations = int(delta_h / TURN_ANGLE) actions += [3] * abs(num_rotations) elif delta_h > 180: #trun left delta_h = 360 - delta_h num_rotations = int(delta_h / TURN_ANGLE) actions += [2] * abs(num_rotations) # move forward actions.append(1) prev_p = p actions.append(0) return (episode['episode_id'], actions, path)
def runAlgorithm(algo): file = open(inputPath,"r") afile = open("analysis.csv","a") afile.write("\nAlgorithm, Puzzle, Cost, Length Of Solution, Length Of Search, Execution, No Solution, Optimality\n") goalstate1 = "1 2 3 4 5 6 7 0" goalstate2 = "1 3 5 7 2 4 6 0" totalLengthOfSolution = 0 totalLengthOfSearch = 0 totalNoSolution = 0 totalCost = 0 totalExecutionTime = 0 counter = 0 averageCost = averageExecutionTime = averageLengthOfSearch = averageLengthOfSolution = averageNoSolution = 0 i = 0 for line in file: lengthOfSolution = 0 lengthOfSearch = 0 duration = 0 cost = 0 analysis = None if algo == ucs: analysis = algoAnalysis(UniformCostSearch(puzzleNumber = i, initial = line.strip(), goal_state1 = goalstate1,goal_state2 = goalstate2)) elif algo == gbfsH0: analysis = algoAnalysis(GBFS(num = i, input = line.strip(), heuristic="h0", openList = [], closedList = [], goalState1=goalstate1, goalState2=goalstate2)) elif algo == gbfsH1: analysis = algoAnalysis(GBFS(num = i, input = line.strip(), heuristic="h1", openList = [], closedList = [], goalState1=goalstate1, goalState2=goalstate2)) elif algo == gbfsH2: analysis = algoAnalysis(GBFS(num = i, input = line.strip(), heuristic="h2", openList = [], closedList = [], goalState1=goalstate1, goalState2=goalstate2)) elif algo == gbfsH3: analysis = algoAnalysis(GBFS(num = i, input = line.strip(), heuristic="h3", openList = [], closedList = [], goalState1=goalstate1, goalState2=goalstate2)) elif algo == astarH0: analysis = algoAnalysis(Astar(puzzleNumber = i, input = line.strip(), heuristic="h0", goalState1=goalstate1, goalState2=goalstate2, monotonic=False)) elif algo == astarH1: analysis = algoAnalysis(Astar(puzzleNumber = i, input = line.strip(), heuristic="h1", goalState1=goalstate1, goalState2=goalstate2, monotonic = False)) elif algo == astarH2: analysis = algoAnalysis(Astar(puzzleNumber = i, input = line.strip(), heuristic="h2", goalState1=goalstate1, goalState2=goalstate2, monotonic=True)) elif algo == astarH3: analysis = algoAnalysis(Astar(puzzleNumber = i, input = line.strip(), heuristic="h3", goalState1=goalstate1, goalState2=goalstate2, monotonic=False)) duration = analysis[5] lengthOfSolution = analysis[1] lengthOfSearch = analysis[2] if analysis[3] == 0: cost = analysis[4] counter = counter + 1 totalCost = totalCost+cost totalExecutionTime = totalExecutionTime + duration totalLengthOfSearch = totalLengthOfSearch + lengthOfSearch if lengthOfSolution != 0: totalLengthOfSolution = totalLengthOfSolution + lengthOfSolution else: totalNoSolution = totalNoSolution + 1 afile.write(algo +", "+ str(i)+", "+str(cost)+", "+str(lengthOfSolution)+", "+str(lengthOfSearch)+", "+str(duration)+","+str(analysis[3])+",,"+line.strip()+"\n") i = i + 1 afile.write("Total-"+algo+", , "+str(totalCost)+","+str(totalLengthOfSolution)+", "+str(totalLengthOfSearch)+", "+str(totalExecutionTime)+","+str(totalNoSolution)+"\n") if(i != 0 and counter != 0): averageCost = totalCost/counter averageExecutionTime = totalExecutionTime/counter averageLengthOfSearch = totalLengthOfSearch/i averageLengthOfSolution = totalLengthOfSolution/counter averageNoSolution = totalNoSolution/i afile.write("Average-"+algo+", , "+str(averageCost)+","+str(averageLengthOfSolution)+", "+str(averageLengthOfSearch)+", "+str(averageExecutionTime)+","+str(averageNoSolution)+"\n") file.close() afile.close()
node7 = Node('Node 7', x_position=90, y_position=90) node8 = Node('Node 8', x_position=120, y_position=120) # create connections node1.add_connection(node2, 7) node1.add_connection(node3, 2) node2.add_connection(node4, 5) node3.add_connection(node5, 3) node4.add_connection(node6, 1) node5.add_connection(node7, 3) node5.add_connection(node8, 4) node7.add_connection(node6, 3) node8.add_connection(node7, 1) # do search ucs = Ucs() print_result_info(ucs.run(start=node1, goal=node6)) Node.reset_nodes() astar = Astar(heuristic_factor=0.05, heuristic=Astar.MANHATTAN_DISTANCE) print_result_info(astar.run(start=node1, goal=node6))
return canvas def draw(self, canvas): self.im.set_data(canvas) self.fig.canvas.draw_idle() plt.pause(0.000001) def show(self, canvas): self.im.set_data(canvas) # self.fig.canvas.draw_idle() # plt.pause(.001) grid = Grid(25, 25) grid.wall() astar = Astar(grid, grid.start, grid.finish, (0, 0, 0, 1)) astar.search() user = input('exit? - enter any key') # agents = [] # num_agent = 10 # for i in range(num_agent): # agents.append(Agent(grid, grid.start, (0, 0, 1, 1))) # agents.append(Agent(grid, grid.finish, (1, 0, 0, 1))) # # fig, ax = plt.subplots(1, 1) # im = ax.imshow(grid.update()) # plt.pause(.1) # while agents[0].searching: # grid.score()
# -*- coding: utf-8 -*- from environment import Environment from dijkstra import Dijkstra from astar import Astar if __name__ == "__main__": resolution = 0.1 botRadius = 0 env = Environment(resolution, botRadius) env.render() alg = Dijkstra(env) alg.executeAlg() env.reset() alg1 = Astar(env) alg1.executeAlg()
def mouse_event(self, event): """ """ if event.type == MOUSEBUTTONDOWN: mouse_pos = (event.pos[0] / SQUARE_SIZE, event.pos[1] / SQUARE_SIZE) if self.mouse_over((WIDTH - 18, 0, 18, 18), event.pos): self.render.menu = not self.render.menu elif not self.render.menu and event.button == 1: if self.round_state == 'CHOICE' and not self.spec and self.render.l_entities[ self.render.me].alive: if self.mouse_over((WIDTH / 2 - 29, HEIGHT - 26, 16, 16), event.pos): self.attack = 'attack' elif self.mouse_over((WIDTH / 2 - 8, HEIGHT - 26, 16, 16), event.pos): self.attack = 'pyrotechnic' elif self.mouse_over((WIDTH / 2 + 13, HEIGHT - 26, 16, 16), event.pos): self.attack = 'windblow' elif self.mouse_over((20, HEIGHT - 40, 90, 20), event.pos): self.buffer_pa.clear() self.render.path = None self.render.target = None elif self.mouse_over((20, HEIGHT - 70, 90, 20), event.pos): while len(self.buffer_pa): self.render.s_queue.put(self.buffer_pa.popleft()) self.render.s_queue.put("CONFIRM_CHOICE") self.confirm = True elif self.entity_on(mouse_pos) is not None: if self.attack == 'attack' and self.reachable( mouse_pos, MELEE_RANGE) and self.render.target is None: self.buffer_pa.append('ATTACK:attack:%d:%d' % mouse_pos) self.render.target = self.render.l_entities.get_by_pos( mouse_pos) elif self.attack == 'pyrotechnic' and self.reachable( mouse_pos, PYRO_RANGE) and self.render.target is None: self.buffer_pa.append('ATTACK:pyrotechnic:%d:%d' % mouse_pos) self.render.target = self.render.l_entities.get_by_pos( mouse_pos) elif self.attack == 'windblow' and self.reachable( mouse_pos, WIND_RANGE) and self.render.target is None: self.buffer_pa.append('ATTACK:windblow:%d:%d' % mouse_pos) self.render.target = self.render.l_entities.get_by_pos( mouse_pos) elif self.render.path is None and self.reachable( mouse_pos, MOVE_DIST): self.buffer_pa.append('MOVE:%d:%d' % mouse_pos) start_pos = self.render.l_entities[self.render.me].pos self.render.path = (Astar(self.get_collide_map(), start_pos, mouse_pos, [0], [BLOCK])) elif self.round_state == 'PLAYERS_CONNECTION' and self.mouse_over( ((WIDTH - 200) / 2, (HEIGHT - 50) / 2, 200, 50), event.pos) and not self.render.rdy: self.render.rdy = True self.render.s_queue.put("READY_TO_PLAY") else: menu_x = (WIDTH - MENU_WIDTH) / 2 menu_y = (HEIGHT - MENU_HEIGHT) / 2 if self.mouse_over((menu_x + 30, menu_y + 50, 10, 10), event.pos): self.render.grid_render = not self.render.grid_render elif self.mouse_over((menu_x + 30, menu_y + 90, 10, 10), event.pos): self.render.fps_render = not self.render.fps_render elif self.mouse_over( (menu_x + MENU_WIDTH - 16, menu_y + 3, 13, 13), event.pos): self.render.menu = not self.render.menu elif self.mouse_over((menu_x + 410, menu_y + 270, 82, 20), event.pos): pygame.event.post(pygame.event.Event(QUIT, {})) elif event.type == MOUSEMOTION and self.render.me is not None: mouse_pos = (event.pos[0] / SQUARE_SIZE, event.pos[1] / SQUARE_SIZE) if self.entity_on( mouse_pos) is not None and self.round_state == 'CHOICE': if self.attack == 'attack' and self.reachable( mouse_pos, MELEE_RANGE): self.render.use_cursor(SWORD) elif self.attack == 'pyrotechnic' and self.reachable( mouse_pos, PYRO_RANGE): self.render.use_cursor(PYRO) elif self.attack == 'windblow' and self.reachable( mouse_pos, WIND_RANGE): self.render.use_cursor(WIND) elif self.render.cursor != ARROW[0]: self.render.use_cursor(ARROW)
while (time.time() - start <= 0.75): self.data.angular.z = 0 self.data.linear.x = lin self.pub.publish(self.data) self.loop_rate.sleep() pass if __name__ == '__main__': rospy.init_node("botmove", anonymous=False) init_x = rospy.get_param('~init_x') init_y = rospy.get_param('~init_y') init_theta = rospy.get_param('~init_t') clrn = [float(rospy.get_param('~clr'))] my_node = Node() initCord = [init_x, init_y, init_theta] actionSet, step = Astar(initCord, clrn) print("\nactions--\n\n", actionSet, "\nstep\n", step) time.sleep(2) if (len(actionSet) != 0): for angle in actionSet[1:]: x = step * 1.0 / 50 if angle > 180: angle = angle - 360 z = angle * 3.14 / 180 my_node.pubData(0.7 * x, z) # my_node.pubData(0.812*x,0.965*z)
def main(): pygame.init() # Taking input from user n = int(input("Enter grid size:")) Sx, Sy = [int(x) for x in input("Enter knight coords:").split()] Tx, Ty = [int(x) for x in input("Enter queen coords:").split()] # Setting up the screen and images res = (n*pixels,n*pixels) gameDisplay = pygame.display.set_mode(res) queen = pygame.image.load("queen.png") knight = pygame.image.load("knight.png") # Initializing the board and the algo createChessboard(gameDisplay,n) placeEntity(queen,Ty,Tx,gameDisplay) d = Astar(n,Sx,Sy,Tx,Ty) # game loop running = True while running: if not d.found: # returns current node and previous node t, prev = d.search() placeEntity(knight,t[1],t[0],gameDisplay) markVisited(prev[1],prev[0],gameDisplay) pygame.display.update() time.sleep(.2) if t == (Tx,Ty): createChessboard(gameDisplay,n) placeEntity(queen,Ty,Tx,gameDisplay) placeEntity(knight,Sy,Sx,gameDisplay) path = d.createPath() for i,j in path[1:]: markVisited(j,i,gameDisplay) pygame.display.update() continue for event in pygame.event.get(): if event.type == pygame.QUIT: running = False break print("Target position ",(Tx,Ty)," reached: ",d.visited[Tx][Ty]) pygame.quit()
def execute_algo(self): for event in pygame.event.get(): if event.type == pygame.QUIT: self.running = False # BFS if self.algorithm_state == 'bfs': self.bfs = BreadthFirst(self, self.start_node_x, self.start_node_y, self.end_node_x, self.end_node_y, self.wall_pos) if self.start_node_x or self.end_node_x is not None: self.bfs.bfs_execute() if self.bfs.route_found: self.draw_path = VisualizePath(self.screen, self.start_node_x, self.start_node_y, self.bfs.route, []) self.draw_path.get_path_coords() self.draw_path.draw_path() else: self.draw_text('NO ROUTE FOUND!', self.screen, [768, 384], 50, RED, FONT, center=True) # DFS elif self.algorithm_state == 'dfs': self.dfs = DepthFirst(self, self.start_node_x, self.start_node_y, self.end_node_x, self.end_node_y, self.wall_pos) if self.start_node_x or self.end_node_x is not None: self.dfs.dfs_execute() if self.dfs.route_found: self.draw_path = VisualizePath(self.screen, self.start_node_x, self.start_node_y, self.dfs.route, []) self.draw_path.get_path_coords() self.draw_path.draw_path() else: self.draw_text('NO ROUTE FOUND!', self.screen, [768, 384], 50, RED, FONT, center=True) # ASTAR elif self.algorithm_state == 'astar': self.astar = Astar(self, self.start_node_x, self.start_node_y, self.end_node_x, self.end_node_y, self.wall_pos) if self.start_node_x or self.end_node_x is not None: self.astar.astar_execute() if self.astar.route_found: self.draw_path = VisualizePath(self.screen, self.start_node_x, self.start_node_y, None, self.astar.route) self.draw_path.draw_path() else: self.draw_text('NO ROUTE FOUND!', self.screen, [768, 384], 50, RED, FONT, center=True) # DIJKSTRA elif self.algorithm_state == 'dijkstra': self.dijkstra = Dijkstra(self, self.start_node_x, self.start_node_y, self.end_node_x, self.end_node_y, self.wall_pos) if self.start_node_x or self.end_node_x is not None: self.dijkstra.dijkstra_execute() if self.dijkstra.route_found: self.draw_path = VisualizePath(self.screen, self.start_node_x, self.start_node_y, None, self.dijkstra.route) self.draw_path.draw_path() else: self.draw_text('NO ROUTE FOUND!', self.screen, [768, 384], 50, RED, FONT, center=True) pygame.display.update() self.state = 'aftermath'
def makepath(self, wrld): pathMaker = Astar(wrld) return pathMaker.findpathtoend(self.x, self.y)
plt.scatter(n1[1] - emin, n1[0] - nmin, c='blue') # draw connected nodes for n1 in g.nodes: plt.scatter(n1[1] - emin, n1[0] - nmin, c='red') plt.xlabel('NORTH') plt.ylabel('EAST') start = list(g.nodes)[0] k = np.random.randint(len(g.nodes)) print(k, len(g.nodes)) goal = list(g.nodes)[k] print('start = ', start, ' goal = ', goal) astar = Astar(g) found, paths = astar.travel(start, goal) if found is False: exit("Unable to find path") path = astar.trace_back(paths) print(path) path_pairs = zip(path[:-1], path[1:]) for (n1, n2) in path_pairs: plt.plot([n1[1] - emin, n2[1] - emin], [n1[0] - nmin, n2[0] - nmin], 'green') plt.show()
ax.set_ylim3d([-300.0, 300.0]) ax.set_zlim3d([-100.0, 300.0]) map_struct = loadmap("maps/final.txt") obstacles = map_struct.obstacles start = np.array([0, 0, 0, 0, 0, 0]) goal = np.array([-1, 0, 1, 0, 0, 0]) max_nodes = 5000 max_iter = 3000 # path, cost = rrt(deepcopy(map_struct), deepcopy(start), deepcopy(goal), max_nodes, max_iter, # stepsize=0.1, neighbour_radius=0.15, bias_ratio=5, bias_radius=0.075, # optimize=True) path = Astar(deepcopy(map_struct), deepcopy(start), deepcopy(goal)) fk = calculateFK() start_pos = fk.forward(path[0])[0] line = ax.plot(start_pos[:, 0], start_pos[:, 1], start_pos[:, 2])[0] visualize.plot_obstacles(ax, obstacles) arm_ani = animation.FuncAnimation(fig, visualize.animate, fargs=(path, line), interval=20, blit=False) plt.show()
use_astar = True elif op in ("-d", "--dstar"): use_dstar = True if use_astar is None and use_dstar is None: print("Error: You need to select one algorithm to plan path: Astar or Dstar!") sys.exit() else: print('Space: ', space_boundary) print('Agents number: ', agents_num) print('Safe Radius: ', safe_R) print('Seed: ', seed) env = Env(space_boundary = space_boundary, agents_num=agents_num, seed = seed, safe_R = safe_R) if use_astar: print('Algorithm: Astar') astar = Astar(env.agents_pos[:, 0], env.agents_targ[:, 0], env.agents_pos, env.space_boundary, env.walk_dirs) pathData = astar.search() pathsData = {"Plan Path": pathData} draw_path(env.space_boundary ,env.agents_pos, pathsData, env.agents_targ, title = "Path Plan with A*") elif use_dstar: print('Algorithm: Dstar') space_map = Map(env.space_boundary, env.walk_dirs) dstar = Dstar(space_map, env.space_boundary) paths = dstar.search( env.agents_pos[:, 0], env.agents_targ[:, 0], env.agents_pos) pathsData = {"Path Without Obstacle": paths[0], "Path With Obstacle": paths[1]} draw_path(env.space_boundary ,env.agents_pos, pathsData, env.agents_targ, title = "Path Plan with D*") print("Finish!") sys.exit()
def render_move(self): """ Simulate and execute all "move" actions and prepare the request to send to Etain. """ # list of (client_id, src_pos, dest_pos, pa_start) # where: # client_id is the client identifier. # src_pos is the tuple (x, y) which represent initial position before the move. # dest_pos is the tuple (x, y) which represent the position where the entity want to go. # pa_start is the action number where the move start. move_actions = self.get_move_actions() # build move flow by actions number self.move_flow = [] finished_move_actions = set() for pa in xrange(NB_ACTIONS): moves_by_pa = {} for move_id, move in enumerate(move_actions): str_client_id, str_src_pos, str_dest_pos, str_pa_start = move client_id = int(str_client_id) src_pos = self.world.get_position_by_object_id(client_id) # src_pos = (int(str_src_pos[0]), int(str_src_pos[1])) dest_pos = (int(str_dest_pos[0]), int(str_dest_pos[1])) pa_start = int(str_pa_start) # if the move has started and isn't finished, then calculate it if move_id not in finished_move_actions and pa_start <= pa: # build A* arguments # we need a world representation which show where # the blocking squares are # # copy world representation world_representation = [] for line in self.world.map: line_clone = [] for item in line: line_clone.append(item) world_representation.append(line_clone) # and add entity as blocking square for entity_x, entity_y in self.world.entities_pos.values(): if entity_y <= len( world_representation) and entity_x <= len( world_representation[0]): world_representation[entity_y][entity_x] = 1 else: # ignore entity which are outside the screen pass free = [0] block = [1] # we have to re calculate the path to follow for # each pa to be able to adapt the path to other # entities' moves. print 'A START MAGIC' a_star_pos_list = Astar(world_representation, src_pos, dest_pos, free, block) pos = a_star_pos_list[0] # first move of one square pos_x, pos_y = pos # collision detection if self.world.square_available(pos_x, pos_y): print 'client n° %d move to (%d, %d)' % (client_id, pos_x, pos_y) self.world.move(client_id, pos_x, pos_y) # generate move action of 1 square moves_by_pa[client_id] = (pos_x, pos_y) if pos == dest_pos: # is the move finished ? print('move is finished') #DEBUG finished_move_actions.add(move_id) else: # collision detected # generate "non-move" action moves_by_pa[client_id] = (None, None) else: print("move is finished or isn't started yet") # end for move in move_actions print('fin iteration PA : moves_by_pa = ', moves_by_pa) self.move_flow.append(moves_by_pa) # end for pa in range(NB_ACTIONS) print("move_flow", self.move_flow)
import sys from map import Map from astar import Astar if __name__ == '__main__': size = len(sys.argv) if size == 2: try: _file = open(sys.argv[1], "r") content = _file.read() _file.close() map = Map(content) except: print("The file is unknown") if map.getStepNumber() >= 2: Astar(map) else: print("The map must have 2 steps at least") else: print("The path of the file is required")
for p in graph.nodes: sub = np.subtract(p, current_point) d = np.linalg.norm(sub) if d < dist: closest_point = p dist = d return closest_point start_ne = (25, 100) goal_ne = (750., 370.) voronoi = VoronoiGraph('colliders.csv') graph = voronoi.create_graph() start_ne = closest_point(graph, start_ne) goal_ne = closest_point(graph, goal_ne) print('################') print(voronoi.edges) print('################') astar = Astar(graph) found, paths = astar.travel(start_ne, goal_ne) path = astar.trace_back(paths) if found else exit("Couldn't find a path") xpoints, ypoints = astar.axis_points(path) voronoi.show(xpoints, ypoints)