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