Constrained Path Planning using Quadratic Programming

Sampling-based planning algorithms have been extensively exploited to solve a wide variety of problems. In recent years, many efforts have been dedicated to extend these tools to solve problems involving constraints, such as geometric loop-closure, which lead the valid Configuration Space (CS) to collapse to a lower-dimensional manifold. One proposed solution considers an approximation of the constrained Configuration Space that is obtained by relaxing constraints up to a desired tolerance. The resulting set has then non-zero measure, allowing to exploit classical planning algorithms to search for a path connecting two given states. When constraints involve kinematic loops in the system, relaxation generally bears to undesired contact forces, which need to be compensated during execution by a proper control action. We propose a new tool that exploits relaxation to plan in presence of constraints. Local motions inside the approximated manifold are found as the result of an iterative scheme that uses Quadratic Optimization to proceed towards a new sample without falling outside the relaxed region. By properly guiding the exploration, paths are found with smaller relaxation factors and the need of a dedicated controller to compensate errors is reduced. We complete the analysis by showing the feasibility of the approach with experiments on a real platform.