ICRA 2014 working version