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