Modified iDynTree interface, created code for conversion between yarp and kdl/eigen