I am writing a 2D discrete motion planner for a task with specific instructions. The instructions for the search algorithm are contained entirely in the docstring for the
My main concerns with the code are the following:
- Did I get the complexity of this code correct ( O(N) )?
- Is the documentation complete?
Please make any recommendations you see relevant, provided they adhere to the search algorithm instructions.
#!/usr/bin/env python import numpy as np class RandomPlanner(object): """Random 2D discrete motion planner. Args: max_step_number (int): Maximum number of steps in search path before search fails. Also memory length is determined by sqrt(max_step_number). Attributes: max_step_number (int): Maximum number of steps in search path before search fails. Also memory length is determined by sqrt(max_step_number). """ def __init__(self, max_step_number): self.max_step_number = max_step_number def search(self, world_state, robot_pose, goal_pose): """Random based discrete motion planner. This search algorithm tries to fina a path to the goal by randomly moving in the environment (only orthogonal moves are legal). If the planner can not find an acceptable solution in less than max_step_number, the search fails. The random planner has a short memory, never attempting to visit a cell that was visited in the last sqrt(max_step_number) steps except if this is the only available option. Complexity of this algorithm is O(N) where N is the max_step_number. Args: world_state (2D array): Grid representation of the environment. The value 0 indicates a navigable space and the value 1 indicates an occupied/obstacle space. robot_pose (2-tuple): Indices (x, y) represent the current pose of the robot in the world_state coordinate system. goal_pose (2-tuple): Indices (x, y) represent the goal in the world_state coordinate system. Returns: List of tuple (x, y) representing a path from the robot_pose to the goal_pose in world_state if successful. None if no path has been found. Raises: AssertionError: if arguments are not of correct shape. """ assert world_state.ndim == 2 assert len(robot_pose) == 2 assert len(goal_pose) == 2 # Append column and row of 1's to enforce boundaries # Note this also enforces the left and top boundaries because an # index of -1 will also map to the appended row or column world_append = np.c_[world_state, np.ones(world_state.shape)] world_append = np.r_[world_append, np.ones((1,world_append.shape))] # Init path list path = [robot_pose] while len(path) < self.max_step_number: # Start at last place in path current_pose = path[-1] current_pose_x = current_pose current_pose_y = current_pose # Establish memory mem = path[-int(np.sqrt(self.max_step_number)):] # Init list of available poses no_obst_poses =  if (not world_append[current_pose_x-1, current_pose_y]): no_obst_poses.append((current_pose_x-1, current_pose_y)) if (not world_append[current_pose_x+1, current_pose_y]): no_obst_poses.append((current_pose_x+1, current_pose_y)) if (not world_append[current_pose_x, current_pose_y-1]): no_obst_poses.append((current_pose_x, current_pose_y-1)) if (not world_append[current_pose_x, current_pose_y+1]): no_obst_poses.append((current_pose_x, current_pose_y+1)) # If we are immediately surrounded by walls/obstacles, search fails if len(no_obst_poses) == 0: return None # If only one choice, take it elif len(no_obst_poses) == 1: path.append(no_obst_poses) else: # Lets check if available_poses are in our memory # Init empty list of available poses available_poses =  # Step through all poses in no_obst_poses for pose in no_obst_poses: # Check if pose is in our memory if not pose in mem: # If not in memory, append to available_poses available_poses.append(pose) # Lets choose one of the available_poses if len(available_poses) > 0: choice = np.random.choice(len(available_poses), 1) path.append(available_poses[choice]) else: choice = np.random.choice(len(no_obst_poses), 1) path.append(no_obst_poses[choice]) # Check if we reached our goal_pose if path[-1] == goal_pose: return path return None if __name__ == "__main__": # If we run this file, run the provided example and print resultant path # to the terminal world_state = np.array([[0, 0, 1, 0, 0, 0], [0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, 1, 1, 1, 0], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0]]) robot_pose = (2, 0) goal_pose = (5, 5) # Note: in provided example (6, 6) is not a valid pose # Set max_step_number max_step_number = 1000 # Instantiate RandomPlanner class planner = RandomPlanner(max_step_number) # Perform path search path = planner.search(world_state, robot_pose, goal_pose) # Print result to screen print "\nRandom planner resultant path\n ", path