COMP550-Project 3 Barking up a Random Tree Solved

The goal of this project is to implement a simple sampling-based motion planner in OMPL to plan for a rigid body and then systematically compare your planner to existing methods in the library.

Random Tree Planner
The algorithm that you will be implementing is the Random Tree Planner, or RTP. The RTP algorithm is a simple sampling-based method that grows a tree in the configuration space of the robot. RTP is loosely based on a random walk of the configuration space and operates as follows:

1.   Select a random configuration qa from the existing Random Tree.

2.   Sample a random configuration qb from the configuration space. With a small probability, select the goal configuration as qb instead of a random one.

3.   Check whether the straight-line path between qa and qb in the C-space is valid (i.e., collision free). If the path is valid, add the path from qa to qb to the tree.

4.   Repeat steps 1 – 3 until the goal state is added to the tree. Extract the final motion plan from the tree.

The tree is initialized with the starting configuration of the robot. You might notice that this procedure seems very similar to other sampling-based algorithms that have been presented in class and in the reading. Many sampling-based algorithms employ a similar core loop that utilizes the basic primitives of selection, sampling, and local planning (checking). RTP is one of the simplest possible approaches, with no additional intelligence in how configurations are sampled, selected, or checked. Improvements to this algorithm are out of scope for this project, simply implement RTP as described above.

Benchmarking Motion Planners
Since many of the state-of-the-art algorithms for motion planning are built upon the concept of random sampling (including RTP), one run is not sufficient to draw statistically meaningful conclusions about the performance of your planner or any others. To help with evaluation, OMPL provides benchmarking functionalities that execute one or more planners many times while recording performance metrics in a database.

The ompl::tools::Benchmark class operates in two phases. First is the planning phase, where all of the planners are executed on the same problem for the given number of runs. Second is the analysis phase,

where the log file emitted by the benchmark class is post-processed into a SQLite database, and statistics for many common metrics are plotted to a PDF using ompl benchmark statistics.py, or using the online analysis available through http://plannerarena.org/. The benchmark facilities are extensively documented at http://ompl.kavrakilab.org/benchmark.html#benchmark code.

Note: ompl benchmark statistics.py requires matplotlib v1.2+ for Python, which can be installed through your favorite package manager or through Python’s pip program. The virtual machine should already have this installed. The script will produce box plots for continuously-valued performance metrics. If you are unfamiliar with these plots, Wikipedia provides a good reference. The script will merge with any existing SQLite database with the same name, so take care to remove any previously existing database files before running the script. You can also upload your SQLite database to http://plannerarena.org/ to interactively view your benchmark data.

Project exercises
1.   Implement RTP for rigid body motion planning. At a minimum, your planner must derive from ompl::base::Planner and correctly implement the solve(), clear(), and getPlannerData() functions. Solve should emit an exact solution path when one is found. If time expires, solve should also emit an approximate path that ends at the closest state to the goal in the tree.

•    Your planner does not need to know the geometry of the robot or the environment, or the exact C-space it is planning in. These concepts are abstracted away in OMPL so that planners can be implemented generically.

2.   Compute valid motion plans for a point robot within the plane and a square robot with known side length that translates and rotates in the plane using OMPL. Note, instead of manually constructing the state space for the square robot, OMPL provides a default implementation of the configuration space R2×S1, called ompl::base::SE2StateSpace.

•    Develop at least two interesting environments for your robot to move in. Bounded environments with axis-aligned rectangular obstacles are sufficient.

•    Collision checking must be exact, and the robot should not leave the bounds of your environment. Inexact collision checking will lead to invalid motion plans. Verify that states and edges are both collision-free and that states do not leave the bounds of your environment.

•    Visualize the world and the path that the robot takes to ensure that your planner and collision checker are implemented correctly. You must include visualizations of your worlds as well as some example paths your planner generated in your report.

3.   Benchmark your implementation of RTP in the 3D Cubicles and Twistycool scenarios from OMPL.app. Make sure to compare your planner against the PRM, EST, and RRT planners over at least 50 independent runs. If all of the planners fail to find a solution, you will need to increase the computation time allowed. See the appropriate OMPL.app .cfg files for the mesh names, start and goal configurations, etc. Use the results from your benchmarking to back up claims in your report about RTP’s performance quantitatively.

•    The ${OMPL DIR}/share/ompl/demos/SE3RigidBodyPlanning.cpp demo shows a code example of how to setup a problem using triangle meshes and benchmark planners. Feel free to use this as a reference for the benchmark exercise. The ompl::app::SE3RigidBodyPlanning class derives from ompl::geometric::SimpleSetup.

Protips
•    It may be helpful to start from an existing planner, like RRT, rather than implementing RTP from scratch.

Check under the directory omplapp/ompl/src/ompl/geometric/planners/rrt if you compiled from source (or the virtual machine). The files are also found at https://bitbucket.org/ompl/ompl/src, and on the online documentation available at http://ompl.kavrakilab.org/.

Make sure you understand what each line of the RRT algorithm is doing. RRT is a much smarter algorithm than RTP, and while a good starting point, requires you to remove what is not required. You

will be penalized if code that is not a part of the RTP algorithm remains in your implementation. Additionally, although RRT has a variant that saves intermediate states, you should not implement a variant of RTP that saves intermediate states.

•    The getPlannerData function is implemented for all planners in OMPL. This method returns a PlannerData object that contains the entire data structure (tree/graph) generated by the planner. getPlannerData is very useful for visualizing and debugging your tree.

•    If your clear() function implemented in RTP is incorrect, you might run into problems when benchmarking as your planner’s internal data structures are not being refreshed.

•    You do not need a nearest neighbor data structure for RTP. RTP has no need for neighbor proximity queries, as it selects states to extend from at random.

•    RTP is not an intelligent planner, and is a very simple algorithm compared to other sampling-based planners. Keep this in mind when observing its performance.

•    For benchmarking, make sure you give the planners a reasonable amount of time to solve the problem. If the time limit is too low, then it is highly unlikely your planner will solve any instances, and the resulting benchmark will not be informative. Make sure you understand when the planner returns an exact solution (it has solved the problem) and when it has returned an approximate solution (it has not solved the problem, and has returned a guess).

•    Solution paths can be easily visualized using the printAsMatrix function from the PathGeometric class. Use any software you want to visualize this path, but provide your script with your submission. See http://ompl.kavrakilab.org/pathVisualization.html for more details.

•    In exercise 3, make sure to link your program against libompl, libompl  app, and libompl app base against the problem instances. The provided Makefile given in the OMPL installation handout and the virtual machine does this.