def solve_slow():
""" First attempt at a solution. Slow, but more general than
solve_fast. It conducts a best-first traversal over all the ways that
the first N keylogs can be interleaved into a password. It terminates
once it finds a password that is consistent with all the keylogs """
start = Node(keylogs[0], 0)
print astar(start)
def findPath(self):
if self.renew:
new_pos = self.model.grid.get_neighborhood(
self.pos, True, False, 1)
candi_pos = [x for x in new_pos
if (not bool(list(self.swi.query(
'full('+str(x)+')')))
and not bool(list(self.swi.query(
'wall('+str(x)+')')))
)]
if len(candi_pos) == 0:
self.possible_steps = []
self.new = False
return
candi_path = []
for pos in candi_pos:
candi_path.append(astar( self.fmap, pos, self.target))
self.possible_steps = candi_path.pop(0)
for path in candi_path:
if len(self.possible_steps) > len(path):
self.possible_steps = path
print("from ", self.possible_steps[0], "to ", self.possible_steps[-1])
self.renew = False
if self.next:
print("from ", self.pos, "to ", self.target)
self.possible_steps = astar(self.fmap, self.pos, self.target)
self.next = False
def solve_slow():
""" First attempt at a solution. Slow, but more general than
solve_fast. It conducts a best-first traversal over all the ways that
the first N keylogs can be interleaved into a password. It terminates
once it finds a password that is consistent with all the keylogs """
start = Node(keylogs[0], 0)
print astar(start)
def AStar(level, testall):
if (testall):
for h in heuristics:
astar.astar(level, h, testall)
else:
heuristic = choose_heuristic()
astar.astar(level, heuristic, testall)
return 1
def main() -> None:
pygame.init()
screen = pygame.display.set_mode(WINDOW_SIZE)
pygame.display.set_caption(ALGO)
screen.fill(BLACK)
done = False
clock = pygame.time.Clock()
game_text = "start"
stop_drawing = False
started = False
draw_grid(screen)
draw_lines(screen)
finished = False
while not done:
position = pygame.mouse.get_pos()
column = position[0] // WIDTH_OF_SQUARE
row = position[1] // HEIGHT_OF_SQUARE - 2
for event in pygame.event.get():
if event.type == pygame.QUIT:
done = True
elif pygame.mouse.get_pressed()[0] and position[
1] > 33 and row < ROW and column < COLUMN and stop_drawing == False:
draw_on_grid(screen, row, column)
elif event.type == pygame.MOUSEBUTTONDOWN and 0 < position[
1] < 33 and (WINDOW_SIZE[0] // 2) - 35 < position[0] < (
WINDOW_SIZE[0] // 2) + 35 and game_text == "start":
game_text = "stop"
started = True
stop_drawing = True
elif event.type == pygame.MOUSEBUTTONDOWN and 0 < position[
1] < 33 and (WINDOW_SIZE[0] // 2) - 35 < position[0] < (
WINDOW_SIZE[0] // 2) + 35 and game_text == "stop":
game_text = "start"
started = False
update_grid(screen)
if started and not finished:
set_neighbours(grid)
if ALGO == "Dijkstra's algorithm":
pass
if ALGO == "A* algorithm":
astar.astar(screen, grid, start, end)
finished = True
if ALGO == "Sample algorithm":
pass
pygame.draw.rect(screen, WHITE,
((WINDOW_SIZE[0] // 2) - 35, 1, 70, 28))
font = pygame.font.SysFont('times new roman', 35)
text = font.render(game_text, 1, (0, 0, 0))
screen.blit(text, ((WINDOW_SIZE[0] // 2) - 32, -7))
clock.tick(1000)
pygame.display.flip()
def getPaths(input_list=[7294, 274, 389]):
# get start and end coords
if len(input_list) == 1:
start = seg_to_coord[input_list[0]][0]
end = seg_to_coord[input_list[-1]][-1]
else:
if seg_to_coord[input_list[0]][0] in seg_to_coord[input_list[1]]:
start = seg_to_coord[input_list[0]][-1]
else:
start = seg_to_coord[input_list[0]][0]
if seg_to_coord[input_list[-1]][0] in seg_to_coord[input_list[-2]]:
end = seg_to_coord[input_list[-1]][-1]
else:
end = seg_to_coord[input_list[-1]][0]
start = tuple(start)
end = tuple(end)
# calculate best paths
bestPaths = []
segblacklist = set()
bestPaths.append(astar(start, end, graph, dist, set()))
for z in range(2):
seg = set()
for i in range(len(bestPaths[-1])-1):
seg.add((bestPaths[-1][i+1], bestPaths[-1][i]))
for i in range(2-z):
if len(seg) < 5:
break
top = max(seg, key=lambda s: dist[s])
segblacklist.add(top)
seg.remove(top)
new = astar(start, end, graph, dist, segblacklist)
if new not in bestPaths:
bestPaths.append(new)
pathCoords = []
for path in bestPaths:
seg = set()
p = list(reversed(path))
coord = []
for i in range(1, len(p)):
seg = coord_to_seg[(p[i-1], p[i])]
c = seg_to_coord[seg]
c = [(b,a) for a, b in c]
coord.extend(c)
pathCoords.append(coord)
return pathCoords
def find_choke_points():
width = len(terrain_map)
height = len(terrain_map[0])
map = gc.starting_map(gc.planet())
my_start_workers = gc.my_units()
enemy_start_workers = [worker for worker in map.initial_units if worker not in my_start_workers]
paths = []
for worker in my_start_workers:
for enemy_worker in enemy_start_workers:
my_worker_location = worker.location.map_location()
enemy_worker_location = enemy_worker.location.map_location()
astar_path = astar.astar(terrain_map, my_units_map, my_worker_location, enemy_worker_location)
if len(astar_path) > 0:
paths.append(astar_path)
choke_points = []
paths_length = 0
for path in paths:
paths_length += len(path)
for location in path:
if is_narrow_point(location) and location not in choke_points:
choke_points.append(location)
average_path_length = 0
if len(paths) > 0:
average_path_length = paths_length / len(paths)
return choke_points, average_path_length
def stepM(self, action_n, rider):
destinationAction = self.destinationPos.pop(action_n)
self.destinations = len(self.destinationPos)
# First we "remove" the riders from the grid
riderPos = self.rider_positions[rider]
self.grid[riderPos[0]][riderPos[1]] = ROAD_N
reward = SIZE_MAP
path = astar(self.grid, riderPos, destinationAction)
if path == None:
print(self.grid, riderPos, destinationAction)
distance = SIZE_MAP
else:
distance = len(path) - 1
reward -= distance
self.grid[destinationAction[0]][destinationAction[1]] = RIDER_N
self.rider_positions[rider] = destinationAction
self.steps += 1
end = self.destinations == 0
newDes = get_random_tuple(1, getFreePositions(self.grid)).pop()
self.destinationPos.append(newDes)
self.grid[newDes[0]][newDes[1]] = 1
return self.returnStateInfo(rider), reward, end, distance
def find_path(self, x0, y0, z0, x, y, z, space=0, timeout=10,
digging=False, debug=None):
def iter_moveable_adjacent(pos):
return self.iter_adjacent(*pos)
def iter_diggable_adjacent(pos):
return self.iter_adjacent(*pos, degrees=1.5)
def is_diggable(current, neighbor):
x0, y0, z0 = current
x, y, z = neighbor
return self.is_diggable(x0, y0, z0, x, y, z)
def is_moveable(current, neighbor):
x0, y0, z0 = current
x, y, z = neighbor
return self.is_moveable(x0, y0, z0, x, y, z)
def block_breaking_cost(p1, p2, weight=7):
x0, y0, z0 = p1
x, y, z = p2
return 1 + len(self.get_blocks_to_break(x0, y0, z0, x, y, z)) * 0.5
# TODO pre-check the destination for a spot to stand
log.info('looking for path from: %s to %s', str((x0, y0, z0)), str((x, y, z)))
if digging:
if not (
self.is_safe_to_break(x, y, z) and
self.is_safe_to_break(x, y + 1, z)
):
return None
neighbor_function = iter_diggable_adjacent
#cost_function = block_breaking_cost
cost_function = euclidean
validation_function = is_diggable
else:
if space == 0 and not self.is_standable(x, y, z):
return None
neighbor_function = iter_moveable_adjacent
cost_function = euclidean
validation_function = is_moveable
start = time.time()
path = astar.astar(
(floor(x0), floor(y0), floor(z0)), # start_pos
neighbor_function, # neighbors
validation_function, # validation
lambda p: euclidean(p, (x, y, z)) <= space, # at_goal
0, # start_g
cost_function, # cost
lambda p: euclidean(p, (x, y, z)), # heuristic
timeout, # timeout
debug, # debug
digging # digging
)
if path is not None:
log.info('Path found in %d sec. %d long.',
int(time.time() - start), len(path))
return path
def test_m_2_ka(self):
goal = ka
heuristic = Heuristic(goal)
path_actual = astar(m, city_graph.neighbors, goal, city_graph.cost,
heuristic)
path_desired = [a, ul, s, ka]
assert path_desired == path_actual
def run(source, destination, show, prediction, seed):
(graph, ways, data) = read_xml(config.osm_path, config.elev_path)
source = sys.argv[1].lower()
dest = sys.argv[2].lower()
try:
source = int(source)
assert source in graph
except:
if source in ways:
source = ways[source][0]
else:
print('Source must be a valid node ID or a street name')
return
try:
dest = int(dest)
assert dest in graph
except:
if dest in ways:
dest = ways[dest][0]
else:
print('Destination must be a valid node ID or a street name')
return
costfunc = None
heuristic = None
if prediction == 'toblers':
costfunc = astar.toblers
heuristic = astar.toblers_heuristic
if prediction in ('linear', 'nearest'):
walks = models.read_walk_data(config.walk_data_path, graph, data)
training_walks, test_walks = models.partition_walks(walks, seed=seed)
train = models.build_dist_elev_examples(training_walks, data)
test = models.build_dist_elev_examples(test_walks, data)
if prediction == 'linear':
model = models.linear_model(train)
costfunc = lambda a, b: model([astar.euclidean(a, b), b.z_m - a.z_m])
heuristic = costfunc
elif prediction == 'nearest':
model = models.nearest_neighbor_model(train)
costfunc = lambda a, b: model([astar.euclidean(a, b), b.z_m - a.z_m])
heuristic = lambda a, b: 0
result = astar.astar(costfunc, heuristic, graph, data, source, dest)
if result is None:
print('No path to destination found')
pathstr = ', '.join((str(nd) for nd in result[0]))
print('\npath: {}\n'.format(pathstr))
print('time: {:.2f} minutes\n'.format(result[1]))
#walk_data = models.read_walk_data(config.walk_data_path, graph, data)
#print(walk_data)
#if seed == None:
# seed = random.random()
#models.compare_models(walk_data, data, seed)
if show:
graphics.display(graph, data, result[0], result[1])
def PathToRandCorner(self):
point = (random.randint(0, 1) * (self._grid._columns - 1),
random.randint(0, 1) * (self._grid._rows - 1))
self._path = astar.astar([[0 for j in range(self._grid._columns)]
for i in range(self._grid._rows)],
self._coordinate_seq[0], point)
def main(): m = 500 # rows n = 700 # cols obs1 = makeObstacle(m, n, intervalToIndices([[100,150], [200,300]])) obs2 = makeObstacle(m, n, intervalToIndices([[20,80], [450,520]])) obs3 = makeObstacle(m, n, intervalToIndices([[300,380], [40,120]])) obs = [obs1, obs2, obs3] W = workspace(m, n, obs) # img = Image.fromarray(W) # img.show() path = astar(W.tolist(), (350,25), (0, 499)) # path = np.array(path) path = makeObstacle(m, n, path) obs = [obs1, obs2, obs3, path] W = workspace(m, n, obs) img = Image.fromarray(W) img.show()
def path_gen(self):
start = (13, 0) # starting position
end = (13, 19) # TODO move this if it's on an obstacle
maze = [] # index of rows of the maze - built as they go
temp = [] # allows for construction of each row independent of size
holdover = [] # elements in the row that were obstacles
for j in range(self.width
): # Astar takes xy backwards for this implementation
for i in range(
self.height): # Build row from the width of the grid input
temp.append(self.raw_map_data[i * self.width + j])
row = [0] * len(temp)
for ind in holdover: # Add extra space for things that were obstacles last time
row[ind] = 1
holdover = []
for index, ele in enumerate(temp): # parse row for obstacles
if ele > self.threshold:
row[index] = 1
holdover.append(index)
try:
row[index + 1] = 1
row[index - 1] = 1
maze[j - 1][index] = 1
except IndexError:
pass
maze.append(row)
temp = []
rospy.loginfo("\n" + str(maze))
if len(maze) > 0:
tuple_path = astar(maze, start, end)
else:
tuple_path = [(0, 0)]
path = make_points(tuple_path)
rospy.loginfo(tuple_path)
return make_gridcells(path)
def search_path(world, start, end):
cells = copy.deepcopy(world.cells)
for trooper in world.troopers:
cells[trooper.x][trooper.y] = 9
def neighbors(pos):
for dx, dy in ((-1, 0), (0, -1), (0, 1), (1, 0)):
x = pos[0] + dx
y = pos[1] + dy
if 0 <= x < world.width and 0 <= y < world.height:
if cells[x][y] == 0:
yield (x, y)
def goal(pos):
return pos[0] == end[0] and pos[1] == end[1]
def cost(posA, posB):
return 1
def heuristic(pos):
dy = abs(end[0] - pos[0])
dx = abs(end[1] - pos[1])
return dy + dx
return astar(start, neighbors, goal, 0, cost, heuristic)
def path_plan(start, finish, weight=0.001, limit=8):
x1 = start[0]
y1 = start[1]
x2 = finish[0]
y2 = finish[1]
max_y = 37.821
min_y = 37.707
max_x = -122.365
min_x = -122.513
rows = 57
cols = 75
pts = astar(start, finish, (min_x, max_x), (min_y, max_y), (cols, rows),
weight)
#path simplification algorithm
short = [(x1, y1)] #assuming first node not returned by A*
for i in range(1, len(pts)):
if not colinear(short[len(short) - 1], pts[i], pts[i - 1]):
short.append(pts[i])
short.append((x2, y2)) #assuming final node not returned by A*
if len(short) < limit: #limit on waypoints
return short
q = (len(short) - 1) / limit
r = (len(short) - 1) % limit
out = []
count = 0
for i in range(limit + 1):
out.append(short[count])
count += q
if i < r:
count += 1
return out
def a_star(source, target):
Roads = graph.load_map_from_csv()
result = astar(Roads,
source,
target,
h_func=lambda junc: h_func_aux(junc, Roads[target]))
return result[0]
def test_units():
# moves = [game.CMD_W, game.CMD_E, game.CMD_SE, game.CMD_SW, game.CMD_CW, game.CMD_CCW]
moves = [game.CMD_W, game.CMD_E, game.CMD_NE, game.CMD_NW, game.CMD_CW, game.CMD_CCW]
def g(n1, n2):
return 1
def nf(g):
def neighbours(u):
nb = []
for d in moves:
new_u = u.action(d)
col, row = new_u.pivot
if 0 <= col < 10 and 0 <= row < 10:
nb.append(new_u)
return nb
return neighbours
def hf(goal):
def h(n):
gc, gr = goal.pivot
nc, nr = n.pivot
return hx.distance(hx.to_hex(gc, gr), hx.to_hex(nc, nr)) + goal.abs_rotation_distance(n)
return h
# start = game.Unit((0, 0), [(0, 0), (1, 0)])
# goal = game.Unit((1, 6), [(1, 6), (1, 7)])
goal = game.Unit.get_or_create_unit((0, 0), [(0, 0), (1, 0)])
start = game.Unit.get_or_create_unit((1, 6), [(1, 6), (1, 7)])
f, p = astar.astar(start, goal, g, hf(goal), nf(None))
moves = [p[i].move_to_reach(p[i + 1], moves) for i in range(len(p) - 1)]
print f, moves
def a_star_time_exp(Roads, source, target):
result = astar(Roads,
source,
target,
h_func=lambda junc: time_h_func_aux(junc, Roads[target]),
cost_func=expected_time)
return result[2], result[1]
def play_1(grids, env):
model_path = './models/model_train1730.ckpt' # change this
dqn_r = dqn_runner(grids, env)
tf.reset_default_graph()
network = DQNetwork(dqn_r.state_size, dqn_r.action_size,
dqn_r.learning_rate)
astar_total = []
qstar_total = []
new_grids = []
stop = 0
# for grid in grids:
for i in range(1000):
env.reset()
curr_grid = env.env.maze.grid
start = env.env.player
end = env.env.target
states_qstar, counter_qstar = qstar.qstar(env, start, end, network,
model_path)
env.fixed_reset(curr_grid)
states_astar, counter_astar = astar.astar(env, start, end)
if counter_astar > 39:
print('states astar: ', counter_astar, ' states qstar: ',
counter_qstar)
qstar_total.append(counter_qstar)
astar_total.append(counter_astar)
new_grids.append(env.env.maze.grid)
stop += 1
if stop == 19:
break
print('Average astar: ' + str(np.mean(astar_total)))
print('Average qstar: ' + str(np.mean(qstar_total)))
pickle.dump(grids, open("grids_20_10.p", "wb"))
return
def astar_move(data, location, target):
# Make maze before sending to a-star pathing function
maze = [[0 for _ in range(data['board']['width'])] for _ in range(data['board']['height'])]
# add all of the other snakes
for snake in data['board']['snakes']:
for seg in snake['body'][:-1]:
maze[seg['x']][seg['y']] = 1
# add self
for seg in data['you']['body'][:-2]:
maze[seg['x']][seg['y']] = 1
path = astar.astar(maze, location, target)
print location
print target
maze[location[0]][location[1]] = "S"
maze[target[0]][target[1]] = "T"
for row in maze:
print row
print "\n\n\n"
print path
return 'down'
def main():
t_start = time.time()
grid_size, = input('Enter the size of the chess board: ')
grid_size = int(grid_size)
# print(grid_size)
#grid_size = 6
#creates and displays a chess board
N, point_list, square_size, allPos, window_size = create_scene(grid_size)
#gives the queens random location in each column and displays on the chess board
InitQueenLoc = initial_scene(N)
#returns the current position of all the queens in a numbered fashion (1, 2, 3, 4, 5, ......) and returns the pair of queens attacking each other
currentScene, attacks = heuristic(InitQueenLoc, N)
print("The initial queen position is: " + str(currentScene))
currentScene = astar(attacks, allPos, currentScene, N)
time_taken = (time.time() - t_start)
print('The time taken to reach the above solution is: ' + str(time_taken) +
" seconds")
draw_graph(currentScene, N, point_list, square_size, window_size)
def action(self):
karbonite_location = self.__outer._nearby_karbonite_locations.pop(
0)
karbonite_map = self.__outer._maps['karbonite_map']
my_units_map = self.__outer._maps['my_units_map']
# Make sure karbonite has not been stolen
while karbonite_map[karbonite_location.x][
karbonite_location.y] == 0 or my_units_map[
karbonite_location.x][karbonite_location.y]:
if len(self.__outer._nearby_karbonite_locations) > 0:
karbonite_location = self.__outer._nearby_karbonite_locations.pop(
0)
else:
self._status = bt.Status.FAIL
return
terrain_map = self.__outer._maps['terrain_map']
my_units_map = self.__outer._maps['my_units_map']
worker = self.__outer.unit()
unit_map_location = worker.location.map_location()
path = astar.astar(terrain_map, my_units_map, unit_map_location,
karbonite_location)
if len(path) > 0:
path.pop(0) # Remove the point the unit is already on
self.__outer._path_to_follow = path
self._status = bt.Status.SUCCESS
else:
self.__outer._path_to_follow = None
self._status = bt.Status.FAIL
def update_path(self):
if not self.dirty:
return
self.dirty = False
self.tag += 1
def neighbors(pos):
cell = self.grid[pos]
if cell.neighbors is None:
y, x = pos
cell.neighbors = []
for neighbor_y, neighbor_x in self.grid.neighbors(y, x):
if self.grid[neighbor_y, neighbor_x].char != '#':
cell.neighbors.append((neighbor_y, neighbor_x))
return cell.neighbors
def goal(pos):
return pos == self.goal
def cost(from_pos, to_pos):
from_y, from_x = from_pos
to_y, to_x = to_pos
return 14 if to_y - from_y and to_x - from_x else 10
def estimate(pos):
y, x = pos
goal_y, goal_x = self.goal
dy, dx = abs(goal_y - y), abs(goal_x - x)
return min(dy, dx) * 14 + abs(dy - dx) * 10
def debug(nodes):
self.nodes = nodes
self.path = astar((self.y, self.x), neighbors, goal, 0, cost, estimate,
self.limit, debug)
def SearchUSA(search_type, src_city, dst_city, filename='roads.txt'):
"This function searches for a path from source city to destination city using given search algorithm"
import bfs, dfs, astar, Roadmap
from HelperFunctions import ReadFileToMap
#Initializing empty map which uses adjacency list data structure to store map information
my_map = Roadmap.Roadmap()
#Loading map from the text file
ReadFileToMap(filename, my_map)
success = False
print("\nApplying '{0}' Algorithm to search path from {1} to {2}\n".format(
search_type.upper(), src_city.upper(), dst_city.upper()))
if (search_type == 'bfs'):
# print("\n{}".format("BFS SEARCH".center(80,'$')))
success = bfs.bfs(src_city, dst_city, my_map)
elif (search_type == 'dfs'):
# print("\n{}".format("DFS SEARCH".center(80,'$')))
success = dfs.dfs(src_city, dst_city, my_map)
elif (search_type == 'astar'):
# print("\n{}".format("ASTAR SEARCH".center(80,'$')))
success = astar.astar(src_city, dst_city, my_map)
else:
print("\nInvalid Search Type: Please Try Again.\n")
if (success):
pass
# print("{}\n".format("SUCESS".center(400,'*')))
else:
print("{}\n".format("FAILURE".center(400, '*')))
def update_path(self):
if not self.dirty:
return
self.dirty = False
self.tag += 1
def neighbors(pos):
cell = self.grid[pos]
if cell.neighbors is None:
y, x = pos
cell.neighbors = []
for neighbor_y, neighbor_x in self.grid.neighbors(y, x):
if self.grid[neighbor_y, neighbor_x].char != '#':
cell.neighbors.append((neighbor_y, neighbor_x))
return cell.neighbors
def goal(pos):
return pos == self.goal
def cost(from_pos, to_pos):
from_y, from_x = from_pos
to_y, to_x = to_pos
return 14 if to_y - from_y and to_x - from_x else 10
def estimate(pos):
y, x = pos
goal_y, goal_x = self.goal
dy, dx = abs(goal_y - y), abs(goal_x - x)
return min(dy, dx) * 14 + abs(dy - dx) * 10
def debug(nodes):
self.nodes = nodes
self.path = astar((self.y, self.x), neighbors, goal, 0, cost,
estimate, self.limit, debug)
def play_3(env):
astar_total = []
qstar_total = []
q_total = []
for i in range(10):
# train on grid
env.reset()
grid = env.env.maze.grid
game = q_runner(grid, env, fixed=True)
game.train()
env.fixed_reset(grid)
start = env.env.player
end = env.env.target
print('astar')
states_astar, counter_astar = astar.astar(env, start, end)
env.fixed_reset(grid)
print('q-learn')
states_q, counter_q = game.test()
env.fixed_reset(grid)
print('qstar')
states_qstar, counter_qstar = qstar_qlearn.qstar(env, start, end, game)
print('states astar: ', counter_astar, ' states qstar: ',
counter_qstar, ' states dqn: ', counter_q)
astar_total.append(counter_astar)
qstar_total.append(counter_qstar)
q_total.append(counter_q)
i += 1
print('Average astar: ' + str(np.mean(astar_total)))
print('Average qstar: ' + str(np.mean(qstar_total)))
print('Average dqn: ' + str(np.mean(q_total)))
return
def a_star_time(source, target):
Roads = graph.load_map_from_csv()
result = astar(Roads,
source,
target,
h_func=lambda junc: time_h_func_aux(junc, Roads[target]),
cost_func=expected_time)
return result[0]
def route(self, from_waypoint, to_waypoint): #print 'Routing from', from_waypoint, 'to', to_waypoint return astar.astar( from_waypoint, lambda wp: wp.connections, lambda wp: wp.heuristic(to_waypoint), to_waypoint )
def FindPath(self, goal_point):
self._path = astar.astar(self._grid._ToNumRepr(),
self._coordinate_seq[0], goal_point)
if self._path == -1:
return False
else:
self._path = deque(self._path)
return True
def astar_path(self):
# Get optimal path sequence
path = astar(self.move_list, self.map_size, self.start, self.goal,
self.walls, self.pits)
for p in path:
if p:
rospy.sleep(1)
self.astar_publisher.publish(list(p))
def get_bottleneck_nodes(dense_G, sparse_G, path_nodes, occ_grid, dis, row,
col, inc):
assert len(path_nodes) > 0
THRESH = 0.1
P_EPS = 10
new_edges = []
start_n = 'p0'
goal_n = 'p' + str(len(path_nodes) - 1)
for count in range(len(path_nodes)):
s1 = dense_G.nodes[path_nodes[count]]['state']
sparse_G.add_node('p' + str(count), state=s1)
if count > 0:
n1 = 'p' + str(count - 1)
n2 = 'p' + str(count)
sparse_G.add_edge(n1, n2)
sparse_G[n1][n2]['weight'] = helper.calc_weight_states(
sparse_G.nodes[n1]['state'], sparse_G.nodes[n2]['state'])
new_edges.append((n1, n2))
for node in sparse_G.nodes():
s2 = sparse_G.nodes[node]['state']
if 'p' in s2:
continue
if helper.calc_weight_states(s1, s2) < THRESH:
n1 = node
n2 = 'p' + str(count)
sparse_G.add_edge(node, 'p' + str(count))
sparse_G[n1][n2]['weight'] = helper.calc_weight_states(
sparse_G.nodes[n1]['state'], sparse_G.nodes[n2]['state'])
new_edges.append((node, 'p' + str(count)))
curr_path_nodes, curr_dis = astar.astar(sparse_G, start_n, goal_n,
occ_grid, row, col, inc)
while curr_dis < P_EPS * dis:
for edge in new_edges:
sparse_G[edge[0]][edge[1]]['weight'] *= 1.2
curr_path_nodes, curr_dis = astar.astar(sparse_G, start_n, goal_n,
occ_grid, row, col, inc)
common_nodes = []
for n in curr_path_nodes:
if 'p' in n:
common_nodes.append(n)
return common_nodes, curr_path_nodes
def construct_full_path(path, id_digraph, id_to_data):
fullpath = []
for l, r in zip(path, path[1:]):
result = astar.astar(astar.toblers, astar.toblers_heuristic, id_digraph, id_to_data, l, r)
if result is None:
return None
else:
fullpath.extend(result[0])
return fullpath
def test_start_node_not_in_graph(self):
start = City('', 0, 0)
goal = start
graph = Graph({goal: {start: 1}})
heuristic = Heuristic(goal)
path_actual = astar(start, graph.neighbors, goal, graph.cost,
heuristic)
path_expected = []
assert path_expected == path_actual
def get_all_shortest_path(maze, start, reachable):
rows, cols = maze.shape
for y in range(rows):
for x in range(cols):
if (x, y) not in reachable:
path = astar(maze, (1, 1), (y, x))
if path and len(path) <= 50:
reachable.append((x, y))
return reachable
def test_two_nodes(self):
start = City('', 0, 0)
goal = City('', 1, 0)
graph = Graph({start: {goal: 1}})
heuristic = Heuristic(goal)
path_actual = astar(start, graph.neighbors, goal, graph.cost,
heuristic)
path_expected = [goal]
assert path_expected == path_actual
def test_empty_graph(self):
start = City('', 0, 0)
goal = start
graph = Graph({})
heuristic = Heuristic(goal)
path_actual = astar(start, graph.neighbors, goal, graph.cost,
heuristic)
path_expected = []
assert path_expected == path_actual
def search_path( self, x, y ):
'''
recherche le meilleur chemin vers le point x,y
@type x: int
@type y: int
@return: liste de point de passage
@rtype: list
'''
self.path = astar.astar( self.land, self.robot_pos, self.get_square( x, y ) )
return self.path
def main():
(target,r1,r2,initialfield,heuristic) = parseUser()
field = map(list,initialfield)
print "\nLEGEND:"
print "\033[1;34m Robot1 path \n\033[1;33m Robot2 path \n\033[0;32m Joined path \033[0m"
print "\n \033[0;34m ======= Robot 2 plays 'nice' ======= \033[0m"
(finalists1,nodes) = astar(r1,target,field,heuristic,1)
total = nodes
field = modifygrid(finalists1,field)
(finalists2,nodes) = astar(r2,target,field,heuristic,2)
total += nodes
flushgrid(field)
designpath("1;34",r1,target,finalists1)
designpath("1;33",r2,target,finalists2)
printpath()
max1 = max(len(finalists1),len(finalists2))
print "Max length in steps:", max1-1
print "\t Robot 1 took:\t\t", len(finalists1)-1, " steps"
print "\t Robot 2 (nice) took:\t", len(finalists2)-1, " steps"
flushgrid(field)
print "\n \033[0;34m ======= Robot 1 plays 'nice' ======= \033[0m"
field = map(list,initialfield)
(finalists2,nodes) = astar(r2,target,field,heuristic,2)
total += nodes
field = modifygrid(finalists2,field)
(finalists1,nodes) = astar(r1,target,field,heuristic,1)
total += nodes
flushgrid(field)
designpath("1;34",r1,target,finalists1)
designpath("1;33",r2,target,finalists2)
printpath()
max2 = max(len(finalists1),len(finalists2))
print "Max length in steps:", max2-1
print "\t Robot 1 (nice) took:\t", len(finalists1)-1, " steps"
print "\t Robot 2 took:\t\t", len(finalists2)-1, " steps"
print "\n ===== RESULT ====="
if max1 < max2:
print "1st strategy (Robot 2 plays nice) is optimal"
elif max2 < max1:
print "2nd strategy (Robot 1 plays nice) is optimal"
else:
print "Both strategies yield same result"
print "Total nodes considered:", total
def __init__(self):
rospy.init_node('Robot', anonymous=True)
self.hog = cv2.HOGDescriptor()
self.hog.setSVMDetector( cv2.HOGDescriptor_getDefaultPeopleDetector() )
self.avg_width = 0.45
self.path_pub = rospy.Publisher(
"results/path_list",
AStarPath,
queue_size = 10
)
self.policy_pub = rospy.Publisher(
"results/policy_list",
PolicyList,
queue_size = 10
)
self.sim_pub = rospy.Publisher(
"map_node/sim_complete",
Bool,
queue_size = 10
)
self.cam_sub = rospy.Subscriber(
"/camera/visible/image",
Image,
self.detect_people
)
path_list = astar.astar()
for pl in path_list:
asp = AStarPath()
asp.data = pl
rospy.sleep(1)
self.path_pub.publish(asp)
markov = mdp()
pl = PolicyList()
pl.data = markov.policy_list
rospy.sleep(1)
rospy.sleep(1)
rospy.sleep(1)
self.policy_pub.publish(pl)
#print markov.policy_list
#for m in markov.policy_list:
#pl = PolicyList()
#pl.data = m
#rospy.sleep(1)
#self.policy_pub.publish(pl)
rospy.sleep(1)
rospy.signal_shutdown("Done.")
def find_path(start, points, end_node):
goal_point = points[end_node]
def goal(point):
return point == goal_point
def neighbors(point):
return [points[connection.id] for connection in point.connections]
def cost(a, b):
return distance_from(a.x, a.y, b.x, b.y)
def heuristic(point):
return cost(goal_point, point)
result = astar(start, neighbors, goal, cost, heuristic)
return [points.index(item) for item in result]
def astar(self, x, y, goalx = None, goaly = None):
if not goalx:
goalx = self.castlex
if not goaly:
goaly = self.castley
mapcoord = x + y * self.mapw
mapgoal = goalx + goaly * self.mapw
ident = (mapcoord, mapgoal)
cached = self.astar_cache.get(ident, None)
if cached:
return cached[:]
def neighbors(pos):
l = pos - 1
r = pos + 1
t = pos - self.mapw
b = pos + self.mapw
neighbors = [l, r, t, b]
if pos < self.mapw or self.mapdata[t] == -1:
neighbors.remove(t)
if pos % self.mapw == 0 or self.mapdata[l] == -1:
neighbors.remove(l)
if pos % self.mapw == self.mapw - 1 or self.mapdata[r] == -1:
neighbors.remove(r)
if pos >= (self.mapw * (self.maph - 1)) or self.mapdata[b] == -1:
neighbors.remove(b)
return neighbors
def goal(pos):
return pos == mapgoal
def cost(from_pos, to_pos):
from_y, from_x = from_pos % self.mapw, from_pos / self.mapw
to_y, to_x = to_pos % self.mapw, to_pos / self.mapw
if to_y - from_y:
return 14 * self.mapdata[to_pos]
return 10 * self.mapdata[to_pos]
def heuristic(pos):
x = pos % self.mapw
y = pos / self.mapw
dy, dx = abs(goaly - y), abs(goalx - x)
return min(dy, dx) * 14 + abs(dy - dx) * 10
def debug(nodes):
print len(nodes), "nodes searched"
calculated_path = astar(mapcoord, neighbors, goal, 0, cost, heuristic, debug = None)
self.astar_cache[ident] = calculated_path
return calculated_path[:]
def main():
maze = Maze.load_maze()
print ("Maze Loaded:")
print (maze)
try:
sy,sx = maze.find_start()
ey,ex = maze.find_end()
print("solving...")
if astar.astar(maze,0,3):
print(maze)
else:
print (" no solution found")
except ValueError:
print ("No start point found.")
def tasks(self):
# A*
intList = astar()
# OUTPUT
# Outputs the locations that must be traversed from the start to the goal including start and goal.
aPub = rospy.Publisher("/results/path_list",AStarPath, queue_size = (self.map_size[0]*self.map_size[1]))
for move in intList:
aPath = AStarPath()
aPath.data = move
rospy.sleep(2)
aPub.publish(aPath)
# MDP
mdp()
def __init__(self):
rospy.init_node('Robot', anonymous=True)
self.path_pub = rospy.Publisher(
"results/path_list",
AStarPath,
queue_size = 10
)
self.policy_pub = rospy.Publisher(
"results/policy_list",
PolicyList,
queue_size = 10
)
self.sim_pub = rospy.Publisher(
"map_node/sim_complete",
Bool,
queue_size = 10
)
path_list = astar.astar()
for pl in path_list:
asp = AStarPath()
asp.data = pl
rospy.sleep(1)
self.path_pub.publish(asp)
markov = mdp()
pl = PolicyList()
pl.data = markov.policy_list
rospy.sleep(1)
rospy.sleep(1)
rospy.sleep(1)
self.policy_pub.publish(pl)
#print markov.policy_list
#for m in markov.policy_list:
#pl = PolicyList()
#pl.data = m
#rospy.sleep(1)
#self.policy_pub.publish(pl)
self.sim_pub.publish(True)
rospy.sleep(1)
rospy.signal_shutdown("Done.")
def beginSimulation(self):
result_astar = astar(self.config)
self.publishAStar(result_astar)
mdp = MDP(self.config)
#INPUT A GRID TODO
print "TESTING"
while mdp.renameThis():
print "Iterate"
result_policy = mdp.iterate()
print "publish"
self.resultsPolicyPub.publish(result_policy)
print "Will Continue?", mdp.renameThis()
self.simulationCompletePub.publish(True)
rospy.sleep(10)
rospy.signal_shutdown("Simulation has Completed")
def ask_for_prediction(algorithm, user, node, limit=None):
same_nodes = graph.get_same_nodes(node)
same_nodes.append(node)
max_conf = None
best_result = None
result = None
expanded = 0
# repeat algorithm for every size of start item
for node in same_nodes:
if algorithm == "dijkstra":
result = dijkstra(user, node)
if result is not None:
expanded += result[4]
elif algorithm == "astar":
result = astar(user, node)
if result is not None:
expanded += result[4]
elif algorithm == "kdirectional":
result = kdirectionalDijkstra(user, node)
if result is not None:
expanded += result[4]
elif algorithm == "perimeter_search":
result = perimeter_search(user, node, limit)
if result is not None:
expanded += result[4]
elif algorithm == "beam_search":
result = beam_search(user, node)
if result is not None:
expanded += result[4]
if result is not None:
if best_result is None or result[2] > max_conf:
max_conf = result[2]
best_result = result
# pick best result out of all starting sizes
print(algorithm)
if best_result is None:
print("Path does not exist")
else:
(best_node, predicted_rating, confidence, length_of_path, _) = best_result
print("best_node: ", best_node, " predicted_rating: ", predicted_rating, " confidence: ", confidence, " length_of_path: ", length_of_path, " expanded nodes: ", expanded)
print("")
def __init__(self):
self.config = read_config()
rospy.init_node("robot")
self.path_publisher = rospy.Publisher(
"/results/path_list",
AStarPath,
queue_size = 10
)
self.mdp_publisher = rospy.Publisher(
"/results/policy_list",
PolicyList,
queue_size = 10
)
self.qlearn_publisher = rospy.Publisher(
"/results/qlearn_policy_list",
PolicyList,
queue_size = 10
)
self.complete_publisher = rospy.Publisher(
"/map_node/sim_complete",
Bool,
queue_size = 10
)
qPolicy = qlearn()
for eachPolicy in qPolicy:
rospy.sleep(1)
self.qlearn_publisher.publish(eachPolicy)
rospy.sleep(1)
paths = astar(self.config)
for eachEntry in paths:
rospy.sleep(1)
self.path_publisher.publish(eachEntry)
mdps = mdp(self.config)
rospy.sleep(1)
self.mdp_publisher.publish(mdps)
rospy.sleep(1)
self.complete_publisher.publish(True)
rospy.sleep(1)
rospy.signal_shutdown("shutting down")
def shortest_path(self, start, end):
from astar import astar
path = astar(self.neighbors, start, end, lambda x,y : 0.0, self.distance)
actions = [self.get_action(path[i],path[i+1]) for i in range(len(path) - 1)]
return path, actions
def test():
sol = astar.path_to_list(astar.astar(TestProblem()).next())
print len(sol)
print sol
return 0
return matrix[spot[0]][spot[1]]
def g(state):
# print getpath(state)
return sum(getcost(spot) for spot in getpath(state))
min_entry = min(min(matrix[r][c] for c in range(len(matrix[r]))) for r in range(len(matrix)))
print "Min entry:", min_entry
def h(((r, c), last)):
return abs(r - (len(matrix) - 1)) + abs(c - (len(matrix[-1]) - 1))
def h82((spot, last)):
if spot == "START":
return min_entry * len(matrix[0])
(r, c) = spot
return min_entry * len(matrix[r]) - 1 - c
# p81
# print astar((0,0), successors, goal81, g, h)
# p82
# print astar('START', successors82, goal82, g, h82)
# p83
print astar((0, 0), successors82, goal81, g, h)
(position[0], position[1]-1),
(position[0]+1, position[1]),
(position[0]-1, position[1])]
is_opened = lambda position: \
0 <= position[0] < len(world[0]) and \
0 <= position[1] < len(world) and \
world[position[1]][position[0]] == 0
opened = list(filter(is_opened, potential))
return opened
def heuristic(position, goal):
return abs(position[0] - goal[0]) + abs(position[1] - goal[1])
a = astar(heuristic, successors)
world = [[1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1],
[1,0,0,0,0,0,0,0,1,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,1,0,0,0,0,0,1,0,1],
[1,0,1,1,1,1,1,1,1,0,1,0,1,1,1,0,1,0,1,1,1,0,1,1,1,0,1,1,1,1,1,1,1,0,1,0,1],
[1,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,1,0,1,0,0,0,0,0,1,0,0,0,0,0,1,0,1,0,0,0,1],
[1,1,1,1,1,0,1,1,1,1,1,0,1,1,1,0,1,0,1,0,1,0,1,1,1,0,1,1,1,1,1,0,1,0,1,1,1],
[1,0,0,0,1,0,1,0,1,0,1,0,0,0,1,0,1,0,1,0,1,0,0,0,1,0,1,0,0,0,1,0,1,0,0,0,1],
[1,0,1,1,1,0,1,0,1,0,1,0,1,1,1,0,1,1,1,1,1,0,1,1,1,0,1,0,1,1,1,0,1,1,1,0,1],
[1,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,0,0,1,0,0,0,1,0,0,0,0,0,1,0,1,0,1,0,0,0,1],
[1,1,1,0,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,0,1,1,1,0,1,1,1,0,1,0,1,0,1,0,1],
[1,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,1,0,1,0,0,0,1,0,0,0,0,0,1,0,1],
[1,0,1,0,1,1,1,1,1,0,1,0,1,1,1,0,1,0,1,0,1,1,1,0,1,0,1,1,1,0,1,1,1,0,1,0,1],
[1,0,1,0,1,0,0,0,0,0,1,0,1,0,1,0,1,0,1,0,0,0,0,0,1,0,0,0,1,0,1,0,1,0,1,0,1],
[1,0,1,0,1,0,1,1,1,1,1,1,1,0,1,0,1,1,1,1,1,1,1,1,1,0,1,1,1,0,1,0,1,1,1,0,1],
[1,0,1,0,1,0,1,0,0,0,0,0,1,0,0,0,1,0,0,0,0,0,1,0,0,0,0,0,1,0,0,0,1,0,0,0,1],
it = pyfirmata.util.Iterator( board )
it.start()
tour = propulsion.Tourelle( config, board )
prop = propulsion.Propulsion( config, board )
import astar
list_map = [
["X", "X", "X", "X", "X", "X", "X", "X", "X", "X", "X", "X", "X", "X", "X", "X"],
["X", ".", "X", "X", "X", "X", "X", ".", ".", ".", ".", "X", ".", "X", ".", "X"],
["X", ".", "X", ".", ".", ".", "X", ".", ".", ".", ".", "X", ".", "X", ".", "X"],
[".", ".", "X", ".", ".", ".", "X", ".", ".", ".", ".", "X", ".", "X", ".", "X"],
["X", ".", "X", ".", "X", ".", "X", ".", ".", ".", ".", "X", ".", "X", ".", "X"],
["X", ".", ".", ".", "X", ".", "X", ".", ".", ".", ".", "X", ".", "X", ".", "X"],
["X", "X", "X", "X", "X", ".", ".", ".", ".", ".", ".", "X", ".", "X", ".", "X"],
["X", ".", ".", ".", "X", ".", ".", ".", ".", ".", ".", "X", ".", "X", ".", "X"],
["X", ".", ".", ".", ".", ".", "X", ".", ".", ".", ".", "X", ".", "X", ".", "X"],
["X", ".", ".", "X", "X", "X", "X", ".", ".", ".", ".", "X", ".", "X", ".", "X"],
[".", ".", ".", ".", ".", ".", "X", ".", "X", "X", ".", "X", ".", "X", ".", "X"],
["X", ".", ".", ".", ".", ".", "X", ".", ".", "X", ".", "X", ".", ".", ".", "X"],
["X", ".", "X", ".", "X", ".", "X", ".", ".", "X", ".", "X", ".", "X", "X", "X"],
["X", ".", "X", ".", "X", ".", "X", ".", ".", "X", ".", "X", ".", "X", ".", "X"],
["X", ".", "X", ".", "X", ".", "X", ".", ".", ".", ".", ".", ".", "X", ".", "X"],
["X", "X", "X", "X", "X", "X", "X", "X", "X", "X", "X", "X", "X", "X", "X", "X"],
]
ma_map = astar.Maze.from_array_2d( list_map )
ch = astar.astar( ma_map, ma_map.get( 0, 10 ), ma_map.get( 0, 3 ) )
for n in ch:
print n.x, n.y
ma_map.print_path( ch )
x, y = p
if is_outside_map(x, y):
return False
if is_block(x, y):
return False
return True
def neighbor_nodes(p):
x, y = p
neighbors = [(x-1, y), (x, y-1), (x+1, y), (x, y+1)]
return filter(is_movable, neighbors)
def heuristic_cost_estimate(p1, p2):
return manhattan_distance(p1, p2)
def manhattan_distance(p1, p2):
return abs(p1[0]-p2[0]) + abs(p1[1]-p2[1])
def euclidean_distance(p1, p2):
return math.sqrt((p1[0]-p2[0])**2 + (p1[1]-p2[1])**2)
if __name__ == '__main__':
start = (2, 2)
goal = (1, 1)
path = astar.astar(start, goal, neighbor_nodes, heuristic_cost_estimate, heuristic_cost_estimate)
if path:
print("====")
for position in reversed(path):
x,y = position
print(x,y)
def compute_path(start,goal):
self.path = astar.astar(self.grid,start,goal)
def __init__(self):
# Configuration setup
self.config = read_config()
rospy.init_node("robot", anonymous=True)
rospy.sleep(3)
# Publishers
self.astar_publisher = rospy.Publisher(
"/results/path_list",
AStarPath,
queue_size = 10
)
self.mdp_publisher = rospy.Publisher(
"/results/policy_list",
PolicyList,
queue_size = 10
)
self.sim_complete_publisher = rospy.Publisher(
"/map_node/sim_complete",
Bool,
queue_size = 1
)
path = astar(
self.config["move_list"],
self.config["start"],
self.config["goal"],
self.config["map_size"],
self.config["walls"],
self.config["pits"]
)
print "A* Path"
print path
rospy.sleep(2)
for p in path:
self.astar_publisher.publish(p)
rospy.sleep(0.1)
rospy.sleep(2)
mdp_object = mdp.mdp(
self.config["move_list"],
self.config["map_size"],
self.config["goal"],
self.config["walls"],
self.config["pits"]
)
oldValues = list()
for x in range(mdp_object.height):
for y in range(mdp_object.width):
oldValues.append(mdp_object.values[x][y])
takeNewValues = True
while mdp_object.iteration < self.config["max_iterations"] and takeNewValues:
newValues = list()
policy = list()
differences = list()
takeNewValues = False
mdp_object.value_iteration()
newValueSum = 0
for x in range(mdp_object.height):
for y in range(mdp_object.width):
newValues.append(mdp_object.values[x][y])
for x in range(mdp_object.height):
for y in range(mdp_object.width):
policy.append(mdp_object.policy[x][y])
for i in range(len(newValues)):
newValueSum += abs(oldValues[i] - newValues[i])
if (newValueSum > self.config["threshold_difference"]):
takeNewValues = True
oldValues = newValues
self.mdp_publisher.publish(policy)
rospy.sleep(0.1)
print policy
rospy.sleep(2)
# Q Value Learning - Final Project
q_object = qlearning.qlearning()
oldQValues = 0.0
takeNewValues = True
while q_object.iteration < self.config["max_iterations"]:
newQValues = 0.0
difference = 0.0
takeNewValues = False
next_state = q_object.update_q_values()
(x, y) = q_object.position
newQValues = sum(q_object.q_values[x][y])
difference = abs(oldQValues - newQValues)
print "Q Learning Iteration: "
print q_object.iteration
pprint.pprint(q_object.q_values)
if difference > self.config["threshold_difference"]:
takeNewValues = True
oldQValues = newQValues
q_object.position = next_state
rospy.sleep(0.1)
self.sim_complete_publisher.publish(True)
rospy.sleep(2)
rospy.signal_shutdown("End")
def clear_paths(voxel_grid, zone_grid, hardness_grid, zone2cells, doors):
V = voxel_grid
Z = zone_grid
H = hardness_grid
save_grid_png(H, 'hardness.png')
zone2hub = {}
for (zone, cells) in zone2cells.iteritems():
hub = Int2.centroid(cells)
if Z.pget(hub) != zone or V.pget(hub).material == None:
spaces = [cell for cell in cells if V.pget(cell).material != None]
if len(spaces) == 0:
# guess we'll have to make one
hub = random.choice(cells)
else:
hub = random.choice(spaces)
assert Z.pget(hub) == zone
zone2hub[zone] = hub
pair2door = {}
for ((z1, z2), _) in doors.iteritems():
h1 = zone2hub[z1]
h2 = zone2hub[z2]
def separates_curr_pair_only(u):
for (v, q) in Z.nbors8(u):
if q not in (z1, z2, EMPTY_ZONE):
return False
return True
def yield_nbors(u):
for (v,q) in Z.nbors4(u):
if q in (z1, z2):
yield v
elif q == EMPTY_ZONE and separates_curr_pair_only(v):
yield v
def edge_cost(u,v):
# always add one for distance
assert Z.pget(v) in (z1, z2, EMPTY_ZONE)
return H.pget(v) + 1
def est_to_target(u):
return Int2.euclidian_dist(u, hub)
print 'astarring from %s to %s, hubs %s to %s' % (z1, z2, str(h1), str(h2))
for u in astar.astar(h1, h2, yield_nbors, edge_cost, est_to_target):
# override whatever is here and force it to be this zone
# this is the "digging"
if Z.pget(u) == EMPTY_ZONE:
vox = V.pget(u)
vox.material = z1
vox.door_pair = (z1, z2)
vox.ceilht = vox.floorht
# this does overwrite any previous cells, but that's ok
# door builder only needs 1 cell for this door to get the right sector
pair2door[ (z1,z2) ] = u
else:
V.pget(u).material = Z.pget(u)
return pair2door
def __init__(self):
self.config = read_config()
rospy.init_node("robot")
self.path_publisher = rospy.Publisher(
"/results/path_list",
AStarPath,
queue_size = 50
)
self.complete_publisher = rospy.Publisher(
"/map_node/sim_complete",
Bool,
queue_size = 10
)
self.policy_publisher = rospy.Publisher(
"/results/policy_list",
PolicyList,
queue_size = 10
)
astarPath = astar(self.config)
path = astarPath.path
rospy.sleep(5)
for point in path:
print point
self.path_publisher.publish(point)
rospy.sleep(3)
map_size = self.config['map_size']
rows = map_size[0]
columns = map_size[1]
goal = self.config['goal']
walls = self.config['walls']
pits = self.config['pits']
move_list = self.config['move_list']
oldV = [[0 for x in range(columns)] for y in range(rows)]
# initialize V values
#oldV = [[0 for x in range(columns)] for y in range(rows)]
for i in range(rows):
for j in range(columns):
tempGrid = [i,j]
if tempGrid == goal:
oldV[i][j] = self.config['reward_for_reaching_goal']
elif tempGrid in walls:
oldV[i][j] = self.config['reward_for_hitting_wall']
elif tempGrid in pits:
oldV[i][j] = self.config['reward_for_falling_in_pit']
else:
#this is redundant
oldV[i][j] = 0
stoppingSum = 0
firstThreshold = False
secondThreshold = False
firstX = 0
secondX = -100
for x in range(self.config['max_iterations']):
mdpPolicy = mdp(self.config,oldV)
policies = mdpPolicy.policies
#policies = ["WALL", "WALL", "WALL", "WALL", "WALL", "WALL", "WALL", "WALL", "WALL", "WALL", "WALL", "S", "S", "S", "W", "PIT", "S", "PIT", "S", "WALL", "E", "E", "S", "W", "W", "W", "S", "E", "S", "WALL", "WALL", "E", "S", "W", "WALL", "PIT", "S", "PIT", "S", "WALL", "WALL", "E", "S", "W", "WALL", "WALL", "S", "WALL", "S", "WALL", "WALL", "E", "S", "W", "WALL", "PIT", "S", "PIT", "S", "WALL", "WALL", "E", "E", "E", "E", "S", "S", "S", "E", "GOAL", "WALL", "E", "N", "W", "WALL", "E", "E", "E", "N", "WALL", "WALL", "N", "N", "N", "E", "N", "N", "N", "N", "WALL", "WALL", "WALL", "WALL", "WALL", "WALL", "WALL", "WALL", "WALL", "WALL", "WALL"]
self.policy_publisher.publish(policies)
for i in range(rows):
for j in range(columns):
stoppingSum += abs(oldV[i][j]-mdpPolicy.newV[i][j])
if(stoppingSum < self.config['threshold_difference']):
secondX = x
if(secondX - 1 == firstX ):
break
if(stoppingSum < self.config['threshold_difference'] ):
firstX = x
firstThreshold = True
oldV = mdpPolicy.newV
rospy.sleep(5)
self.complete_publisher.publish(True)
rospy.sleep(3)
rospy.signal_shutdown("sim complete")
def path(self, p_0, p_1):
HA_STEP = 0.5
DE_STEP = 10
bag = {}
def cost(p_a, p_b):
return sqrt((p_b.ha - p_a.ha) ** 2 + (p_b.de - p_a.de) ** 2)
def heuristic(p):
return cost(p, p_1)
def neighbor_positions(p):
local_grid = [
(p.ha + HA_STEP, p.de),
(p.ha, p.de + DE_STEP),
(p.ha - HA_STEP, p.de),
(p.ha, p.de - DE_STEP),
]
for coords in local_grid:
bag[coords] = bag.get(coords, Position(coords[0], coords[1]))
return [bag[coords] for coords in local_grid if self.contains(bag[coords].point())]
def goal(p):
near_area = Polygon([
(p.ha - HA_STEP/2, p.de - DE_STEP/2),
(p.ha - HA_STEP/2, p.de + DE_STEP/2),
(p.ha + HA_STEP/2, p.de + DE_STEP/2),
(p.ha + HA_STEP/2, p.de - DE_STEP/2),
])
return near_area.contains(p_1.point()) or near_area.touches(p_1.point()) # touches if point exactly on the grid
def smooth(path):
n = len(path) - 1
i = 0
direct_path = [0]
def find_furthest_direct():
j = n
k = n
best_j = i + 1
while True:
line = LineString([(path[i].ha, path[i].de), (path[j].ha, path[j].de)])
if self.contains(line):
best_j = max(j, best_j)
j += int(floor((k-j)/2))
if k - best_j < 2:
return best_j
else:
k = j
j -= int(floor((j-best_j)/2))
if k - j < 1:
return best_j
while n - i > 2:
j = find_furthest_direct()
direct_path.append(j)
i = j
return [path[i] for i in range(0, len(path)) if i in direct_path]
path = astar(p_0, neighbor_positions, goal, 0, cost, heuristic)
if path == []:
path = [p_0, p_1]
else:
path[-1] = p_1
return smooth(path)
