Update icubWholeBodyModel.h