Modified iDynTree