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()
