def dijkstra_plan_optimal(product,
beta=10,
start_set=None,
pose=None,
time_limit=None,
segment='lasso'):
start = time.time()
#print 'dijkstra plan optimal started!'
runs = {}
accept_set = product.graph['accept']
if start_set == None:
init_set = product.graph['initial']
else:
init_set = start_set
#print 'number of accepting states %d' %(len(accept_set))
#print 'number of initial states %d' %(len(init_set))
loop_dict = {}
for init_prod_node in init_set:
for (prefix, precost) in dijkstra_targets(product, init_prod_node,
accept_set):
# for reachable accepting state
# print 'accept node reached %s' %(str(prefix[-1]))
if pose:
precost += distance(pose, init_prod_node[0][0])
if prefix[-1] in loop_dict:
suffix, sufcost = loop_dict[prefix[-1]]
else:
suffix, sufcost = dijkstra_loop(product, prefix[-1])
print suffix, sufcost
loop_dict[prefix[-1]] = (suffix, sufcost)
if suffix:
runs[(prefix[0], prefix[-1])] = (prefix, precost, suffix,
sufcost)
#print 'find run from %s to %s and back' %(str(init_prod_node), str(prefix[-1]))
if (time_limit) and (time.time() - start >
time_limit): # time limit has reached
break
if runs:
if segment == 'lasso':
prefix, precost, suffix, sufcost = min(
runs.values(), key=lambda p: p[1] + beta * p[3])
run = ProdAut_Run(product, prefix, precost, suffix, sufcost,
precost + beta * sufcost)
#print '\n==================\n'
print 'optimal_dijkstra_olf done within %.2fs: precost %.2f, sufcost %.2f' % (
time.time() - start, precost, sufcost)
return run, time.time() - start
elif segment == 'prefix':
prefix, precost, suffix, sufcost = min(runs.values(),
key=lambda p: p[1])
run = ProdAut_Run(product, prefix, precost, suffix, sufcost,
precost + beta * sufcost)
#print '\n==================\n'
print 'optimal_dijkstra_olf done within %.2fs: precost %.2f, sufcost %.2f' % (
time.time() - start, precost, sufcost)
return run, time.time() - start
else:
print 'no accepting run found in optimal planning!'
return None, None
def replan(self, temporary_task_):
new_run = improve_plan_given_history(self.product, self.trace,
self.run, self.index)
if new_run:
self.run = new_run
self.index = 0
self.segment = 'line'
self.trace = []
self.run_history = []
if (len(temporary_task_.combinations)) > 0:
current_state = list(
initial_state_given_history(self.product, self.run_history,
self.run, self.index))
current_state = current_state[0]
run_temp = temporary_task_.find_temporary_run(
current_state, self.product)
end_temporary = set()
end_temporary.add(run_temp.prefix[-1])
new_run, time = dijkstra_plan_optimal(self.product, 10,
end_temporary)
prefix = run_temp.prefix[0:-1] + new_run.prefix
precost = run_temp.precost + new_run.precost
suffix = run_temp.suffix + new_run.suffix
sufcost = new_run.sufcost
totalcost = precost + self.beta * sufcost
self.run = ProdAut_Run(self.product, prefix, precost, suffix,
sufcost, totalcost)
def dijkstra_plan_bounded(product, time_limit=3, beta=10):
start = time.time()
print 'dijkstra plan started!'
runs = {}
accept_set = product.graph['accept']
init_set = product.graph['initial']
print 'number of accepting states %d' % (len(accept_set))
print 'number of initial states %d' % (len(init_set))
loop_dict = {}
for init_prod_node in init_set:
for (prefix, precost) in dijkstra_targets(product, init_prod_node,
accept_set):
#print 'accept node reached %s' %(str(prefix[-1]))
if prefix[-1] in loop_dict:
suffix, sufcost = loop_dict[prefix[-1]]
else:
suffix, sufcost = dijkstra_loop(product, prefix[-1])
loop_dict[prefix[-1]] = (suffix, sufcost)
#print suffix, sufcost
if suffix:
runs[(prefix[0], prefix[-1])] = (prefix, precost, suffix,
sufcost)
#print 'find run from %s to %s and back' %(str(init_prod_node), str(prefix[-1]))
if time.time() - start > time_limit: # time limit has reached
if runs:
prefix, precost, suffix, sufcost = min(
runs.values(), key=lambda p: p[1] + beta * p[3])
run = ProdAut_Run(product, prefix, precost, suffix,
sufcost, precost + beta * sufcost)
print 'optimal_dijkstra done within %.2fs: precost %.2f, sufcost %.2f' % (
time.time() - start, precost, sufcost)
return run, time.time() - start
print 'no accepting run found in optimal planning!'
def dijkstra_plan_networkX(product, beta=10):
# requires a full construct of product automaton
start = time.time()
runs = {}
loop = {}
# minimal circles
for prod_target in product.graph['accept']:
#print 'prod_target', prod_target
# accepting state in self-loop
if prod_target in product.predecessors(prod_target):
loop[prod_target] = (
product.edge[prod_target][prod_target]["weight"],
[prod_target, prod_target])
continue
else:
cycle = {}
loop_pre, loop_dist = dijkstra_predecessor_and_distance(
product, prod_target)
for target_pred in product.predecessors_iter(prod_target):
if target_pred in loop_dist:
cycle[target_pred] = product.edge[target_pred][
prod_target]["weight"] + loop_dist[target_pred]
if cycle:
opti_pred = min(cycle, key=cycle.get)
suffix = compute_path_from_pre(loop_pre, opti_pred)
loop[prod_target] = (cycle[opti_pred], suffix)
# shortest line
if not product.graph['initial']:
print 'Initial set empty'
for prod_init in product.graph['initial']:
line = {}
line_pre, line_dist = dijkstra_predecessor_and_distance(
product, prod_init)
for target in loop.iterkeys():
if target in line_dist:
line[target] = line_dist[target] + beta * loop[target][0]
if line:
opti_targ = min(line, key=line.get)
prefix = compute_path_from_pre(line_pre, opti_targ)
precost = line_dist[opti_targ]
runs[(prod_init,
opti_targ)] = (prefix, precost, loop[opti_targ][1],
loop[opti_targ][0])
# best combination
if runs:
prefix, precost, suffix, sufcost = min(
runs.values(), key=lambda p: p[1] + beta * p[3])
run = ProdAut_Run(product, prefix, precost, suffix, sufcost,
precost + beta * sufcost)
print '=================='
print 'Dijkstra_plan_networkX done within %.2fs: precost %.2f, sufcost %.2f' % (
time.time() - start, precost, sufcost)
return run, time.time() - start
#print '\n==================\n'
print '=================='
print 'No accepting run found in optimal planning!'
return None, None
def find_temporary_run(self, current_state, product):
cost = float('inf')
for i in range(0, len(self.combinations)):
run_prefix = []
cost_temp = 0
cost_with_factor = 0
distance = 0
propos_planner_index = []
task_end_index = []
task_end_planner_index = []
#comb = []
for j in range(0, len(self.combinations[i])):
target_set = self.build_target_set(self.combinations[i][j], product)
runs = {}
if j == 0:
for (prefix, precost) in dijkstra_targets(product, current_state, target_set):
runs[(prefix[0], prefix[-1])] = (prefix, precost)
#print(prefix)
else:
for (prefix, precost) in dijkstra_targets(product, run_prefix[-1], target_set):
runs[(prefix[0], prefix[-1])] = (prefix, precost)
prefix_temp, cost_ = min(runs.values(), key = lambda p: p[1])
run_prefix = run_prefix + prefix_temp
cost_temp = cost_temp + cost_
for m in range(1, len(prefix_temp)):
#print(prefix_temp[m-1][0][0][0])
distance = distance + self.euclidean_distance(prefix_temp[m-1][0][0][0], prefix_temp[m][0][0][0])
#print('---distance---')
#print(distance)
#print(self.combinations[i][j])
propos_planner_index.append(len(run_prefix) - 1)
#comb.append([self.combinations[i][j]])
if self.combinations[i][j] in self.final_propos:
task_index = self.final_propos.index(self.combinations[i][j])
planner_index = len(run_prefix) - 1
task_end_index.append(task_index)
task_end_planner_index.append(planner_index)
cost_with_factor = cost_with_factor + cost_temp*(distance/0.3 + (rospy.Time.now()-self.task_time[task_index][0]).to_sec())/self.task_time[task_index][1]
print('---costwithfactor---')
print(cost_with_factor)
print(cost)
if cost_with_factor < cost:
cost = cost_with_factor
prefix_ = run_prefix
self.chosen_comb = deepcopy(self.combinations[i])
self.propos_planner_index = propos_planner_index
self.task_end_index = task_end_index
self.task_end_planner_index = task_end_planner_index
print('---run_temp---')
print(prefix_[0])
run_temp = ProdAut_Run(product, prefix_, cost, [], [], cost_temp)
return run_temp
def dijkstra_plan_networkX(product,
beta=10,
start_set=None,
pose=None,
segment='lasso'):
# requires a full construct of product automaton
# start_set can be used to specify other initial states
#
start = time.time()
runs = {}
loop = {}
cycle = {}
if start_set == None:
init_set = product.graph['initial']
else:
init_set = start_set
########################################
# minimal circles
for prod_target in product.graph['accept']:
loop_pre, loop_dist = dijkstra_predecessor_and_distance(
product, prod_target)
for target_pred in product.predecessors_iter(prod_target):
if target_pred in loop_dist:
cycle[target_pred] = product.edge[target_pred][prod_target][
"weight"] + loop_dist[target_pred]
if cycle:
opti_pred = min(cycle, key=cycle.get)
suffix = compute_path_from_pre(loop_pre, opti_pred) + [
prod_target,
]
loop[prod_target] = (cycle[opti_pred], suffix)
########################################
# shortest line
for prod_init in init_set:
line = {}
line_pre, line_dist = dijkstra_predecessor_and_distance(
product, prod_init)
for target in loop.iterkeys():
if target in line_dist:
if pose:
line_dist[target] += 0.1 * distance(pose, prod_init[0][0])
line[target] = line_dist[target] + beta * loop[target][0]
if line:
opti_targ = min(line, key=line.get)
prefix = compute_path_from_pre(line_pre, opti_targ)
precost = line_dist[opti_targ]
runs[(prod_init,
opti_targ)] = (prefix, precost, loop[opti_targ][1],
loop[opti_targ][0])
########################################
# best combination
if runs:
if segment == 'lasso':
prefix, precost, suffix, sufcost = min(
runs.values(), key=lambda p: p[1] + beta * p[3])
run = ProdAut_Run(product, prefix, precost, suffix, sufcost,
precost + beta * sufcost)
#print '\n==================\n'
print 'dijkstra_plan_networkX done within %.2fs: precost %.2f, sufcost %.2f' % (
time.time() - start, precost, sufcost)
return run, time.time() - start
#print '\n==================\n'
elif segment == 'prefix':
prefix, precost, suffix, sufcost = min(runs.values(),
key=lambda p: p[1])
run = ProdAut_Run(product, prefix, precost, suffix, sufcost,
precost + beta * sufcost)
#print '\n==================\n'
print 'dijkstra_plan_networkX done within %.2fs: precost %.2f, sufcost %.2f' % (
time.time() - start, precost, sufcost)
return run, time.time() - start
#print '\n==================\n'
else:
print 'no accepting run found in optimal planning!'
return None, None