Sampling-Based Planning

This page describes the Sampling-Based Planning project from the Coursera course "Modern Robotics, Course 4: Robot Motion Planning and Control."

The environment for motion planning for a point robot moving in the plane.

Introduction

In this programming assignment, you will implement a sampling-based planner, either an RRT or a PRM, to find a path for a point robot through the cluttered planar environment shown on the right.

Your project will be graded on:

• An evaluation of your description of your planner. It will be graded on clarity and completeness.
• An evaluation of your source code, including its clarity for the reader.
• The correctness of your code's output, according to the specification below.
• The correctness of your CoppeliaSim screenshot of your solution path on the graph.

Details

In this assignment you will use the same CoppeliaSim visualization scene as in the previous assignment, the csv motion planning kilobot scene (Scene 5). You have the choice of programming either an RRT or a PRM to find a path for a point robot moving in a plane among the obstacles shown in the image at the right.

You can download the files for this obstacle-cluttered environment in the zipped folder here. Only the obstacles.csv file is relevant, since it is the input to your planner. The other files in the folder, nodes.csv, edges.csv, and path.csv, are placeholders; your program should write these files as output. The information in the obstacles.csv file is also replicated below:

0.0, 0.0, 0.2
0.0, 0.1, 0.2
0.3, 0.2, 0.2
-0.3, -0.2, 0.2
-0.1, -0.4, 0.2
-0.2, 0.3, 0.2
0.3, -0.3, 0.2
0.1, 0.4, 0.2

Your program will choose its own random samples in the (x, y) C-space, which is the square [-0.5, 0.5] x [-0.5, 0.5]. You will also employ a straight-line planner as the local planner, regardless of whether you use the RRT or PRM. The distance between two configurations is simply the Euclidean distance ${\sqrt {(x_{1}-x_{2})^{2}+(y_{1}-y_{2})^{2}}}$.

The start configuration is at (x, y) = (-0.5, -0.5), the bottom left corner of the square environment, and the goal configuration is at (x, y) = (0.5, 0.5), the top right corner of the square environment.

You are responsible for collision checking. Given a straight line segment from (x1, y1) to (x2, y2), you must write your own collision checker to see if the line segment intersects a circle (all the obstacles are circles). This is a fairly simple mathematical exercise. If your solution involves finding the intersection of a line with a circle (there are 0, 1, or 2 intersections), make sure to test if the intersection(s) are within the length of the line segment! A less desirable, but still acceptable, solution could test sample points finely on the line segment between (x1, y1) and (x2, y2). You should treat the robot as a point; in other words, if the radius of the robot is actually $r$ and the radius of an obstacle is $R$, you can assume the radius of the robot has been shrunk to 0 and the radius of the obstacle has been grown to $R+r$.

Your program should take as input the obstacles.csv file that you created by cutting and pasting the obstacle description above. The output of your program should be three files: nodes.csv, edges.csv, and path.csv. (This page has information on writing csv files in Python, MATLAB, and Mathematica.) All of these should be in the format described on the wiki page for the csv motion planning kilobot scene. When you put your input file and your three output files in a directory (named "results," for example), then when you call the CoppeliaSim scene with this directory as input, you should see the obstacles, your graph or tree, and the solution path, as well as an animation of the motion.

You may optionally have another input file, perhaps called "parameters.csv" or "parameters.txt," that contain other input to your motion planner. Examples of such input could be the number of nodes to create in a PRM graph or the likelihood (as a percentage) that an RRT "random" sample is chosen as the goal node (to attempt to complete the motion planning problem).

Your program will not be judged on efficiency! Ideally you would employ efficient data structures and algorithms to find the nearest nodes to a given configuration, or to check whether a given straight-line path collides with an obstacle. But the point of this assignment is to demonstrate your understanding of the motion planning technique, not to demonstrate mastery of efficient programming, nor to write software that runs quickly on large problems. For example, to find the nearest node to a particular configuration, you could simply calculate the distance to each of the nodes.

However, your program will be judged on clarity! Remember that your code may be reviewed by someone unfamiliar with your programming language. Also, you must write your code modularly with multiple functions. For example, a function for testing whether a path from (x1, y1) to (x2, y2) is in collision with any obstacle could call another function which checks if it is in collision with a specific circular obstacle. This single-obstacle collision-detection function could then call a function that returns the intersection points of a line with a circle, then another function that checks if these intersections (if they exist) belong to the line segment or are on the line containing the line segment but are outside the segment.

If you choose to program an RRT

Sampling: You are free to choose your sampling method, though sampling from a uniform random distribution over the square [-0.5, 0.5] x [-0.5, 0.5] is a good choice. Every so often, perhaps 10% of the time (this is up to you, though), your sample should be at the goal configuration, to see if you can connect your tree to the goal.

Finding the nearest node to a sampled configuration: As mentioned above, you can simply test the Euclidean distance to all nodes in the tree.

Local planner: By default the local planner should be a straight-line planner, from the closest node to the sampled configuration. If you decide to do something else, clearly document it.

If you choose to program a PRM

If you decide to use a PRM planner, where you first build a graph representing the C-space before searching it, you should use the A* search you wrote for the previous assignment, the weight between nodes should be the Euclidean distance between the nodes, and the optimistic cost-to-go from a node should be the Euclidean distance to the goal configuration.

Phase 1, sampling: You are free to choose your sampling method, though sampling from a uniform random distribution over the square [-0.5, 0.5] x [-0.5, 0.5] is a good choice. Since you will always use the same start and goal configurations, as described above, you should also choose samples at these configurations, so they are built into the tree. It is up to you to choose the number of nodes in your graph, but you should choose enough that the start and goal are likely to be in the same connected component of the graph.

Phase 2, creating edges: It is up to you to choose the number of neighbors $k$ to try to connect to each node.

Phase 3, searching the graph: You should use A* search.

What to submit

Once your program is working, and you are able to consistently find solutions to the path planning problem, you will submit a single .zip file with the following contents:

1. A README file, preferably as a plain text .txt file or as a .pdf file. This file should describe your solution to the motion planning problem: your choice of an RRT or PRM, decisions you made in your implementation, information on any supplementary files the planner needs as input (other than obstacles.csv), and any other comments that help the reader understand your solution. This file should typically be one page or less.
2. Your commented code in a directory called "code." Your code should be lightly commented, so it is clear to the reader what the code is doing. No need to go overboard, but keep in mind your reviewer may not be fluent in your programming language. Your code comments must include an example of how to use the code. You can write your program in any language, provided it is clearly structured, reasonably modular, easy to understand for a reader with no experience in your programming language, and with sufficient comments. Your program can span multiple files, or it can be a single file, if appropriate. If your code is in Mathematica, turn in (a) your .nb notebook file and (b) a .pdf printout of your code, so a reviewer can read your code without having to have the Mathematica software.
3. A directory called "results" with (at least) four files in it: nodes.csv, edges.csv, obstacles.csv, and path.csv. Your results directory should include the obstacles.csv input file as well as output files nodes.csv, edges.csv, and path.csv produced by your planner. These output files must correspond to the screenshot you submit. Your results directory can optionally have another file or files with other input needed by your planner, provided these files are clearly described in the README.
4. A CoppeliaSim screenshot of your solution path on the graph/tree. The graph among the obstacles should be clearly visible.
5. (OPTIONAL) A second results folder (e.g., "results-extra") with another interesting motion planning problem, its solution, and a CoppeliaSim screenshot.