Motion control overhaul (uses cmd_vel)