Fixed bug in icubWholeBodyStatesLocal.cpp