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 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)
