Two papers accepted at ICRA 2025