publishing code for Zhao's IROS 2013 paper