We present an evaluation of several representative sampling-based and optimization-based motion planners, and then introduce an integrated motion planning system which incorporates recent advances in trajectory optimization into a sparse roadmap framework. Through experiments in 4 common application scenarios with 5000 test cases each, we show that optimization-based or sampling-based planners alone are not effective for realistic problems where fast planning times are required. To the best of our knowledge, this is the first work that presents such a systematic and comprehensive evaluation of state-of-the-art motion planners, which are based on a significant amount of experiments. We then combine different stand-alone planners with trajectory optimization. The results show that the combination of our sparse roadmap and trajectory optimization provides superior performance over other standard sampling-based planners' combinations. By using a multi-query roadmap instead of generating completely new trajectories for each planning problem, our approach allows for extensions such as persistent control policy information associated with a trajectory across planning problems. Also, the sub-optimality resulting from the sparsity of roadmap, as well as the unexpected disturbances from the environment, can both be overcome by the real-time trajectory optimization process.
We present a parallel Cartesian planning algorithm for redundant robot arms and manipulators. We pre-compute a roadmap, that takes into account static obstacles in the environment as well as singular configurations. At runtime, multiple paths in this roadmap are computed as initial trajectories for an optimization-based planner that tends to satisfy various constraints corresponding to demands on the trajectory, including end-effector constraints, collision-free, and non-singular. We highlight and compare the performance of our parallel planner using 7-DOF arms with other planning algorithms. To the best of our knowledge, this is the first approach that can compute smooth and collision-free trajectories in complex environments with dynamic obstacles.