Interleaving Discrete Search and Continuous Optimization for Kinodynamic Motion Planning

Motion planning for dynamically complex robotic tasks requires explicit reasoning within constraints on velocity, acceleration, force/torque, and kinematics such as avoiding obstacles. To meet these constraints, planning algorithms may have to simultaneously make high-level discrete decisions and low-level continuous decisions. For example, pushing a heavy object involves making discrete decisions about contact locations and continuous decisions about fine interactions. Most existing methods tackle this challenge by employing different planners hierarchically. A common strategy is to search on a graph constructed by sampling or systematically discretizing the planning space to make discrete high-level decisions, such as a sequence of waypoints or contact locations. Once the high-level plan is set, trajectory optimization constrained by these high-level decisions is used for continuous low-level refinement. Our hypothesis is that the lack of bidirectional communication between these hierarchies inevitably leads to suboptimalities or infeasibilities during the refinement stage and produces inefficient plans and failures. Consequently, this thesis argues that by alternating searching in this discrete high-level decision space with selective reasoning in the continuous planning space, we can efficiently discover global, long-horizon, dynamically rich capabilities to complete a task.The goal of this thesis is to develop a single unified framework that seamlessly blends discrete high-level and continuous low-level decision making for the automatic discovery of well-reasoned long-horizon plans for complex dynamical systems. To this end, the first half of the thesis presents the INterleaved Search And Trajectory optimization (INSAT) algorithm. INSAT combines the benefit of graph search-based planning algorithms to find paths over non-convex state spaces and that of trajectory optimization to find dynamically feasible trajectories in high-dimensional spaces. We demonstrate the effectiveness of INSAT in two challenging domains 1) aggressive quadrotor flight in large environments (in simulation) and 2) contact-rich manipulation of heavy objects in confined spaces. We also show that, by interleaving graph search and trajectory optimization, INSAT solves planning problems that a naively initialized trajectory optimization or standalone graph search does not solve.The second half of the thesis explores three different ways of accelerating the INSAT planner. CPU Parallelization: By leveraging recent advances in parallelized graph search, we enable CPU acceleration for optimization-based edge computation in INSAT. Richer Planning Representation: We use sparse and optimization-friendly planning space representation called graphs of convex sets (GCS) to speed up planning with INSAT. We also demonstrate that using INSAT as a planner on graphs of convex sets (IxG) provides additional theoretical guarantees and leads to gains in runtime efficiency in certain domains. Preprocessing: By computing a motion library offline we can guarantee constant-time solutions for any planning query to IxG.These methods collectively enhance the performance and efficiency of the INSAT planner, making it more capable of addressing complex planning tasks.