FTME140 Code in course Dynamics of Multibody Systems (FTME140), 2015, Chalmers University of Technology, Gothenburg, Sweden.