Final version, submitted at IROS