The paper is currently under review at the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2023).