Aisha's Lab 7 notes

Aisha's Notes on Lab 7 (created Mar 16, 2005)
API of current code
**These notes are incomplete, but I am stopping here so that I can prepare the release documents for the lab.

Lab 7 Navigation
Assumptions:
    1.) Obstacles are 2D convex polygons.
    2.) Using integers to represent coordinates, where 1 = 10^-2 meters, ie. 1 cm (sorry Nick)
    3.) Left-Handed coordinate system
    4.) Input Map is defined in all positive coordinates
Hints: Create main program in each class that will test/verify the methods in that class. Think about your data structures. It's useful to take a look at java Collections. Be careful of the java drawPolygon method because you want a convex polygon, so you can run your Convex Hull to guarantee that java draws the right Polygon. Be careful of namespaces between Carmen and java, qualify your clasess with java.awt.Point or Carmen.Point for example.

Lab 7 Steps (path planner)
0. My Ideas of what should be in the release for the studens
1. Read a Map From a File and Store the Values
2. Convert Map to Configuration Space
3. Create a Grid
4. Grid-based Path Planner (Label the Grid Cells According to Their Distance From the Goal )
5. View All Steps in the Lab 7 GUI (created by our spftware LA David Blau)
6. Deploying Path Planner on Robot (using Carmen)

0 My Release Notes
The current imple

1 Map
The current implementation for the map of the world, the environment in which the robot navigates, is in PPMap.java <add API>

1.1 Read a map from an input file (all input is specified in integers)
The input file format is specified below. There can be any number of convex 2D polygons, with one polygon a line:

( startPoint.X, startPoint.Y )	( goalPoint.X, goalPoint.Y )	robotLength x robotWidth
( Obstacle1Vertex1.X, Obstacle1Vertex1.Y )	( Obstacle1Vertex2.X, Obstacle1Vertex2.Y )	(Obstacle1Vertex3.X, Obstacle1Vertex3.Y ) ......
( Obstacle2Vertex1.X, Obstacle1Vertex1.Y )	( Obstacle2Vertex2.X, Obstacle2Vertex2.Y )	(Obstacle2Vertex3.X, Obstacle3Vertex3.Y )......

(note:there can be more than three points on each line that representa a polygon)

The first line contains the start and goal points of the robot, and the length and width of the robot. The start and goal points should refer to the center of the robot. The starting point of the robot is used as the reference point to compute the configuration space. Below is an example input file.
(0,100) (500, 500) 25 x 30
(0,10) (0,20) (20,40) (50,40) (60,20) (60,10)

Currently the format is being interpreted by a regular expression defined in the ParseFile.java class . Below is an example of how to use this class. I assume that the students can use this to parse their input, or write their own parser, using the StringTokenizer or something of that sort. The use of the parser is shown in bold.

import java.awt.*;
import java.io.*;
import java.util.*
.....
public class Map{
    private LinkedList obstaclePolygons;
    private Rectangle robotDimensions;
    private Rectangle robotBoundingSquare;
    private Point startPoint, goalPoint;
    ...
   public Map(String mapFile) {
        ParseFile parser = new ParseFile(mapFile);
        obstaclePolygons = new LinkedList();
        for(int i=0; i < parser.getNumberOfObstacles(); i++){
            obstaclePolygons.add(parser.getObstacle(i));
        }
        robotDimensions = new Rectangle(parser.getRobotDimension());
        Point startPoint = new Point(parser.getRobotStart());
        goalPoint = new Point(parser.getRobotGoal());
        ...
    }
    //Accessor methods and other methods here
    ...
    public static void main(String[] args){
        Map m = new Map("mapinput.txt");
        m.printMap();
    }
}

1.1 Storing the Obstacles and the Robot (ie. using java data structures)
The obstacles are stored as Polygons using java's java.awt.Polygon class. The robot is stored as a Rectangle using the java.awt.Rectangle class. Another option is to store the robot is to use the java.awt.Dimension class. The robot start and goal points are stored as Points from java.awt.Point.

1.2 Creating a Bounding Box Around the Robot
To create a bounding box (square) that encompasses the robot including its rotation a very simple method is used. Take the length of the diagonal of the robot. Then use the 1/2 the diagonal length to determine a circle of that radius and bound it with a box.

2 Configuration Space
The current implementation for the configuration space implementation is in ConfigurationSpace.java

