Dynamic control and motion planning technique for a class of nonlinear systems with drift