Paper accepted at ICRA'19!