Working control on robot but dirty
Working 2nd_ord integration with command to set of velocity size
Deleted command to set velocity size (automatically set by setState)