Fix bug in icubWholeBodySensor with IMU readings