# 2D discrete motion planner

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

• 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... – Graipher Jan 25 '18 at 17:07
• 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. – mmmfarrell Jan 25 '18 at 17:47
• Nice use of asserts! – Bailey Parker Jan 26 '18 at 0:10

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