Added joint ids. Reorganized files and renamed functions.