Clean up the code for https://github.com/robotology/codyco/issues/31