Fix rodriguez rotation formula 
Fix rodriguez rotation formula 

See https://it.sharelatex.com/project/564ddaaf9cec09503ca12023
robotology/idyntree,robotology/idyntree,robotology/idyntree,robotology/idyntree,robotology/idyntree