2.1 Helper Methods
Depending on how the students represent their map, they will probably need a few helper methods that they can write or we can provide for them. Below are some of the helper method prototypes that are used in the current Lab 7 solutions.
    public static Polygon[] makePolygonArray(Collection c)
    public static Point[] makePointArray(Collection c)
    public static void printPointArray(Point[] srcArray)
    public static Polygon pointArrayToPolygon(Point []points)
    public static Point[] removeDuplicatePoints(Point []points)//this is required in order to use GeomUtils.java

2.2 Computing the Minkowski Sum
Computing the Minkowski sum in simplified when using the bounding square of the robot. That is the enclosing square in which the robot can rotate in place. The students can compute the length of the diagonal of the robot's bonding square, and use polar coordinates to map out points from each vertex of an obstacle polygon. This will create four points for every vertex of an obstacle polygon including duplicate points which need to be thrown out prior to convex hull.

2.3 Computing the Convex Hull (reference Computational Geometry M. de Berg, M. van Krevald, M. Overmars, O. Schwarzkopf)
The current implementation support the algorithm presented in the reference given above. The algorithm is concise and straight-forward and I think the students can understand it by tracing through the code using a set of points that they make-up. To sort an Array of points, I have defined a PointComparator class that compares two points. Here is an example of how it's used.

    public static Point[] convexHull( Point points[] ){
        ...
        //1. Sort points by x-coords, if tie use highest y
        Arrays.sort( points, new PointComparator() );

There is one potentially difficult method required to implement the algorithm and that is to detecting with there has been a right turn between three points. Fortunately there is some code that can take care of that in GeomUtils.java . Below is an example of how to use it.
Point p1 = new Point(5,5);
Point p2 = new Point(10,0);
Point p3 = new Point(10,10);
if(GeomUtils.typeOfTurn(p1, p2, p3) == GeomUtils.TO_THE_RIGHT ){ ... }

3 Creating a Grid
In the current implementation, I create a Grid class and a GridCell class. The Grid class contains a 2-D array of GridCell objects. The resolution of a grid cell is defined in the Grid class, while the dimensions of the world are defined in World.java.

3.1 Definition of the GridCell class
The purpose of this class is used to store information about each cell in the grid. A GridCell is determined to be in free space or in a obstacle region. It also knows its distance from the goal (ie. manhatten distance from the goal. A GridCell also knows which row and column it referes to in the Gred.

3.2 Specifying Free Space
In order to specify the free space and the obstacle space, method to declare which areas of the grid are occupied by the CSpace obstacles are needed. One of the main reason I used Java's Polygon class to represent my obstacle was that the class has a intersects methods the determines wether or not a rectangle intersects the Polygon. The method is conservative in that if a line segment of the polygon intersect a line segment or just one point of the rectangle, then there is an intersection. This results in a conservative representaition of free space. Below is an example on how to use the method.
     Rectangle tempRectangle = new Rectangle();
     ....
     tempRectangle.setBounds(x,y,this.gridCellSize,this.gridCellSize);
     if(polygon.intersects(tempRectangle)) {
        gridCell.setFree(false);
     }

In addition a bunch of helper methods are implemented. For example a method that maps a world (x,y) coordinate to a grid cell.

4 Grid-based Path Planner
The Grid-based Path Planner has the job of assigning the values to each cell based on the cell's distance from the goal. It also is able to determine if there exisits a path from start to goal and marks the grid cells that are on the path, if a path exists.

4.1 Assigning Grid Cell Values
This is the crux of the grid based path planner that Prof. Nick Roy presented in class. Here GridCells are marked with their distance from the goal (ie. manhatten). This is done by doing a breadth first search (see Algorithms book CLRS) and labelling each neighbor of a GridCell with its distance from the goal, starting by labeling the goal with 0. A method called getNeighbor cells is implemented that returns a LinkedList of GridCells that are the neighbors of the specified cell. This method only returns grid cells that in free space. I use a queue data structure that I found on the internet. An example is given below. There is an exmple showing how to use the queue class in Test.java.
       ....
        this.goalCell.setDistanceFromGoal(0);
        Queue myQueue = new Queue();
        myQueue.enqueue(goalCell);
       ....
4.2 Traversing the grid to find a path from start to goal

5 Integration/Visualization with the GUI
David Blau has written a great GUI to visualize the steps involved in developing the lab 7 path planner, see Lab7GUI.java. Be careful of the java drawPolygon method because you want a convex polygon, so you can run your Convex Hull to guarantee that java draws the right Polygon.