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, alpha): buchi = mission_to_buchi(hard_spec, soft_spec) self.product = ProdAut(ts, buchi, alpha) 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 reset_alpha(self, alpha): self.product.graph['alpha'] = alpha def margin_optimal(self, opt_path, style='ready'): self.product.build_full_margin(opt_path) self.run, plantime = dijkstra_plan_networkX(self.product, self.beta) 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) elif styple == 'repeat': self.run, plantime = dijkstra_plan_networkX( 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.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!'