def case1_synthesis(formula, ts_file): _, dfa_0, dfa_inf, bdd = twtl.translate(formula, kind='both', norm=True) logging.debug('alphabet: {}'.format(dfa_inf.props)) for u, v, d in dfa_inf.g.edges_iter(data=True): logging.debug('({}, {}): {}'.format(u, v, d)) dfa_inf.visualize(draw='matplotlib') plt.show() logging.debug('\nEnd of translate\n\n') logging.info('The bound of formula "%s" is (%d, %d)!', formula, *bdd) logging.info('Translated formula "%s" to normal DFA of size (%d, %d)!', formula, *dfa_0.size()) logging.info('Translated formula "%s" to infinity DFA of size (%d, %d)!', formula, *dfa_inf.size()) logging.debug('\n\nStart policy computation\n') ts = Ts(directed=True, multi=False) ts.read_from_file(ts_file) ets = expand_duration_ts(ts) for name, dfa in [('normal', dfa_0), ('infinity', dfa_inf)]: logging.info('Constructing product automaton with %s DFA!', name) pa = ts_times_fsa(ets, dfa) logging.info('Product automaton size is: (%d, %d)', *pa.size()) if name == 'infinity': for u in pa.g.nodes_iter(): logging.debug('{} -> {}'.format(u, pa.g.neighbors(u))) pa.visualize(draw='matplotlib') plt.show() # compute optimal path in PA and then project onto the TS policy, output, tau = compute_control_policy(pa, dfa, dfa.kind) logging.info('Max deadline: %s', tau) if policy is not None: logging.info('Generated output word is: %s', [tuple(o) for o in output]) policy = [x for x in policy if x not in ets.state_map] out = StringIO.StringIO() for u, v in zip(policy[:-1], policy[1:]): print >> out, u, '->', ts.g[u][v][0]['duration'], '->', print >> out, policy[-1], logging.info('Generated control policy is: %s', out.getvalue()) out.close() logging.info('Relaxation is: %s', twtl.temporal_relaxation(output, formula=formula)) else: logging.info('No control policy found!')
def generate(formula, traces_file, no_traces=100): np.random.seed(1001) alphabet, dfa = translate(formula, kind=DFAType.Infinity, optimize=True) alphabet = [set([s]) for s in alphabet] + [set()] print 'alphabet:', alphabet data = {'positive': [], 'negative': []} both = 0 for k in range(no_traces): print 'k:', k state = iter(dfa.init).next() w = [] while True: r = np.random.randint(0, len(alphabet)) n = int(np.ceil(1 * np.random.exponential(3))) # print r, n symbol = alphabet[r] for _ in range(n): w.append(symbol) states = dfa.next_states_of_fsa(state, symbol) assert len(states) <= 1 if not states: break state = states[0] if state in dfa.final: ch = np.random.randint(0, 2) if len(w) < 62: data['positive'].append(w) elif len(w) > 82: data['negative'].append(w) elif ch: both += 1 data['positive'].append(w) else: both += 1 data['negative'].append(w) break print 'done', len(data['positive']), len(data['negative']), both # for w in data['positive']: # print 'length:', len(w) # print # for w in data['negative']: # print 'length:', len(w) with open(traces_file, 'w') as fout: pprint(data, fout, width=5000)
def case2_verification(formula, ts_file): _, dfa_inf, bdd = twtl.translate(formula, kind=DFAType.Infinity, norm=True) logging.info('The bound of formula "%s" is (%d, %d)!', formula, *bdd) logging.info('Translated formula "%s" to infinity DFA of size (%d, %d)!', formula, *dfa_inf.size()) ts = Ts.load(ts_file) ts.g.add_edges_from(ts.g.edges(), weight=1) for u, v in ts.g.edges_iter(): print u, '->', v result = verify(ts, dfa_inf) logging.info('The result of the verification procedure is %s!', result)
def case1_synthesis(formulas, ts_files): startG = timeit.default_timer() dfa_dict = {} for ind, f in enumerate(formulas): _, dfa_inf, bdd = twtl.translate(f, kind=DFAType.Infinity, norm=True) dfa_dict[ind + 1] = copy.deepcopy(dfa_inf) ts_dict = {} ets_dict = {} for ind, ts_f in enumerate(ts_files): ts_dict[ind + 1] = Ts(directed=True, multi=False) ts_dict[ind + 1].read_from_file(ts_f) ets_dict[ind + 1] = expand_duration_ts(ts_dict[ind + 1]) # Get the nominal PA logging.info('Constructing product automaton with infinity DFA!') startPA = timeit.default_timer() pa_dict = {} for key in dfa_dict: logging.info('Constructing product automaton with infinity DFA!') pa = ts_times_fsa(ets_dict[key], dfa_dict[key]) # Give initial weight attribute to all edges in pa nx.set_edge_attributes(pa.g, "weight", 1) logging.info('Product automaton size is: (%d, %d)', *pa.size()) # Make a copy of the nominal PA to change pa_dict[key] = copy.deepcopy(pa) # Calculate PA for the entire system pa_tuple = (pa_dict[1], pa_dict[2]) ets_tuple = (ets_dict[1], ets_dict[2]) team_ts = ts_times_ts(ets_tuple) team_pa = pa_times_pa(pa_tuple, team_ts) # pdb.set_trace() stopPA = timeit.default_timer() print 'Product automaton size is:', team_pa.size() print 'Run Time (s) to get PA is: ', stopPA - startPA startPath = timeit.default_timer() # Compute the optimal path in PA and project onto the TS pa_policy_multi = compute_multiagent_policy(team_pa) stopPath = timeit.default_timer() print 'Run Time (s) to get optimal paths for agents is: ', stopPath - startPath stopG = timeit.default_timer() print 'Run Time (s) for full algorithm: ', stopG - startG print 'PA Policy for 2 agents: ', pa_policy_multi
def learn_deadlines(formula, traces_p, traces_n): '''Infers deadlines for a given TWTL formula from the lists of positive and negative traces. The procedure generates the annotated DFA corresponding to the given formula just once and uses it to compute tight deadlines for all traces, positive and negative. Lastly, the deadlines are aggregated using a simple heuristic that ignores interdependencies between deadlines. ''' # 1. construct annotated DFA _, dfa = translate(formula, kind=DFAType.Infinity, optimize=True) # 2. compute tightest deadlines for all traces D_p = [get_deadlines(word, dfa) for word in traces_p] D_n = [get_deadlines(word, dfa) for word in traces_n] logging.debug('Tight deadlines for positive traces: %s', D_p) logging.debug('Tight deadlines for negative traces: %s', D_n) m = len(D_p[0]) d = [-float('Inf')] * m for k in range(m): deadlines = sorted(set([b[k] for b in it.chain(D_p, D_n)])) min_dl = None mcr = float('Inf') for dl in deadlines: FP = [dl_n[k] for dl_n in D_n if dl_n[k] <= dl] FN = [dl_p[k] for dl_p in D_p if dl_p[k] > dl] logging.debug('Deadline id: %d, value %d, MCR: %d, FP: %s, FN: %s', k, dl, len(FP) + len(FN), FP, FN) if len(FP) + len(FN) < mcr: mcr = len(FP) + len(FN) min_dl = dl logging.debug('Tightest deadline: %d, MCR: %d', mcr, min_dl) d[k] = min_dl # compute actual MCR - FIXME: this won't work with disjunction d_vec = np.asarray(d) MCR = np.sum([np.any(np.asarray(d_p) > d_vec) for d_p in D_p]) \ + np.sum([np.all(np.asarray(d_n) <= d_vec) for d_n in D_n]) return d, MCR
def learn_deadlines(formula, traces_p, traces_n): '''Infers deadlines for a given TWTL formula from the lists of positive and negative traces. The procedure generates the annotated DFA corresponding to the given formula just once and uses it to compute tight deadlines for all traces, positive and negative. Lastly, the deadlines are aggregated using a simple heuristic that ignores interdependencies between deadlines. ''' # 1. construct annotated DFA _, dfa = translate(formula, kind=DFAType.Infinity, optimize=True) # 2. compute tightest deadlines for all traces D_p = [get_deadlines(word, dfa) for word in traces_p] D_n = [get_deadlines(word, dfa) for word in traces_n] logging.debug('Tight deadlines for positive traces: %s', D_p) logging.debug('Tight deadlines for negative traces: %s', D_n) m = len(D_p[0]) d = [-float('Inf')]*m for k in range(m): deadlines = sorted(set([b[k] for b in it.chain(D_p, D_n)])) min_dl = None mcr = float('Inf') for dl in deadlines: FP = [dl_n[k] for dl_n in D_n if dl_n[k] <= dl] FN = [dl_p[k] for dl_p in D_p if dl_p[k] > dl] logging.debug('Deadline id: %d, value %d, MCR: %d, FP: %s, FN: %s', k, dl, len(FP) + len(FN), FP, FN) if len(FP) + len(FN) < mcr: mcr = len(FP) + len(FN) min_dl = dl logging.debug('Tightest deadline: %d, MCR: %d', mcr, min_dl) d[k] = min_dl # compute actual MCR - FIXME: this won't work with disjunction d_vec = np.asarray(d) MCR = np.sum([np.any(np.asarray(d_p) > d_vec) for d_p in D_p]) \ + np.sum([np.all(np.asarray(d_n) <= d_vec) for d_n in D_n]) return d, MCR
def prep_for_learning(ep_len, m, n, h, init_states, obstacles, pick_up_state, delivery_state, rewards, rew_val, custom_flag, custom_task): # Create the environment and get the TS # ts_start_time = timeit.default_timer() disc = 1 TS, obs_mat, state_mat = create_ts(m, n, h) path = '../data/ts_' + str(m) + 'x' + str(n) + 'x' + str(h) + '_1Ag_1.txt' paths = [path] bases = {init_states[0]: 'Base1'} obs_mat = update_obs_mat(obs_mat, state_mat, m, obstacles, init_states[0]) TS = update_adj_mat_3D(m, n, h, TS, obs_mat) create_input_file(TS, state_mat, obs_mat, paths[0], bases, disc, m, n, h, 0) ts_file = paths ts_dict = Ts(directed=True, multi=False) ts_dict.read_from_file(ts_file[0]) ts = expand_duration_ts(ts_dict) ts_timecost = timeit.default_timer() - ts_start_time # Get the DFA # dfa_start_time = timeit.default_timer() pick_ups = pick_up_state[0][0] * n + pick_up_state[0][1] deliveries = delivery_state[0][0] * n + delivery_state[0][1] pick_up = str(pick_ups) # Check later delivery = str(deliveries) tf = str((ep_len - 1) / 2) # time bound if custom_flag == 1: phi = custom_task else: phi = '[H^1 r' + pick_up + ']^[0, ' + tf + '] * [H^1 r' + delivery + ']^[0,' + tf + ']' # Construc the task according to pickup/delivery )^[0, ' + tf + ']' _, dfa_inf, bdd = twtl.translate( phi, kind=DFAType.Infinity, norm=True ) # states and sim. time ex. phi = '([H^1 r47]^[0, 30] * [H^1 r31]^[0, 30])^[0, 30]' dfa_timecost = timeit.default_timer( ) - dfa_start_time # DFAType.Normal for normal, DFAType.Infinity for relaxed # Get the PA # pa_start_time = timeit.default_timer() alpha = 1 nom_weight_dict = {} weight_dict = {} pa_or = ts_times_fsa(ts, dfa_inf) # Original pa edges_all = nx.get_edge_attributes(ts_dict.g, 'edge_weight') max_edge = max(edges_all, key=edges_all.get) norm_factor = edges_all[max_edge] for pa_edge in pa_or.g.edges(): edge = (pa_edge[0][0], pa_edge[1][0], 0) nom_weight_dict[pa_edge] = edges_all[edge] / norm_factor nx.set_edge_attributes(pa_or.g, 'edge_weight', nom_weight_dict) nx.set_edge_attributes(pa_or.g, 'weight', 1) pa = copy.deepcopy(pa_or) # copy the pa time_weight = nx.get_edge_attributes(pa.g, 'weight') edge_weight = nx.get_edge_attributes(pa.g, 'edge_weight') for pa_edge in pa.g.edges(): weight_dict[pa_edge] = alpha * time_weight[pa_edge] + ( 1 - alpha) * edge_weight[pa_edge] nx.set_edge_attributes(pa.g, 'new_weight', weight_dict) pa_timecost = timeit.default_timer() - pa_start_time # Compute the energy of the states # energy_time = timeit.default_timer() compute_energy(pa) energy_dict = nx.get_node_attributes(pa.g, 'energy') energy_pa = [] for ind in range(len(pa.g.nodes())): energy_pa.append(pa.g.nodes([0])[ind][1].values()[0]) # projection of pa on ts # init_state = [init_states[0][0] * n + init_states[0][1]] pa2ts = [] for i in range(len(pa.g.nodes())): if pa.g.nodes()[i][0] != 'Base1': pa2ts.append(int(pa.g.nodes()[i][0].replace("r", ""))) else: pa2ts.append(init_state[0]) i_s = i # Agent's initial location in pa energy_timecost = timeit.default_timer() - pa_start_time # TS adjacency matrix and source-target TS_adj = TS TS_s = [] TS_t = [] for i in range(len(TS_adj)): for j in range(len(TS_adj)): if TS_adj[i, j] != 0: TS_s.append(i) TS_t.append(j) # pa adjacency matrix and source-target pa_adj_st = nx.adjacency_matrix(pa.g) pa_adj = pa_adj_st.todense() pa_s = [] # source node pa_t = [] # target node for i in range(len(pa_adj)): for j in range(len(pa_adj)): if pa_adj[i, j] == 1: pa_s.append(i) pa_t.append(j) # PA rewards matrix rewards_ts = np.zeros(m * n) #-0.25# rewards_pa = np.zeros(len(pa2ts)) rewards_ts_indexes = [] for i in range(len(rewards)): rewards_ts_indexes.append( rewards[i][0] * n + rewards[i][1] ) # rewards_ts_indexes[i] = rewards[i][0] * n + rewards[i][1] rewards_ts[rewards_ts_indexes[i]] = rew_val for i in range(len(rewards_pa)): rewards_pa[i] = rewards_ts[pa2ts[i]] # # Display some important info print('##### PICK-UP and DELIVERY MISSION #####' + "\n") print('Initial Location : ' + str(init_states[0]) + ' <---> Region ' + str(init_state[0])) print('Pick-up Location : ' + str(pick_up_state[0]) + ' <---> Region ' + pick_up) print('Delivery Location : ' + str(delivery_state[0]) + ' <---> Regions ' + delivery) print('Reward Locations : ' + str(rewards) + ' <---> Regions ' + str(rewards_ts_indexes) + "\n") print('State Matrix : ') print(state_mat) print("\n") print('Mission Duration : ' + str(ep_len) + ' time steps') print('TWTL Task : ' + phi + "\n") print('Computational Costst : TS created in ' + str(ts_timecost) + ' seconds') # print(' TS created in ' + str(ts_timecost) + ' seconds') print(' DFA created in ' + str(dfa_timecost) + ' seconds') print(' PA created in ' + str(pa_timecost) + ' seconds') print(' Energy of PA states calculated in ' + str(energy_timecost) + ' seconds') return i_s, pa, pa_s, pa_t, pa2ts, energy_pa, rewards_pa, pick_up, delivery, pick_ups, deliveries, pa.g.nodes( )
Based on ... ''' # refine partition of states by reversed neighborhoods partition = PartitionRefinement(dfa.g.nodes_iter()) partition.refine(dfa.final) unrefined = dict([(id(p), p) for p in partition]) while unrefined: part = unrefined.pop(unrefined.iterkeys().next()) for symbol in dfa.alphabet: neighbors = set([ v for v, _, d in dfa.g.in_edges_iter(part, data=True) if symbol in d['input'] ]) for new, old in partition.refine(neighbors): if id(old) in unrefined or len(new) < len(old): unrefined[id(new)] = new else: unrefined[id(old)] = old print dfa.g.number_of_nodes() print len(list(partition)) # convert partition to DFA nx.condensation if __name__ == '__main__': phi1 = '[H^2 R2 & [H^2 R1]^[0, 8]]^[0, 20] & [H^2 R2 & [H^2 R3]^[0, 14]]^[0, 70]' # phi1 = '[H^2 R2]^[0, 20]' _, dfa, bdd = twtl.translate(phi1, kind=DFAType.Normal, norm=True) dfa = minimize(dfa)
def case1_synthesis(formulas, ts_files, alpha, radius, time_wp, lab_testing, always_active): startFull = timeit.default_timer() startOff = timeit.default_timer() dfa_dict = {} for ind, f in enumerate(formulas): _, dfa_inf, bdd = twtl.translate(f, kind=DFAType.Infinity, norm=True) logging.debug('\nEnd of translate\n\n') logging.info('The bound of formula "%s" is (%d, %d)!', f, *bdd) logging.info( 'Translated formula "%s" to infinity DFA of size (%d, %d)!', f, *dfa_inf.size()) dfa_dict[ind + 1] = copy.deepcopy( dfa_inf) # The key is set to the agent number logging.debug('\n\nStart policy computation\n') ts_dict = {} ets_dict = {} for ind, ts_f in enumerate(ts_files): ts_dict[ind + 1] = Ts(directed=True, multi=False) ts_dict[ind + 1].read_from_file(ts_f) ets_dict[ind + 1] = expand_duration_ts(ts_dict[ind + 1]) for ind in ts_dict: print 'Size of TS:', ets_dict[ind].size() # Get the nominal PA for each agent pa_nom_dict = {} norm_factor = {} startPA = timeit.default_timer() for key in dfa_dict: logging.info('Constructing product automaton with infinity DFA!') pa = ts_times_fsa(ets_dict[key], dfa_dict[key]) # Give length and weight attributes to all edges in pa nom_weight_dict = {} edges_all = nx.get_edge_attributes(ts_dict[key].g, 'edge_weight') max_edge = max(edges_all, key=edges_all.get) norm_factor[key] = edges_all[max_edge] for pa_edge in pa.g.edges(): edge = (pa_edge[0][0], pa_edge[1][0], 0) nom_weight_dict[pa_edge] = edges_all[edge] / norm_factor[key] nx.set_edge_attributes(pa.g, 'edge_weight', nom_weight_dict) nx.set_edge_attributes(pa.g, 'weight', 1) logging.info('Product automaton size is: (%d, %d)', *pa.size()) # Make a copy of the nominal PA to change pa_nom_dict[key] = copy.deepcopy(pa) stopPA = timeit.default_timer() print 'Run Time (s) to get all three PAs is: ', stopPA - startPA for key in pa_nom_dict: print 'Size of PA:', pa_nom_dict[key].size() # Use alpha to perform weighted optimization of time and edge_weight and make this a # new edge attribute to find "shortest path" over for key in pa_nom_dict: weight_dict = {} time_weight = nx.get_edge_attributes(pa_nom_dict[key].g, 'weight') edge_weight = nx.get_edge_attributes(pa_nom_dict[key].g, 'edge_weight') for pa_edge in pa_nom_dict[key].g.edges(): weight_dict[pa_edge] = alpha * time_weight[pa_edge] + ( 1 - alpha) * edge_weight[pa_edge] # Append the multi-objective cost to the edge attribtues of the PA nx.set_edge_attributes(pa_nom_dict[key].g, 'new_weight', weight_dict) # Compute the energy (multi-objective cost function) for each agent's PA at every node startEnergy = timeit.default_timer() for key in pa_nom_dict: compute_energy(pa_nom_dict[key]) stopEnergy = timeit.default_timer() print 'Run Time (s) to get the moc energy function for all three PA: ', stopEnergy - startEnergy # Compute optimal path in PA and project onto the TS ts_policy_dict_nom = {} pa_policy_dict_nom = {} tau_dict_nom = {} for key in pa_nom_dict: ts_policy_dict_nom[key], pa_policy_dict_nom[key], tau_dict_nom[key] = \ compute_control_policy(pa_nom_dict[key], dfa_dict[key], dfa_dict[key].kind) # Perform initial check on nominal control policies for key in ts_policy_dict_nom: if ts_policy_dict_nom[key] is None: logging.info('No control policy found!') # set empty control policies that will be iteratively updated ts_control_policy_dict = {} pa_control_policy_dict = {} # Initialize policy variables for key in ts_policy_dict_nom: ts_control_policy_dict[key] = [] pa_control_policy_dict[key] = [] # Concatenate nominal policies for searching policy_match, key_list, policy_match_index = update_policy_match( ts_policy_dict_nom) # Initialize vars, give nominal policies iter_step = 0 running = True traj_length = 0 ts_policy = copy.deepcopy(ts_policy_dict_nom) pa_policy = copy.deepcopy(pa_policy_dict_nom) tau_dict = tau_dict_nom # Choose parameter for n-horizon local trajectory, must be at least 2 num_hops = 2 # Get agent priority based on lowest energy prev_states = {} for key in ts_policy_dict_nom: prev_states[key] = pa_policy_dict_nom[key][0] priority = get_priority(pa_nom_dict, pa_policy_dict_nom, prev_states, key_list) # Create Agent energy dictionary for post-processing # Create Termination indicator to assign terminated agents lowest priority F_indicator = {} agent_energy_dict = {} for key in ts_policy_dict_nom: agent_energy_dict[key] = [] F_indicator[key] = False # Print time statistics stopOff = timeit.default_timer() print 'Offline run time for all initial setup: ', stopOff - startOff startOnline = timeit.default_timer() # Execute takeoff command for all crazyflies in lab testing if lab_testing: startTakeoff = timeit.default_timer() os.chdir("/home/ryan/crazyswarm/ros_ws/src/crazyswarm/scripts") os.system( "/home/ryan/crazyswarm/ros_ws/src/crazyswarm/scripts/twtl_takeoff.py" ) # make sure executable os.chdir("/home/ryan/Desktop/pyTWTL/src") stopTakeoff = timeit.default_timer() print 'Takeoff time, should be ~2.7sec: ', stopTakeoff - startTakeoff ############################################################################ # Iterate through all policies sequentially while running: while policy_match: for p_ind, p_val in enumerate(priority): if p_ind < 1: weighted_nodes = {} for i in range(num_hops): weighted_nodes[i] = [] else: # Get local neighborhood (n-hop) of nodes to search for a conflict for k_c, key_c in enumerate(key_list): if p_val == key_c: node = policy_match[0][k_c] break # Receive path information from 2*H neighborhood local_set = get_neighborhood(node, ts_dict[p_val], 2 * num_hops) # Get constraints for each transition weighted_nodes = {} for pty in priority[0:p_ind]: for key in key_list: if pty == key: ts_length = len(ts_policy[key]) if ts_length >= num_hops: for i in range(num_hops): if ts_policy[key][i] in local_set: try: weighted_nodes[i].append( ts_policy[key][i]) # Add downwash nodes to constraint, for drone experiments downwash_nodes = downwash_checkDP( ets_dict[key], ts_policy[key][i], radius) if downwash_nodes: for downwash_node in downwash_nodes: if downwash_node not in weighted_nodes[ i]: weighted_nodes[ i].append( downwash_node ) except KeyError: weighted_nodes[i] = [ ts_policy[key][i] ] downwash_nodes = downwash_checkDP( ets_dict[key], ts_policy[key][i], radius) if downwash_nodes: for downwash_node in downwash_nodes: if downwash_node not in weighted_nodes[ i]: weighted_nodes[ i].append( downwash_node ) else: for i in range(ts_length): if ts_policy[key][i] in local_set: try: weighted_nodes[i].append( ts_policy[key][i]) # Add downwash nodes to constraint, for drone experiments downwash_nodes = downwash_checkDP( ets_dict[key], ts_policy[key][i], radius) if downwash_nodes: for downwash_node in downwash_nodes: if downwash_node not in weighted_nodes[ i]: weighted_nodes[ i].append( downwash_node ) except KeyError: weighted_nodes[i] = [ ts_policy[key][i] ] downwash_nodes = downwash_checkDP( ets_dict[key], ts_policy[key][i], radius) if downwash_nodes: for downwash_node in downwash_nodes: if downwash_node not in weighted_nodes[ i]: weighted_nodes[ i].append( downwash_node ) for i in range(num_hops): try: weighted_nodes[i] except KeyError: weighted_nodes[i] = [] # Update constraint set with intersecting transitions if traj_length >= 1: for p_ind2, p_val2 in enumerate(priority[0:p_ind]): for k, key in enumerate(key_list): if p_val2 == key: # initialize previous state comp_prev_state = ts_control_policy_dict[ key][-1] cur_prev_state = ts_control_policy_dict[ key_c][-1] cur_ts_policy_length = len( ts_policy[key_c]) ts_length = len(ts_policy[key]) if ts_length >= num_hops: for i in range(num_hops): comp_next_state = ts_policy[key][i] if i < cur_ts_policy_length: cur_next_state = ts_policy[ key_c][i] if comp_next_state in local_set: # Check if the trajectories cross during transition (or use same transition) cross_weight = check_intersectDP(ets_dict[key], cur_prev_state, cur_next_state, \ comp_prev_state, comp_next_state, radius, time_wp) if cross_weight: for cross_node in cross_weight: if cross_node not in weighted_nodes[ i]: weighted_nodes[ i].append( cross_node ) # Check if using same transition in updated case if comp_next_state == cur_prev_state: if comp_prev_state not in weighted_nodes[ i]: weighted_nodes[ i].append( comp_prev_state ) # Set previous state for next iteration comp_prev_state = ts_policy[ key][i] cur_prev_state = ts_policy[ key_c][i] else: break else: for i in range(ts_length): comp_next_state = ts_policy[key][i] if i < cur_ts_policy_length: cur_next_state = ts_policy[ key_c][i] if comp_next_state in local_set: # Check if the trajectories cross during transition (or use same transition) cross_weight = check_intersectDP(ets_dict[key], cur_prev_state, cur_next_state, \ comp_prev_state, comp_next_state, radius, time_wp) if cross_weight: for cross_node in cross_weight: if cross_node not in weighted_nodes[ i]: weighted_nodes[ i].append( cross_node ) # Check if using same transition in updated case if comp_next_state == cur_prev_state: if comp_prev_state not in weighted_nodes[ i]: weighted_nodes[ i].append( comp_prev_state ) # Set previous state for next iteration comp_prev_state = ts_policy[ key][i] cur_prev_state = ts_policy[ key_c][i] else: break # Generate receding horizon path and check for termination if traj_length >= 1: init_loc = pa_control_policy_dict[p_val][-1] ts_temp = ts_policy[p_val] pa_temp = pa_policy[p_val] # Compute receding horizon shortest path ts_policy[p_val], pa_policy[ p_val], D_flag = local_horizonDP( pa_nom_dict[p_val], weighted_nodes, num_hops, init_loc) # Check for deadlock, and if so resolve deadlock if p_ind > 0: if D_flag == True: # Agent in deadlock is to remain stationary ts_policy[p_val] = [ ts_control_policy_dict[p_val][-1], ts_control_policy_dict[p_val][-1] ] pa_policy[p_val] = [ pa_control_policy_dict[p_val][-1], pa_control_policy_dict[p_val][-1] ] # Assign deadlock node x_d = ts_control_policy_dict[p_val][-1] x_d_val = p_val x_d_flag = True hp_set = priority[0:p_ind] while x_d_flag == True and hp_set: x_d_flag = False for hp in hp_set: if ts_policy[hp][0] == x_d: if hp == priority[0]: # Make all agents stationary and perform Dijkstra's shortest path for j in priority[1:p_ind]: ts_policy[j] = [ ts_control_policy_dict[j] [-1], ts_control_policy_dict[j] [-1] ] pa_policy[j] = [ pa_control_policy_dict[j] [-1], pa_control_policy_dict[j] [-1] ] occupied_nodes = [ ts_control_policy_dict[x_d_val] [-1] ] for j in priority[0:p_ind]: occupied_nodes.append( ts_control_policy_dict[j] [-1]) init_loc = pa_control_policy_dict[ x_d_val][-1] ts_policy[x_d_val], pa_policy[ x_d_val] = deadlock_path( pa_nom_dict[x_d_val], occupied_nodes, init_loc) for j in priority[1:p_ind]: for ind, node in enumerate( ts_policy[x_d_val] [:-1]): if ts_policy[j][0] == node: ts_policy[j] = [ ts_policy[x_d_val][ ind + 1], ts_policy[x_d_val][ ind + 1] ] # Find the actual state on agent's PA that corresponds to this node neighbors = pa_nom_dict[ j].g.neighbors( pa_policy[j] [0]) for node in neighbors: if node[0] == ts_policy[ j][0]: pa_policy[ j] = [ node, node ] break else: ts_policy[hp] = [ ts_control_policy_dict[hp][-1], ts_control_policy_dict[hp][-1] ] pa_policy[hp] = [ pa_control_policy_dict[hp][-1], pa_control_policy_dict[hp][-1] ] x_d = ts_control_policy_dict[hp][ -1] x_d_val = hp x_d_flag = True hp_set.remove(hp) break # Increase iteration step (for statistics at end) iter_step += 1 # Update policy match policy_match, key_list, policy_match_index = update_policy_match( ts_policy) # Account for agents which have finished, also accounts for other finished agents through agent ID ordering if always_active == True: finished_ID = [] for key in F_indicator: if F_indicator[key] == True: finished_ID.append(key) current_node = ts_control_policy_dict[key][-1] hp_nodes_avoid = [] for k in key_list: hp_nodes_avoid.append(ts_policy[k][0]) hp_nodes_avoid.append( ts_control_policy_dict[k][-1]) for fID in finished_ID[:-1]: hp_nodes_avoid.append( ts_control_policy_dict[fID][-1]) if current_node in hp_nodes_avoid: local_set = ts_dict[key].g.neighbors(current_node) for node in local_set: if node not in hp_nodes_avoid: ts_control_policy_dict[key].append(node) break else: ts_control_policy_dict[key].append(current_node) # Append trajectories for key in ts_policy: agent_energy_dict[key].append( pa_nom_dict[key].g.node[pa_policy[key][0]]['energy']) ts_control_policy_dict[key].append(ts_policy[key].pop(0)) pa_policy_temp = list(pa_policy[key]) pa_control_policy_dict[key].append(pa_policy_temp.pop(0)) pa_policy[key] = tuple(pa_policy_temp) ts_write = policy_match.pop(0) traj_length += 1 # publish waypoint to a csv file write_to_csv_iter(ts_dict, ts_write, key_list, time_wp) # Execute waypoint in crazyswarm lab testing if lab_testing: startWaypoint = timeit.default_timer() os.chdir("/home/ryan/crazyswarm/ros_ws/src/crazyswarm/scripts") os.system( "/home/ryan/crazyswarm/ros_ws/src/crazyswarm/scripts/twtl_waypoint.py" ) # make sure executable os.chdir("/home/ryan/Desktop/pyTWTL/src") stopWaypoint = timeit.default_timer() print 'Waypoint time, should be ~2.0sec: ', stopWaypoint - startWaypoint # Update policy_match now that a trajectory has finalized and policy_match is empty if ts_policy: # Remove keys from policies that have terminated land_keys = [] for key, val in ts_policy.items(): if len(val) == 0: F_indicator[key] = True land_keys.append(key) del ts_policy[key] del pa_policy[key] # publish to the land csv file when finished (for experiments) if land_keys: if lab_testing: write_to_land_file(land_keys) os.chdir( "/home/ryan/crazyswarm/ros_ws/src/crazyswarm/scripts" ) os.system( "/home/ryan/crazyswarm/ros_ws/src/crazyswarm/scripts/twtl_land.py" ) # make sure executable os.chdir("/home/ryan/Desktop/pyTWTL/src") if not ts_policy: running = False break # Update policy match policy_match, key_list, policy_match_index = update_policy_match( ts_policy) # Get agent priority based on lowest energy for key in key_list: prev_states[key] = pa_control_policy_dict[key][-1] priority = get_priority(pa_nom_dict, pa_policy, prev_states, key_list) else: running = False # Print run time statistics stopOnline = timeit.default_timer() print 'Online run time for safe algorithm: ', stopOnline - startOnline stopFull = timeit.default_timer() print 'Full run time for safe algorithm: ', stopFull - startFull # Print other statistics from simulation print 'Number of iterations for run: ', iter_step print 'Average time for itertion is: ', (stopOnline - startOnline) / iter_step print 'Number of full updates in run: ', traj_length print 'Average update time for single step: ', (stopOnline - startOnline) / traj_length # Print energy graph for each agent and the system from run plot_energy(agent_energy_dict) # Not exact, but gives insight for key in pa_nom_dict: tau_dict[key] = tau_dict_nom[key] + len( ts_control_policy_dict[key]) - len(ts_policy_dict_nom[key]) # Write the nominal and final control policies to a file for key in pa_nom_dict: write_to_control_policy_file(ts_policy_dict_nom[key], pa_policy_dict_nom[key], \ tau_dict_nom[key], dfa_dict[key],ts_dict[key],ets_dict[key],\ ts_control_policy_dict[key], pa_control_policy_dict[key], tau_dict[key], key) # Write the CSV files for experiments for key in pa_nom_dict: write_to_csv(ts_dict[key], ts_control_policy_dict[key], key, time_wp)
def case1_synthesis(formulas, ts_files, alpha, gamma, radius, time_wp, lab_testing): startFull = timeit.default_timer() startOff = timeit.default_timer() dfa_dict = {} for ind, f in enumerate(formulas): _, dfa_inf, bdd = twtl.translate(f, kind=DFAType.Infinity, norm=True) logging.debug('\nEnd of translate\n\n') logging.info('The bound of formula "%s" is (%d, %d)!', f, *bdd) logging.info( 'Translated formula "%s" to infinity DFA of size (%d, %d)!', f, *dfa_inf.size()) dfa_dict[ind + 1] = copy.deepcopy( dfa_inf) # Note that the key is set to the agent number logging.debug('\n\nStart policy computation\n') ts_dict = {} ets_dict = {} for ind, ts_f in enumerate(ts_files): ts_dict[ind + 1] = Ts(directed=True, multi=False) ts_dict[ind + 1].read_from_file(ts_f) ets_dict[ind + 1] = expand_duration_ts(ts_dict[ind + 1]) for ind in ts_dict: print 'Size of TS:', ets_dict[ind].size() # Get the nominal PA for each agent pa_nom_dict = {} norm_factor = {} for key in dfa_dict: logging.info('Constructing product automaton with infinity DFA!') pa = ts_times_fsa(ets_dict[key], dfa_dict[key]) # Give length and weight attributes to all edges in pa nom_weight_dict = {} edges_all = nx.get_edge_attributes(ts_dict[key].g, 'edge_weight') max_edge = max(edges_all, key=edges_all.get) norm_factor[key] = edges_all[max_edge] for pa_edge in pa.g.edges(): edge = (pa_edge[0][0], pa_edge[1][0], 0) nom_weight_dict[pa_edge] = edges_all[edge] / norm_factor[key] nx.set_edge_attributes(pa.g, 'edge_weight', nom_weight_dict) nx.set_edge_attributes(pa.g, 'weight', 1) logging.info('Product automaton size is: (%d, %d)', *pa.size()) # Make a copy of the nominal PA to change pa_nom_dict[key] = copy.deepcopy(pa) for key in pa_nom_dict: print 'Size of PA:', pa_nom_dict[key].size() # Use alpha to perform weighted optimization of time and edge_weight and make this a # new edge attribute to find "shortest path" over for key in pa_nom_dict: weight_dict = {} time_weight = nx.get_edge_attributes(pa_nom_dict[key].g, 'weight') edge_weight = nx.get_edge_attributes(pa_nom_dict[key].g, 'edge_weight') for pa_edge in pa_nom_dict[key].g.edges(): weight_dict[pa_edge] = alpha * time_weight[pa_edge] + ( 1 - alpha) * edge_weight[pa_edge] # Append the multi-objective cost to the edge attribtues of the PA nx.set_edge_attributes(pa_nom_dict[key].g, 'new_weight', weight_dict) # Compute the energy (multi-objective cost function) for each agent's PA at every node startEnergy = timeit.default_timer() for key in pa_nom_dict: compute_energy(pa_nom_dict[key]) stopEnergy = timeit.default_timer() print 'Run Time (s) to get the energy function for all three PA: ', stopEnergy - startEnergy # Compute optimal path in Pa_Prime and project onto the TS, and initial policy based on new_weight ts_policy_dict_nom = {} pa_policy_dict_nom = {} tau_dict_nom = {} for key in pa_nom_dict: ts_policy_dict_nom[key], pa_policy_dict_nom[key], tau_dict_nom[key] = \ compute_control_policy(pa_nom_dict[key], dfa_dict[key], dfa_dict[key].kind) for key in pa_nom_dict: ts_policy_dict_nom[key], pa_policy_dict_nom[key] = \ compute_control_policy3(pa_nom_dict[key], dfa_dict[key], pa_policy_dict_nom[key][0]) # Perform initial check on nominal control policies for key in ts_policy_dict_nom: if ts_policy_dict_nom[key] is None: logging.info('No control policy found!') # set empty control policies that will be iteratively updated ts_control_policy_dict = {} pa_control_policy_dict = {} # Initialize policy variables for key in ts_policy_dict_nom: ts_control_policy_dict[key] = [] pa_control_policy_dict[key] = [] # Concatenate nominal policies for searching policy_match, key_list, policy_match_index = update_policy_match( ts_policy_dict_nom) # Initialize vars, give nominal policies iter_step = 0 running = True traj_length = 0 ts_policy = copy.deepcopy(ts_policy_dict_nom) pa_policy = copy.deepcopy(pa_policy_dict_nom) tau_dict = tau_dict_nom # Choose parameter for n-horizon local trajectory and information sharing, # must be at least 2 num_hops = 3 # Get agent priority based on lowest energy prev_priority = key_list prev_states = {} for key in ts_policy_dict_nom: prev_states[key] = pa_policy_dict_nom[key][0] priority = get_priority(pa_nom_dict, pa_policy_dict_nom, prev_states, key_list, prev_priority) # Create Agent energy dictionary for post-processing agent_energy_dict = {} for key in ts_policy_dict_nom: agent_energy_dict[key] = [] # Print time statistics stopOff = timeit.default_timer() print 'Offline run time for all initial setup: ', stopOff - startOff startOnline = timeit.default_timer() # Execute takeoff command for all crazyflies in lab testing if lab_testing: startTakeoff = timeit.default_timer() os.chdir("/home/ryan/crazyswarm/ros_ws/src/crazyswarm/scripts") os.system( "/home/ryan/crazyswarm/ros_ws/src/crazyswarm/scripts/twtl_takeoff.py" ) # make sure file is an executable os.chdir("/home/ryan/Desktop/pyTWTL/src") stopTakeoff = timeit.default_timer() print 'Takeoff time, should be ~2.7sec: ', stopTakeoff - startTakeoff # Iterate through all policies sequentially while running: while policy_match: for p_ind, p_val in enumerate(priority): if p_ind < 1: weighted_nodes = [] weighted_soft_nodes = {} for i in range(num_hops - 1): weighted_soft_nodes[i + 1] = [] else: # Get local neighborhood (n-hop) of nodes to search for a conflict for k, key in enumerate(key_list): if p_val == key: node = policy_match[0][k] break local_set = get_neighborhood(node, ts_dict[p_val], num_hops) one_hop_set = ts_dict[p_val].g.neighbors(node) # Assign hard constraint nodes in local neighborhood weighted_nodes = [] for pty in priority[0:p_ind]: for k, key in enumerate(key_list): if pty == key: prev_node = policy_match[0][k] if prev_node in one_hop_set: weighted_nodes.append(prev_node) # Check if downwash constraint needs to be added, mostly for physical testing downwash_weight = downwash_check(k, ets_dict[key], policy_match[0], \ priority[0:k], key_list, radius) if downwash_weight: for downwash_node in downwash_weight: if downwash_node not in weighted_nodes: weighted_nodes.append( downwash_node) break # Get soft constraint nodes from sharing n-hop trajectory soft_nodes = {} for pty in priority[0:p_ind]: for k, key in enumerate(key_list): if pty == key: ts_length = len(ts_policy[key]) if ts_length >= num_hops: for i in range(num_hops - 1): if ts_policy[key][i + 1] in local_set: try: soft_nodes[i + 1] soft_nodes[i + 1].append( ts_policy[key][i + 1]) except KeyError: soft_nodes[i + 1] = [ ts_policy[key][i + 1] ] else: for i in range(ts_length - 1): if ts_policy[key][i + 1] in local_set: try: soft_nodes[i + 1] soft_nodes[i + 1].append( ts_policy[key][i + 1]) except KeyError: soft_nodes[i + 1] = [ ts_policy[key][i + 1] ] for i in range(num_hops - 1): try: soft_nodes[i + 1] except KeyError: soft_nodes[i + 1] = [] # Assign soft constraint nodes weighted_soft_nodes = soft_nodes # Update weights if transitioning between same two nodes ts_prev_states = [] ts_index = [] if len(policy_match[0]) > 1 and traj_length >= 1: for key in ts_control_policy_dict: if len(ts_control_policy_dict[key]) == traj_length: ts_prev_states.append( ts_control_policy_dict[key][-1]) if ts_prev_states: for p_ind2, p_val2 in enumerate(priority[0:p_ind]): if p_ind2 > 0: for k_c, key in enumerate(key_list): if p_val2 == key: node = policy_match[0][k_c] break # Check if the trajectories will cross each other in transition cross_weight = check_intersect(k_c, ets_dict[key], ts_prev_states, policy_match[0], \ priority[0:p_ind2], key_list, radius, time_wp) if cross_weight: for cross_node in cross_weight: if cross_node not in weighted_nodes: weighted_nodes.append(cross_node) # Check if agents using same transition for p_ind3, p_val3 in enumerate( priority[0:p_ind2]): for k, key in enumerate(key_list): if p_val3 == key: if ts_prev_states[k] == node: if policy_match[0][ k] == ts_prev_states[ k_c]: temp_node = policy_match[ 0][k] if temp_node not in weighted_nodes: weighted_nodes.append( temp_node) if node not in weighted_nodes: weighted_nodes.append( node) break else: continue break else: continue break else: # Check if agents using same transition for p_ind3, p_val3 in enumerate( priority[0:p_ind2]): for k, key in enumerate(key_list): if p_val3 == key: if ts_prev_states[k] == node: if policy_match[0][ k] == ts_prev_states[ k_c]: temp_node = policy_match[ 0][k] if temp_node not in weighted_nodes: weighted_nodes.append( temp_node) if node not in weighted_nodes: weighted_nodes.append( node) break else: continue break else: continue break # Compute local horizon function to account for receding horizon all the time # while checking for termination if traj_length >= 1: init_loc = pa_control_policy_dict[p_val][-1] # Compute receding horizon shortest path ts_policy[p_val], pa_policy[p_val] = local_horizon(pa_nom_dict[p_val], weighted_nodes,\ weighted_soft_nodes, num_hops, init_loc, gamma) # Write updates to file # iter_step += 1 # write_to_iter_file(ts_policy[p_val], ts_dict[p_val], ets_dict[p_val], p_val, iter_step) # Update policy match policy_match, key_list, policy_match_index = update_policy_match( ts_policy) # Append trajectories for key in ts_policy: agent_energy_dict[key].append( pa_nom_dict[key].g.node[pa_policy[key][0]]['energy']) ts_control_policy_dict[key].append(ts_policy[key].pop(0)) pa_policy_temp = list(pa_policy[key]) pa_control_policy_dict[key].append(pa_policy_temp.pop(0)) pa_policy[key] = tuple(pa_policy_temp) ts_write = policy_match.pop(0) traj_length += 1 # publish this waypoint to a csv file write_to_csv_iter(ts_dict, ts_write, key_list, time_wp) # Execute waypoint in crazyswarm lab testing if lab_testing: startWaypoint = timeit.default_timer() os.chdir("/home/ryan/crazyswarm/ros_ws/src/crazyswarm/scripts") os.system( "/home/ryan/crazyswarm/ros_ws/src/crazyswarm/scripts/twtl_waypoint.py" ) os.chdir("/home/ryan/Desktop/pyTWTL/src") stopWaypoint = timeit.default_timer() print 'Waypoint time, should be ~2.0sec: ', stopWaypoint - startWaypoint # Update policy_match now that a trajectory has finalized and policy_match is empty if ts_policy: # Remove keys from policies that have terminated land_keys = [] for key, val in ts_policy.items(): if len(val) == 0: land_keys.append(key) del ts_policy[key] del pa_policy[key] # publish to the land csv file for lab testing if land_keys: if lab_testing: write_to_land_file(land_keys) os.chdir( "/home/ryan/crazyswarm/ros_ws/src/crazyswarm/scripts" ) os.system( "/home/ryan/crazyswarm/ros_ws/src/crazyswarm/scripts/twtl_land.py" ) os.chdir("/home/ryan/Desktop/pyTWTL/src") if not ts_policy: running = False break # Update policy match policy_match, key_list, policy_match_index = update_policy_match( ts_policy) # Get agent priority based on lowest energy for key in key_list: prev_states[key] = pa_control_policy_dict[key][-1] priority = get_priority(pa_nom_dict, pa_policy, prev_states, key_list, priority) else: running = False # Print run time statistics stopOnline = timeit.default_timer() print 'Online run time for safe algorithm: ', stopOnline - startOnline stopFull = timeit.default_timer() print 'Full run time for safe algorithm: ', stopFull - startFull # Print energy statistics from run plot_energy(agent_energy_dict) # Possibly just set the relaxation to the nominal + additional nodes added *** Change (10/28) for key in pa_nom_dict: tau_dict[key] = tau_dict_nom[key] + len( ts_control_policy_dict[key]) - len(ts_policy_dict_nom[key]) # Write the nominal and final control policies to a file for key in pa_nom_dict: write_to_control_policy_file(ts_policy_dict_nom[key], pa_policy_dict_nom[key], \ tau_dict_nom[key], dfa_dict[key],ts_dict[key],ets_dict[key],\ ts_control_policy_dict[key], pa_control_policy_dict[key], tau_dict[key], key) # Write the CSV files for experiments for key in pa_nom_dict: write_to_csv(ts_dict[key], ts_control_policy_dict[key], key, time_wp)