BITKOMO: Combining Sampling and Optimization for Fast Convergence in Optimal Motion Planning
Jay Kamat, Joaquim Ortiz-Haro, Marc Toussaint, Florian T. Pokorny, Andreas Orthey
Abstract
Optimal sampling based motion planning and trajectory optimization are two competing frameworks to generate optimal motion plans. Both frameworks have complementary properties: Sampling based planners are typically slow to converge, but provide optimality guarantees. Trajectory optimizers, however, are typically fast to converge, but do not provide global optimality guarantees in nonconvex problems, e.g. scenarios with obstacles. To achieve the best of both worlds, we introduce a new planner, BITKOMO, which integrates the asymptotically optimal Batch Informed Trees (BIT*) planner with the K-Order Markov Optimization (KOMO) trajectory optimization framework. Our planner is anytime and maintains the same asymptotic optimality guarantees provided by BIT*, while also exploiting the fast convergence of the KOMO trajectory optimizer. We experimentally evaluate our planner on manipulation scenarios that involve high dimensional configuration spaces, with up to two 7-DoF manipulators, obstacles and narrow passages. BITKOMO performs better than KOMO by succeeding even when KOMO fails, and it outperforms BIT* in terms of convergence to the optimal solution.
Aggregate plot over all our experiments, showing how BITKOMO converges faster than BIT* while keeping a similar success rate.
Contributions
BITKOMO: A planner integrating BIT* and KOMO.
We integrate sampling and optimization to obtain fast convergence to the global optimum while maintaining the guarantees provided by the sampler.
Relaxed edge collision checking
A method for BIT* that allows edges partially in collision to be included in the motion tree with a collision penalty .
The edge is first subdivided and then checked for collision level by level. The number on the nodes represents the level of the node.