Introduction to robotics : mechanics and control. - Craig, John J.