def search(self,start,end,Glist): a=A_star(start,end,Glist) self.path=a.solve() exitGame=False pos_x=self.start_point_x pos_y=self.start_point_y self.de_occupy_grid(pos_x,pos_y) self.screen.blit(self.ship,(pos_x,pos_y)) pygame.display.update() while not exitGame: for event in pygame.event.get(): if event.type==pygame.QUIT: pygame.quit() quit() for x,y in self.path: self.screen.blit(self.held_image,(0,0)) for i in self.objects: i.print_obj() for i in self.obstacles: i.print_obj() self.screen.blit(self.ship,(pos_x,pos_y)) pos_x=x pos_y=y #head=pygame.transform.rotate(self.ship,270) if it goes to right pygame.display.update() self.clock.tick(self.FPS) exitGame=True pygame.display.update()
def main(): allVertexes, t_gui = gui.getVertexes() sys.setrecursionlimit(5000) k = int(sys.argv[2]) graph = gui.Graph(None, allVertexes) for vertex in graph.graph: vertex.domain = [ 'Green', 'Red', 'Blue', 'Yellow', 'Black', 'Brown', 'Pink', 'Cyan', 'Magenta', 'Orange' ][:k] csp = CSP(graph, t_gui) csp.initialice(graph.graph) while not csp.isFinish(): a_star = A_star('astar', graph, csp) print "ASTAR STARTED" a_star.mainLoop() for v in csp.graph: print v.domain color = v.domain csp.gui.canvas.itemconfig('a' + str(v.index), fill=color) print "IS CORRECT: ", csp.is_correct() t_gui.mainloop()
def show_shortest_path(request, start, end): path1 = A_star(int(start), int(end), H, G) if not path1: return HttpResponse("Path not found") path2 = A_star(int(end), int(start), H, G) path2.reverse() d1 = find_distance(path1) d2 = find_distance(path2) if d1<d2: path = path1 else: path = path2 stages = [Stage.objects.get(id=sid) for sid in path] changeovers = [] for i in xrange(0,len(stages) - 1): startStage = stages[i] endStage = stages[i+1] rc = ChangeOver( start_stage = startStage, end_stage = endStage, routes=direct_routes_between(startStage, endStage)) changeovers.append(rc) return direct_to_template(request, "show_shortest_path.html", { 'changeovers':changeovers, 'start_stage':Stage.objects.get(id=start), 'end_stage':Stage.objects.get(id=end) })
def search(self, start, end, Glist): a = A_star(start, end, Glist) self.path = a.solve() exitGame = False pos_x = self.start_point_x pos_y = self.start_point_y self.de_occupy_grid(pos_x, pos_y) self.screen.blit(self.ship, (pos_x, pos_y)) pygame.display.update() while not exitGame: for event in pygame.event.get(): if event.type == pygame.QUIT: pygame.quit() quit() for x, y in self.path: self.screen.blit(self.held_image, (0, 0)) for i in self.objects: i.print_obj() for i in self.obstacles: i.print_obj() self.screen.blit(self.ship, (pos_x, pos_y)) pos_x = x pos_y = y #head=pygame.transform.rotate(self.ship,270) if it goes to right pygame.display.update() self.clock.tick(self.FPS) exitGame = True pygame.display.update()
def high_plan(self, start, end): """ Create a high level plan to bais sampling Inputs: start = start position of the sub end = goal position of the sub """ start = self.locate_region(start) end = self.locate_region(end) count = 0 for k in range(len(self.z) - 1): for i in range(len(self.x) - 1): for j in range(len(self.y) - 1): # go to the right if not count % self.dis_size == self.dis_size - 1: index_count = self.get_connection_index( count, count + 1) if index_count: # compute the cost of transition for each region self.W[index_count] = self.cost( count, count + 1, index_count) # go to the bottom if count % self.dis_size_big not in range( self.dis_size_big - self.dis_size, self.dis_size_big): index_count = self.get_connection_index( count, count + self.dis_size) if index_count: self.W[index_count] = self.cost( count, count + self.dis_size, index_count) #go to the back if not count > (self.dis_size**3 - self.dis_size_big - 1): index_count = self.get_connection_index( count, count + self.dis_size_big) if index_count: self.W[index_count] = self.cost( count, count + self.dis_size_big, index_count) count += 1 astar = A_star() # occationally choose a random path that will go to goal but not optimal path_type = np.random.choice([0, 1], p=[0.05, 0.95]) if path_type: h = [0] * len(self.R) else: # make random costs h = np.random.uniform(0, max(self.W), len(self.R)) # use A* to compute the high level path self.high_path = astar.construct_path_fast( [self.R, self.edges, self.W], start, end, h)
def main(): variables, rows, cols, gui_draw = gui.getVariables() node = gui.Node(None, variables) csp = CSP(node) csp.initialice(node.variables) while not csp.isFinish(): #csp.initialice(node.variables) a_star = A_star('astar', node, csp) print "Astar Started: " a_star.mainLoop() finishedRows = [] for key in csp.variables: variable = variables[key] if 'row' in key: finishedRows.append(variable) if variable.index[1] > 9: print "key: ", variable.index, "domain: ", variable.domain, " len: ", len( variable.domain), "fill: ", variable.fill else: print "key: ", variable.index, " domain: ", variable.domain, " len: ", len( variable.domain), "fill: ", variable.fill else: if variable.index[1] > 9: print "key: ", variable.index, "domain: ", variable.domain, " len: ", len( variable.domain), "fill: ", variable.fill else: print "key: ", variable.index, " domain: ", variable.domain, " len: ", len( variable.domain), "fill: ", variable.fill finishedRows.reverse() for variable in finishedRows: for i in range(cols): gui_draw.canvas.itemconfig( 'a' + str(variable.index[1]) + '-' + str(i), fill=gui.setColor(variable.domain[0][i])) gui_draw.after(5, gui_draw.update()) gui_draw.mainloop()
def gen_new_map(val): # Make new obstacles global obs, obs_x, obs_y, path, path_x, path_y, slider obs = generate_obstacles() path = A_star(start, goal, obs, width=360 // grid_size, height=540 // grid_size, grid_size=grid_size) (path_x, path_y) = get_path(path) (obs_x, obs_y) = get_obs(obs, 75 // 15) p_obs.set_data(obs_x, obs_y) p_path.set_data(path_x, path_y) slider.set_val(0) plt.draw()
def main(): matrix_path = sys.argv[1] start_arg = sys.argv[2].split(',') end_arg = sys.argv[3].split(',') start = (int(start_arg[0]), int(start_arg[1])) end = (int(end_arg[0]), int(end_arg[1])) matrix = Grid(matrix_path).get_grid() result, matrix = A_star(matrix, start, end).find_path() print("Mapa da trajetória (maracado com 'x'):") for line in matrix: print(line) print() print("Lista com as coordenadas:") print(result)
def main(): isScenario = raw_input("Scenarios? y=>YES or ENTER=> NO : ") if isScenario: scenario = input("type scenario 0-7: ") tempScenario = case[scenario] else: dim = input("Dimension: ") start = input("Start Node: ") end = input("End Node: ") tempScenario = [dim, start, end] if_obstacle = raw_input("Obstacle?,y=>YES ENTER=> NO: ") while if_obstacle: obstacle = input("Obstacle: start,end,breadth,height: ") tempScenario.append(obstacle) if_obstacle = raw_input("Contiunue? y=>YES, ENTER=>NO") type_of_search = raw_input("type astar, bfs or dfs: ") isGui = raw_input("With GUI? y or ENTER=> NO: ") if isGui: speed = raw_input( "Choose speed: f=>FAST :: MEDIUM=>m :: SLOW=>s :: VERY SLOW=>vs ") if speed == 'f': speed = 5 elif speed == 'm': speed = 100 elif speed == 's': speed = 500 elif speed == 'vs': speed = 2000 allObstacles = [] for obstacles in tempScenario[3:]: allObstacles.append(obstacles) board = Board(tempScenario[0], tempScenario[1], tempScenario[2]) board.createBoard() addObstacles(board, allObstacles) if isGui: draw = Draw(board) a_star = A_star(board, draw, type_of_search, speed) else: a_star = A_star(board, False, type_of_search, None) start = clock() a_star.mainLoop() stop = clock() print round((stop - start) * 1000, 2), "ms" if isGui: draw.mainloop()
def show_shortest_path(request, city, start, end): path = A_star(int(start), int(end)) if not path: return HttpResponse("Path not found") stages = [Stage.objects.get(id=sid) for sid in path] changeovers = [] for i in xrange(0, len(stages) - 1): startStage = stages[i] endStage = stages[i + 1] rc = ChangeOver(start_stage=startStage, end_stage=endStage, routes=direct_routes_between(startStage, endStage)) changeovers.append(rc) return direct_to_template( request, "show_shortest_path.html", { 'changeovers': changeovers, 'city': city, 'start_stage': Stage.objects.get(id=start), 'end_stage': Stage.objects.get(id=end) })
def search(self, start, end, Glist): exitGame = False pos_x = self.start_point_x pos_y = self.start_point_y self.de_occupy_grid(pos_x, pos_y) for i in range(end[0] - 30, end[0] + 30, 30): for j in range(end[1] - 30, end[1] + 30, 30): self.de_occupy_grid(i, j) a = A_star(end, start, Glist) self.path = a.solve() first_x, first_y = self.path[0] while not exitGame: for event in pygame.event.get(): if event.type == pygame.QUIT: pygame.quit() quit() for x, y in self.path: if x > first_x and y > first_y: #direction='down_right' ship = pygame.transform.rotate(self.ship, 225) elif x > first_x and y == first_y: #direction=='right' ship = pygame.transform.rotate(self.ship, 270) elif x > first_x and y < first_y: #direction='up_right' ship = pygame.transform.rotate(self.ship, 315) elif x == first_x and y < first_y: ship = self.ship elif x == first_x and y == first_y: ship = self.ship elif x == first_x and y > first_y: #direction='down' ship = pygame.transform.rotate(self.ship, 180) elif x < first_x and y > first_y: #direction='down_left' ship = pygame.transform.rotate(self.ship, 135) if x < first_x and y == first_y: #direction='left' ship = pygame.transform.rotate(self.ship, 90) if x < first_x and y < first_y: #direction='up_left' ship = pygame.transform.rotate(self.ship, 45) self.t_current = time.time() if (self.t_current - self.t1) > self.seconds: exitGame = True break self.screen.blit(self.held_image, (0, 0)) first_x = x first_y = y for i in self.objects: i.print_obj() for i in self.obstacles: i.print_obj() pos_x = x pos_y = y self.screen.blit(ship, (pos_x, pos_y)) pygame.display.update() self.clock.tick(self.FPS) for i in self.good_list: if end == i.get_coord(): x = i self.good_list.remove(x) self.objects.remove(x) self.sort_list.append(x) exitGame = True pygame.display.update()
def solve(self, start, end, x_dim, y_dim, z_dim, obs, place=None, task=False): """Solve RoboSub Motion Planning""" self.dis_size = 11 self.dis_size_big = self.dis_size**2 self.cov_size = int(self.dis_size / 2) self.cov_size_big = self.cov_size**2 self.T = [] self.T_edges = [] self.came_from = {} self.L = .5 # used to calculate the final high level region end_mid = [ np.mean(end[0]), np.mean(end[1]), np.mean(end[2]), np.mean(end[3]), np.mean(end[4]), np.mean(end[5]), np.mean(end[6]), np.mean(end[7]), np.mean(end[8]), np.mean(end[9]), np.mean(end[10]), np.mean(end[11]) ] # construct the cspace for the car self.c_space_obs(obs) # create the high level discritization if task: self.discritize_task(x_dim, y_dim, z_dim, start, end_mid) else: self.discritize(x_dim, y_dim, z_dim) self.update_converage(start) self.calc_freevol() if task: self.place = place self.high_plan_task(start, end_mid, place) else: self.place = 0 self.high_plan(start, end_mid) #update high level selections for i in range(len(self.high_path) - 1): connection_index = self.get_connection_index( self.high_path[i], self.high_path[i + 1]) if connection_index: self.high_selection[connection_index] += 1 self.available = [] for R in reversed(self.high_path): if self.cov[R] != 0: self.available.append(R) self.stop = False # for high_run in tqdm(range(100),ncols=100): while not self.stop: continue_high = np.random.choice([0, 1], p=[0.125, 0.875]) if not continue_high: if task: self.high_plan_task(start, end_mid, place) else: self.high_plan(start, end_mid) #update high level selections for i in range(len(self.high_path) - 1): connection_index = self.get_connection_index( self.high_path[i], self.high_path[i + 1]) if connection_index: self.high_selection[connection_index] += 1 self.available = [] for R in reversed(self.high_path): if self.cov[R] != 0: self.available.append(R) probs = [] for R in self.available: probs.append((self.freevol[R]**4) / ((1 + self.cov[R]) * (1 + self.nsel[R]**2))) suma = sum(probs) probs = [x / suma for x in probs] # select which R to propagate samples R_select = np.random.choice(self.available, p=probs) self.nsel[R_select] += 1 self.explore(R_select, end) self.path = None if self.stop: h = [0] * len(self.T) astar = A_star() # print('finding path') # backpropagate from the final point total_path = [self.final] current = tuple(self.final) while current != tuple(start): current = tuple(self.came_from[current]) total_path.insert(0, list(current)) self.path = total_path
def high_plan_task(self, start, end, place): """ Create a high level plan to bais sampling Inputs: start = start position of the sub end = goal position of the sub """ closest_start = self.locate_region(start) self.closest_start = 'x' + str(closest_start) + 'q' + str(place) closest_end = self.locate_region(end) self.closest_end = 'x' + str(closest_end) + 'q' + str(place + 1) count = 0 for k in range(len(self.z) - 1): for i in range(len(self.x) - 1): for j in range(len(self.y) - 1): # go to the right if not count % self.dis_size == self.dis_size - 1: index_count = self.get_connection_index( count, count + 1) if index_count: # compute the cost of transition for each region if index_count in self.edges_task_index.keys(): indicies = self.edges_task_index[index_count] current_cost = self.cost( count, count + 1, index_count) for ind in indicies: self.W_task[ind] = current_cost # go to the bottom if count % self.dis_size_big not in range( self.dis_size_big - self.dis_size, self.dis_size_big): index_count = self.get_connection_index( count, count + self.dis_size) if index_count: if index_count in self.edges_task_index.keys(): indicies = self.edges_task_index[index_count] current_cost = self.cost( count, count + self.dis_size, index_count) for ind in indicies: self.W_task[ind] = current_cost #go to the back if not count > (self.dis_size**3 - self.dis_size_big - 1): index_count = self.get_connection_index( count, count + self.dis_size_big) if index_count: if index_count in self.edges_task_index.keys(): indicies = self.edges_task_index[index_count] current_cost = self.cost( count, count + self.dis_size_big, index_count) for ind in indicies: self.W_task[ind] = current_cost count += 1 astar = A_star() # occationally choose a random path that will go to goal but not optimal path_type = np.random.choice([0, 1], p=[0.05, 0.95]) if path_type: h = [0] * len(self.R_task) else: # make random costs h = np.random.uniform(0, max(self.W_task), len(self.R_task)) # use A* to compute the high level path self.high_path = astar.construct_path_fast( [self.R_task, self.edges_task, self.W_task], self.closest_start, self.closest_end, h) # print('path',self.high_path) self.high_path = [int(x[1:-2]) for x in self.high_path]
import sys import time from client import DroidClient import courses from a_star import A_star import maneuver # connect to Sphero droid = DroidClient() droid.connect_to_R2D2() # get course, find path G = courses.grid_1 start = (0, 0) goal = (3, 0) path = A_star(G, start, goal) # ☑ =start node, ☒ =goal node # ☐ ══☐ ☐ ══☐ # ║ ║ ║ ║ # ☐ ☐ ══☐ ☐ # ║ ║ ║ ║ # ☐ ══☐ ☐ ══☐ # ║ ║ ║ # ☑ ☐ ══☐ ☒ # traverse path speed = 0x88 # half speed maneuver.follow_path(droid, path, speed, dist_constant=0.75) droid.animate(10) droid.quit()
for p in path: if not waypoints or prev_ang == None or p.a != prev_ang: prev_ang = p.a path_x.append(p.x) path_y.append(p.y) path_x.append(path[-1].x) path_y.append(path[-1].y) return (path_x, path_y) if show_plot: obs = generate_obstacles() path = A_star(start, goal, obs, width=360 // grid_size, height=540 // grid_size, grid_size=grid_size) (path_x, path_y) = get_path(path) (obs_x, obs_y) = get_obs(obs, 75 // 15) fig, ax = plt.subplots(figsize=(5.4, 8.1)) plt.subplots_adjust(left=0.1, bottom=0.35) p_obs, = plt.plot(obs_x, obs_y, 'o', color="green") p_path, = plt.plot(path_x, path_y, 'o', color="blue") plt.plot([start.x, goal.x], [start.y, goal.y], 'o', color="red") plt.axis([0, 360 // grid_size, 0, 540 // grid_size]) ax_slider = plt.axes([0.1, 0.2, 0.8, 0.05]) slider = Slider(ax_slider, "Angle",
def search(self,start,end,Glist): exitGame=False pos_x=self.start_point_x pos_y=self.start_point_y self.de_occupy_grid(pos_x,pos_y) for i in range(end[0]-30,end[0]+30,30): for j in range(end[1]-30,end[1]+30,30): self.de_occupy_grid(i,j) a=A_star(end,start,Glist) self.path=a.solve() first_x,first_y=self.path[0] while not exitGame: for event in pygame.event.get(): if event.type==pygame.QUIT: pygame.quit() quit() for x,y in self.path: if x>first_x and y>first_y: #direction='down_right' ship=pygame.transform.rotate(self.ship,225) elif x>first_x and y==first_y: #direction=='right' ship=pygame.transform.rotate(self.ship,270) elif x>first_x and y<first_y: #direction='up_right' ship=pygame.transform.rotate(self.ship,315) elif x==first_x and y<first_y: ship=self.ship elif x==first_x and y==first_y: ship=self.ship elif x==first_x and y>first_y: #direction='down' ship=pygame.transform.rotate(self.ship,180) elif x<first_x and y>first_y: #direction='down_left' ship=pygame.transform.rotate(self.ship,135) if x<first_x and y==first_y: #direction='left' ship=pygame.transform.rotate(self.ship,90) if x<first_x and y<first_y: #direction='up_left' ship=pygame.transform.rotate(self.ship,45) self.t_current=time.time() if (self.t_current-self.t1)>self.seconds: self.breakloop=1 exitGame=True break self.screen.blit(self.held_image,(0,0)) first_x=x first_y=y for i in self.objects: i.print_obj() self.message_display_price(str(i.get_price()),i.get_x(),i.get_y()) for i in self.obstacles: i.print_obj() pos_x=x pos_y=y self.screen.blit(ship,(pos_x,pos_y)) pygame.display.update() self.clock.tick(self.FPS) for i in self.good_list: if end==i.get_coord(): x=i self.good_list.remove(x) self.objects.remove(x) self.sort_list.append(x) if self.breakloop==1: self.sort_list.remove(x) exitGame=True pygame.display.update()
def search(self,start,end,Glist): exitGame=False pos_x=self.start_point_x pos_y=self.start_point_y self.de_occupy_grid(pos_x,pos_y) for i in range(end[0]-30,end[0]+30,30): for j in range(end[1]-30,end[1]+30,30): self.de_occupy_grid(i,j) a=A_star(end,start,Glist) self.path=a.solve() try: first_x,first_y=self.path[0] except: print("TypeError: 'NoneType' object is not subscriptable") while not exitGame: for event in pygame.event.get(): if event.type==pygame.QUIT: pygame.quit() quit() for x,y in self.path: if x>first_x and y>first_y: #direction='down_right' ship=pygame.transform.rotate(self.ship,225) elif x>first_x and y==first_y: #direction=='right' ship=pygame.transform.rotate(self.ship,270) elif x>first_x and y<first_y: #direction='up_right' ship=pygame.transform.rotate(self.ship,315) elif x==first_x and y<first_y: ship=self.ship elif x==first_x and y==first_y: ship=self.ship elif x==first_x and y>first_y: #direction='down' ship=pygame.transform.rotate(self.ship,180) elif x<first_x and y>first_y: #direction='down_left' ship=pygame.transform.rotate(self.ship,135) if x<first_x and y==first_y: #direction='left' ship=pygame.transform.rotate(self.ship,90) if x<first_x and y<first_y: #direction='up_left' ship=pygame.transform.rotate(self.ship,45) self.t_current=time.time() if (self.t_current-self.t1)>self.seconds: self.breakloop=1 exitGame=True break self.screen.blit(self.held_image,(0,0)) first_x=x first_y=y for i in self.objects: i.print_obj() self.message_display_price(str(i.get_price()),i.get_x(),i.get_y()) for i in self.obstacles: i.print_obj() pos_x=x pos_y=y self.screen.blit(ship,(pos_x,pos_y)) # Call the asteroid update function, checking if ship hits an asteroid. self.asteroidMovement(self.A_list, pos_x, pos_y) if self.health >=70: healthLabel = self.freeSansBold.render("Ship health: " + str(self.health), 1, (255,255,255)) elif self. health >= 50 and self.health < 70: healthLabel = self.freeSansBold.render("Ship health: " + str(self.health), 1, (255,255,0)) elif self. health >= 25 and self.health < 50: healthLabel = self.freeSansBold.render("Ship health: " + str(self.health), 1, (255,162,0)) else: healthLabel = self.freeSansBold.render("Ship health: " + str(self.health), 1, (255,0,0)) self.screen.blit(healthLabel, (30, 600)) pygame.display.update() self.clock.tick(self.FPS) a=A_star(end,(pos_x,pos_y),Glist) self.path=a.solve() first_x,first_y=self.path[0] for i in self.good_list: if end==i.get_coord(): x=i self.good_list.remove(x) self.objects.remove(x) self.sort_list.append(x) if self.breakloop==1: self.sort_list.remove(x) exitGame=True pygame.display.update()