ICRA paper accepted