Some changes in the beginning.
Merge branch 'master' of https://github.com/hangkaiyu/WS_ICRA2014