Constellation - An algorithm for finding robot configurations that satisfy multiple constraints

Planning motion for humanoid robots requires obeying simultaneous constraints on balance, collision-avoidance, and end-effector pose, among others. Several algorithms are able to generate configurations that satisfy these constraints given a good initial guess, i.e. a configuration which is already close to satisfying the constraints. However, when selecting goals for a planner a close initial guess is rarely available. Methods that attempt to satisfy all constraints through direct projection from a distant initial guess often fail due to opposing gradients for the various constraints, joint-limits, or singularities. We approach the problem of generating a constrained goal by searching for a configuration in the intersection of all constraint manifolds in configuration space (C-space). Starting with an initial guess, our algorithm, Constellation, builds a graph in C-space whose nodes are configurations that satisfy one or more constraints and whose cycles determine where the algorithm explores next. We compare the performance of our approach to direct projection and a previously-proposed cyclic projection method on reaching tasks for a humanoid robot with 33 DOF. We find that Constellation performs the best in terms of the number of solved queries across a wide range of problem difficulty. However, this success comes at higher computational cost.