def optimal_action(self, state=None):
if state is None:
state = self.state
# TODO: Save actions
grid = Grid(matrix=(1 - self.grid))
start = grid.node(state.agent_pos[1], state.agent_pos[0])
end = grid.node(state.goal_pos[1], state.goal_pos[0])
finder = AStarFinder(diagonal_movement=DiagonalMovement.never)
path, runs = finder.find_path(start, end, grid)
dif_h = path[1][1] - path[0][1]
dif_w = path[1][0] - path[0][0]
# Down, Up, Right, Left
if dif_w != 0:
if dif_w > 0:
return 2
elif dif_w < 0:
return 3
elif dif_h != 0:
if dif_h > 0:
return 0
elif dif_h < 0:
return 1
def generate_move(self, agent):
matrix = list(
map(map_list_bool_to_int,
np.array(agent._game.state) != 1))
grid = Grid(matrix=matrix)
agent_location = agent._current_location
pacman_location = agent._game.pacman._current_location
dist = math.sqrt(
pow(agent_location[0] - pacman_location[0], 2) +
pow(agent_location[1] - pacman_location[1], 2))
if dist > 6:
return ValidRandom().generate_move(agent)
start = grid.node(agent_location[0], agent_location[1])
end = grid.node(pacman_location[0], pacman_location[1])
finder = AStarFinder(diagonal_movement=DiagonalMovement.never)
path, runs = finder.find_path(start, end, grid)
target_next = path[1]
if agent_location[0] < target_next[0]:
return 'R'
elif agent_location[0] > target_next[0]:
return 'L'
if agent_location[1] < target_next[1]:
return 'D'
elif agent_location[1] > target_next[1]:
return 'U'
def find_path_to_snack(Snake_bodies,Start_pos,Snack_pos):
Clear_Grid=[]
Grid_factor=4
Grid_size=[int(16*Grid_factor),int(9*Grid_factor)]
for loop in range(Grid_size[1]):
Clear_Grid.append([ 1 ]*Grid_size[0])
for position in Snake_bodies:
#print(position)
#print(Clear_Grid[position[1]])
# if position[0]==Grid_size[0] and position[1]<Grid_size[1]:
# position=(63,position[1])
try:
Clear_Grid[position[1]][position[0]]=0
except:
print('Error snake off grid')
pass
grid = Grid(matrix=Clear_Grid)
start = grid.node(Start_pos[0], Start_pos[1])
end = grid.node(Snack_pos[0], Snack_pos[1])
finder = AStarFinder()
path, runs = finder.find_path(start, end, grid)
#print(path)
#print(grid.grid_str(path=path, start=start, end=end))
#print('found path')
return path
#print(find_path_to_snack(Clear_Grid,[(25,22),(12,20)],(2,0),(10,30)))
#find_path_to_snack(
#print(find_path_to_snack([(25, 2), (2, 33), (1, 33), (0, 33), (63, 33)], (3, 33) ,(60, 7)))
def calc_path_astar(self, ordered):
"""
Calculate the walking path using the A* algorithm
:param ordered: ordered set of coordinates.
:return: path, distance
"""
distance = 0
full_path = []
# make sure all destination points are walkable
for o in ordered:
x, y = o
self.slayout.walkable[x, y] = 1
# calculate path
for i in range(len(ordered)-1):
# define the grid and the solver
grid = Grid(matrix=self.slayout.walkable)
finder = AStarFinder(diagonal_movement=DiagonalMovement.always)
start = ordered[i]
start_node = grid.node(start[1], start[0])
end = ordered[i+1]
end_node = grid.node(end[1], end[0])
path, runs = finder.find_path(start_node, end_node, grid)
distance += len(path)
full_path.extend(path)
return full_path, distance
def finder_path(field, gps_actual, gps_target, finder=AStarFinder):
"""Shortest pathfinding algorithms.
Args:
field (np.ndarray):
The current field of fake environment.
gps_actual (tulple):
The current position of RL agent.
gps_actual (tulple):
The target position.
finder (optional):
Choose different algorithms. Defaults to AStarFinder.
Returns:
Each grid position of shortest path.
"""
grid = field.copy()
grid[grid > 0] = 99
grid[grid == 0] = 1
grid[grid == 99] = 0
grid = grid.astype(int)
grid = grid.tolist()
grid = Grid(matrix=grid)
# find path
sx, sy = gps_actual
tx, ty = gps_target
start = grid.node(sy, sx)
end = grid.node(ty, tx)
finder = finder()
path, _ = finder.find_path(start, end, grid)
return path
def findDistToGoal(mapList, pos, goal):
#invert map for A* purposes. In this library, 0 is obstacle 1 is free. so invert
mapMatrix = np.ones((len(mapList), len(mapList[0])))
for i in range(0, len(mapList)):
for j in range(0, len(mapList[0])):
if (mapList[i][j] == 0):
mapMatrix[i][j] = 1
else:
mapMatrix[i][j] = 0
grid = Grid(matrix=mapMatrix)
start = grid.node(pos[0], pos[1])
end = grid.node(goal[0], goal[1])
finder = AStarFinder(diagonal_movement=DiagonalMovement.never)
path, runs = finder.find_path(start, end, grid)
print(grid.grid_str(path=path, start=start, end=end))
print(path)
dist = 0
if (len(path) > 2):
dist = len(path) - 1
elif (len(path) == 2):
dist = 1
elif (len(path) == 1):
dist = 0
else:
dist = -1
return dist
def find(self, start, end, angle=None):
print(start, end)
self.img = self.warehouse.getImage()
preproc = self.preprocess(self.img)
angle = None
if angle is not None:
preproc = self.buildPerimiter(start, angle, 35, preproc)
plt.imsave("pre.png", preproc)
self.imshow(preproc)
# 7.34 and 9.04 are actual dimension in the sim env
grid = Grid(matrix=preproc)
startGrid = grid.node(*start)
endGrid = grid.node(*end)
finder = AStarFinder(diagonal_movement=DiagonalMovement)
print("Finding")
path, _ = finder.find_path(startGrid, endGrid, grid)
if len(path) == 0:
print("exit")
return ([], [])
print(path[0])
print("found")
path = path[0::4]
self.path = [[p[0], p[1]] for p in path]
self.path = self.smooth(30, 0.6, 40)
actualPath = []
for point in path:
actualPath.append(self.warehouse.img_to_warehouse(*point))
return (self.path, actualPath)
def find_path(matrix):
grid = Grid(matrix=matrix)
start = grid.node(0, 0) # Given in (x, y) or (col, row)
end = grid.node(3, 6) # TODO: must change this
finder = AStarFinder()
path, runs = finder.find_path(start, end, grid)
return path
def find_path(data, A, B):
grid = Grid(matrix=data)
finder = BreadthFirstFinder(
diagonal_movement=DiagonalMovement.only_when_no_obstacle)
A['x'] = int(A['x'] / 16)
A['y'] = int(A['y'] / 16)
B['x'] = int(B['x'] / 16)
B['y'] = int(B['y'] / 16)
if A['x'] < B['x']:
start = grid.node(A['x'], A['y'])
end = grid.node(B['x'], B['y'])
elif A['x'] > B['x']:
end = grid.node(A['x'], A['y'])
start = grid.node(B['x'], B['y'])
else:
if A['y'] < B['y']:
start = grid.node(A['x'], A['y'])
end = grid.node(B['x'], B['y'])
elif A['y'] > B['y']:
print(A, B)
end = grid.node(A['x'], A['y'])
start = grid.node(B['x'], B['y'])
else:
return ValueError("Can't calculate path from A to B when A==B")
path, runs = finder.find_path(start, end, grid)
# print('operations:', runs, 'path length:', len(path))
# print(grid.grid_str(path=path, start=start, end=end))
new_path = [(point[0] * 16, point[1] * 16) for point in path]
return new_path
def find_path(self, a, b, grid):
path_grid = Grid(matrix=grid)
start = path_grid.node(a[0], a[1])
end = path_grid.node(b[0], b[1])
finder = BestFirst(diagonal_movement=DiagonalMovement.always)
path, _runs = finder.find_path(start, end, path_grid)
return path
def find_path(challenge, start, end):
# Convert paths appropriately
for y in range(len(challenge)):
for x in range(len(challenge[y])):
if challenge[y][x] == "-":
challenge[y][x] = 1
else:
challenge[y][x] = 0
# Use library to find our path
grid = Grid(matrix=challenge)
start = grid.node(start[1], start[0])
end = grid.node(end[1], end[0])
finder = AStarFinder(diagonal_movement=DiagonalMovement.never)
path_nodes, _ = finder.find_path(start, end, grid)
# Convert path to wasd
previous = None
path = ""
for step in path_nodes:
if previous is not None:
if step[1] < previous[1]:
path += "w"
if step[0] < previous[0]:
path += "a"
if step[1] > previous[1]:
path += "s"
if step[0] > previous[0]:
path += "d"
previous = step
# Return path
return path
def index():
matrix = request.json['matrix']
start_pos = request.json['start']
end_pos = request.json['end']
finder_name = request.json['finder']
is_diagonal = request.json['isDiagonal']
grid = Grid(matrix=matrix)
start = grid.node(start_pos[0], start_pos[1])
end = grid.node(end_pos[0], end_pos[1])
diagonal_movement = DiagonalMovement.always if is_diagonal else DiagonalMovement.never
finder = None
if finder_name == 'a_star':
finder = AStarFinder(diagonal_movement=diagonal_movement)
elif finder_name == 'breadth_first':
finder = BreadthFirstFinder(diagonal_movement=diagonal_movement)
elif finder_name == 'bi_a_star':
finder = BiAStarFinder(diagonal_movement=diagonal_movement)
elif finder_name == 'best_first':
finder = BestFirst(diagonal_movement=diagonal_movement)
elif finder_name == 'dijkstra':
finder = DijkstraFinder(diagonal_movement=diagonal_movement)
elif finder_name == 'ida_star':
finder = IDAStarFinder(diagonal_movement=diagonal_movement)
path, runs = finder.find_path(start, end, grid)
return jsonify({
"path": path,
"steps": len(path),
"finder_name": finder_name
})
def load(self, path):
im = Image.open(path)
w, h = im.size
v = list(
map(lambda p: 1 if (p[0] + p[1] + p[2]) // 3 > 127 else 0,
im.getdata()))
self.grid = Grid(matrix=[v[i * w:(i + 1) * w] for i in range(h)])
def get_lowest_risk_path(map):
grid = Grid(matrix=map)
finder = AStarFinder(diagonal_movement=DiagonalMovement.never)
start = grid.node(0, 0)
end = grid.node(len(map[0]) - 1, len(map) - 1)
path, runs = finder.find_path(start, end, grid)
return sum([map[node[1]][node[0]] for node in path[1:]])
class Map:
def __init__(self):
self.map = []
self.finder = AStarFinder(diagonal_movement=DiagonalMovement.never)
def load(self, path):
im = Image.open(path)
w, h = im.size
v = list(
map(lambda p: 1 if (p[0] + p[1] + p[2]) // 3 > 127 else 0,
im.getdata()))
self.grid = Grid(matrix=[v[i * w:(i + 1) * w] for i in range(h)])
def width(self):
return len(self.map[0])
def height(self):
return len(self.map)
def find_path(self, a, b):
start = self.grid.node(a[0], a[1])
end = self.grid.node(b[0], b[1])
path, runs = self.finder.find_path(start, end, self.grid)
self.grid.cleanup()
return path
def find_path(state, board_matrix, foodx, foody):
height = state["board"]["height"]
y = state['you']["body"][0]["y"]
x = state['you']["body"][0]["x"]
grid = Grid(width=height, height=height, matrix=board_matrix)
start = grid.node(x, y)
end = grid.node(foodx, foody)
finder = AStarFinder(diagonal_movement=DiagonalMovement.never)
path, runs = finder.find_path(start, end, grid)
if len(path) > 1:
pathx = path[1][0]
pathy = path[1][1]
# go up
if ((y - 1) == pathy) and (x == pathx):
return 'up'
# go down
if ((y + 1) == pathy) and (x == pathx):
return 'down'
# go left
if ((x - 1) == pathx) and (y == pathy):
return 'left'
# go right
if ((x + 1) == pathx) and (y == pathy):
return 'right'
else:
# If there are no valid moves to food pick a random move
return random_move()
def generate_hall_1(self, matrix):
doors = self.doors
if len(doors) == 2:
door1, door2 = doors
mini = np.negative(matrix.copy()).tolist()
for s, y in enumerate(mini):
for t, x in enumerate(y):
if mini[s][t] == 0 or mini[s][t] == -4:
mini[s][t] = 1
grid = Grid(matrix=mini)
finder = BestFirst(diagonal_movement=DiagonalMovement.never,
heuristic=manhatten)
start = grid.node(door1[0], door1[1])
end = grid.node(door2[0], door2[1])
pos, _ = finder.find_path(start, end, grid)
for p in pos:
if (p[1] == doors[1][1]) and (p[0] == doors[1][0]):
matrix[p[1], p[0]] = 4
elif (p[1] == doors[0][1]) and (p[0] == doors[0][0]):
matrix[p[1], p[0]] = 4
else:
matrix[p[1], p[0]] = 2
self.meta = doors + pos
return matrix
def solve_maze(maze):
level = []
for row in maze:
newrow = []
for element in row:
if (element == '#'):
newrow.append(0)
elif (element == ' '):
newrow.append(1)
elif (element == 'P'):
newrow.append(2)
elif (element == 'G'):
newrow.append(3)
level.append(newrow)
grid = Grid(matrix=level)
start_y, start_x = index_2d(level, 2)
end_y, end_x = index_2d(level, 3)
start_node = grid.node(start_x, start_y)
end_node = grid.node(end_x, end_y)
finder = AStarFinder()
path, runs = finder.find_path(start_node, end_node, grid)
output = parse_list_to_op_string(path)
return output
def find_path(game_state, board_matrix, x, y, foodx, foody):
height = game_state["board"]["height"]
grid = Grid(width=height, height=height, matrix=board_matrix)
start = grid.node(x, y)
end = grid.node(foodx, foody)
finder = AStarFinder(diagonal_movement=DiagonalMovement.never)
path, runs = finder.find_path(start, end, grid)
if (len(path) > 0):
pathx = path[1][0]
pathy = path[1][1]
y = game_state['you']["body"][0]["y"]
x = game_state['you']["body"][0]["x"]
# go up
if ((y - 1) == pathy) and (x == pathx):
directions["up"] += 20
print("Pick: UP")
# go down
if ((y + 1) == pathy) and (x == pathx):
directions["down"] += 20
print("Pick: down")
# go left
if ((x - 1) == pathx) and (y == pathy):
directions["left"] += 20
print("Pick: left")
# go right
if ((x + 1) == pathx) and (y == pathy):
directions["right"] += 20
print("Pick: right")
def find_path(game_state, board_matrix, x, y, targetx, targety):
height = game_state["board"]["height"]
grid = Grid(width=height, height=height, matrix=board_matrix)
start = grid.node(x, y)
end = grid.node(targetx, targety)
finder = AStarFinder(diagonal_movement=DiagonalMovement.never)
path, runs = finder.find_path(start, end, grid)
if (len(path) > 0):
pathx = path[1][0]
pathy = path[1][1]
# print("Next Move Coordinates "+ str(path[1][0]) + ", " + str(path[1][1]))
y = game_state['you']["body"][0]["y"]
x = game_state['you']["body"][0]["x"]
# go up
if ((y - 1) == pathy) and (x == pathx):
directions["up"] += 20
return "up"
# go down
if ((y + 1) == pathy) and (x == pathx):
directions["down"] += 20
return "down"
# go left
if ((x - 1) == pathx) and (y == pathy):
directions["left"] += 20
return "left"
# go right
if ((x + 1) == pathx) and (y == pathy):
directions["right"] += 20
return "right"
def path_finder(player, opponent, object_tiles, new_pos):
matrix = create_matrix(player, opponent, object_tiles)
grid = Grid(matrix=matrix)
start = grid.node(player.moved_hero.pos[0], player.moved_hero.pos[1])
end = grid.node(*new_pos)
finder = AStarFinder(diagonal_movement=DiagonalMovement.never)
path, runs = finder.find_path(start, end, grid)
return add_side(path)
def find_path(self, grid, final):
x = self.location[0]
y = self.location[1]
start = grid.node(x, y)
end = grid.node(final[0], final[1])
finder = AStarFinder()
self.path, runs = finder.find_path(start, end, grid)
Grid.cleanup(grid)
def findPath(map, startx, starty, endx, endy): grid = Grid(matrix=map) # (x, y) = (col, row) start = grid.node(startx, starty) end = grid.node(endx, endy) finder = AStarFinder(diagonal_movement=DiagonalMovement.never) path, runs = finder.find_path(start, end, grid) return path
def print_grid(self, grid=True, players=None):
"""Print the grid"""
matrix = self.matrix.copy()
if players is not None:
for p in players.players:
i, j = self.pos_player_in_grid(p)
matrix[i][j] = -1
grid = Grid(matrix=matrix)
print(grid.grid_str())
def astar(array, start, goal):
start = list(start)
goal = list(goal)
grid = Grid(matrix=array)
finder = AStarFinder(diagonal_movement=DiagonalMovement.always)
path, runs = finder.find_path(grid.node(start[1], start[0]), grid.node(goal[1], goal[0]), grid)
return path
def path_find(self, start, end):
grid = Grid(matrix=self.mapn)
start_node = grid.node(start[0], start[1])
end_node = grid.node(end[0], end[1])
path, runs = AStarFinder(
diagonal_movement=DiagonalMovement.never).find_path(
start_node, end_node, grid)
return path
def get_good_place(self, places):
grid = Grid(matrix=self._matrix)
self._grid = grid
path_list = []
for place in places:
path = self.get_path(place, grid)
grid.cleanup()
if path:
path_list.append(path)
return path_list
def get_distance_path(field, start, end):
field = np.where(field != 0, -1, 1).T
grid = Grid(matrix=field)
start = grid.node(start[0], start[1])
end = grid.node(end[0], end[1])
finder = AStarFinder()
path, runs = finder.find_path(start, end, grid)
#print(grid.grid_str(path=path, start=start, end=end))
#print(len(path))
return path
def test_path_diagonal():
# test diagonal movement
for scenario in data:
for find in finders:
grid = Grid(matrix=scenario['matrix'])
start = grid.node(scenario['startX'], scenario['startY'])
end = grid.node(scenario['endX'], scenario['endY'])
finder = find(diagonal_movement=DiagonalMovement.always)
path, runs = finder.find_path(start, end, grid)
print(find.__name__, runs, len(path))
print(grid.grid_str(path=path, start=start, end=end))
def find_moves(mob, range, grid):
matrix = grid_to_matrix(grid)
results = list()
for square in range:
maze = Grid(matrix=matrix)
start = maze.node(mob[1], mob[2])
end = maze.node(square[0], square[1])
path, runs = finder.find_path(start, end, maze)
if len(path) > 0:
results.append([square[0], square[1], len(path) - 1, path[1]])
return (results)
