iDynTree protype upgrade