First working code for the real robot with the algorithms I plan to submit to a Journal
Code of the paper submitted to IROS 2017