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