Introduction to robotics mechanics and control