Introduction to robotics: Mechanics and control