In this assignment you will use the same V-REP visualization scene as in the previous assignment, the csv motion planning kilobot scene. 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 .
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).
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 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:
- 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.
- 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.
- 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.
- A V-REP screenshot of your solution path on the graph/tree. The graph among the obstacles should be clearly visible.