Continue planner optimizations

[amalnanavati]

#204 focused on creating a planner benchmark tool and comparing different planners. The results can be found here (access for anyone with a UW CSE account). The outcome of this was that we will use RRT* for the time being, but should pick up this investigation later to understand why RRTConnect con-con, with shortcutting and hybridization, generates paths that are so much longer than RRT*.

The above doc also has some code pointers to accelerate the follow-up work on this.