Submitted to ICRA 2024