Description
Planning a path from one point of the environment to the other while avoiding collisions with the obstacles is a complicated problem. As the number of degrees of freedom increases the problem soon becomes very hard to solve. As computer power increases, probabilistic methods are being used more and more often to tackle these problems.
This project deals with a method for path planning known as Probabilistic Road Maps. The basic idea with probabilistic road maps is to randomly select a set of nodes/points in the environment that are free from collision and try to connect them with straight lines that are also free from collisions. A solution to the problem might then be found by finding a path through the graph from the start point to the end point.
One of the key components in such an algorithm is the ability to check for collisions with the environment. This project assignment is about implementing a world model that allows the path planning algorithm to query if a certain point in space is free from obstacles.
For more in detail information about planning have a look at Steve LaValle's book on Planning Algorithms that is available online.
Specification
You are given are given a number of files packed into a zip-file. On a linux/unix system you unpack this file with the command unzip prm-files.zip which will give you a directoy called prm-files with the following files
$ ls prm-files
AStarNode.cc PRM.cc SingleCircleWorld.cc
AStarNode.hh PRM.hh SingleCircleWorld.hh
plan_problem1.png PRMNode.cc solvePlanningProblem.cc
plan_problem2.png PRMNode.hh World.cc
plan_problem3.png problem1.txt World.hh
plan_singlecircle.png problem2.txt
Position.cc problem3.txt
Position.hh README
The .cc and .hh files are the source files that you are given as a start for this project. Study them so that you understand what they do. Briefly, the solvePlanningProblem.cc file is the application, PRM.hh[cc] implements a simple version of the probabilistic road map method and World.hh[cc] is an abstract base class which you are supposed to inherit from and implement something that can read a problem specification file on the format below.
Problem specification file
The world is defined in a specification file that give the start and goal for the path as well as the obstacles in their. An example of such a file is
START_AND_GOAL 0.5 0.5 9 9
CIRCLE 0 0 0.5
CIRCLE 1 1 0.3
CIRCLE 1 2 0.3
CIRCLE 2 2 0.3
CIRCLE 4 4 1
CIRCLE 4 7 1.5
CIRCLE 7 5.5 1.5
CIRCLE 10 10 0.5
RECTANGLE 1 3 1 1 0
Each obstacles is on a new line in the file. The format for the circle
and rectangle is
CIRCLE centerX centerY radius
RECTANGLE centerX centerY width height angle
where units are meters and radians and
The first thing you should do is to run the code that has been provided to you. It contains a very simple World implementation that contains a single circle with a fixed position.
Compile the code
./solvePlanningProblem
The progran will output some information about the setting, like the start and goal position, number of nodes, etc. To see what options are available issue
./solvePlanningProblem -h
You can look at the result of the planning with the automatically generated matlab program dispPRM.m. Just open matlab and run the program and you will see the nodes and path if it was found. An example this is seen in the image below
As stated above you are to implement a subclass to World that is able to read a problem specification file, answered collision queries and output information in matlab format. When you have implemented your code you have to modify the solvePlanningProblem.cc file slightly to allow it to use you code. This is done by expanding the if-statement where the World object is created.
if (worldModel == "SingleCircleWorld") {
world = new SingleCircleWorld;
// By adding to this if-statement you can easily make the program
// create an instance of your own class
} else {
std::cerr << "worldModel \"" << worldModel << "\" is unknown,"
<< "specify a correct model with -w option or leave out"
<< std::endl;
return -1;
}
You should then be able to select your new world model with the -w option on the command line.
Requirement
To fulfil the requirements for this project you have to
- Understand how the provided code works (This would normally not be the case as the beauty of object oriented programming is that all you really have to understand in this case is the interface that the class World provides. However, since this is a course you should practice understanding code written by someone as well)
- Implement a base-class for an obstacle and provide it with an appropriate interface
- Implement subclasses to your base-class from above that match the obstacle types in this project
- Implement a subclass to World that aggregates obstacles and makes use of object oriented design so that it does not need to differentiate different obstacles when it prints them to file and checks for collisions.
- The design can be summarised in the following figure
Your program should be able to
- read a problem specification file (test with problem1.txt - problem3.txt using -p option). To do this your world file needs be able to read problem files
- answer collision queries so that a correct solution is found for problem1.txt - problem3.txt
- output matlab display code so that the obstacle map can be displayed in matlab
NOTE: No changes are supposed to be made to the provided code except for solvePlanningProblem.cc to add you world.
Together with your code you should submit a document that clearly states how to execute the code and gives a brief overview of how the project was solved.
Example
The following three images show and example of how the generated path can look for problem1-3 respectively
Good Luck!