One paper accepted by IROS 2019