Paper published in IROS 2020