Description
A broad class of motion planning algorithms consider only geometric constraints (e.g., bug algorithms, visibility graph methods, PRM). These algorithms compute paths that check only whether the robot is in collision with itself or its environment. In the motion planning literature, this problem is also known as rigid body motion planning or the piano mover’s problem. Physical constraints on the robot (e.g., velocity and acceleration limits, steering speed) are ignored in this formulation. While considering only geometric constraints may seem simplistic, many manipulators can safely ignore dynamical effects during planning due to their relatively low momentum or high torque motors. Formally, this is known as a quasistatic assumption. 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. 1 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 R 2 ×S 1 , 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. 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. 2 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/. • 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. • 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. • 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. • 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. Deliverables This project may be completed in pairs. Submissions are due Thursday October 5th at 1pm. To submit your project, clean your build space with make clean, zip up the project directory into a file named Project2 <partner’s NetID>.zip, and submit to Canvas. Your code must compile within a modern Linux environment. If your code compiled on the virtual machine, then it will be fine. If you deem it necessary, also include a README with details on compiling and executing your code. In addition to the archive, submit a short report that summarizes your experience from this project. The report should be no longer than 5 pages in PDF format, and contain the following information: • (5 points) A succinct statement of the problem that you solved. • (5 points) A short description of the robots (their geometry) and configuration spaces you tested in exercise 2. • (5 points) Images of your environments and a description of the start-goal queries you tested in exercise 2. • (5 points) Summarize your experience in implementing the planner and testing in exercise 2. How would you characterize the performance of your planner in these instances? What do the solution paths look like? • (15 points) Compare and contrast the solutions of your RTP with the PRM, EST, and RRT planners from exercise 3. Elaborate on the performance of your RTP. Conclusions must be presented quantitatively from the benchmark data. Consider the following metrics: computation time, path length, and the number of states sampled (graph states). • (5 points) Rate the difficulty of each exercise on a scale of 1–10 (1 being trivial, 10 being impossible). Give an estimate of how many hours you spent on each exercise, and detail what was the hardest part of the assignment. 3 Take time to complete your write-up. It is important to proofread and iterate over your thoughts. Reports will be evaluated for not only the raw content, but also the quality and clarity of the presentation. When referencing benchmarking results you must include the referenced data as figures within your report. Additionally, you will be graded upon your implementation. Your code must compile, run, and solve the problem correctly. Correctness of the implementation is paramount, but succinct, well-organized, well-written, and well-documented code is also taken into consideration. Visualization is an important component of providing evidence that your code is functioning properly. The breakdown of the grading of the implementation is as follows: 1. (30 points) A correct implementation of RTP that generates valid motion plans. 2. (30 points) Benchmarking of RTP against other planners in all prescribed environments. Those completing the project in pairs need only provide one submission. 4
