The paper is currently under review at the IEEE International Conference on Robotics and Automation (ICRA 2024).