Based on chapter 7 of Robotics: Modelling, Planning and Control by Siciliano, Sciavicco, Villani and Oriolo.