Added code related to KDL - iDyn compatibility and iCub KDL::Tree model (a bit messy)