Latest Version of the Paper

The paper was accepted in Robotics: Science and Systems (RSS 2024).