This is the author accepted version of a paper we published in Robotics and Automation Letters and presented at ICRA 2021. The final published version is available in IEEE Xplore.