Robot Modeling and Control, Mark W. Spong, Seth Hutchinson, M. Vidyasagar