Rigig Body Dynamic Library (modify from  http://rbdl.bitbucket.org/)