First working version of iDynTree