A penalty function method for constrained motion planning

Establishes necessary and sufficient conditions under which manipulation constraints are holonomic. Then the authors present a systematic approach to motion planning in the presence of manipulation constraints deriving from this theory. Its principle is to replace a constrained problem by a convergent series of less constrained subproblems increasingly penalizing motions that do not satisfy the constraints. Each subproblem is solved using a standard path planner. The authors use the method of variational dynamic programming for solving the subproblems. The implemented planner has solved manipulation planning problems of unprecedented complexity