def get_spatial_finish_blocks(self, cur_sit, cl_lv, size, ag_name): sit = deepcopy(self.task.goal_state) ag_pose = sit['objects'][self.name] sit['objects'] = deepcopy(cur_sit['objects']) sit['objects'][self.name] = ag_pose region_map, cell_map, cell_location, near_loc, cell_coords, size, cl_lv = signs_markup( sit, self.task.additions[3], ag_name, size=size, cl_lv=cl_lv) agent_state_action = state_prediction(self.task.signs[ag_name], sit, self.task.signs) conditions = sit['conditions'] sit['conditions'] = conditions action_situation = define_situation('*goal-sit*-' + ag_name, cell_map, conditions, agent_state_action, self.task.signs) action_map = define_map('*goal-map-*-' + ag_name, region_map, cell_location, near_loc, self.task.additions[1], self.task.signs) state_fixation(action_situation, cell_coords, self.task.signs, 'cell') return action_situation, action_map, cl_lv, sit
def create_situation(description, agent, whose='They', size_prev=None): region_map, cell_map_pddl, cell_location, near_loc, cell_coords, size, cl_lv = ut.signs_markup( description, self.additions[3], agent.name, size=size_prev) if cl_lv < description['cl_lv'] and not size_prev: #it can be only iff actions's cl_lv more than after markup agcx = description['objects'][agent.name]['x'] agcy = description['objects'][agent.name]['y'] while cl_lv != description['cl_lv']: size_x = (size[2] - size[0]) // 6 size_y = (size[3] - size[1]) // 6 size = [ agcx - size_x, agcy - size_y, agcx + size_x, agcy + size_y ] cl_lv += 1 region_map, cell_map_pddl, cell_location, near_loc, cell_coords, size, cl_lv = ut.signs_markup( description, self.additions[3], agent.name, size=size) agent_state_action = ut.state_prediction(agent, description, self.signs) conditions_new = description['conditions'] global SIT_COUNTER SIT_COUNTER += 1 act_sit = ut.define_situation( 'situation_' + whose + '_' + str(SIT_COUNTER), cell_map_pddl, conditions_new, agent_state_action, self.signs) act_map = ut.define_map('map_' + whose + '_' + str(SIT_COUNTER), region_map, cell_location, near_loc, self.additions[1], self.signs) ut.state_fixation(act_sit, cell_coords, self.signs, 'cell') return act_sit, act_map, size
def change_start(self, map, act): if map != self.task.additions[0][max(self.task.additions[0])]: place = self.task.additions[0][max( self.task.additions[0])]['objects'][self.name] self.task.additions[0][max(self.task.additions[0])] = map self.task.additions[0][max( self.task.additions[0])]['objects'][self.name] = place region_map, cell_map_pddl, cell_location, near_loc, cell_coords, size, cl_lv = signs_markup( self.task.additions[0][max(self.task.additions[0])], self.task.additions[3], self.name) agent_state_action = state_prediction( self.task.signs[self.name], self.task.additions[0][max(self.task.additions[0])], self.task.signs) active_situation = define_situation( '*start-sit-*-' + act + '-' + self.name, cell_map_pddl, self.task.additions[0][max( self.task.additions[0])]['conditions'], agent_state_action, self.task.signs) active_map = define_map('*start-map-*-' + act + '-' + self.name, region_map, cell_location, near_loc, self.task.additions[1], self.task.signs) state_fixation(active_situation, cell_coords, self.task.signs, 'cell') I_img = self.task.signs['I'].add_image() active_situation.replace('image', self.task.signs[self.name], I_img) self.task.start_situation = active_situation.sign self.task.start_map = active_map.sign
def get_spatial_sit_blocks(self, action, cur_sit, prev_size, prev_cl): if action[3].name == 'I': ag_name = self.name else: ag_name = action[3].name new_sit = deepcopy(cur_sit) ag_pos = new_sit['objects'][ag_name] ag_activity = new_sit[ag_name]['activity'] ground_block = None clear_block = None move_block = None # we need to know coords of block in effect for el in action[2].effect: el_signs = el.get_signs() if (action[1] == 'pick-up' or action[1] == 'unstack')\ and 'holding' in [sign.name for sign in el_signs]: move_block = [ sign for sign in el_signs if sign.name != 'holding' and sign.name != ag_name ][0] break elif (action[1] == 'stack' or action[1] == 'put-down') and 'clear' in [ sign.name for sign in el_signs ]: clear_block = [ sign for sign in el_signs if sign.name != 'clear' ][0] break if action[1] == 'stack': for el in action[2].cause: el_signs = el.get_signs() if 'clear' in [sign.name for sign in el_signs]: ground_block = [ sign for sign in el_signs if sign.name != 'clear' ][0] break if action[1] == 'unstack': for el in action[2].effect: el_signs = el.get_signs() if 'clear' in [sign.name for sign in el_signs]: ground_block = [ sign for sign in el_signs if sign.name != 'clear' ][0] break if action[1] == 'put-down' or action[1] == 'stack': obj = clear_block else: obj = move_block def get_pos(ag_pos, obj_pos): """ Thus function was implemented to foresee the agent's position after it will manipulate with object. """ if ag_pos['x'] < obj_pos['x']: if ag_pos['y'] < obj_pos['y'] - ag_pos['r']: x = obj_pos['x'] - obj_pos['r'] - 4 * ag_pos['r'] y = obj_pos['y'] - obj_pos['r'] - 4 * ag_pos['r'] orient = 'below-right' elif ag_pos['y'] > obj_pos['y'] + ag_pos['r']: x = obj_pos['x'] - obj_pos['r'] - 10 * ag_pos['r'] y = obj_pos['y'] + obj_pos['r'] + 10 * ag_pos['r'] orient = 'above-right' else: x = obj_pos['x'] - obj_pos['r'] - 4 * ag_pos['r'] y = obj_pos['y'] orient = 'right' elif ag_pos['x'] > obj_pos['x']: if ag_pos['y'] < obj_pos['y'] - ag_pos['r']: x = obj_pos['x'] + obj_pos['r'] + 4 * ag_pos['r'] y = obj_pos['y'] - obj_pos['r'] - 4 * ag_pos['r'] orient = 'below-left' elif ag_pos['y'] > obj_pos['y'] + ag_pos['r']: x = obj_pos['x'] + obj_pos['r'] + 4 * ag_pos['r'] y = obj_pos['y'] + obj_pos['r'] + 4 * ag_pos['r'] orient = 'above-left' else: x = obj_pos['x'] + obj_pos['r'] + 2 * ag_pos['r'] y = obj_pos['y'] orient = 'left' else: if ag_pos['y'] < obj_pos['y']: x = ag_pos['x'] y = obj_pos['y'] - obj_pos['r'] - 2 * ag_pos['r'] orient = 'below' else: x = ag_pos['x'] y = obj_pos['y'] + obj_pos['r'] + 2 * ag_pos['r'] orient = 'above' return x, y, orient # todo change to set of applicable poses size = None cl_lv = 0 if action[1] == 'pick-up' or action[1] == 'unstack': obj_pos = new_sit['objects'][obj.name] x, y, orient = get_pos(ag_pos, obj_pos) ag_pos['x'] = x ag_pos['y'] = y # check old size, on which agent can perform an action _, _, _, _, _, size, cl_lv = signs_markup(new_sit, self.task.additions[3], ag_name) new_sit['objects'].pop(obj.name) for pred, signif in new_sit[ag_name].copy().items(): if pred == 'orientation': new_sit[ag_name].pop(pred) new_sit[ag_name][pred] = orient elif pred == 'handempty': new_sit[ag_name].pop(pred) new_sit[ag_name]['holding'] = { 'cause': [ag_name, move_block.name], 'effect': [] } elif action[1] == 'put-down': new_sit['objects'][obj.name] = {} obj_pos = new_sit['objects'][obj.name] obj_pos['x'] = ag_pos['x'] + 2 * ag_pos['r'] + ag_activity // 2 obj_pos['y'] = ag_pos['y'] + 2 * ag_pos['r'] + ag_activity // 2 obj_pos['r'] = self.task.additions[0][0]['objects'][obj.name]['r'] elif action[1] == 'stack': ground_coords = new_sit['objects'][ground_block.name] x, y, orient = get_pos(ag_pos, ground_coords) # if we need to move, or stay on prev position if math.sqrt((ag_pos['y'] - y)**2 + (ag_pos['x'] - x)**2) >= ag_pos['r'] * 2: ag_pos['x'] = x ag_pos['y'] = y new_sit['objects'][obj.name] = {} obj_pos = new_sit['objects'][obj.name] obj_pos['x'] = ground_coords['x'] obj_pos['y'] = ground_coords['y'] obj_pos['r'] = self.task.additions[0][0]['objects'][obj.name]['r'] for pred, signif in new_sit[ag_name].copy().items(): if pred == 'orientation': new_sit[ag_name].pop(pred) new_sit[ag_name][pred] = orient elif pred == 'holding': new_sit[ag_name].pop(pred) new_sit[ag_name]['handempty'] = {'cause': [], 'effect': []} region_map, cell_map_pddl, cell_location, near_loc, cell_coords, size, cl_lv = signs_markup( new_sit, self.task.additions[3], ag_name, size=size, cl_lv=cl_lv) agent_state_action = state_prediction(self.task.signs[ag_name], new_sit, self.task.signs) conditions_new = get_conditions(new_sit, action, obj.name, ground_block) new_sit['conditions'] = conditions_new action_situation = define_situation('*goal-sit-*-' + action[1], cell_map_pddl, conditions_new, agent_state_action, self.task.signs) action_map = define_map('*goal-map-*-' + action[1], region_map, cell_location, near_loc, self.task.additions[1], self.task.signs) state_fixation(action_situation, cell_coords, self.task.signs, 'cell') return action_situation, action_map, cl_lv, new_sit, size
def spatial_ground(problem, plagent, agents, exp_signs=None, backward = False): global signs initial_state = problem.initial_state initial_state.update(problem.map) goal_state = problem.goal_state goal_state.update(problem.map) # Prepare states to plagent init_state = {key: value for key, value in deepcopy(initial_state).items() if key != plagent} init_state['I'] = initial_state[plagent] init_state['objects']['I'] = init_state['objects'].pop(plagent) go_state = {key: value for key, value in deepcopy(goal_state).items() if key != plagent} go_state['I'] = goal_state[plagent] go_state['objects']['I'] = go_state['objects'].pop(plagent) agents.remove(plagent) agents.append('I') obj_signifs = {} obj_means = {} # Create agents and communications agent_type = None types = problem.domain['types'] roles = problem.domain['roles'] if exp_signs: signs, types = ut._clear_model(exp_signs, signs, types) signs, roles = ut._clear_model(exp_signs, signs, roles) I_sign = exp_signs['I'] signs['I'] = I_sign They_sign = exp_signs['They'] signs['They'] = They_sign Clarify = exp_signs['Clarify'] signs['Clarify'] = Clarify Abstract = exp_signs['Abstract'] signs['Abstract'] = Abstract signs['situation'] = exp_signs['situation'] else: I_sign = Sign("I") They_sign = Sign("They") obj_means[I_sign] = I_sign.add_meaning() obj_signifs[I_sign] = I_sign.add_significance() signs[I_sign.name] = I_sign obj_means[They_sign] = They_sign.add_meaning() obj_signifs[They_sign] = They_sign.add_significance() signs[They_sign.name] = They_sign Clarify = Sign('Clarify') signs[Clarify.name] = Clarify Abstract = Sign('Abstract') signs[Abstract.name] = Abstract signs['situation'] = Sign('situation') # ground types for type_name, smaller in types.items(): if not type_name in signs and not exp_signs: type_sign = Sign(type_name) signs[type_name] = type_sign else: if exp_signs: type_sign = exp_signs[type_name] else: type_sign = signs[type_name] if smaller: for obj in smaller: if not obj in signs: obj_sign = Sign(obj) signs[obj] = obj_sign else: obj_sign = signs[obj] obj_signif = obj_sign.add_significance() obj_signifs[obj_sign] = obj_signif tp_signif = type_sign.add_significance() connector = tp_signif.add_feature(obj_signif, zero_out=True) obj_sign.add_out_significance(connector) # Assign I to agent if obj_sign.name == plagent: connector = obj_signif.add_feature(obj_signifs[I_sign], zero_out=True) I_sign.add_out_significance(connector) agent_type = type_name else: obj_signifs[type_sign] = type_sign.add_significance() # Assign other agents others = {signs[ag] for ag in agents if ag != 'I'} for subagent in others: if not They_sign in subagent.significances[1].get_signs(): signif = obj_signifs[They_sign] if signif.is_empty(): They_signif = signif else: They_signif = They_sign.add_significance() connector = subagent.significances[1].add_feature(They_signif, zero_out=True) They_sign.add_out_significance(connector) obj_means[subagent] = subagent.add_meaning() # Signify roles for role_name, smaller in roles.items(): role_sign = Sign(role_name) signs[role_name] = role_sign for object in smaller: obj_sign = signs[object] obj_signif = obj_sign.significances[1] role_signif = role_sign.add_significance() connector = role_signif.add_feature(obj_signif, zero_out=True) obj_sign.add_out_significance(connector) if object == agent_type: agent_type = role_name # Find and signify walls if exp_signs: if not 'wall' in signs: signs['wall'] = exp_signs['wall'] ws = signs['wall'] views = [] if ws.images: for num, im in ws.images.items(): if len(im.cause): for view in im.cause[0].coincidences: if view.view: views.append(view.view) if 'wall' in problem.map: for wall in problem.map['wall']: if wall not in views: cimage = ws.add_image() cimage.add_feature(wall, effect=False, view = True) else: logging.warning('There are no walls around! Check your task!!!') sys.exit(1) # Ground predicates and actions if not exp_signs: ut._ground_predicates(problem.domain['predicates'], signs) ut._ground_actions(problem.domain['actions'], obj_means, problem.constraints, signs, agents, agent_type) else: #TODO check if new action or predicate variation appeared in new task for pred in problem.domain['predicates'].copy(): if pred in exp_signs: signs[pred] = exp_signs[pred] problem.domain['predicates'].pop(pred) for act in problem.domain['actions'].copy(): if act in exp_signs: signs[act] = exp_signs[act] problem.domain['actions'].pop(act) ut._ground_predicates(problem.domain['predicates'], signs) ut._ground_actions(problem.domain['actions'], obj_means, problem.constraints, signs, agents, agent_type) #Copy all experience subplans and plans for sname, sign in exp_signs.items(): if sname not in signs: signs[sname] = sign # Define start situation maps = {} if backward: maps[0] = problem.goal_state else: maps[0] = problem.initial_state ms = maps[0].pop('map-size') walls = maps[0].pop('wall') static_map = {'map-size': ms, 'wall': walls} # Create maps and situations for planning agents regions_struct = ut.get_struct() additions = [] additions.extend([maps, regions_struct, static_map]) cells = {} agent_state = {} for agent in agents: region_map, cell_map_start, cell_location, near_loc, cell_coords, size, cl_lv_init = ut.signs_markup(init_state, static_map, agent) agent_state_start = ut.state_prediction(signs[agent], init_state, signs) start_situation = ut.define_situation('*start-sit*-'+agent, cell_map_start, problem.initial_state['conditions'], agent_state_start, signs) start_map = ut.define_map('*start-map*-'+agent, region_map, cell_location, near_loc, regions_struct, signs) ut.state_fixation(start_situation, cell_coords, signs, 'cell') # Define goal situation region_map, cell_map_goal, cell_location, near_loc, cell_coords, size, cl_lv_goal = ut.signs_markup(go_state, static_map, agent) agent_state_finish = ut.state_prediction(signs[agent], go_state, signs) goal_situation = ut.define_situation('*goal-sit*-'+agent, cell_map_goal, problem.goal_state['conditions'], agent_state_finish, signs) goal_map = ut.define_map('*goal-map*-'+agent, region_map, cell_location, near_loc, regions_struct, signs) ut.state_fixation(goal_situation, cell_coords, signs, 'cell') #fixation map map_size = ut.scale(ms) rmap = [0, 0, map_size[0], map_size[1]] region_location, _ = ut.locater('region-', rmap, initial_state['objects'], walls) ut.state_fixation(start_map, region_location, signs, 'region') ut.signify_connection(signs) if agent == 'I': additions[0][0][plagent]['cl_lv_init'] = cl_lv_init additions[0][0][plagent]['cl_lv_goal'] = cl_lv_goal else: additions[0][0][agent]['cl_lv_init'] = cl_lv_init additions[0][0][agent]['cl_lv_goal'] = cl_lv_goal if backward: cell_map_goal['cell-4'] = {plagent} cells[agent] = {0:cell_map_goal} else: cell_map_start['cell-4'] = {plagent} cells[agent] = {0: cell_map_start} agent_state[agent] = {'start-sit':start_situation.sign, 'goal-sit': goal_situation.sign, 'start-map':start_map.sign, 'goal-map': goal_map.sign} additions.insert(2, cells) return SpTask(problem.name, signs, agent_state, additions, problem.initial_state, {key:value for key, value in problem.goal_state.items() if key not in static_map}, static_map, plagent)
def get_spatial_sit_blocks(self, action, cur_sit, prev_size, prev_cl): if action[3].name == 'I': ag_name = self.name else: ag_name = action[3].name new_sit = deepcopy(cur_sit) ag_pos = new_sit['objects'][ag_name] ag_activity = new_sit[ag_name]['activity'] ground_block = None clear_block = None move_block = None # we need to know object's coords in effect for el in action[2].effect: el_signs = el.get_signs() if (action[1] == 'pick-up' or action[1] == 'unstack')\ and 'holding' in [sign.name for sign in el_signs]: move_block = [ sign for sign in el_signs if sign.name != 'holding' and sign.name != ag_name ][0] break elif (action[1] == 'stack' or action[1] == 'put-down') and 'clear' in [ sign.name for sign in el_signs ]: clear_block = [ sign for sign in el_signs if sign.name != 'clear' ][0] break if action[1] == 'stack': for el in action[2].cause: el_signs = el.get_signs() if 'clear' in [sign.name for sign in el_signs]: ground_block = [ sign for sign in el_signs if sign.name != 'clear' ][0] break if action[1] == 'unstack': for el in action[2].effect: el_signs = el.get_signs() if 'clear' in [sign.name for sign in el_signs]: ground_block = [ sign for sign in el_signs if sign.name != 'clear' ][0] break if action[1] == 'put-down' or action[1] == 'stack': obj = clear_block else: obj = move_block def get_pos(ag_pos, obj_pos, borders): """ Thus function was implemented to foresee the agent's position after it will manipulate with object. """ def check_borders(x, y, borders): for border in borders: if (border[0] <= x <= border[2]) and (border[1] <= y <= border[3]): return False return True def get_random_side(obj_pos, ag_pos): """ get the closest path without borders. To make it random - delete path. """ csides = ['ar', 'br', 'r', 'bl', 'al', 'l', 'a', 'b'] path = [] while csides: side = csides[0] x, y, orient = getattr(self, side)(obj_pos, ag_pos) if check_borders(x, y, borders): p = math.sqrt((x - ag_pos['x'])**2 + (y - ag_pos['y'])**2) path.append(((x, y, orient), p)) csides.remove(side) else: csides.remove(side) min_dist = None for place, dist in path: if not min_dist: min_dist = dist if min_dist > dist: min_dist = dist return [place for place, path in path if path == min_dist][0] if ag_pos['x'] < obj_pos['x']: if ag_pos['y'] < obj_pos['y'] - ag_pos['r']: x, y, orient = self.br(obj_pos, ag_pos) if check_borders(x, y, borders): return x, y, orient else: return get_random_side(obj_pos, ag_pos) elif ag_pos['y'] > obj_pos['y'] + ag_pos['r']: x, y, orient = self.ar(obj_pos, ag_pos) if check_borders(x, y, borders): return x, y, orient else: return get_random_side(obj_pos, ag_pos) else: x, y, orient = self.r(obj_pos, ag_pos) if check_borders(x, y, borders): return x, y, orient else: return get_random_side(obj_pos, ag_pos) elif ag_pos['x'] > obj_pos['x']: if ag_pos['y'] < obj_pos['y'] - ag_pos['r']: x, y, orient = self.bl(obj_pos, ag_pos) if check_borders(x, y, borders): return x, y, orient else: return get_random_side(obj_pos, ag_pos) elif ag_pos['y'] > obj_pos['y'] + ag_pos['r']: x, y, orient = self.al(obj_pos, ag_pos) if check_borders(x, y, borders): return x, y, orient else: return get_random_side(obj_pos, ag_pos) else: x, y, orient = self.l(obj_pos, ag_pos) if check_borders(x, y, borders): return x, y, orient else: return get_random_side(obj_pos, ag_pos) else: if ag_pos['y'] < obj_pos['y']: x, y, orient = self.b(obj_pos, ag_pos) if check_borders(x, y, borders): return x, y, orient else: return get_random_side(obj_pos, ag_pos) else: x, y, orient = self.a(obj_pos, ag_pos) if check_borders(x, y, borders): return x, y, orient else: return get_random_side(obj_pos, ag_pos) # todo change to set of applicable poses size = None cl_lv = 0 if action[1] == 'pick-up' or action[1] == 'unstack': obj_pos = new_sit['objects'][obj.name] x, y, orient = get_pos(ag_pos, obj_pos, self.task.additions[3]['wall']) ag_pos['x'] = x ag_pos['y'] = y # check old size, on which agent can perform an action _, _, _, _, _, size, cl_lv = signs_markup(new_sit, self.task.additions[3], ag_name) new_sit['objects'].pop(obj.name) for pred, signif in new_sit[ag_name].copy().items(): if pred == 'orientation': new_sit[ag_name].pop(pred) new_sit[ag_name][pred] = orient elif pred == 'handempty': new_sit[ag_name].pop(pred) new_sit[ag_name]['holding'] = { 'cause': [ag_name, move_block.name], 'effect': [] } elif action[1] == 'put-down': new_sit['objects'][obj.name] = {} obj_pos = new_sit['objects'][obj.name] obj_pos['x'] = ag_pos['x'] + 2 * ag_pos['r'] + ag_activity // 2 obj_pos['y'] = ag_pos['y'] + 2 * ag_pos['r'] + ag_activity // 2 obj_pos['r'] = self.task.additions[0][0]['objects'][obj.name]['r'] elif action[1] == 'stack': ground_coords = new_sit['objects'][ground_block.name] x, y, orient = get_pos(ag_pos, ground_coords, self.task.additions[3]['wall']) # if we need to move, or stay on prev position if math.sqrt((ag_pos['y'] - y)**2 + (ag_pos['x'] - x)**2) >= ag_pos['r'] * 2: ag_pos['x'] = x ag_pos['y'] = y new_sit['objects'][obj.name] = {} obj_pos = new_sit['objects'][obj.name] obj_pos['x'] = ground_coords['x'] obj_pos['y'] = ground_coords['y'] obj_pos['r'] = self.task.additions[0][0]['objects'][obj.name]['r'] for pred, signif in new_sit[ag_name].copy().items(): if pred == 'orientation': new_sit[ag_name].pop(pred) new_sit[ag_name][pred] = orient elif pred == 'holding': new_sit[ag_name].pop(pred) new_sit[ag_name]['handempty'] = {'cause': [], 'effect': []} region_map, cell_map_pddl, cell_location, near_loc, cell_coords, size, cl_lv = signs_markup( new_sit, self.task.additions[3], ag_name, size=size, cl_lv=cl_lv) agent_state_action = state_prediction(self.task.signs[ag_name], new_sit, self.task.signs) conditions_new = get_conditions(new_sit, action, obj.name, ground_block) new_sit['conditions'] = conditions_new action_situation = define_situation('*goal-sit-*-' + action[1], cell_map_pddl, conditions_new, agent_state_action, self.task.signs) action_map = define_map('*goal-map-*-' + action[1], region_map, cell_location, near_loc, self.task.additions[1], self.task.signs) state_fixation(action_situation, cell_coords, self.task.signs, 'cell') return action_situation, action_map, cl_lv, new_sit, size