Paper accepted to ICRA 2016