Sampling-Based Planning

In this assignment you will use the same V-REP visualization scene as in the previous assignment. 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.csv file below (you can cut and paste this into your obstacles.csv file):

0.0, 0.0, 0.2
0.0, 0.1, 0.2
0.4, 0.3, 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 (nodes in the graph or tree) is simply the Euclidean distance $$\sqrt{(x_1-x_2)^2 + (y_1-y_2)^2}$$ (for finding the nearest nodes in the RRT or 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, your weight between nodes should be the Euclidean distance between the nodes, and your optimistic cost-to-go from a node should be the Euclidean distance to the goal configuration.

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 elegant, but still acceptable, solution could test sample points finely on the line segment between (x1, y1) and (x2, y2).

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. 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 V-REP 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 percentage of time an RRT planner should attempt to connect to the goal node or the number of nodes to create in the PRM graph.

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.

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 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

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, 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. If you did not already include the start and goal nodes in the graph, you should add them to the graph first, by trying to connect them to their $$k$$ nearest neighbors.

What to submit

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

A README file, preferably as plain text .txt 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 will help the reader understand your solution.

code directory

results directory Your results directory should include the obstacles.csv input file as well as the 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.

screen shot