Rebased the paper on Severin's IROS2013 paper