Motion planning and controlling algorithm for grasping and manipulating moving objects in the presence of obstacles