def init_planner(self): logger.info("adding atomic operators to pyhop") self.init_state() # note: can call declare_operators multiple times for current situation hop.declare_operators(self.move_forward, self.turn_left, self.turn_right, self.break_block) hop.print_operators(hop.get_operators()) # for the main task hop.declare_methods('get_resource', self.get_resource) # for each sub-task of the main task hop.declare_methods('find_route', self.find_route_to_resource) hop.declare_methods('navigate', self.navigate_to_resource) hop.declare_methods('acquire', self.acquire_resource) hop.print_methods(hop.get_methods())
def init_planner(self): logger.info("adding atomic operators to pyhop") self.init_state() # note: can call declare_operators multiple times for current situation hop.declare_operators( self.move_forward, self.turn_left, self.turn_right, self.break_block, self.search_placeholder, ) # self.equip_agent) hop.print_operators(hop.get_operators()) # for the main task hop.declare_methods('get_resource', self.get_resource) # for each sub-task of the main task hop.declare_methods('search_for_gold', self.search_for_gold) hop.declare_methods('move_closer', self.move_closer) hop.declare_methods('acquire_gold', self.acquire_resource) # hop.declare_methods('break_wall', self.break_wall) hop.print_methods(hop.get_methods())
def place(state, r, o, x): # print(state.objects['tray']) # pick_from_tray(state, r, o) if (state.loc[o] == r and state.gripperfree == False and state.loc[r] == x and state.cupboardisopen == True): state.gripperfree = True state.loc[o] = x state.objects['cupboard'] += 1 else: return False return state #initial state hop.declare_operators(look_cupboard, open, look_table, goto, pick, place) print('') hop.print_operators(hop.get_operators()) #x and y are locations, o is object and r is robot def task1(state, r, o, x, y): tasklist = [('look_cupboard', r), ('goto', r, '', x), ('open', r, x), ('look_table', r), ('goto', r, x, y)] X1 = [('pick', r, o, y), ('goto', r, y, x), ('place', r, o, x), ('goto', r, x, y)] for k in range(5): for j in range(len(X1)): tasklist.append(X1[j]) return tasklist
and state.loc[r] == x and state.cupboardisopen == True): state.gripperfree = True state.loc[o] = x state.objects[x] += 1 state.objectsLocation[key_objects[state.i]] = x else: return False objs[key_objects[state.i]] = x state.i += 1 # state.loc['objects'] = 'cupboard' state.isTable = False return state #initial state hop.declare_operators(look_cupboard, open, look_table, goto, pick, place, perceiveObjects) print('') hop.print_operators(hop.get_operators()) #x and y are locations, o is object and r is robot def task1(state, r, o, x, y, i): tasklist = [('look_cupboard', r), ('goto', r, '', x), ('open', r, x), ('look_table', r), ('goto', r, x, y), ('perceiveObjects', y)] X1 = [('pick', r, o, y), ('goto', r, y, x), ('place', r, o, x), ('goto', r, x, y)] for k in range(len(state.objectsLocation)): for j in range(len(X1)): tasklist.append(X1[j]) return tasklist
def ride_taxi(state,a,x,y): if state.loc['taxi']==x and state.loc[a]==x: state.loc['taxi'] = y state.loc[a] = y state.owe[a] = taxi_rate(state.dist[x][y]) return state else: return False def pay_driver(state,a): if state.cash[a] >= state.owe[a]: state.cash[a] = state.cash[a] - state.owe[a] state.owe[a] = 0 return state else: return False hop.declare_operators(walk, call_taxi, ride_taxi, pay_driver) print('') hop.print_operators(hop.get_operators()) def travel_by_foot(state,a,x,y): if state.dist[x][y] <= 2: return [('walk',a,x,y)] return False def travel_by_taxi(state,a,x,y): if state.cash[a] >= taxi_rate(state.dist[x][y]): return [('call_taxi',a,x), ('ride_taxi',a,x,y), ('pay_driver',a)] return False hop.declare_methods('travel',travel_by_foot,travel_by_taxi) print('')
return False # state.gripper = True # state.loc[o] = tray # return state def open(state, r, x): if (state.gripper == True and state.cupDoorisopen == False and state.loc[r] == x): state.cupDoorisopen = True return state #initial state hop.declare_operators(open, goto, pick, place) print('') hop.print_operators(hop.get_operators()) def task1(state, r, o, x, y): return [('open', r, x), ('goto', r, x, y), ('pick', r, o, y), ('goto', r, y, x), ('place', r, o, x)] #def task2(state, r,o,x,y): #return [('goto',r, x, y), ('pick',r,o,y), ('place',r,o,x)] hop.declare_methods('task1', task1) print('') hop.print_methods(hop.get_methods())
def runRobot(): """Runs the robot Initializes all the variables needed for the application, calls for a plan, makes the behaviour tree and runs it. """ domain = parser.parse(sys.argv[1], sys.argv[2]) hop.declare_operators(*(domain.taskList)) for k in domain.methodList.keys(): hop.declare_methods(k, domain.methodList[k]) #Calculates the plan for the given state and goal plan = hop.plan(copy.deepcopy(domain.state),domain.getGoals(), hop.get_operators(),hop.get_methods(),verbose=0) if plan != None: print('PLAN =',plan,'\n') #Launchs the ros node, and set it the shutdown function rospy.init_node("planner_node", anonymous=False) rospy.on_shutdown(shutdown) #Sets the Global variables and the user defined robot configuration global_vars.init() getConfig() #Initializes the simulator init_environment() initBattery = rospy.ServiceProxy("battery_simulator/set_battery_level", SetBatteryLevel) initBattery(100) #Computes the behaviour tree global_vars.black_board.setWorld(copy.deepcopy(domain.state)) Tree = makeTree(plan, domain.state) print_tree(Tree) #Runs the tree while not rospy.is_shutdown() and global_vars.black_board.finished() == False: while not rospy.is_shutdown() and global_vars.black_board.finished() == False and global_vars.black_board.rePlanNeeded() == False: Tree.run() rospy.sleep(0.1) if global_vars.black_board.rePlanNeeded() == True: print ("Replanning...") global_vars.move_base.cancel_all_goals() global_vars.cmd_vel_pub.publish(Twist()) plan = hop.plan(copy.deepcopy(global_vars.black_board.getWorld()), domain.getGoals(),hop.get_operators(),hop.get_methods(),verbose=0) if plan != None: print('** New plan =',plan,'\n') Tree = makeTree(plan, copy.deepcopy(global_vars.black_board.getWorld())) print_tree(Tree) else: print("Empty plan generated") raise ValueError("Empty plan generated") else: print("Empty plan generated") raise ValueError("Empty plan generated")
state.pos[b] = 'hand' state.clear[b] = False state.holding = b state.clear[c] = True return state else: return False def putdown(state,b): if state.pos[b] == 'hand': state.pos[b] = 'table' state.clear[b] = True state.holding = False return state else: return False def stack(state,b,c): if state.pos[b] == 'hand' and state.clear[c] == True: state.pos[b] = c state.clear[b] = True state.holding = False state.clear[c] = False return state else: return False """ Below, 'declare_operators(pickup, unstack, putdown, stack)' tells Pyhop what the operators are. Note that the operator names are *not* quoted. """ hop.declare_operators(pickup, unstack, putdown, stack)
if __name__ == '__main__': if len(sys.argv) < 3: print( "Usage: rosrun pfg_utils domain_checker.py domain.pddl problem.pddl" ) else: domPddl = sys.argv[1] probPddl = sys.argv[2] print "\nParsing domain: " + domPddl print "Parsing problem: " + probPddl domain = parser.parse(domPddl, probPddl) print "\nINITIAL STATE: \n" domain.printState() print "\n" hop.declare_operators(*(domain.taskList)) hop.print_operators(hop.get_operators()) for k in domain.methodList.keys(): hop.declare_methods(k, domain.methodList[k]) hop.print_methods(hop.get_methods()) print "\n" plan = hop.plan(domain.state, domain.getGoals(), hop.get_operators(), hop.get_methods(), verbose=2)