Reviewed for IEEE IROS 2022