Simulink Toolbox for rapid prototyping of Whole Body Robot Controllers