def main():
#random.seed(1)
if len(sys.argv)==4 and sys.argv[3]=="VCD":
E = parse(sys.argv[1])
print("......Doing VCD......")
G=VCD(E)
path=A_star(G, 0, 1)
elif len(sys.argv)==5 and sys.argv[3]=="RRT":
E = parse(sys.argv[1])
I=int(sys.argv[4])
print("......Doing RRT with "+str(I)+" iterations......")
G = RRT(E,I)
path=A_star(G, 0, G.VertexNumber-1)
else:
print("[ERROR] Input arguments incorrect. Use command like 'python main.py input.txt output.txt VCD' or 'python main.py input.txt output.txt RRT 1000' and try again.")
sys.exit()
#print("\n\n")
spath=''
for i in path:
spath += str(i)+","
#print(str(G)+"\n"+spath[:-1])
file = open(sys.argv[2],"w")
file.truncate()
file.write(str(G)+"\n"+spath[:-1])
print("[Success] Output has been written into 'output.txt'.")
file.close()
def unconstrained_heuristic(self, node):
"""
this is heuristic from A star, not consider vehicle constraint, consider obstacle
"""
start = Node_2D(self.goal.x, self.goal.y)
goal = Node_2D(node.x, node.y)
a_star = A_star(start, goal, self.map, self.grid_resolution)
a_star.run()
result = a_star.goal.cost_so_far
return result
def player_move_to_dest(self, x,y):
dest = [x,y]
A = A_star.A_star_path(self.game)
path = A.get_path_to(dest)
self.player.in_move = True
self.player.next_steps = path
self.player_auto_move()
def tmp(agent_host, tree, dataMap, depthMap, expansionFactor):
start = dataMap.indexFromPoint(startPos)
tmp = dataMap.indexFromPoint(
(startPos[0] + 50, startPos[1], startPos[2] + 50))
end = (tmp[0], depthMap[tmp[0]][tmp[2]], tmp[2]
) # search goal (per for loops)
waypoints = A_star_graph.search(start, end, tree, expansionFactor)
cur = (waypoints[0][0], waypoints[0][1], waypoints[0][2])
curIndex = 0
t = time.time() # store timestamp
while (True):
for i in range(curIndex + 1, len(waypoints)):
print "{}/{}, {}".format(i, len(waypoints) - 1, curIndex)
tmp = (waypoints[i][0], waypoints[i][1], waypoints[i][2]
) # my A* requires tuples, not arrays
path = A_star.search(cur, tmp, dataMap.data, 2) # 2 second timeout
if path:
walkPath(agent_host, path)
cur = tmp
curIndex = i
break
if curIndex == len(waypoints) - 2:
print "done!"
break
# print time.time() - t
return waypoints
def searchTesting(dataMap, depthMap):
results = []
#load saved map
start = dataMap.indexFromPoint(startPos)
# run A* 40^2 times, each time to a different point
t0 = time.time()
count = 1
for i in range(-200, 200, 10):
for j in range(-200, 200, 10):
tmp = dataMap.indexFromPoint(
(startPos[0] + i, startPos[1], startPos[2] + j))
end = (tmp[0], depthMap[tmp[0]][tmp[2]], tmp[2]
) #search goal (per for loops)
t = time.time() #store timestamp
path = A_star.search(start, end, dataMap.data,
15) #15 second timeout
# print results thus far
# if(path):
# s = "{},{}: | Ellapsed: {:f} | Current: {:f} | Percentage: {:f}% |"
# print s.format(i, j, time.time() - t0, time.time() - t, count/16.)
# else:
# s = "{},{}: | Ellapsed: {:f} | TIMEOUT | Percentage: {:f}% |"
# print s.format(i, j, time.time() - t0, count/16.)
# count = count + 1
print("{},{}".format(i, j))
if (path):
results.append((i, j, time.time() - t, len(path)))
else:
results.append((i, j, time.time() - t, 0))
pickle.dump(results, open("A_star_test_results.p", "wb"))
def get_astar_path(sx,sy,ex,ey,mazeX,mazeY,sparsity):
obs,arr=Maze.creat_map([sx,sy],[ex,ey],mazeX,mazeY,sparsity)
plt.plot(ex,ey,'xm')
plt.plot(sx,sy,'xm')
path_planner=A_star.A_Star(obs,[sx,sy],[ex, ey])
# Plot Mesh
plt.pcolormesh(arr)
plt.axes().set_aspect('equal') #set the x and y axes to the same scale
plt.xticks([]) # remove the tick marks by setting to an empty list
plt.yticks([]) # remove the tick marks by setting to an empty list
plt.axes().invert_yaxis() #invert the y-axis so the first row of data is at the top
start_time=timeit.default_timer()
path=path_planner.A_Star()
# If Path found plot the path
if path!=None:
nppath=np.asarray(path)
end_time=timeit.default_timer()
print("timing",end_time-start_time)
plt.plot(nppath[:,0],nppath[:,1],'r-')
plt.savefig('./frames/astar_final')
plt.show()
else:
print("path not found")
plt.show()
return path
def auto_entry():
o_pos = si.col_vec([2,2,0])
o_r = 1.1
## o_h = float('Inf')
o_h = 50
obst = A_star.obstacle(o_pos, o_r, o_h)
start = A_star.node(si.col_vec([0,5,0]))
payload = A_star.node(si.col_vec([5,1,0]))
end = A_star.node(si.col_vec([0,5,3]))
n = 3
return(obst, start, payload, end, n)
def manual_entry():
print("Enter the coordinates of the obstacle.")
o_pos = si.get_coords()
o_r = si.get_real_number("Enter the radius of the obstacle >>> ", lower = 0)
o_h = si.get_real_number("Enter the height of the obstacle. (leave blank for infinite height). >>> ", lower = 0)
obst = A_star.obstacle(o_pos, o_r, o_h)
print("Enter the starting position. >>> ")
start = A_star.node(si.get_coords())
print("Enter the payload position. >>> ")
end = A_star.node(si.get_coords())
n = si.get_integer("Enter the approximate number of nodes along the path. >>> ", lower = 0)
return(obst, start, end, n)
def get_all_available_targets(self):
"""
this function checks for which monsters and mixtures there is available a direct path for the player
:param LogicEngine:
:return: list of [x,y] attributes of mixtures and monsters the player can get to
"""
potential_targets = self.get_mixtures_positions() + self.get_monsters_positions()
A_star_instance = A_star.A_star_path(self.game)
return [target for target in potential_targets if (
A_star_instance.get_path_to(target) is not [[]] and (
(A_star_instance.get_path_to(target))))]
def resolve_algorithm(flag_value, puzzle, puzzle_solution, function,
indexes_solution):
if flag_value == "a*":
return A_star.resolve(puzzle.puzzle, puzzle.dimension, puzzle_solution,
function, indexes_solution)
elif flag_value == "uniform_cost":
return Uniform_cost_search.resolve(puzzle.puzzle, puzzle.dimension,
puzzle_solution)
elif flag_value == "greedy_search":
return Greedy_search.resolve(puzzle.puzzle, puzzle.dimension,
puzzle_solution, function,
indexes_solution)
else:
return None
"ucs_" + file) # calea fisierul de iesire
g = open(outputFile, 'w+')
graf = UCS.Graph(fileInfo, g)
UCS.NodParcurgere.gr = graf
timeResult = UCS.UCS(graf, nSol, timeout=timeout)
if timeResult == "TLE":
print(file + " - UCS : TLE")
g.write("\nTLE\n")
else:
print(file + " - UCS : Success")
g.close()
"""""" """ A* """ """"""
outputFile = os.path.join(outputFolder,
"a_star_" + file) # calea fisierul de iesire
g = open(outputFile, 'w+')
graf = A_star.Graph(fileInfo, g)
A_star.NodParcurgere.gr = graf
timeResult = A_star.a_star(
graf,
nrSolutiiCautate=nSol,
tip_euristica="euristica admisibila 2",
timeout=timeout
) # functia intoarce "TLE" daca nu a rezolvat problema in timp
if timeResult == "TLE":
print(file + " - A* : TLE")
g.write("\nTLE\n")
else:
print(file + " - A* : Success")
g.close()
"""""" """ A* optimizat """ """"""
outputFile = os.path.join(outputFolder, "a_star_opt_" + file)
plt.title('Start End Points Difineded')
exec_time = int(round(time.time() * 1000))
filename = './' + str(exec_time) + '.png'
plt.savefig(filename)
plt.show()
#%% List of obstacles
obs = np.where(bidiimg==True)
obs1 = np.vstack(obs).T
obs_points = []
for obs_point in obs1:
obs_points.append(point.Point(obs_point[0],obs_point[1]))
size = [80, 147]
a_star = A_star.AStar(size, obs_points, p_start, p_end)
print("OK")
#%%
start_time = time.time()
# 初始化起点以及起点的优先值，并加入open_set
a_star.p_start.cost = 0
a_star.open_set.append(a_star.p_start)
while True:
index = a_star.SelectPointInOpenList() # 返回open_set中优先级最高的索引
if index < 0:
print('No path found, algorithm failed!!!')
def g_path_FSM(speed, verbose=True):
while True:
# Step 1: line following
path_following(speed, verbose=verbose) # INSERT JOACHIM'S CODE
# Step 2: wall following
wall_following(speed, verbose=verbose)
# Define the start and end goal
start = (0, 0)
goal = (20, 40)
resize_factor = 6 # Resize occupancy grid
occupancy_grid = A_star.get_map('map.jpg', resize_factor)
max_x, max_y = occupancy_grid.shape # Size of the map
max_val = [max_x, max_y]
# List of all coordinates in the grid
x, y = np.mgrid[0:max_x:1, 0:max_y:1]
pos = np.empty(x.shape + (2, ))
pos[:, :, 0] = x
pos[:, :, 1] = y
pos = np.reshape(pos, (x.shape[0] * x.shape[1], 2))
coords = list([(int(x[0]), int(x[1])) for x in pos])
# Define the heuristic, here = distance to goal ignoring obstacles
h = np.linalg.norm(pos - goal, axis=-1)
h = dict(zip(coords, h))
def get_optimal_route():
nonlocal original_status
nonlocal whether_set_original
spac = 0
if whether_set_original:
for i in range(len(original_status)):
if original_status[i] == 0:
spac = i
break
print(spac)
else:
full_arr = {}
with open("./even_sequence_result.json","r") as ff:
full_arr = json.load(ff)
num = random.randint(0,181439)
original_status = full_arr.get(str(num))
# spac = 0
for i in range(len(original_status)):
if original_status[i] == 0:
spac = i
break
print(spac)
print(original_status)
# change the picture original status
nums = original_status[:]
for i in range(len(nums)):
path2 = "./image/" + str(nums[i])+".png"
photo = ImageTk.PhotoImage(Image.open(path2))
if i == 0:
Lab1.config(image= photo)
Lab1.image = photo
elif i == 1:
Lab2.config(image=photo)
Lab2.image = photo
elif i == 2:
Lab3.config(image=photo)
Lab3.image = photo
elif i == 3:
Lab4.config(image=photo)
Lab4.image = photo
elif i == 4:
Lab5.config(image=photo)
Lab5.image = photo
elif i == 5:
Lab6.config(image=photo)
Lab6.image = photo
elif i == 6:
Lab7.config(image=photo)
Lab7.image = photo
elif i == 7:
Lab8.config(image=photo)
Lab8.image = photo
elif i == 8:
Lab9.config(image=photo)
Lab9.image = photo
original_status = " ".join([str(x) for x in original_status])
print("original status: " + json.dumps(original_status))
goal_status = list("123456780")
goal_status = " ".join(goal_status)
# print(original_status)
# print(goal_status)
# write the valid original and goal sequence to the file
with open("./eight.txt", "w") as f:
f.write(original_status)
f.write("\n")
f.write(str(spac))
f.write("\n")
f.write(goal_status)
f.write("\n")
# find the optimal route
A_star.main()
# store the result in the global route
nonlocal routes
with open("./route_result.json", "r") as json_f:
routes = json.load(json_f)
nonlocal steps
steps = len(routes)
# modify the text of the move status label board to show the current status .
move_status_label["text"] = "Need "+ str(steps-1) + " times of move totally, now it's the 0th status"
print("the optimal routes is : "+json.dumps(routes))
print("the current step is: "+json.dumps(cur_step))
def main(guiinit):
if guiinit:
gui = GUI()
head = Snake(50, 50)
body = [head]
body.append(Snake(40, 50))
body.append(Snake(30, 50))
body.append(Snake(20, 50))
food = Food()
aStar = A_star()
tmppath = aStar.run(body, food.tile)
if tmppath is not None:
path = tmppath
path.pop()
while True:
if guiinit and not gui.handleQuit():
break
if len(path) == 0:
tmppath = aStar.run(body, food.tile)
if tmppath is not None:
path = tmppath
path.pop()
if tmppath is not None:
lastelement = path.pop()
## print("lastelement: "+str(lastelement.posx) + ", " + str(lastelement.posy))
if lastelement.posx > head.tile.posx:
head.direction = 3
elif lastelement.posx < head.tile.posx:
head.direction = 4
elif lastelement.posy > head.tile.posy:
head.direction = 2
elif lastelement.posy < head.tile.posy:
head.direction = 1
for i in path:
print(str(i.posx) + ", " + str(i.posy))
#Handles key movement. Manual movement messes with the algorithm, so i commented it out.
## keys = pygame.key.get_pressed()
##
## if keys[pygame.K_UP] and head.direction != 2:
## head.direction = 1
## elif keys[pygame.K_DOWN] and head.direction != 1:
## head.direction = 2
## elif keys[pygame.K_RIGHT] and head.direction != 4:
## head.direction = 3
## elif keys[pygame.K_LEFT] and head.direction != 3:
## head.direction = 4
head.move(body)
#If snake gets to the food, then it adds to the end of snake body.
if head.tile.posx == food.tile.posx and head.tile.posy == food.tile.posy:
sx = body[(len(body) - 1)].tile.posx
sy = body[(len(body) - 1)].tile.posy
if head.direction == 1:
body.append(Snake(sx, (sy + 10)))
if head.direction == 2:
body.append(Snake(sx, (sy - 10)))
if head.direction == 3:
body.append(Snake((sx - 10), sy))
if head.direction == 4:
body.append(Snake((sx + 10), sy))
food.generateFood(body)
#Sets up the game rules. Cannot go outside the window and can't collide with itself
if head.tile.posx < 0 or head.tile.posx >= 200 or head.tile.posy < 0 or head.tile.posy >= 200:
pygame.time.wait(500)
break
if hasCollided(body):
pygame.time.wait(500)
break
gui.update(body, food)
if guiinit:
gui.exitGame()
approach = calc_approach(payload, 1.5)
approach2 = calc_approach(payload, 1.5)
t0 = time.time()
error_flag = False
print("Checking path ends")
temp1 = obst.collision_detect(start)
temp2 = obst.collision_detect(end)
temp3 = obst.collision_detect(approach)
error_flag = temp1 or temp2 or temp3
print("Done checking.")
if not error_flag:
path1 = A_star.generate_path(start, approach, n, obst)
print("Path:", path1)
# Send path1
path2 = A_star.generate_path(approach, payload, n, obst)
print("Path:", path2)
# Send path2
# Close gripper
path3 = A_star.generate_path(payload, approach2, n, obst)
print("Path:", path3)
# Send path3
path4 = A_star.generate_path(approach2, end, n, obst)
print("Path:", path4)
# Open gripper.
t1 = time.time()
print("Time: ",t1-t0)
def main():
#----------------------------- Potential field path planning AND A* path planning -------------------------
# gird : start = (21,20), goal = (21,30)
# convert position in grid to real 2.15 2.05 2.15 3.05
# TODO Set the position!!!
sx_real = 1.55
sy_real = 2.05
gx_real = 1.55
gy_real = 4.05
paths_pfp = pfp.potentialField(sx=sx_real,
sy=sy_real,
gx=gx_real,
gy=gy_real,
ox=[],
oy=[],
grid_reso=grid_reso,
robot_radius=robot_radius,
grid_width=grid_width,
grid_height=grid_height)
paths_Astar = Astar.aStar(sx=sx_real,
sy=sy_real,
gx=gx_real,
gy=gy_real,
d_diagnoal=14,
d_straight=10,
grid_reso=grid_reso,
grid_width=grid_width,
grid_height=grid_height)
# paths_Astar_modified = Astar_modified.aStar(sx = sx_real, sy = sy_real, gx = gx_real, gy = gy_real, d_diagnoal = 14, d_straight = 10, grid_reso = grid_reso, grid_width = grid_width, grid_height = grid_height)
fix_path_list = [] # first is pfp, second is A*
fix_path_list.append(paths_pfp)
fix_path_list.append(paths_Astar)
# fix_path_list.append(paths_Astar_modified)
#---------------------------------------------------------------------------------------------------------
measured_path_list = []
# TODO 29
# allPaths_pos_list = [] # store the 1000 times pos for all steps for all paths
# TODO 30
disErrorMean_list = []
disErrorStd_list = []
# TODO 2
xyError_list_AllPaths = [] # store xyError_list for all paths
Rmat_error_mean_list_AllPaths = []
tvec_error_mean_list_AllPaths = []
Rmat_error_std_list_AllPaths = []
tvec_error_std_list_AllPaths = []
for fix_path in fix_path_list:
print "======================LOOP start one time================================="
# measured_path, allPos_list, disErrorMean, disErrorStd = computeDistanceErrorMeanStd(fix_path)
measured_path, disErrorMean, disErrorStd, xyError_list, Rmat_error_list_mean_iters, \
tvec_error_list_mean_iters, Rmat_error_list_std_iters, tvec_error_list_std_iters = computeDistanceErrorMeanStd(fix_path)
print "fix_path\n", fix_path
print "measured_path\n", measured_path
print "disErrorStd\n", disErrorStd
print "disErrorMean\n", disErrorMean
measured_path_list.append(measured_path)
# TODO 29
# allPaths_pos_list.append(allPos_list)
# TODO 30
disErrorMean_list.append(disErrorMean)
disErrorStd_list.append(disErrorStd)
# TODO 2
xyError_list_AllPaths.append(xyError_list)
Rmat_error_mean_list_AllPaths.append(Rmat_error_list_mean_iters)
Rmat_error_std_list_AllPaths.append(Rmat_error_list_std_iters)
tvec_error_mean_list_AllPaths.append(tvec_error_list_mean_iters)
tvec_error_std_list_AllPaths.append(tvec_error_list_std_iters)
print "======================LOOP end one time================================="
# ---------------------------- Plot-----------------------------------------------
plotPath.plotComparePaths(fix_path_list, disErrorMean_list,
disErrorStd_list, Rmat_error_mean_list_AllPaths,
tvec_error_mean_list_AllPaths,
Rmat_error_std_list_AllPaths,
tvec_error_std_list_AllPaths)
plotPath.plotFixedMeasuredFillBetween(fix_path_list, disErrorMean_list)
# ---------------------------- Plot with Mayavi ------------------------------------
plotPath_mayavi.plotComparePaths_DisError_3DSurface(xyError_list_AllPaths,
grid_reso=grid_reso)
plotPath_mayavi.plotComparePaths_R_error_3DSurface(
fix_path_list,
Rmat_error_mean_list_AllPaths,
grid_reso=grid_reso,
width=grid_width,
height=grid_height)
plotPath_mayavi.plotComparePaths_t_error_3DSurface(
fix_path_list,
tvec_error_mean_list_AllPaths,
grid_reso=grid_reso,
width=grid_width,
height=grid_height)
from Maze import *
import A_star
import sys
from Node import Node
#"AStar": a_star,
size = 15
seed = 2019
maze = getProblemInstance(size, seed)
root = Node(None, None, None, 0, "", 0, size)
root.filler(maze)
root.show_maze()
print("------------------------")
A_star.a_star(root)
def __init__(self,
wall_describe,
model_map,
exit_list,
people_list,
wall_list,
a_star_map_name,
thickness,
encounter_mode=False,
group_bound=0,
exit_point=None):
#################
# encounter_mode: weather is for encounter mode
# group_bound: the border of two group
# exit_point: two point -- two center of two exit
self.wall_describe = wall_describe
self.model_map = model_map
self.exit_list = exit_list
self.people_list = people_list
self.wall_list = wall_list
self.thickness = thickness
self.encounter_mode = encounter_mode
self.velocity_list = np.zeros(shape=(len(people_list), 2))
self.map_height, self.map_width = self.model_map.shape
self.color_list = np.ones(shape=len(people_list), dtype=np.int32)
self.color_list[0:group_bound] = 0
# constant
self.const_number = 10
self.velocity_i_0 = 0.8 * self.const_number # units of measurement: dm/s
self.A_i = 2000 # units of measurement: N
self.B_i = 0.08 # units of measurement: m
self.k = 1.2 * 10**5 # units of measurement: kg(s**-2)
self.k_body_effect_coefficient = 2.4 * 10**5 / self.const_number # units of measurement: kg(dm**-1)(s**-1)
self.radius = 0.2 * self.const_number # units of measurement: dm
self.radius_wall = 0.05 * self.const_number # units of measurement: dm
self.t_gap = 0.05 # units of measurement: s
self.mass = 80 # units of measurement: kg
self.velocity_list[0:len(people_list), 0:2] = 0
# parameters for control
self.print_n = 0
self.easy_model = 1
# a_star_map preset
if encounter_mode:
insert_point = a_star_map_name.index('.')
file_name_1 = a_star_map_name[:
insert_point] + "_1" + a_star_map_name[
insert_point:]
file_name_2 = a_star_map_name[:
insert_point] + "_2" + a_star_map_name[
insert_point:]
if os.path.isfile(file_name_1) and os.path.isfile(file_name_2):
self.a_star_map_0 = np.load(file_name_1)
self.a_star_map_1 = np.load(file_name_2)
print("Loaded the preset encounter a_star_map")
else:
self.a_star_map_0 = np.zeros(shape=(self.map_height,
self.map_width, 2))
self.a_star_map_1 = np.zeros(shape=(self.map_height,
self.map_width, 2))
for x in tqdm(range(self.map_height)):
for y in range(self.map_width):
if model_map[x][y] == 1:
continue
# astar = A_star.A_star(self.model_map, x, y, exit_point[0][0], exit_point[0][1])
# path = np.array(astar.get_path())
# self.a_star_map_0[x][y] = path[0]
self.a_star_map_0[x][y] = [0, 1]
np.save(file_name_1, self.a_star_map_0)
print("First map preset! ")
for x in tqdm(range(self.map_height)):
for y in range(self.map_width):
if model_map[x][y] == 1:
continue
# astar = A_star.A_star(self.model_map, x, y, exit_point[1][0], exit_point[1][1])
# path = np.array(astar.get_path())
# self.a_star_map_1[x][y] = path[0]
self.a_star_map_1[x][y] = [0, -1]
np.save(file_name_2, self.a_star_map_1)
print("Second map preset! ")
print("The progress of presetting a_star_map finished! ")
else:
if os.path.isfile(a_star_map_name):
self.a_star_map = np.load(a_star_map_name)
print("Loaded the preset a_star_map")
else:
self.a_star_map = np.zeros(shape=(self.map_height,
self.map_width, 2))
# a = np.zeros(shape=(self.map_height, self.map_width)) # for observing
for x in tqdm(range(self.map_height)):
for y in range(self.map_width):
if model_map[x][y] == 1:
continue
astar = A_star.A_star(self.model_map, x, y,
self.exit_list[4][0],
self.exit_list[4][1])
path = np.array(astar.get_path())
self.a_star_map[x][y] = path[0]
# a[x][y] = path[0][0]*10 + path[0][1]
np.save(a_star_map_name, self.a_star_map)
print("The progress of presetting a_star_map finished! ")
#!/usr/bin/env python
""" Library for creating 2D graphs, and then determining free space and connections to generate a roadmap """
import sys
sys.path.insert(0,'../src')
import A_star as asp
import roadmap_builder as rb
roadmap = rb.RoadmapBuilder()
roadmap.construct_square(10)
roadmap.set_obstacles(10)
roadmap.init_roadmap()
roadmap.set_neighbors()
astar = asp.AStar(roadmap)
start = 0
goal = len(astar.roadmap.roadmap)-1
plan = astar.get_plan(start,goal)
idx_cur = goal
while idx_cur != start:
print(astar.roadmap.roadmap[idx_cur].backpointer)
print(astar.roadmap.roadmap[idx_cur].coord)
idx_cur = astar.roadmap.roadmap[idx_cur].backpointer
print(plan)
initial_state = State()
goal_state = initial_state
length = 0
while length < 27:
path_option = goal_state.get_actions()
opt = [
initial_state.get_manhattan_distance(goal_state.apply_action(act))
for act in path_option
]
if np.random.rand(1)[0] <= 0.1:
goal_state = goal_state.apply_action(
np.random.permutation(path_option)[0])
else:
goal_state = goal_state.apply_action(path_option[np.argmax(opt)])
puzzle = Puzzle(initial_state, goal_state)
length = len(As.solve(puzzle)[0]) - 1
print(goal_state.to_string())
solution_start_time = datetime.datetime.now()
[plane_A_star, states_vis_A] = As.solve(puzzle)
print('time to solve A_star{}'.format(datetime.datetime.now() -
solution_start_time))
print('length of plan A_star {}'.format(len(plane_A_star) - 1))
print('states visited A_star {}'.format(states_vis_A))
print("----------------------------")
solution_start_time = datetime.datetime.now()
[plane_dijkstra, states_vis_dijkstra] = dij.solve(puzzle)
print('time to solve dijkstra {}'.format(datetime.datetime.now() -
solution_start_time))
def blocked_cells(self, event):
list4 = A_star.Main()
xsize = int((event.width - 1) / self.columns)
ysize = int((event.height - 1) / self.rows)
self.size = min(xsize, ysize)
self.canvas.delete("square")
for x in range(120):
for y in range(160):
x1 = (y * self.size)
y1 = (x * self.size)
x2 = x1 + self.size
y2 = y1 + self.size
status = list4[(x * 160) + y].status
if status == "0":
self.canvas.create_rectangle(x1,
y1,
x2,
y2,
outline="black",
fill="black",
tags="")
elif status == "1":
self.canvas.create_rectangle(x1,
y1,
x2,
y2,
outline="black",
fill="green",
tags="")
elif status == "2":
self.canvas.create_rectangle(x1,
y1,
x2,
y2,
outline="black",
fill="yellow",
tags="")
elif status == "a":
self.canvas.create_rectangle(x1,
y1,
x2,
y2,
outline="black",
fill="blue",
tags="")
elif status == "b":
self.canvas.create_rectangle(x1,
y1,
x2,
y2,
outline="black",
fill="red",
tags="")
elif status == "s":
print "-----------------"
print "start:", [x, y]
self.canvas.create_rectangle(x1,
y1,
x2,
y2,
outline="orange",
fill="orange",
tags="")
elif status == "g":
print "-----------------"
print "goal:", [x, y]
self.canvas.create_rectangle(x1,
y1,
x2,
y2,
outline="pink",
fill="pink",
tags="")
elif status == "w":
self.canvas.create_rectangle(x1,
y1,
x2,
y2,
outline="white",
fill="white",
tags="")
_, isExit, _, isClear = getCity(clickPoint, tempText, isExit, isClear)
if isClear:
isStartSelected = False
isDestSelected = False
startCityText.setText('None')
destCityText.setText('None')
resultText.setText(' ')
isClear = False
for i in range(20):
cityButton[i].setFill("red")
elif not isStartSelected:
isStartSelected, isExit, startCityNum, isClear = getCity(
clickPoint, startCityText, isExit, isClear)
elif not isDestSelected:
isDestSelected, isExit, destCityNum, isClear = getCity(
clickPoint, destCityText, isExit, isClear)
resultText.setText("Click Anywhere")
else:
print('>>>> Start algorithm <<<<')
resultText.setText("Please wait ...")
############### Algorithm ##################
resultList = aStarModule.A_starSearch(heuristic, startCityNum,
destCityNum)
############################################
resultText.setText('Path ' + ','.join(str(p) for p in resultList) +
' is the best way !!')
#drawLine(resultList)
print('<<<< Finished >>>>')
win.close()
def calc_approach(node, payload_radius):
approach = A_star.node(si.col_vec([node.loc.x, node.loc.y, node.loc.z+payload_radius]))
return(approach)
)
matriz_atual = [
[1, 0, 0], # estado inicial da torre de hanoi
[2, 0, 0],
[3, 0, 0]
]
matriz_Goal = [
[0, 0, 1], # estado objetivo da torre de hanoi
[0, 0, 2],
[0, 0, 3]
]
Hanoi_Tower_Searches.Greedy_Hanoi(matriz_atual, matriz_Goal,
Heuristics.heuristic_1)
print(" ")
print(
" ------------------------------------------------------------------------------------------------"
)
print(" ")
print(
" Busca A*------------------------------------------------------------------------------------------------"
)
A_star.Start()
print("")
print(
"------------------------------------------------------------------------------------------------"
)
print(" ")
a = int(input())
def gameplayState(window, clock):
#Creates snake head and a couple body parts to start out with.
head = Snake(50, 50)
body = [head]
body.append(Snake(40, 50))
body.append(Snake(30, 50))
body.append(Snake(20, 50))
food = Food()
aStar = A_star()
tmppath = aStar.run(body, food.tile)
if tmppath is not None:
path = tmppath
path.pop()
#Main Game loop
while True:
#Checks to see if window was quit
for event in pygame.event.get():
if event.type == pygame.QUIT:
return False
#Handles A* path
if len(path) == 0:
tmppath = aStar.run(body, food.tile)
if tmppath is not None:
path = tmppath
path.pop()
if tmppath is not None:
lastelement = path.pop()
## print("lastelement: "+str(lastelement.posx) + ", " + str(lastelement.posy))
if lastelement.posx > head.tile.posx:
head.direction = 3
elif lastelement.posx < head.tile.posx:
head.direction = 4
elif lastelement.posy > head.tile.posy:
head.direction = 2
elif lastelement.posy < head.tile.posy:
head.direction = 1
#Handles key movement. Manual movement messes with the algorithm, so i commented it out.
## keys = pygame.key.get_pressed()
##
## if keys[pygame.K_UP] and head.direction != 2:
## head.direction = 1
## elif keys[pygame.K_DOWN] and head.direction != 1:
## head.direction = 2
## elif keys[pygame.K_RIGHT] and head.direction != 4:
## head.direction = 3
## elif keys[pygame.K_LEFT] and head.direction != 3:
## head.direction = 4
head.move(body)
#If snake gets to the food, then it adds to the end of snake body.
if head.tile.posx == food.tile.posx and head.tile.posy == food.tile.posy:
sx = body[(len(body) - 1)].tile.posx
sy = body[(len(body) - 1)].tile.posy
if head.direction == 1:
body.append(Snake(sx, (sy + 10)))
if head.direction == 2:
body.append(Snake(sx, (sy - 10)))
if head.direction == 3:
body.append(Snake((sx - 10), sy))
if head.direction == 4:
body.append(Snake((sx + 10), sy))
food.generateFood()
#Sets up the game rules. Cannot go outside the window and can't collide with itself
if head.tile.posx < 0 or head.tile.posx >= 200 or head.tile.posy < 0 or head.tile.posy >= 200:
pygame.time.wait(500)
return True
if hasCollided(body):
pygame.time.wait(500)
return True
#Draws and updates screen
for x in body:
pygame.draw.rect(window, (80, 80, 80),
(x.tile.posx, x.tile.posy, x.width, x.height))
pygame.draw.rect(window, (248, 131, 121),
(food.tile.posx, food.tile.posy, 10, 10))
pygame.display.update()
window.fill((255, 255, 255))
clock.tick(10)
#! /usr/bin/env python3 import A_star import easy_grid_converter A_star.test() easy_grid_converter.easy_grid()
# Create Spots
for i in range(cols):
for j in range(row):
x = node(i, j)
grid[i][j] = x
x.draw()
graph[i][j] = 0
run = True
findPath = False
while run:
# pygame.time.delay(1)
if findPath:
path = A_star.search(graph, start, end)
if path == None:
print("no path")
run = False
else:
for i in range(0, len(path)):
for j in range(0, len(path[i])):
if path[i][j] != -1:
node(i, j).changeColorToRed()
for event in pygame.event.get():
if event.type == pygame.QUIT:
run = False
if pygame.mouse.get_pressed()[0] & (not findPath):
try:
def disp():
global img
cv2.imshow("Maze_runner", img)
cv2.setMouseCallback('Maze_runner', mouse_event)
u = 0
while True and flag == 0:
cv2.imshow("Maze_runner", img)
cv2.waitKey(1)
img = cv2.imread("MAZE_.png", cv2.IMREAD_GRAYSCALE)
img = resize_image(img)
_, img = cv2.threshold(img, 120, 255, cv2.THRESH_BINARY)
img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
h, w = img.shape[:2]
t = threading.Thread(target=disp, args=())
t.start()
while p < 2:
pass
t2 = time.time()
#b.bfs(s,e,h,w,img)
a_star.a_s_(s, e, h, w, img)
print(time.time() - t2)
t.join()
sys.exit(1)
cv2.waitKey(0)
y = np.random.randint(0,151,(n,2))
def heuristique1(c1):
a = c1.dernier_point()
arrivee = c1.arrivee()
i = abs(a[0] - arrivee[0])
j = abs(a[1] - arrivee[1])
return(ftt.force_to_temps(c1.u()*i + j*c1.v()))
def heuristique2(c1):
return(0)
t1 = time.time()
for i in range(n):
A_star.A_star(grib,(x[i][0],y[i][0]),(x[i][1],y[i][1]),heuristique1)
print((time.time()-t1)/n)
t1 = time.time()
for i in range(n):
A_star.A_star(grib,(x[i][0],y[i][0]),(x[i][1],y[i][1]), heuristique2)
print((time.time() - t1)/n)
t1 = time.time()
for i in range(n):
Dijkstra.dijkstra(grib,(x[i][0],y[i][0]),(x[i][1],y[i][1]))
print((time.time()-t1)/n)