Revert to old behavior of set control mode

See https://github.com/robotology-playground/yarp-wholebodyinterface/issues/9
See https://github.com/robotology-playground/WBI-Toolbox/issues/41