Add robot state and init function, refactor commands
Fix default get-new-robot-velocity