Lab 5: Motion Planning (Wavefront Algorithm)

Lead TAs: Donni Cober , Alex Styler,
                   Ally Naaktgeboren

Lab 5 Demo Times

Lab 5 PowerPoint

Introduction:

The purpose of this lab is to show you how to use the wavefront algorithm to allow your robot to move between two given points while avoiding obstacles.

Challenge Statement:

You will be given representations of two possible worlds that have both obstacles and open space. The total size of the world will be 4' x 8' and will be divided up into 6" x 6" squares.

You will need to write a program that will allow your robot to take in 2 sets of coordinates, a start and a goal and using the wavefront algorithm travel from the start to the goal on the shortest path while avoiding all obstacles.

You can chose the starting orientation for the robot, but once you place the robot down, you cannot touch it. Also, you place the robot down and then plan the path.

The Wavefront Algorithm

The wavefront algorithm involves a breadth first search of the graph beginning at the destination point until it reaches the start point. First, obstacles are marked with a 1 and your goal point is marked with a 2. You can optionally surround the entire world with 1s as well to tell your robot to avoid those squares, and/or "expand" the size of the obstacle to avoid collisions due to dead-reckoning errors:
  

The following is one way of implenting the wavefront algorithm:

7
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 1 1 1 1 1 1 1 1 0 0 0 0
0 0 0 0 1 1 1 1 1 1 1 1 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2
6
5
4
3
2
1
0
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15


To create the "wave" of values, begin at the destination, and assign a distance of 3 to every square adjacent to the goal. Then assign a distance of 4 to every square adjacent to the squares of distance 3. Continue to do this until you reach the start point. Once this is complete, you can simply follow the numbers in reverse order until to reach the goal, avoiding any square with the value 1. You are free to chose if you would like your robot to be able to move diagonally or just in 4 directions.
  

7
18 17 16 15 14 13 12 11 10 9 9 9 9 9 9 9
17 17 16 15 14 13 12 11 10 9 8 8 8 8 8 8
17 16 16 15 14 13 12 11 10 9 8 7 7 7 7 7
17 16 15 15 1 1 1 1 1 1 1 1 6 6 6 6
17 16 15 14 1 1 1 1 1 1 1 1 5 5 5 5
17 16 15 14 13 12 11 10 9 8 7 6 5 4 4 4
17 16 15 14 13 12 11 10 9 8 7 6 5 4 3 3
17 16 15 14 13 12 11 10 9 8 7 6 5 4 3 2
6
5
4
3
2
1
0
0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15

In the above graph, the ones represent an obstacle. The goal and start points are labelled - 2 and 18 - and all other squarse are labelled according to their distance from the goal. A possible path (assuming diagonal travel is not used) would be:

(0,7) -> (1,7) -> (2,7) -> (3,7) -> (4,7) -> (5,7) -> (6,7) -> (7,7) -> (8,7) -> (9,7) -> (10,7) -> (10,6) -> (11,6) -> (11,5) -> (12,5) -> (12,4) -> (12,3) -> (13,3) -> (13,2) -> (14,2) -> (14,1) -> (15,1) -> (15,0)

which is the goal. This is just one of the many paths that can be used to reach the goal, and any path that follows the descending numbers correctly will be acceptable.

Evaluation:

At the time of the demo you will assigned to one of the two worlds and will be given 2 sets of coordinates, a starting position and a goal representing the center of a given square. You must enter the world and your start/goal cordinates on your robot. You may then place your robot at the center of the start square with any orientation you choose with the center of your robot in the center of the square. Your robot must then reach the goal without running off the end of the world or hitting the obstacles. You will be graded on the efficiency of the path you choose to the goal, your success in avoiding obstacles, and your proximity to the goal. Your robot's position will be defined as its center. After you have placed the robot you may hit a start switch but after that you may not touch the robot or give it any input until it has either failed, or reached its goal.

The grades will be based on the performance of the robot as follows:

  • 70%: software runs
  • 30%: hardware runs:
  • 30/30: center of the robot is in the goal square
  • 20/30: center of the robot is one square away.
  • 10/30: center of the robot is 2 squares away. 
  • You will have a maximum of 3 tries to demonstrate your robot's abilities. A failed try occurs when
  • (a) The robot hits an obstacle
  • (b) The robot goes off the world
  • (c) The robot is no longer heading towards the goal.

Click here for the UPDATED work spaces.

Tips

  • If you have too many variables Interactive C may give you a stack overflow error. Try declaring your variables as global instead (your world arrays will need to be global).
  • Similarly, if you try using recursion, you could easily run into stack overflow errors. Try implementing your wavefront algorithm iteratively.
  • Make sure to allow for the size of your robot when declaring obstacles and also when dealing with the edge of the environment so your robot doesn't go off the end.
  • FAQ from Lab
  • Grading sheet