Algorithms for constrained motion planning