Update idyn2kdl_icub.cpp