Paper accepted at IROS 2023!