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. Selecta random configuration qa from the existing Random Tree. 2. Samplea random configuration qb from the configuration space. With a small probability, select the goal configuration as qb instead of a random one. 3. Checkwhether the straight-line path between qa and qb in theC-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-basedalgorithmsemployasimilarcoreloopthatutilizesthebasicprimitivesofselection,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 https://plannerarena.org/. The benchmark facilities are extensively documented at https://ompl.kavrakilab.org/benchmark.html#benchmark code.
Note: ompl benchmark statistics.py requires matplotlib v1.2+ for Python, which can be installed throughyourfavoritepackagemanagerorthroughPython’s pip program. Thevirtualmachineshouldalready 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 https://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 plannerdoesnotneed 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. 3. Benchmark your implementation of RTP in the 3D Cubicles and Twistycool scenarios from OMPL.app. Makesuretocompareyourplanneragainstthe PRM, EST,and RRT plannersoveratleast50independent 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 • Itmaybehelpfultostartfromanexistingplanner,like RRT,ratherthanimplementing RTP fromscratch. 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 https://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 https://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. • Inexercise3,makesuretolinkyourprogramagainstlibompl,libompl app,andlibompl 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. SubmissionsaredueThursdayOctober5that1pm.
To submit your project, clean your build space with make clean, zip up the project directory into a file named Project2