First (not working) version of new gazebo_yarp_forcetorque