Added tests for iDynTree