Robotics: Vision and Control – Peter Corke