New paper appearing on IEEE Transactions on Robotics