Improving Relaxation-based Constrained Path Planning via Quadratic Programming

Many robotics tasks involve a set of constraints that limit the valid configurations the system can assume. Some of these constraints, such as loop-closure or orientation constraints to name some, can be described by a set of implicit functions which cause the valid Configuration Space of the robot to collapse to a lower-dimensional manifold. Sampling-based planners, which have been extensively studied in the last two decades, need some adaptations to work in this context. A proposed approach, known as relaxation, introduces constraint violation tolerances, thus approximating the manifold with a non-zero measure set. The problem can then be solved using classical approaches from the randomized planning literature. The relaxation needs however to be sufficiently high to allow planners to work in a reasonable amount of time, and violations are counterbalanced by controllers during actual motion. We present in this paper a new component for relaxation-based path planning under differentiable constraints. It exploits Quadratic Optimization to simultaneously move towards new samples and keep close to the constraint manifold. By properly guiding the exploration, both running time and constraint violation are substantially reduced.