add robustness to icubWholeBodyStates