4
\$\begingroup\$

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 search function.

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[0])]
        world_append = np.r_[world_append, np.ones((1,world_append.shape[1]))]

        # 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[0]
            current_pose_y = current_pose[1]

            # 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[0])
            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)[0]
                    path.append(available_poses[choice])
                else:

                    choice = np.random.choice(len(no_obst_poses), 1)[0]
                    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
\$\endgroup\$
  • \$\begingroup\$ Could you set max_step_number = world_state.shape[0] * world_state.shape[1] - world_state.sum(), since you know that the longest path is visiting each cell once or do you explicitly want to allow self-intersecting paths? They will never be the shortest path anyways, because you can exclude the loop that does not reach the goal... \$\endgroup\$ – Graipher Jan 25 '18 at 17:07
  • \$\begingroup\$ I like this idea, but I think they want me to allow self-intersecting paths. Using the value you suggest, the search algorithm fails more frequently than it succeeds because the robot often finds itself in corners that force it to intersect its previous path. \$\endgroup\$ – mmmfarrell Jan 25 '18 at 17:47
  • \$\begingroup\$ Nice use of asserts! \$\endgroup\$ – Bailey Parker Jan 26 '18 at 0:10
3
\$\begingroup\$
  1. Good documentation, I like it.

  2. I'm not keen on the use of the word "pose". Normally the word "pose" refers to the angles of the robot's joints. But the code here uses "pose" to mean the robot's position in the world. I would prefer to abbreviate "position" to "pos" since there is no "e" in "position".

  3. The max_step_number argument should be documented in the docstring for the __init__ method, not in the class docstring.

  4. I think it's a bad idea to copy out the description of the max_step_number argument. When you have duplicate documentation like this, there is a risk that when you change something you will edit one copy and forget to edit the other. So why not write something like this:

    The max_step_number attribute has the same meaning as the
    argument to the constructor, q.v.
    

    (You don't actually have to write "q.v." but it makes you sound more sophisticated.)

  5. It would be a good idea to cache values like int(np.sqrt(self.max_step_number)) in local variables, to save recomputing them on every step:

    memory_size = int(np.sqrt(self.max_step_number))
    
  6. There's a lot of repetition in the construction of the list of available positions to move to. I would suggest making a list of directions in which the robot can move:

    DIRECTIONS = [(0, 1), (1, 0), (0, -1), (-1, 0)]
    

    and looping over those directions, testing each one as you go:

    free_positions = []
    for dx, dy in DIRECTIONS:
        new_position = x + dx, y + dy
        if not world_append[new_position]:
            free_positions.append(new_position)
    
  7. The complexity analysis needs work. First, the construction of world_append takes time proportional to the size of the world, and this is probably bigger than \$n\$. (Putting boundaries on the world is a good idea in order to avoid bounds-checking in the loop, but adding the boundaries should be done just once, not once per search.) Second, the line

    mem = path[-int(np.sqrt(self.max_step_number)):]
    

    copies out the memory, taking \$Θ(\sqrt n)\$, and this is done again for each step in the path, taking \$Θ(n^{3\over2})\$ overall. Third, the test

    if not pose in mem:
    

    may have to search through all elements in the memory in order to conclude that pose is not present, taking \$O(\sqrt n)\$, and again, this may be done every step, thus taking \$O(n^{3\over2})\$ overall.

    To get the runtime down to \$O(n)\$ you need to use a different data structure for the robot's memory. A simple approach would be to represent the memory as a mapping from position to the number of the most recent step of the search at which that position was visited. You'd start with an empty mapping (no positions visited yet):

    memory = {} # mapping from position to step number
    

    and then after taking a step to position you'd update the memory like this:

    path.append(position)
    memory[position] = len(path)
    

    and then to see if a position is in the memory you see if it was visited recently enough:

    if memory.get(position, float('-inf')) > len(path) - memory_size:
        # position is in memory
    
\$\endgroup\$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.