Fixing bug in icubWholeBodySensors