Fixed several stability issues

Added walkto autobot command, but not complete.