One Paper accepted by IROS 2019