daily pastebin goal
47%
SHARE
TWEET

Untitled

a guest May 17th, 2018 99 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. from PlanParser import PlanParser
  2. from pddlsim.local_simulator import LocalSimulator
  3. from pddlsim.executors.executor import Executor
  4. import math
  5. from Plan import *
  6. import Queue
  7.  
  8. goal_position = (1,5)
  9.  
  10. class Calculator(object):
  11.  
  12.     current_lift = None
  13.     next_lift=None
  14.  
  15.     @classmethod
  16.     def distance(cls, tile1, tile2):
  17.         '''
  18.         :param tile1: src
  19.         :param tile2: dst
  20.         :return: distance between src to dest
  21.         '''
  22.         return math.hypot(int(tile2.x)-int(tile1.x),int(tile2.y)-int(tile1.y))
  23.  
  24.  
  25.  
  26.     @classmethod
  27.     def choose_neighbor(cls, src, neighbors, dst):
  28.         if src.label == dst.label:
  29.             return None
  30.         min_neighbor = neighbors[0]
  31.         min_distance=Calculator.distance(min_neighbor, dst)
  32.         for n in neighbors:
  33.             temp=Calculator.distance(n, dst)
  34.             if temp<min_distance:
  35.                 min_distance=temp;
  36.                 min_neighbor=n
  37.         return min_neighbor
  38.  
  39.  
  40.  
  41.  
  42.  
  43.  
  44.  
  45.  
  46. class Actions(object):
  47.     @classmethod
  48.     def move(cls,from_location,target):
  49.         s='(move ' + from_location.label + ' ' + target.label + ')'
  50.         print('current action: '+s)
  51.         return s
  52.  
  53.     @classmethod
  54.     def kick(cls,ball,from_location,target):
  55.         s='(kick '+ball+' '+from_location.label+' '+target.label+')'
  56.         print('current action: ' + s)
  57.         return s
  58.  
  59.     @classmethod
  60.     def kick_with_leg(cls,leg, ball, from_location, target):
  61.         #(kick - right ball2 g3 g2 right)
  62.         s='(kick-' + leg + ' ' + ball + ' ' + from_location.label + ' ' + target.label +' '+leg+ ')'
  63.         print('current action: ' + s)
  64.         return s
  65.  
  66.  
  67.     @classmethod
  68.     def lift(cls, foot):
  69.         s='(lift-' + foot + ' ' + foot + ')'
  70.         print('current action: ' + s)
  71.         return s
  72.  
  73.  
  74.     @classmethod
  75.     def move_with_leg(cls,leg,from_location,target):
  76.         s= '(move-'+leg+'-leg'+' '+from_location.label+' '+target.label+ ')'
  77.         print('current action: ' + s)
  78.         return s
  79.  
  80.  
  81.  
  82.  
  83.  
  84.  
  85.  
  86. class tile(object):
  87.     def __init__(self,x,y):
  88.         self.x=x
  89.         self.y=y
  90.         self.label=None
  91.         self.ball_name = None
  92.         self.dist_from_goal = math.hypot(int(goal_position[0])-x,int(goal_position[1])-y)
  93.         self.neighbors = None
  94.  
  95.     def set_label(self,label):
  96.         print("setting label to: "+label)
  97.         self.label=label
  98.  
  99.     def set_neighbors(self,neighbors):
  100.         self.neighbors = neighbors
  101.  
  102.     def set_ball(self,ball_name):
  103.         self.ball_name=ball_name
  104.  
  105. class PlanDispatcher(Executor):
  106.     counter=0
  107.     def __init__(self):
  108.         self.did_lift=False
  109.         super(PlanDispatcher,self).__init__()
  110.         self.steps=[]
  111.         #path = 'simple_football_plan.xml'
  112.         path = 'complex_football_plan.xml'
  113.         temp_hierarchy=PlanParser(path).getPlan()
  114.         self.hierarchy_root=None
  115.         for node in temp_hierarchy.nodes:
  116.             print(node.action)
  117.             #assuming only one root has not action
  118.             if node.nodeName == 'score_goal':
  119.                 self.hierarchy_root=node
  120.  
  121.         print('got hierarchy')
  122.         self.current_ball=None
  123.         self.robot_tile=None
  124.  
  125.     def print_neighbors_and_grid(self):
  126.         print("----------------------------------------------------")
  127.         print("PRINTING NEIGHBORS FOR TEST")
  128.         print()
  129.         for i in range(3):
  130.             for j in range(6):
  131.                 item = self.grid[i][j]
  132.                 n = item.neighbors
  133.                 print("printing for ({0},{1},{2})".format(item.label, item.x, item.y))
  134.                 if not n is None:
  135.                     print([(a.label, a.x, a.y) for a in n])
  136.                 else:
  137.                     print("no neighbors, i'm x")
  138.  
  139.  
  140.  
  141.         print("----------------------------------------------------")
  142.         print("grid is:\n")
  143.         for raw in self.grid:
  144.             print([(j.label, j.dist_from_goal) for j in raw])
  145.  
  146.     def initialize_grid_and_dicts(self):
  147.         self.label_to_coor = dict()
  148.         self.label_to_tile = dict()
  149.         # self.ball_to_coor = dict()
  150.         self.grid = [[0 for x in range(6)] for x in range(3)]
  151.         for i in range(3):
  152.             for j in range(6):
  153.                 self.grid[i][j] = tile(i, j)
  154.  
  155.         self.grid[0][0].set_label("x")
  156.         self.grid[1][0].set_label("x")
  157.         self.grid[1][0].set_label("start_tile")
  158.         self.label_to_coor["start_tile"] = (1, 0)
  159.         self.label_to_tile["start_tile"] = self.grid[1][0]
  160.         self.grid[1][5].set_label("goal_tile")
  161.         self.label_to_coor["goal_tile"] = (1, 5)
  162.         self.label_to_tile["goal_tile"] = self.grid[1][5]
  163.         self.grid[0][1].set_label("g0")
  164.         self.label_to_coor["g0"] = (0, 1)
  165.         self.label_to_tile["g0"] = self.grid[0][1]
  166.         self.grid[0][2].set_label("g1")
  167.         self.label_to_coor["g1"] = (0, 2)
  168.         self.label_to_tile["g1"] = self.grid[0][2]
  169.         self.grid[0][3].set_label("g2")
  170.         self.label_to_coor["g2"] = (0, 3)
  171.         self.label_to_tile["g2"] = self.grid[0][3]
  172.         self.grid[0][4].set_label("g3")
  173.         self.label_to_coor["g3"] = (0, 4)
  174.         self.label_to_tile["g3"] = self.grid[0][4]
  175.         self.grid[0][5].set_label("x")
  176.         self.grid[1][1].set_label("c0")
  177.         self.label_to_coor["c0"] = (1, 1)
  178.         self.label_to_tile["c0"] = self.grid[1][1]
  179.         self.grid[1][2].set_label("c1")
  180.         self.label_to_coor["c1"] = (1, 2)
  181.         self.label_to_tile["c1"] = self.grid[1][2]
  182.         self.grid[1][3].set_label("c2")
  183.         self.label_to_coor["c2"] = (1, 3)
  184.         self.label_to_tile["c2"] = self.grid[1][3]
  185.         self.grid[1][4].set_label("c3")
  186.         self.label_to_coor["c3"] = (1, 4)
  187.         self.label_to_tile["c3"] = self.grid[1][4]
  188.         self.grid[2][0].set_label("x")
  189.         self.grid[2][1].set_label("d0")
  190.         self.label_to_coor["d0"] = (2, 1)
  191.         self.label_to_tile["d0"] = self.grid[2][1]
  192.         self.grid[2][2].set_label("d1")
  193.         self.label_to_coor["d1"] = (2, 2)
  194.         self.label_to_tile["d1"] = self.grid[2][2]
  195.         self.grid[2][3].set_label("d2")
  196.         self.label_to_coor["d2"] = (2, 3)
  197.         self.label_to_tile["d2"] = self.grid[2][3]
  198.         self.grid[2][4].set_label("d3")
  199.         self.label_to_coor["d3"] = (2, 4)
  200.         self.label_to_tile["d3"] = self.grid[2][4]
  201.         self.grid[2][5].set_label("x")
  202.         #start tile
  203.         self.grid[1][0].set_neighbors([self.grid[1][1]])
  204.         #g0 [0,1]
  205.         self.grid[0][1].set_neighbors([self.grid[0][2],self.grid[1][1]])
  206.         # c0 [1,1]
  207.         self.grid[1][1].set_neighbors([self.grid[1][0], self.grid[0][1],self.grid[1][2],self.grid[2][1]])
  208.         # d0 [2,1]
  209.         self.grid[2][1].set_neighbors([self.grid[1][1], self.grid[2][2]])
  210.         # g1 [0,2]
  211.         self.grid[0][2].set_neighbors([self.grid[0][1], self.grid[1][2],self.grid[0][3]])
  212.         # c1 [1,2]
  213.         self.grid[1][2].set_neighbors([self.grid[1][1], self.grid[0][2], self.grid[2][2],self.grid[1][3]])
  214.         # d1 [2,2]
  215.         self.grid[2][2].set_neighbors([self.grid[2][1], self.grid[1][2], self.grid[2][3]])
  216.         # g2 [0,3]
  217.         self.grid[0][3].set_neighbors([self.grid[0][2], self.grid[1][3], self.grid[0][4]])
  218.         # c2 [1,3]
  219.         self.grid[1][3].set_neighbors([self.grid[1][2], self.grid[0][3], self.grid[1][4],self.grid[2][3]])
  220.         # d2 [2,3]
  221.         self.grid[2][3].set_neighbors([self.grid[2][2], self.grid[1][3], self.grid[2][4]])
  222.         # g3 [0,4]
  223.         self.grid[0][4].set_neighbors([self.grid[0][3], self.grid[1][4]])
  224.         # c3 [1,4]
  225.         self.grid[1][4].set_neighbors([self.grid[1][3], self.grid[0][4],self.grid[1][5],self.grid[2][4]])
  226.         # d3 [2,4]
  227.         self.grid[2][4].set_neighbors([self.grid[1][4], self.grid[2][3]])
  228.         # goal tile [1,5]
  229.         self.grid[1][5].set_neighbors([self.grid[1][4]])
  230.         #self.print_neighbors_and_grid()
  231.  
  232.  
  233.         print("----------------------------------------------------")
  234.         print("PRINTING (label,ball) in grid:")
  235.         for raw in self.grid:
  236.             print([(j.label, j.ball_name) for j in raw])
  237.         print("----------------------------------------------------")
  238.         #sorting tiles to get minimum n balls (closest to goal)
  239.  
  240.  
  241.  
  242.         # remove items from the head of the queue
  243.         #while not self.balls_queue.empty():
  244.         #    print self.balls_queue.get()
  245.  
  246.  
  247.  
  248.     def initialize(self,services):
  249.         self.services = services
  250.         state = self.services.perception.get_state()
  251.         # for key, val in state.items():
  252.         #    print (key, val)
  253.         balls = dict()
  254.         for ball in state['at-ball']:
  255.             balls[ball[0]] = ball[1] #ball[0]=name , ball[1]=label
  256.  
  257.         self.initialize_grid_and_dicts()
  258.         state = self.services.perception.get_state()
  259.         self.robot_tile = self.label_to_tile[state['at-robby'].pop()[0]]
  260.         self.new_state = dict(robby=self.robot_tile, balls=balls)
  261.         for key, val in self.new_state.items():
  262.             print (key, val)
  263.         for ball in self.new_state['balls'].keys():
  264.             x = self.label_to_coor[self.new_state['balls'][ball]][0]
  265.             y = self.label_to_coor[self.new_state['balls'][ball]][1]
  266.             self.grid[x][y].set_ball(ball)
  267.         tiles = list(set().union(*self.grid))
  268.         tiles.sort(key=lambda x: x.dist_from_goal, reverse=False)
  269.         all_balls=[x for x in tiles if not x.ball_name==None]
  270.         target_balls=all_balls[:2] #TODO: n from args instead of 2
  271.         self.balls_queue = Queue.Queue()
  272.         # put items at the end of the queue
  273.         for ball in target_balls:
  274.             self.balls_queue.put(ball.ball_name)
  275.         plan_path = "simple_football_plan.xml"
  276.         plan = PlanParser(plan_path).getPlan()
  277.         print("----------------------------------------------------")
  278.         print("PRINTING (ball,dist from goal) in grid sorted in ascending order:")
  279.         print([(j.label, j.dist_from_goal) for j in tiles])
  280.         print("----------------------------------------------------")
  281.         #va=self.services.valid_actions.get()
  282.         self.steps=services.planner(
  283.             services.pddl.domain_path, services.pddl.problem_path)
  284.         print('STEPS: '+str(self.steps))
  285.  
  286.  
  287.  
  288.     def choose(self,target_ball_tile):
  289.         where_to_go = Calculator.choose_neighbor(self.robot_tile,self.robot_tile.neighbors, target_ball_tile)
  290.         if where_to_go is None:
  291.             #robot is on same tile with ball, should kick
  292.             where_to_kick=Calculator.choose_neighbor(target_ball_tile,target_ball_tile.neighbors,self.label_to_tile['goal_tile'])
  293.             action=Actions.kick(self.current_ball,self.robot_tile,where_to_kick)
  294.             if where_to_kick.label == 'goal_tile':
  295.                 print("current ball is now None")
  296.                 self.current_ball=None
  297.         else:
  298.             action = Actions.move(self.robot_tile, where_to_go)
  299.         return action
  300.  
  301.     def choose_complex(self,target_ball_tile):
  302.         if Calculator.current_lift is None:
  303.             # first movment
  304.             from random import randint
  305.             r=randint(0, 1)
  306.             Calculator.current_lift = self.hierarchy_root.hierarchicalChildren[r]
  307.             Calculator.next_lift = self.hierarchy_root.hierarchicalChildren[1-r]
  308.             self.did_lift = True
  309.             return Actions.lift(Calculator.current_lift.action.split('-')[1])
  310.         else:
  311.             if self.did_lift:
  312.             # choose wether to kick or move
  313.                 where_to_go = Calculator.choose_neighbor(self.robot_tile, self.robot_tile.neighbors, target_ball_tile)
  314.                 if where_to_go is None:
  315.                     #robot is on same tile with ball, should kick
  316.                     where_to_kick = Calculator.choose_neighbor(target_ball_tile, target_ball_tile.neighbors,
  317.                                                            self.label_to_tile['goal_tile'])
  318.                     temp=None
  319.                     for node in Calculator.current_lift.hierarchicalChildren:
  320.                         if node.action.split('-')[0]=='kick':
  321.                             temp=node
  322.                     current_sub_lift=temp
  323.                     #do action
  324.                     action=Actions.kick_with_leg(current_sub_lift.action.split('-')[1], self.current_ball, self.robot_tile, where_to_kick)
  325.                     if where_to_kick.label == 'goal_tile':
  326.                         print("current ball is now None")
  327.                         self.current_ball = None
  328.                 else:
  329.                     #now moving
  330.                     temp = None
  331.                     for node in Calculator.current_lift.hierarchicalChildren:
  332.                         if node.action.split('-')[0] == 'move':
  333.                             temp = node
  334.                     current_sub_lift = temp
  335.                     action = Actions.move_with_leg(current_sub_lift.action.split('-')[1], self.robot_tile, where_to_go)
  336.  
  337.                 #finish, and return to lift, change leg
  338.                 temp = Calculator.current_lift
  339.                 Calculator.current_lift=Calculator.next_lift
  340.                 Calculator.next_lift=temp
  341.                 self.did_lift = False
  342.                 return action
  343.             else:
  344.                 self.did_lift = True
  345.                 return Actions.lift(Calculator.current_lift.action.split('-')[1])
  346.  
  347.  
  348.     def do_action(self):
  349.         if self.current_ball is None:
  350.             self.current_ball=self.balls_queue.get()
  351.         print("going to put {0} in goal".format(self.current_ball))
  352.         state = self.services.perception.get_state()
  353.         for ball in state['at-ball']:
  354.             if ball[0] == self.current_ball:
  355.                 target_ball_tile =self.label_to_tile[ball[1]] #ball[0]=name , ball[1]=label
  356.  
  357.         #choose condition
  358.         #return self.choose(target_ball_tile)
  359.         return self.choose_complex(target_ball_tile)
  360.  
  361.  
  362.     def next_action(self):
  363.         #get robot and ball location
  364.         state = self.services.perception.get_state()
  365.         self.robot_tile = self.label_to_tile[state['at-robby'].pop()[0]]
  366.  
  367.         print("Hi, I'm robby, at {0}".format(self.robot_tile.label))
  368.         for ball in state['at-ball']:
  369.                 print("hi, i'm ball:{0} an i'm in {1}".format(ball[0],self.label_to_tile[ball[1]].label))  # ball[0]=name , ball[1]=label
  370.         PlanDispatcher.counter += 1
  371.         print("in next action. valid actions are")
  372.         print(self.services.valid_actions.get())
  373.         # TODO: get state, match valid actions to bot and balls positions
  374.         #  get possible actions, pick one, do stuff, go home :D
  375.  
  376.         #self.test_stuff()
  377.         '''with open('state'+str(PlanDispatcher.counter)+'.log', 'w') as log:
  378.             log.write('state: {}\n'.format(self.services.perception.get_state()))
  379.             log.write('pre conditions: {}\n'.format(self.hierarchy.nodes[1].preConds))
  380.             log.write('valid_actions: {}\n'.format(self.services.valid_actions.get()))'''
  381.  
  382.         va = self.services.valid_actions.get()
  383.         #return self.services.valid_actions.get()[0]'''
  384.         #if len(self.steps)>0:
  385.         #   return self.steps.pop(0).lower()
  386.  
  387.         if self.balls_queue.empty() and self.current_ball is None:
  388.             return None
  389.         else:
  390.             return self.do_action()
  391.  
  392.  
  393. #domain_path,problem_path='simple_football_domain.pddl',\
  394. #                        'simple_football_problem.pddl'
  395. domain_path,problem_path='complex_football_domain.pddl',\
  396.                         'complex_football_problem.pddl'
  397. print(LocalSimulator(False,).run(
  398.     domain_path,problem_path,PlanDispatcher()))
RAW Paste Data
Top