Paper Accepted at ICRA 2024