Motion planning and control of mobile robots