This repository contains the code for the paper submitted at IROS 2014. The code will be released upon paper acceptance.