paper to be submitted to 2014 IROS