Introduction To Robotics Mechanics And Control By John J. Craig