Fixed shit
Merge branch 'master' into robotSim_greenleaf
Ready for 2.0 release!