Does some minor changes I made while debugging.
Code for IROS demo.