@reister, @peller This is the branch from our ICRA paper. Does any of this need to be merged into the master? Otherwise, we should delete the branch