The paper is currently under review at the 39th IEEE Conference on Robotics and Automation (ICRA 2022).