class ltl_planner(object):
def __init__(self, ts, hard_spec, soft_spec):
buchi = mission_to_buchi(hard_spec, soft_spec)
self.product = ProdAut(ts, buchi)
self.Time = 0
self.cur_pose = None
self.trace = [] # record the regions been visited
self.traj = [] # record the full trajectory
self.opt_log = []
# record [(time, prefix, suffix, prefix_cost, suffix_cost, total_cost)]
self.com_log = []
# record [(time, no_messages)]
def optimal(self, beta=10, style='static'):
self.beta = beta
if style == 'static':
# full graph construction
self.product.graph['ts'].build_full()
self.product.build_full()
self.run, plantime = dijkstra_plan_networkX(self.product, self.beta)
elif style == 'ready':
self.product.build_full()
self.run, plantime = dijkstra_plan_networkX(self.product, self.beta)
elif style == 'on-the-fly':
# on-the-fly construction
self.product.build_initial()
self.product.build_accept()
self.run, plantime = dijkstra_plan_optimal(self.product, self.beta)
if self.run == None:
print '---No valid has been found!---'
print '---Check you FTS or task---'
return
#print '\n'
print '------------------------------'
print 'the prefix of plan **states**:'
print [n for n in self.run.line]
class ltl_planner(object):
def __init__(self, ts, hard_spec, soft_spec):
buchi = mission_to_buchi(hard_spec, soft_spec)
self.product = ProdAut(ts, buchi)
self.Time = 0
self.cur_pose = None
self.trace = [] # record the regions been visited
self.run_history = [] # record executed plan
self.traj = [] # record the full trajectory
self.opt_log = []
# record [(time, prefix, suffix, prefix_cost, suffix_cost, total_cost)]
self.com_log = []
# record [(time, no_messages)]
self.num_changed_regs = 0
def optimal(self, beta=10, style='static'):
self.beta = beta
if style == 'static':
# full graph construction
self.product.graph['ts'].build_full()
self.product.build_full()
self.run, plantime = dijkstra_plan_networkX(
self.product, self.beta)
elif style == 'ready':
self.product.build_full()
self.run, plantime = dijkstra_plan_networkX(
self.product, self.beta)
elif style == 'on-the-fly':
# on-the-fly construction
self.product.build_initial()
self.product.build_accept()
self.run, plantime = dijkstra_plan_optimal(self.product, self.beta)
if self.run == None:
print '---No valid has been found!---'
print '---Check you FTS or task---'
return
#print '\n'
print '------------------------------'
print 'the prefix of plan **states**:'
print[n for n in self.run.line]
print 'the suffix of plan **states**:'
print[n for n in self.run.loop]
#print '\n'
print '------------------------------'
print 'the prefix of plan **actions**:'
print[n for n in self.run.pre_plan]
print 'the suffix of plan **actions**:'
print[n for n in self.run.suf_plan]
self.opt_log.append(
(self.Time, self.run.pre_plan, self.run.suf_plan, self.run.precost,
self.run.sufcost, self.run.totalcost))
self.index = 0
self.segment = 'line'
self.next_move = self.run.pre_plan[self.index]
return plantime
def find_next_move(self):
if self.segment == 'line' and self.index < (len(self.run.pre_plan) -
2):
self.trace.append(self.run.line[self.index])
self.run_history.append(self.run.pre_plan[self.index])
self.index += 1
self.next_move = self.run.pre_plan[self.index]
elif (self.segment
== 'line') and ((self.index >= len(self.run.pre_plan) - 2) or
(len(self.run.pre_plan) <= 2)):
self.trace.append(self.run.line[self.index])
self.run_history.append(self.run.pre_plan[self.index])
self.index = 0
self.segment = 'loop'
self.next_move = self.run.suf_plan[self.index]
elif self.segment == 'loop' and self.index < len(
self.run.suf_plan) - 1:
self.trace.append(self.run.loop[self.index])
self.run_history.append(self.run.suf_plan[self.index])
self.index += 1
self.segment = 'loop'
self.next_move = self.run.suf_plan[self.index]
elif (self.segment
== 'loop') and ((self.index >= len(self.run.suf_plan) - 1) or
(len(self.run.suf_plan) <= 2)):
self.trace.append(self.run.loop[self.index])
self.run_history.append(self.run.suf_plan[self.index])
self.index = 1
self.segment = 'loop'
self.next_move = self.run.suf_plan[self.index]
return self.next_move
def update_add(self, object_name, region_label):
MotionFts = self.product.graph['ts'].graph['region']
for ts_node in MotionFts.nodes():
if region_label in MotionFts.node[ts_node]['label']:
MotionFts.node[ts_node]['label'].add(object_name)
def update_remove(self, object_name, region_label):
MotionFts = self.product.graph['ts'].graph['region']
for ts_node in MotionFts.nodes():
if region_label in MotionFts.node[ts_node]['label']:
if object_name in MotionFts.node[ts_node]['label']:
MotionFts.node[ts_node]['label'].remove(object_name)
return True
else:
return False
def update_knowledge(self, sense_info):
changed_regs = self.product.graph['ts'].graph[
'region'].update_ts_after_sense_info(sense_info)
if changed_regs:
changed_states = self.product.update_prod_aut_after_ts_update(
sense_info)
self.num_changed_regs = self.num_changed_regs + len(changed_regs)
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 validate_and_revise(self):
validate_run_and_revise(self.product, self.run)
def replan_simple(self, pose):
self.product.graph['ts'].graph['region'].set_initial(pose)
self.optimal(10, style='static')
self.index = 0
self.segment = 'loop'
self.next_move = self.run.suf_plan[self.index]
class ltl_planner(object): def __init__(self, ts, hard_spec, soft_spec): buchi = mission_to_buchi(hard_spec, soft_spec) self.product = ProdAut(ts, buchi) self.Time = 0 self.cur_pose = None self.trace = [] # record the regions been visited self.traj = [] # record the full trajectory self.opt_log = [] # record [(time, prefix, suffix, prefix_cost, suffix_cost, total_cost)] self.com_log = [] # record [(time, no_messages)] def optimal(self, beta=10, style='static'): self.beta = beta if style == 'static': # full graph construction self.product.graph['ts'].build_full() self.product.build_full() self.run, plantime = dijkstra_plan_networkX(self.product, self.beta) elif style == 'ready': self.product.build_full() self.run, plantime = dijkstra_plan_networkX(self.product, self.beta) elif style == 'on-the-fly': # on-the-fly construction self.product.build_initial() self.product.build_accept() self.run, plantime = dijkstra_plan_optimal(self.product, self.beta) print 'the plan prefix:\n' print [n for n in self.run.pre_plan] #print '\n' print 'the plan suffix:\n' print [n for n in self.run.suf_plan] #print '\n' self.opt_log.append((self.Time, self.run.pre_plan, self.run.suf_plan, self.run.precost, self.run.sufcost, self.run.totalcost)) self.last_time = self.Time self.acc_change = 0 self.index = 1 self.segment = 'line' self.next_move = self.run.pre_plan[self.index] return plantime def find_next_move(self): if self.segment == 'line' and self.index < len(self.run.pre_plan)-1: self.trace.append(self.run.line[self.index]) self.index += 1 self.next_move = self.run.pre_plan[self.index] elif self.segment == 'line' and self.index == len(self.run.pre_plan)-1: self.trace.append(self.run.line[self.index]) self.index = 0 self.segment = 'loop' self.next_move = self.run.suf_plan[self.index] elif self.segment == 'loop' and self.index < len(self.run.suf_plan)-1: self.trace.append(self.run.loop[self.index]) self.index += 1 self.segment = 'loop' self.next_move = self.run.suf_plan[self.index] elif self.segment == 'loop' and self.index == len(self.run.suf_plan)-1: self.trace.append(self.run.loop[self.index]) self.index = 0 self.segment = 'loop' self.next_move = self.run.suf_plan[self.index] return self.next_move def update(self,object_name): MotionFts = self.product.graph['ts'].graph['region'] cur_region = MotionFts.closest_node(self.cur_pose) sense_info = dict() sense_info['label'] = set([(cur_region,set([object_name,]),set()),]) changes = MotionFts.update_after_region_change(sense_info,None) if changes: return True def replan(self): new_run = improve_plan_given_history(self.product, self.trace) if (new_run) and (new_run.pre_plan !=self.run.pre_plan[self.index:-1]): self.run = new_run self.index = 1 self.segment = 'line' self.next_move = self.run.pre_plan[self.index] print 'Plan adapted!'
class ltl_planner(object):
def __init__(self, ts, hard_spec, soft_spec):
buchi = mission_to_buchi(hard_spec, soft_spec)
self.product = ProdAut(ts, buchi)
self.Time = 0
self.cur_pose = None
self.trace = [] # record the regions to be visited
self.traj = [] # record the full trajectory
self.opt_log = []
# record [(time, prefix, suffix, prefix_cost, suffix_cost, total_cost)]
self.com_log = []
# record [(time, no_messages)]
self.contract_time = 0 # 其他机器人协助动作的大约执行时间
self.ETA_current_collaboration = 0
self.delay = False
def optimal(self, beta=10, style='static'):
self.beta = beta
if style == 'static':
# full graph construction
self.product.graph['ts'].build_full()
# write_dot(self.product.graph['ts'], "./ts.dot")
self.product.build_full()
self.run, plantime = dijkstra_plan_networkX(
self.product, self.beta)
elif style == 'ready':
self.product.build_full()
self.run, plantime = dijkstra_plan_networkX(
self.product, self.beta)
elif style == 'on-the-fly':
# on-the-fly construction
self.product.build_initial()
self.product.build_accept()
self.run, plantime = dijkstra_plan_optimal(self.product, self.beta)
if self.run is None:
print '---No valid has been found!---'
print '---Check you FTS or task---'
return
# print '\n'
print '------------------------------'
print 'the prefix of plan **states**:'
print[n for n in self.run.line]
print 'the suffix of plan **states**:'
print[n for n in self.run.loop]
print '------------------------------'
print 'the prefix of plan **aps**:'
print[self.product.graph['ts'].node[n]['label'] for n in self.run.line]
print 'the suffix of plan **aps**:'
print[self.product.graph['ts'].node[n]['label'] for n in self.run.loop]
# print '\n'
print '------------------------------'
# print 'the prefix of plan **actions**:'
# print [n for n in self.run.pre_plan]
# print 'the suffix of plan **actions**:'
# print [n for n in self.run.suf_plan]
self.opt_log.append(
(self.Time, self.run.pre_plan, self.run.suf_plan, self.run.precost,
self.run.sufcost, self.run.totalcost))
self.last_time = self.Time
self.acc_change = 0
self.index = 1
self.segment = 'line'
self.next_move = self.run.pre_plan[self.index]
return plantime
def find_next_move(self):
# if self.delay and self.contract_time <= 0:
# return self.next_move
if self.segment == 'line' and self.index < len(self.run.pre_plan) - 2:
# print 'index:', self.index
# print 'pre_plan:', self.run.pre_plan
# print 'line:', self.run.line
self.trace.append(self.run.line[self.index])
self.index += 1
self.next_move = self.run.pre_plan[self.index]
elif (self.segment
== 'line') and ((self.index == len(self.run.pre_plan) - 2) or
(len(self.run.pre_plan) <= 2)):
self.trace.append(self.run.line[self.index])
self.index = 0
self.segment = 'loop'
self.next_move = self.run.suf_plan[self.index]
elif self.segment == 'loop' and self.index < len(
self.run.suf_plan) - 2:
self.trace.append(self.run.loop[self.index])
self.index += 1
self.segment = 'loop'
self.next_move = self.run.suf_plan[self.index]
elif (self.segment
== 'loop') and ((self.index == len(self.run.suf_plan) - 2) or
(len(self.run.suf_plan) <= 2)):
self.trace.append(self.run.loop[self.index])
self.index = 0
self.segment = 'loop'
self.next_move = self.run.suf_plan[self.index]
return self.next_move
def update(self, object_name):
MotionFts = self.product.graph['ts'].graph['region']
cur_region = MotionFts.closest_node(self.cur_pose)
sense_info = dict()
sense_info['label'] = set([
(cur_region, set([
object_name,
]), set()),
])
changes = MotionFts.update_after_region_change(sense_info, None)
if changes:
return True
def replan(self):
new_run = improve_plan_given_history(self.product, self.trace)
if new_run and (new_run.pre_plan != self.run.pre_plan[self.index:-1]):
self.run = new_run
self.index = 1
self.segment = 'line'
self.next_move = self.run.pre_plan[self.index]
print 'Plan adapted!'
def cooperative_action_in_horizon(self, dep, horizon):
s = 0
Tm = 0
Request = []
init_run = self.run.prefix + self.run.suffix[1:]
if self.segment == 'line':
index = self.index - 1
else:
index = self.index + len(self.run.pre_plan) - 1
# print 'prefix:', self.run.prefix
# print 'suffix:', self.run.suffix
# print 'init_run:', init_run
while Tm < horizon and index + s < len(init_run) - 1:
current_node = init_run[index + s]
next_node = init_run[index + s + 1]
# print 'current:', current_node, 'next:', next_node
Tm = Tm + self.product.edges[current_node, next_node]['weight']
# 0 represents 'ts' in next_node, and 0 represents region in next_node[0], 1 represents action
reg = next_node[0][0]
act = next_node[0][1]
if not Request and act in dep:
for action in dep[act]:
Request.append([action, reg, Tm])
s += 1
return Request
def evaluate_request(self, request, dep, alpha):
MAX = float(10000)
reply = {}
run = {}
for req in request:
act = req[0]
reg = req[1]
Tm = req[2]
if self.contract_time <= 0 and not self.delay and not isinstance(
self.next_move, basestring):
if self.segment == 'line':
current_node = self.run.prefix[self.index - 1]
accept_node = self.run.prefix[-1]
else:
if self.index == 0:
current_node = self.run.prefix[-1]
else:
current_node = self.run.suffix[self.index - 1]
accept_node = self.run.suffix[-1]
sd = []
sc = []
for prod_node in self.product.nodes:
if reg == prod_node[0][0] and act == prod_node[0][1]:
sd.append(prod_node)
for col in dep:
if col == prod_node[0][1] or (prod_node[0][1] != act
and prod_node[0][1]
in dep[col]):
sc.append(prod_node)
cost = 0
index = self.index - 1
if self.segment == 'line':
while index < len(self.run.prefix) - 1:
cost += self.product.edges[
self.run.prefix[index],
self.run.prefix[index + 1]]['weight']
index += 1
else:
while index < len(self.run.suffix) - 1:
cost += self.product.edges[
self.run.suffix[index],
self.run.suffix[index + 1]]['weight']
index += 1
for path1, cost1 in dijkstra_targets(self.product,
current_node, sd, sc):
# print 'path1:', path1
for path2, cost2 in dijkstra_targets_reverse(
self.product, accept_node, sd):
# print 'path2:', path2
if path1 and path2 and path1[-1] == path2[-1]:
cost3 = abs(cost1 -
Tm) + alpha * (cost1 + cost2 - cost)
run[path1[-1]] = (path1 +
list(reversed(path2))[1:], cost3,
cost1)
if run:
path, total_cost, pre_cost = min(run.values(),
key=lambda p: p[1])
reply[act] = [path, True, pre_cost]
else:
reply[act] = [[], False, MAX]
else:
reply[act] = [[], False, MAX]
return reply
def confirmation(self, request, Reply):
start = time.time()
aj = [key for key in Reply]
d = [act[0] for act in request]
confirm = {}
t = {}
b = {}
Tm = {}
for key in Reply:
for req in request:
t[req[0], key] = Reply[key][req[0]][2]
if Reply[key][req[0]][1]:
b[req[0], key] = 1
else:
b[req[0], key] = 0
Tm[req[0]] = req[2]
m = Model("confirm")
c = m.addVars(d, aj, vtype=GRB.BINARY, name="c")
tx = m.addVars(d, aj, name='tx')
fm = m.addVar(name="fm")
m.setObjective(fm, GRB.MINIMIZE)
m.addGenConstrMax(fm, [tx[i, j] for i in d for j in aj], Tm[d[0]])
m.addConstrs((c.prod(b, '*', j) <= 1 for j in aj), "constr1")
m.addConstrs((c.prod(b, i, '*') == 1 for i in d), "constr2")
m.addConstrs((c[i, j] * t[i, j] * b[i, j] - tx[i, j] == 0 for i in d
for j in aj), "constr3")
m.optimize()
try:
solution = m.getAttr('x', c)
except GurobiError:
return confirm, 0
else:
print 'solution:', solution
has_solution = False
for num in aj:
conf = {}
for act in d:
if solution[act, num] == 1:
conf[act] = [
Reply[num][act][0], Reply[num][act][1],
fm.getAttr('x')
]
has_solution = True
else:
conf[act] = [[], False, float('inf')]
confirm[num] = conf
if has_solution:
self.delay = False
self.contract_time = fm.getAttr('x')
return confirm, time.time() - start
else:
# self.contract_time = 0
return confirm, 0
def adapt_plan(self, confirm):
for act in confirm:
message = confirm[act]
if message[1]:
self.contract_time = message[2]
new_path = message[0]
if self.segment == 'line':
print 'old_prefix:', self.run.prefix
print 'current_node:', self.run.prefix[self.index - 1]
# print 'path:', new_path
if self.index == 1:
self.run.prefix = self.run.prefix[:self.
index] + new_path
else:
self.run.prefix = self.run.prefix[:self.index -
1] + new_path
print 'new_prefix:', self.run.prefix
else:
if self.index == 1:
self.run.suffix = self.run.suffix[:self.
index] + new_path
else:
self.run.suffix = self.run.suffix[:self.index -
1] + new_path
# print 'suffix:', self.run.suffix
self.run.plan_output(self.product)
if self.segment == 'line':
self.next_move = self.run.pre_plan[self.index]
else:
self.next_move = self.run.suf_plan[self.index]
else:
continue
def delay_cooperation(self, DELAY, STEP):
self.contract_time = 0
self.delay = True
class ltl_planner(object):
def __init__(self, ts, hard_spec, soft_spec):
buchi = mission_to_buchi(hard_spec, soft_spec)
self.product = ProdAut(ts, buchi)
self.Time = 0
self.cur_pose = None
self.trace = [] # record the regions been visited
self.traj = [] # record the full trajectory
self.opt_log = []
# record [(time, prefix, suffix, prefix_cost, suffix_cost, total_cost)]
self.com_log = []
# record [(time, no_messages)]
def optimal(self, beta=10, style='static'):
self.beta = beta
if style == 'static':
# full graph construction
self.product.graph['ts'].build_full()
self.product.build_full()
self.run, plantime = dijkstra_plan_networkX(
self.product, self.beta)
elif style == 'ready':
self.product.build_full()
self.run, plantime = dijkstra_plan_networkX(
self.product, self.beta)
elif style == 'on-the-fly':
# on-the-fly construction
self.product.build_initial()
self.product.build_accept()
self.run, plantime = dijkstra_plan_optimal(self.product, self.beta)
if self.run == None:
print '---No valid has been found!---'
print '---Check you FTS or task---'
return
if isinstance(self.run.pre_plan[1], str):
init_pose = (rospy.get_param('amcl_initial_pose_x'),
rospy.get_param('amcl_initial_pose_y'),
rospy.get_param('amcl_initial_pose_a'))
self.run.line = [(init_pose, 'None')] + self.run.line
print[n for n in self.run.line]
self.run.pre_plan = [init_pose] + self.run.pre_plan
print[n for n in self.run.pre_plan]
rospy.logwarn("Prefix plan corrected")
#print '\n'
print '------------------------------'
print 'the prefix of plan **states**:'
print[n for n in self.run.line]
print 'the suffix of plan **states**:'
print[n for n in self.run.loop]
#print '\n'
print '------------------------------'
print 'the prefix of plan **actions**:'
print[n for n in self.run.pre_plan]
print 'the suffix of plan **actions**:'
print[n for n in self.run.suf_plan]
self.opt_log.append(
(self.Time, self.run.pre_plan, self.run.suf_plan, self.run.precost,
self.run.sufcost, self.run.totalcost))
self.last_time = self.Time
self.acc_change = 0
self.index = 1
self.segment = 'line'
self.next_move = self.run.pre_plan[self.index]
return plantime
def find_next_move(self):
if self.segment == 'line' and self.index < len(self.run.pre_plan) - 2:
self.trace.append(self.run.line[self.index])
self.index += 1
self.next_move = self.run.pre_plan[self.index]
elif (self.segment
== 'line') and ((self.index == len(self.run.pre_plan) - 2) or
(len(self.run.pre_plan) <= 2)):
self.trace.append(self.run.line[self.index])
self.index = 0
self.segment = 'loop'
self.next_move = self.run.suf_plan[self.index]
elif self.segment == 'loop' and self.index < len(
self.run.suf_plan) - 2:
self.trace.append(self.run.loop[self.index])
self.index += 1
self.segment = 'loop'
self.next_move = self.run.suf_plan[self.index]
elif (self.segment
== 'loop') and ((self.index == len(self.run.suf_plan) - 2) or
(len(self.run.suf_plan) <= 2)):
self.trace.append(self.run.loop[self.index])
self.index = 0
self.segment = 'loop'
self.next_move = self.run.suf_plan[self.index]
return self.next_move
def update(self, object_name):
MotionFts = self.product.graph['ts'].graph['region']
cur_region = MotionFts.closest_node(self.cur_pose)
sense_info = dict()
sense_info['label'] = set([
(cur_region, set([
object_name,
]), set()),
])
changes = MotionFts.update_after_region_change(sense_info, None)
if changes:
return True
def replan(self):
new_run = improve_plan_given_history(self.product, self.trace)
if (new_run) and (new_run.pre_plan !=
self.run.pre_plan[self.index:-1]):
self.run = new_run
self.index = 1
self.segment = 'line'
self.next_move = self.run.pre_plan[self.index]
print 'Plan adapted!'
class ltl_planner(object):
def __init__(self, ts, hard_spec, soft_spec, beta):
buchi = mission_to_buchi(hard_spec, soft_spec)
self.product = ProdAut(ts, buchi, beta)
self.Time = 0
self.cur_pose = None
self.trace = [] # record the regions been visited
self.traj = [] # record the full trajectory
self.opt_log = []
# record [(time, prefix, suffix, prefix_cost, suffix_cost, total_cost)]
self.com_log = []
# record [(time, no_messages)]
self.beta = beta
def reset_beta(self, beta):
self.beta = beta
self.product.graph['beta'] = beta
def set_to_suffix(self):
self.segment = 'loop'
self.index = 0
self.next_move = self.run.suf_plan[self.index]
def start_suffix(self):
if ((self.segment == 'loop') and (self.index == 0)):
return True
else:
return False
def in_suffix(self):
if ((self.segment == 'loop')):
return True
else:
return False
def optimal(self, gamma=10, style='static'):
self.gamma = gamma
if style == 'static':
# full graph construction
self.product.graph['ts'].build_full()
self.product.build_full()
self.run, plantime = dijkstra_plan_networkX(self.product, self.gamma)
elif style == 'ready':
self.product.build_full()
self.run, plantime = dijkstra_plan_networkX(self.product, self.gamma)
elif style == 'on-the-fly':
# on-the-fly construction
self.product.build_initial()
self.product.build_accept()
self.run, plantime = dijkstra_plan_optimal(self.product, self.gamma)
elif style == 'repeat':
self.run, plantime = dijkstra_plan_networkX(self.product, self.gamma)
if self.run == None:
print '---No valid has been found!---'
print '---Check you FTS or task---'
return
#print '\n'
# print '------------------------------'
# print 'the prefix of plan **states**:'
# print [n for n in self.run.line]
# print 'the suffix of plan **states**:'
# print [n for n in self.run.loop]
# #print '\n'
# print '------------------------------'
# print 'the prefix of plan **actions**:'
# print [n for n in self.run.pre_plan]
# print 'the suffix of plan **actions**:'
# print [n for n in self.run.suf_plan]
# self.opt_log.append((self.Time, self.run.pre_plan, self.run.suf_plan, self.run.precost, self.run.sufcost, self.run.totalcost))
self.last_time = self.Time
self.acc_change = 0
self.index = 0
self.segment = 'line'
self.next_move = self.run.pre_plan[self.index]
return plantime
def find_next_move(self):
if self.segment == 'line' and self.index < len(self.run.pre_plan)-2:
self.trace.append(self.run.line[self.index])
self.index += 1
self.next_move = self.run.pre_plan[self.index]
elif (self.segment == 'line') and ((self.index == len(self.run.pre_plan)-2) or (len(self.run.pre_plan) <= 2)):
self.trace.append(self.run.line[self.index])
self.index = 0
self.segment = 'loop'
self.next_move = self.run.suf_plan[self.index]
elif self.segment == 'loop' and self.index < len(self.run.suf_plan)-2:
self.trace.append(self.run.loop[self.index])
self.index += 1
self.segment = 'loop'
self.next_move = self.run.suf_plan[self.index]
elif (self.segment == 'loop') and ((self.index == len(self.run.suf_plan)-2) or (len(self.run.suf_plan) <= 2)):
self.trace.append(self.run.loop[self.index])
self.index = 0
self.segment = 'loop'
self.next_move = self.run.suf_plan[self.index]
return self.next_move
def reach_ts_node(self, pose, reach_bound):
for n in self.product.graph['ts'].nodes():
if ((reach_waypoint(n[0], pose, reach_bound))
and (n[1] == 'None')):
return n
return None
def update_reachable(self, reachable_states, ts_node):
new_reachable = set()
for f_s in reachable_states:
for t_s in self.product.successors(f_s):
if t_s[0] == ts_node:
new_reachable.add(t_s)
return new_reachable
def update_runs(self, prev_runs, ts_node):
new_runs = set()
for run in prev_runs:
f_s = run[-1]
for t_s in self.product.successors(f_s):
if t_s[0] == ts_node:
new_run = list(run)
new_run.append(t_s)
new_runs.add(tuple(new_run))
return new_runs
def prod_dist_to_trap(self, pose, reachable_set):
mini_dist_reg = min(self.product.graph['ts'].graph['region'].nodes(),
key = lambda s: distance(pose, s))
mini_dist = distance(mini_dist_reg, pose)
new_reachable = self.update_reachable(reachable_set, (mini_dist_reg, 'None'))
if self.check_trap(new_reachable):
return mini_dist
else:
return -1
def check_trap(self, reachable_set):
for s in reachable_set:
if has_path_to_accept(self.product, s):
return False
return True
def check_accept(self, reachable_set):
accept_set = self.product.graph['accept']
if accept_set.intersection(reachable_set):
return True
else:
return False
def intersect_accept(self, reachable_set, reach_ts):
accept_set = self.product.graph['accept']
inter_set = set([s for s in accept_set if s[0] == reach_ts])
return inter_set
def update(self,object_name):
MotionFts = self.product.graph['ts'].graph['region']
cur_region = MotionFts.closest_node(self.cur_pose)
sense_info = dict()
sense_info['label'] = set([(cur_region,set([object_name,]),set()),])
changes = MotionFts.update_after_region_change(sense_info,None)
if changes:
return True
def replan(self):
new_run = improve_plan_given_history(self.product, self.trace)
if (new_run) and (new_run.pre_plan !=self.run.pre_plan[self.index:-1]):
self.run = new_run
self.index = 1
self.segment = 'line'
self.next_move = self.run.pre_plan[self.index]
print 'Plan adapted!'
def compute_path_cost(self, path):
ac_c = 0
ac_d = 0
for i in range(len(path)-1):
e = (path[i], path[i+1])
ac_d += self.product.edge[e[0]][e[1]]['distance']
ac_c += self.product.edge[e[0]][e[1]]['cost']
return [ac_c, ac_d]
def margin_opt_path(self, opt_path, beta):
self.reset_beta(beta)
self.product.build_full_margin(opt_path)
#marg_path = dijkstra_path_networkX(self.product, opt_path[0], opt_path[-1])
self.run, plantime = dijkstra_plan_networkX(self.product, self.gamma)
return self.run.suffix
def opt_path_match(self, path1, path2):
score = 0
for i,s in enumerate(path1):
if ((i< len(path2)) and (path2[i] == s)):
score += 1
return score
def find_opt_path(self, ts_path, reachable_prod_states):
if self.segment == 'line':
print 'In prefix'
opt_path = opt_path_in_prefix(self.product, ts_path, reachable_prod_states)
return opt_path
elif self.segment == 'loop':
print 'In suffix'
opt_path = opt_path_in_suffix(self.product, ts_path, reachable_prod_states)
return opt_path
def find_opt_paths_jit(self, posb_runs):
opt_path = opt_path_jit(self.product, posb_runs)
return opt_path
def irl_jit(self, posb_runs):
print '------------------------------'
print 'Find beta via IRL starts'
t0 = time.time()
opt_path = self.find_opt_paths_jit(posb_runs)
opt_cost = self.compute_path_cost(opt_path)
opt_ac_d = opt_cost[1]
beta_seq = []
beta = 100.0
beta_p = self.beta
count = 0
lam = 1.0
alpha = 1.0
match_score = []
count = 0
while ((abs(beta_p-beta)>0.3) and (count <20)):
if beta_p < 0:
break
print 'Iteration --%d--'%count
beta = beta_p
marg_path = self.margin_opt_path(opt_path, beta)
marg_cost = self.compute_path_cost(marg_path)
marg_ac_d = marg_cost[1]
print '(opt_ac_d-marg_ac_d)', opt_ac_d-marg_ac_d
#gradient = beta + lam*(opt_ac_d-marg_ac_d)
gradient = lam*(opt_ac_d-marg_ac_d)
if count <10:
beta_p = beta - (alpha)*gradient
else:
beta_p = beta - (alpha/(count+1))*gradient
print 'gradient:%.2f and beta_dif:%.2f' %(gradient, beta-beta_p)
count += 1
print 'old beta: %.2f ||| new beta: %.2f' %(beta, beta_p)
score = self.opt_path_match(opt_path, marg_path)
beta_seq.append(beta_p)
match_score.append(score)
print '--------------------'
print 'Find beta via IRL done, time %.2f' %(time.time()-t0)
print 'In total **%d** para_dijkstra run ||| beta sequence: %s' %(count, str(beta_seq))
print 'Opt_path length: %d, match score sequence: %s' %(len(opt_path), str(match_score))
print '--------------------'
if beta <0:
beta = 0
self.reset_beta(beta)
self.optimal(style='ready')
opt_suffix = list(self.run.suffix)
self.set_to_suffix()
print 'opt_suffix updated to %s' %str(opt_suffix)
print '-----------------'
return beta_seq, match_score
def irl(self, ts_path, reachable_prod_states):
print '------------------------------'
print 'Find beta via IRL starts'
t0 = time.time()
opt_path = self.find_opt_path(ts_path, reachable_prod_states)
opt_cost = self.compute_path_cost(opt_path)
opt_ac_d = opt_cost[1]
beta_seq = []
beta = 100.0
beta_p = 0.0
count = 0
lam = 1.0
alpha = 1.0
match_score = []
count = 0
while ((abs(beta_p-beta)>0.3) or (count <20)):
print 'Iteration --%d--'%count
beta = beta_p
marg_path = self.margin_opt_path(opt_path, beta)
marg_cost = self.compute_path_cost(marg_path)
marg_ac_d = marg_cost[1]
print '(opt_ac_d-marg_ac_d)', opt_ac_d-marg_ac_d
#gradient = beta + lam*(opt_ac_d-marg_ac_d)
gradient = lam*(opt_ac_d-marg_ac_d)
beta_p = beta - (alpha/(count+1))*gradient
print 'gradient:%.2f and beta_dif:%.2f' %(gradient, beta-beta_p)
count += 1
print 'old beta: %.2f ||| new beta: %.2f' %(beta, beta_p)
score = self.opt_path_match(opt_path, marg_path)
beta_seq.append(beta_p)
match_score.append(score)
print '--------------------'
print 'Find beta via IRL done, time %.2f' %(time.time()-t0)
print 'In total **%d** para_dijkstra run ||| beta sequence: %s' %(count, str(beta_seq))
print 'Opt_path length: %d, match score sequence: %s' %(len(opt_path), str(match_score))
print '--------------------'
self.reset_beta(beta)
self.optimal(style='ready')
opt_suffix = list(self.run.suffix)
self.set_to_suffix()
print 'opt_suffix updated to %s' %str(opt_suffix)
print '-----------------'
return beta_seq, match_score
def add_temp_task(self, temp_task):
reg_s = (temp_task[0],temp_task[1])
reg_g = (temp_task[2],temp_task[3])
t_sg = temp_task[4]
best_line, best_precost = add_temp_task(self.product, self.run, self.index, self.segment, reg_s, reg_g, t_sg)
self.run.update_line(best_line, best_precost)
print 'Temporary task Incorporated in plan! new_pre_plan:%s' %str(self.run.pre_plan)
self.index = 0
self.segment = 'line'
self.next_move = self.run.pre_plan[self.index]
print 'Index reset and start new_line execution'
class ltl_planner(object):
def __init__(self, ts, hard_spec, soft_spec):
buchi = mission_to_buchi(hard_spec, soft_spec)
self.product = ProdAut(ts, buchi)
self.Time = 0
self.cur_pose = None
self.trace = [] # record the regions been visited
self.traj = [] # record the full trajectory
self.opt_log = []
# record [(time, prefix, suffix, prefix_cost, suffix_cost, total_cost)]
self.com_log = []
# record [(time, no_messages)]
def optimal(self, beta=10, style='static'):
self.beta = beta
if style == 'static':
# full graph construction
self.product.graph['ts'].build_full()
self.product.build_full()
self.run, plantime = dijkstra_plan_networkX(
self.product, self.beta)
elif style == 'ready':
self.product.build_full()
self.run, plantime = dijkstra_plan_networkX(
self.product, self.beta)
elif style == 'on-the-fly':
# on-the-fly construction
self.product.build_initial()
self.product.build_accept()
self.run, plantime = dijkstra_plan_optimal(self.product, self.beta)
print 'the plan prefix:\n'
#print [[n, self.product.graph['ts'].graph['region'].node[n]['label']] for n in self.run.pre_plan]
print[n for n in self.run.pre_plan]
print '\n'
print 'the plan suffix:\n'
#print [[n, self.product.graph['ts'].graph['region'].node[n]['label']] for n in self.run.suf_plan]
print[n for n in self.run.suf_plan]
print '\n'
self.opt_log.append(
(self.Time, self.run.pre_plan, self.run.suf_plan, self.run.precost,
self.run.sufcost, self.run.totalcost))
self.last_time = self.Time
self.acc_change = 0
self.index = 1
self.segment = 'line'
self.next_move = self.run.pre_plan[self.index]
self.next_state = self.run.line[self.index]
return plantime
def find_next_move(self):
if self.segment == 'line' and self.index < len(self.run.pre_plan) - 1:
self.index += 1
self.next_move = self.run.pre_plan[self.index]
return self.run.line[self.index - 1]
elif self.segment == 'line' and self.index == len(
self.run.pre_plan) - 1:
self.index = 0
self.segment = 'loop'
self.next_move = self.run.suf_plan[self.index]
return self.run.loop[self.index - 1]
elif self.segment == 'loop' and self.index < len(
self.run.suf_plan) - 1:
self.index += 1
self.next_move = self.run.suf_plan[self.index]
return self.run.loop[self.index - 1]
elif self.segment == 'loop' and self.index == len(
self.run.suf_plan) - 1:
self.index = 0
self.segment = 'loop'
self.next_move = self.run.suf_plan[self.index]
return self.run.loop[self.index - 1]
def update(self, object_name, region_label):
MotionFts = self.product.graph['ts'].graph['region']
for ts_node in MotionFts.nodes_iter():
if object_name in MotionFts.node[ts_node]['label']:
MotionFts.node[ts_node]['label'].remove(object_name)
if region_label in MotionFts.node[ts_node]['label']:
MotionFts.node[ts_node]['label'].add(object_name)
def replan(self):
self.run = improve_plan_given_history(self.product, self.trace)
self.index = 0
self.segment = 'line'
self.next_move = self.run.pre_plan[self.index]
def replan_simple(self):
self.product.graph['ts'].graph['region'].set_initial(self.pose)
self.product.graph['ts'].build_full()
self.product.build_full()
self.optimal(10)