Reviewed for IEEE RA-L & ICRA 2022