Fundamentals of Robotics: Analysis and Control