Full Paper [Presented at IEEE/RSJ IROS 2022]