def main(argv):
maze = argv[0] if len(argv) > 0 else "../Mazes/maze01.txt"
model = argv[1] if len(argv) > 1 else "../Models/auto-gen-c.pkl"
show_freq = int(
argv[2]) if len(argv) > 2 else 0 # frequency to show frames
directory_name = argv[5] if len(argv) > 5 else "tmp_diagnostics"
print("DIR NAME: " + directory_name)
env = PycastWorldEnv(maze, 320, 240)
observation = env.reset()
path = Path("../")
model_inf = load_learner(model)
prev_move = None
prev_image_data = None
frame = 0
num_static = 0
prev_x, prev_y = env.world.x(), env.world.y()
animation_frames = [observation.copy()]
prev_pred = 0
outcome = "At goal? "
stuck = False
# Initialize maximum number of steps in case the robot travels in a completely incorrect direction
max_steps = 3000
step_count = 0
# Initialize Maze Check
maze_rvs, _, _, maze_directions, _ = read_maze_file(maze)
start_x, start_y, _ = maze_directions[0]
end_x, end_y, _ = maze_directions[-1]
_, maze_path = bfs_dist_maze(maze_rvs, start_x, start_y, end_x, end_y)
while not env.world.at_goal() and num_static < 5:
if is_on_path(maze_path, int(env.world.x()), int(
env.world.y())) is False:
print("Off Path")
break
# Get image
image_data = np.array(env.world)
# Convert image_data and give to network
pred_angle, _, _ = model_inf.predict(observation)
pred_angle = math.ceil(pred_angle[0])
num_movements = abs(int(pred_angle / 20))
if num_movements == 0:
action_index = 1
observation, reward, done, info = env.step(action_index)
prev_pred = pred_angle
continue
# move robot
if (prev_pred > 0 and pred_angle < 0) or (prev_pred < 0
and pred_angle > 0):
action_index = 1
observation, reward, done, info = env.step(action_index)
prev_pred = pred_angle
continue
action_index = 1
if pred_angle > 0 and num_movements > 0:
for i in range(num_movements):
action_index = 0 # turn left
observation, reward, done, info = env.step(action_index)
elif pred_angle < 0 and num_movements > 0:
for i in range(num_movements):
action_index = 2 # turn right
observation, reward, done, info = env.step(action_index)
prev_pred = pred_angle
# Check if we reached the end goal
# prev_move = move
env.world.update()
curr_x, curr_y = round(env.world.x(), 5), round(env.world.y(), 5)
if show_freq != 0 and frame % show_freq == 0:
if curr_x == prev_x and curr_y == prev_y:
num_static += 1
else:
num_static = 0
animation_frames.append(image_data.copy())
prev_x = curr_x
prev_y = curr_y
frame += 1
prev_image_data = image_data
if frame == max_steps:
print("Exceeds step limit")
break
# this chunk gets the completion percentage
lost = False
if num_static >= 5:
stuck = True
if frame >= max_steps:
lost = True
outcome = ("At Goal? " + str(env.world.at_goal()) + "\n Stuck? " +
str(stuck) + "\n Exceed step limit? " + str(lost))
print(outcome)
completion_per = percent_through_maze(maze_rvs, int(env.world.x()),
int(env.world.y()), start_x, start_y,
end_x, end_y)
# plt.imshow(image_data)
# plt.show()
# print("DIR NAME: ")
animate(animation_frames, model, directory_name)
if num_static >= 5 and not env.world.at_goal(
): # model failed to navigate maze
return frame, False, completion_per
else: # model successfully navigated maze
return frame, True, completion_per
def process_maze(
maze_filepath: str,
) -> Tuple[List[Tuple[Pt, Pt, Pt, str, int, int]], List[Tuple[Pt, str, str]]]:
# Loop to find the first change in direction
def get_next_dir(maze_directions, curr_heading):
turnx, turny, next_heading = next(maze_directions)
while next_heading == curr_heading:
try:
turnx, turny, next_heading = next(maze_directions)
except StopIteration:
break
return turnx, turny, next_heading
# Compute path from maze file
maze, _, _, maze_directions, _ = read_maze_file(maze_filepath)
x_start, y_start, _ = maze_directions[0]
x_end, y_end, _ = maze_directions[-1]
_, correct_path = bfs_dist_maze(maze, x_start, y_start, x_end, y_end)
# Compute the direction and next_turn cell for each cell along the path
maze_directions = iter(maze_directions)
_, _, curr_heading = next(maze_directions) # First directions
curr_heading = curr_heading
# Set these so that they are at reasonable defaults for the first iteration
turnx, turny, next_heading = get_next_dir(maze_directions, curr_heading)
prev_heading = ""
prev_action = None
next_action = DIRS_TO_TURN.get((curr_heading, next_heading), "FORWARD")
path_cells = []
turn_cells = []
for (x, y) in correct_path[:-1]: # We don't need images for the final cell
# Update information when we make a turn
if x == turnx and y == turny:
is_turn_cell = True
prev_heading = curr_heading
curr_heading = next_heading
prev_action = next_action
turnx, turny, next_heading = get_next_dir(maze_directions,
curr_heading)
# Default to FORWARD when curr_heading toward goal cell and we don't
# have a next heading
next_action = DIRS_TO_TURN.get((curr_heading, next_heading),
"FORWARD")
else:
is_turn_cell = False
# Right and left corners depend on curr_heading
rightx = turnx if curr_heading in ("SOUTH", "EAST") else turnx + 1
righty = turny if curr_heading in ("NORTH", "EAST") else turny + 1
right_corner = Pt(rightx, righty)
leftx = turnx if curr_heading in ("NORTH", "EAST") else turnx + 1
lefty = turny if curr_heading in ("NORTH", "WEST") else turny + 1
left_corner = Pt(leftx, lefty)
# Add 0.5 offset to center position in the hallway
pos = Pt(x + 0.5, y + 0.5)
cell_info = (
pos,
right_corner,
left_corner,
curr_heading,
prev_heading,
prev_action,
)
path_cells.append(cell_info)
if is_turn_cell:
turn_cells.append(cell_info)
return path_cells, turn_cells
def main(argv):
maze = argv[0] if len(argv) > 0 else "../Mazes/maze01.txt"
model = argv[1] if len(argv) > 1 else "../Models/auto-gen-c.pkl"
show_freq = int(argv[2]) if len(argv) > 2 else 0 # frequency to show frames
directory_name = argv[5] if len(argv) > 5 else "tmp_diagnostics"
print("DIR NAME: " + directory_name)
model_type = (
argv[3] if len(argv) > 3 else "c"
) # 'c' for classification, 'r' for regresssion
stacked = (
bool(distutils.util.strtobool(argv[4])) if len(argv) > 4 else False
) # True for stacked input
world = PycastWorld(320, 240, maze)
path = Path("../")
model_inf = load_learner(model)
prev_move = None
prev_image_data = None
frame = 0
num_static = 0
prev_x, prev_y = world.x(), world.y()
animation_frames = []
outcome = "At goal? "
stuck = False
# Initialize maximum number of steps in case the robot travels in a completely incorrect direction
max_steps = 3000
step_count = 0
# Initialize Maze Check
maze_rvs, _, _, maze_directions, _ = read_maze_file(maze)
start_x, start_y, _ = maze_directions[0]
end_x, end_y, _ = maze_directions[-1]
_, maze_path = bfs_dist_maze(maze_rvs, start_x, start_y, end_x, end_y)
while not world.at_goal() and num_static < 5:
if is_on_path(maze_path, int(world.x()), int(world.y())) is False:
print("Off Path")
break
# Get image
image_data = np.array(world)
# Convert image_data and give to network
if model_type == "c":
if stacked:
move = model_inf.predict(stacked_input(prev_image_data, image_data))[0]
else:
move = model_inf.predict(image_data)[0]
elif model_type == "r":
if stacked:
pred_coords, _, _ = model_inf.predict(
stacked_input(prev_image_data, image_data)
)
else:
pred_coords, _, _ = model_inf.predict(image_data)
move = reg_predict(pred_coords)
# print(move)
if move == "left" and prev_move == "right":
move = "straight"
elif move == "right" and prev_move == "left":
move = "straight"
# Move in world
if move == "straight":
world.walk(Walk.Forward)
world.turn(Turn.Stop)
elif move == "left":
world.walk(Walk.Stop)
world.turn(Turn.Left)
else:
world.walk(Walk.Stop)
world.turn(Turn.Right)
prev_move = move
world.update()
curr_x, curr_y = round(world.x(), 5), round(world.y(), 5)
if show_freq != 0 and frame % show_freq == 0:
if curr_x == prev_x and curr_y == prev_y:
num_static += 1
else:
num_static = 0
animation_frames.append(image_data.copy())
# plt.imshow(image_data)
# plt.show()
# update previous coordinates
prev_x = curr_x
prev_y = curr_y
frame += 1
prev_image_data = image_data
if frame == max_steps:
print("Exceeds step limit")
break
# this chunk gets the completion percentage
lost = False
if num_static >= 5:
stuck = True
if frame >= max_steps:
lost = True
outcome = (
"At Goal? "
+ str(world.at_goal())
+ "\n Stuck? "
+ str(stuck)
+ "\n Exceed step limit? "
+ str(lost)
)
print(outcome)
completion_per = percent_through_maze(
maze_rvs, int(world.x()), int(world.y()), start_x, start_y, end_x, end_y
)
plt.imshow(image_data)
plt.show()
print("DIR NAME: ")
animate(animation_frames, model, directory_name)
if num_static >= 5 and not world.at_goal(): # model failed to navigate maze
return frame, False, completion_per
else: # model successfully navigated maze
return frame, True, completion_per
def main():
ANG_NOISE_MAG = radians(30)
POS_NOISE_MAG = 0.4
arg_parser = ArgumentParser("Run a maze utility")
arg_parser.add_argument("maze_filepath", help="Path to a maze file.")
arg_parser.add_argument("save_dir", help="Save location (directory must exist).")
arg_parser.add_argument(
"num_straight_images", type=int, help="Number of straightaway images."
)
arg_parser.add_argument(
"num_turn_images", type=int, help="Number of turning specific images."
)
arg_parser.add_argument(
"--image_width", type=int, default=224, help="Width of generated images."
)
arg_parser.add_argument(
"--image_height", type=int, default=224, help="Height of generated images."
)
# TODO: currently unused
arg_parser.add_argument(
"--sequence", type=int, default=1, help="Number of images per sequence."
)
arg_parser.add_argument(
"--demo", action="store_true", help="Display images instead of saving."
)
args = arg_parser.parse_args()
# Compute path from maze file
maze, _, _, maze_directions, _ = read_maze_file(args.maze_filepath)
x_start, y_start, _ = maze_directions[0]
x_end, y_end, _ = maze_directions[-1]
_, correct_path = bfs_dist_maze(maze, x_start, y_start, x_end, y_end)
dirs_to_turn = {
("NORTH", "EAST"): "RIGHT",
("NORTH", "WEST"): "LEFT",
("EAST", "NORTH"): "LEFT",
("EAST", "SOUTH"): "RIGHT",
("SOUTH", "EAST"): "LEFT",
("SOUTH", "WEST"): "RIGHT",
("WEST", "NORTH"): "RIGHT",
("WEST", "SOUTH"): "LEFT",
}
dir_to_angle = {
"EAST": radians(0),
"WEST": radians(180),
"NORTH": radians(90),
"SOUTH": radians(270),
}
# Compute the direction and next_turn cell for each cell along the path
maze_directions = iter(maze_directions)
_, _, direction = next(maze_directions) # First directions
# Loop to find the next change in direction
turnx, turny, next_direction = next(maze_directions)
while next_direction == direction:
turnx, turny, next_direction = next(maze_directions)
# Set these so that they are at reasonable defaults for the first iteration
is_turn_cell = False
prev_direction = ""
prev_turn = None
prev_heading = ""
upcoming_turn = None
path_cells = []
turn_cells = []
for (x, y) in correct_path[:-1]: # We don't need images for the final cell
if x == turnx and y == turny:
is_turn_cell = True
prev_direction = direction
prev_turn = upcoming_turn
direction = next_direction
try:
turnx, turny, next_direction = next(maze_directions)
while next_direction == direction:
turnx, turny, next_direction = next(maze_directions)
except StopIteration:
pass
heading = direction[4:]
next_heading = next_direction[4:]
prev_heading = prev_direction[4:]
# Default to FORWARD when heading toward goal cell
upcoming_turn = dirs_to_turn.get((heading, next_heading), "FORWARD")
# Right and left corners depend on heading
rightx = turnx if heading in ("SOUTH", "EAST") else turnx + 1
righty = turny if heading in ("NORTH", "EAST") else turny + 1
right_corner = Pt(rightx, righty)
leftx = turnx if heading in ("NORTH", "EAST") else turnx + 1
lefty = turny if heading in ("NORTH", "WEST") else turny + 1
left_corner = Pt(leftx, lefty)
# Add 0.5 offset to center in the hallway
pos = Pt(x + 0.5, y + 0.5)
path_cells.append((pos, right_corner, left_corner, heading, turnx, turny,))
if is_turn_cell:
turn_cells.append((pos, prev_heading, prev_turn))
# Reset for the next cell
is_turn_cell = False
# Create world
world = PycastWorld(320, 240, args.maze_filepath)
# FOV = radians(66) TODO: figure this out
# Select position along path
for i in range(args.num_straight_images):
# TODO: not using turnx or turny
pos, right_corner, left_corner, heading, turnx, turny = choice(path_cells)
# Perturb the position and heading
perturbed_pos = pos + Pt.rand((-POS_NOISE_MAG, POS_NOISE_MAG))
x, y = perturbed_pos.xy
ang_noise = ANG_NOISE_MAG * (2 * random() - 1)
angle = angle_from_zero_to_2pi(dir_to_angle[heading] + ang_noise)
world.set_position(x, y)
world.set_direction(angle)
# Compute angles to the two corners of the turn
angle_to_right = angle_from_zero_to_2pi(Pt.angle(right_corner - perturbed_pos))
angle_to_left = angle_from_zero_to_2pi(Pt.angle(left_corner - perturbed_pos))
# Angles can straddle 0/360 when facing EAST
if angle_to_right > angle_to_left:
angle = angle_from_negpi_to_pospi(angle)
angle_to_right = angle_from_negpi_to_pospi(angle_to_right)
angle_to_left = angle_from_negpi_to_pospi(angle_to_left)
# Compute the "correct" action
if angle < angle_to_right:
action = "LEFT"
elif angle > angle_to_left:
action = "RIGHT"
else:
action = "FORWARD"
print(
f"{i:>6} : ",
f"{x:5.2f}",
f"{y:5.2f}",
heading.rjust(5),
f"{degrees(angle_to_right): 7.2f}",
f"{degrees(angle_to_left): 7.2f}",
f"{degrees(angle): 7.2f}",
action.rjust(7),
)
filename = f"{i:>06}.png"
if args.demo:
import matplotlib.pyplot as plt
import numpy as np
image = np.array(world)
plt.imshow(image)
plt.show()
print(f"File not saved in demo mode ({filename}).")
else:
world.save_png(str(Path(args.save_dir) / action.lower() / filename))
for i in range(args.num_turn_images):
i += args.num_straight_images
pos, heading, turn = choice(turn_cells)
# Perturb the position and heading
perturbed_pos = pos + Pt.rand((-POS_NOISE_MAG, POS_NOISE_MAG))
x, y = perturbed_pos.xy
ang_noise_mag = radians(30)
ang_noise = ang_noise_mag * (2 * random() - 1)
angle = angle_from_zero_to_2pi(dir_to_angle[heading] + ang_noise)
# TODO: check if arrow is in field of view
world.set_position(x, y)
world.set_direction(angle)
action = turn
print(
f"{i:>6} : ",
f"{x:5.2f}",
f"{y:5.2f}",
heading.rjust(5),
f"{degrees(angle): 7.2f}",
action.rjust(7),
)
filename = f"{i:>06}.png"
if args.demo:
import matplotlib.pyplot as plt
import numpy as np
image = np.array(world)
plt.imshow(image)
plt.show()
print(f"File not saved in demo mode ({filename}).")
else:
world.save_png(str(Path(args.save_dir) / action.lower() / filename))
def main(argv):
maze = argv[0] if len(argv) > 0 else "../Mazes/maze01.txt"
model = argv[1] if len(argv) > 1 else "../Models/auto-gen-c.pkl"
show_freq = int(
argv[2]) if len(argv) > 2 else 0 # frequency to show frames
directory_name = argv[5] if len(argv) > 5 else "tmp_diagnostics"
# cmd_in = bool(distutils.util.strtobool(argv[6]) if len(argv) > 6 else False
print("DIR NAME: " + directory_name)
model_type = (argv[3] if len(argv) > 3 else "c"
) # 'c' for classification, 'r' for regresssion
stacked = (
bool(distutils.util.strtobool(argv[4])) if len(argv) > 4 else False
) # True for stacked input
world = PycastWorld(320, 240, maze)
if model_type == "cmd" or model_type == "rnn":
model_inf = ConvRNN()
model_inf.load_state_dict(torch.load(model))
else:
path = Path("../")
model_inf = load_learner(model)
prev_move = None
prev_image_data = None
frame = 0
frame_freq = 5
num_static = 0
prev_x, prev_y = world.x(), world.y()
animation_frames = []
outcome = "At goal? "
stuck = False
# Initialize maximum number of steps in case the robot travels in a completely incorrect direction
max_steps = 3500
# Initialize Maze Check
maze_rvs, _, _, maze_directions, _ = read_maze_file(maze)
start_x, start_y, _ = maze_directions[0]
end_x, end_y, _ = maze_directions[-1]
_, maze_path = bfs_dist_maze(maze_rvs, start_x, start_y, end_x, end_y)
on_path = is_on_path(maze_path, int(world.x()), int(world.y()))
print("Predicting...")
while not world.at_goal() and num_static < 5 and on_path:
# Get image
image_data = np.array(world)
# Convert image_data and give to network
if model_type == "c":
if stacked:
move = model_inf.predict(
stacked_input(prev_image_data, image_data))[0]
else:
move = model_inf.predict(image_data)[0]
elif model_type == "r":
if stacked:
pred_coords, _, _ = model_inf.predict(
stacked_input(prev_image_data, image_data))
else:
pred_coords, _, _ = model_inf.predict(image_data)
move = reg_predict(pred_coords)
elif model_type == "cmd":
model_inf.eval()
# Predict
tmp_move_indx = 2 if prev_move == "straight" else 1 if prev_move == "right" else 0
img = (tensor(image_data) / 255).permute(2, 0, 1).unsqueeze(0)
cmd = tensor([tmp_move_indx])
output = model_inf((img, cmd))
# Assuming we always get batches
if output.size()[0] > 0:
for i in range(output.size()[0]):
# Getting the predicted most probable move
action_index = torch.argmax(output[i])
move = 'left' if action_index == 0 else 'right' if action_index == 1 else 'straight'
else:
# is there any reason for us to believe batch sizes can be empty?
move = 'straight'
elif model_type == "rnn":
model_inf.eval()
img = (tensor(image_data) / 255).permute(
2, 0, 1).unsqueeze(0).unsqueeze(0)
output = model_inf(img)
# Assuming we always get batches
for i in range(output.size()[0]):
# Getting the predicted most probable move
action_index = torch.argmax(output[i])
move = 'left' if action_index == 0 else 'right' if action_index == 1 else 'straight'
if move == "left" and prev_move == "right":
move = "straight"
elif move == "right" and prev_move == "left":
move = "straight"
# Move in world
if move == "straight":
world.walk(Walk.Forward)
world.turn(Turn.Stop)
elif move == "left":
world.walk(Walk.Stop)
world.turn(Turn.Left)
else:
world.walk(Walk.Stop)
world.turn(Turn.Right)
prev_move = move
world.update()
curr_x, curr_y = round(world.x(), 5), round(world.y(), 5)
if show_freq != 0 and frame % show_freq == 0:
if int(curr_x) == int(prev_x) and int(curr_y) == int(prev_y):
num_static += 1
else:
maze_path.remove((int(prev_x), int(prev_y)))
num_static = 0
prev_x = curr_x
prev_y = curr_y
if frame % frame_freq == 0:
animation_frames.append(image_data.copy())
on_path = is_on_path(maze_path, int(world.x()), int(world.y()))
frame += 1
prev_image_data = image_data
if frame == max_steps:
print("Exceeds step limit")
break
# this chunk gets the completion percentage
lost = False
if num_static >= 5:
stuck = True
if frame >= max_steps:
lost = True
outcome = ("At Goal? " + str(world.at_goal()) + "\n Stuck? " + str(stuck) +
"\n Exceed step limit? " + str(lost) + "\n On path? " +
str(on_path))
print(outcome)
completion_per = percent_through_maze(maze_rvs, int(world.x()),
int(world.y()), start_x, start_y,
end_x, end_y)
animate(animation_frames, model, directory_name)
if num_static >= 5 and not world.at_goal(
): # model failed to navigate maze
return frame, False, completion_per
else: # model successfully navigated maze
return frame, True, completion_per