Foundations of Robotics: Analysis and Control