added code relative to a paper submitted to IROS2014