modified version for the IROS2013 video
modified verion for IROS 2013 with changes from lab video