• API
• FAQ
• Tools
• Archive
SHARE
TWEET

# Untitled

a guest May 17th, 2018 104 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.
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
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.

Top