def update_agents(agents, operators, methods): for ag, tasks in agents.items(): state = hatpehda.State(ag + "_init") state.types = { "Agent": ["isHolding"], "DtCube": ["isInContainer", "isHeldBy", "isReachableBy"], "DtBox": [], "ReachableDtBox": [], "ReceiverReachableDtBox": [], "VisibleDtBox": [], "ReceiverVisibleDtBox": [], "DirectorVisibleDtBox": [], "DirectorReachableDtBox": [], "DtDirector": [], "DtReceiver": [], "DtThrowBox": [] } state_filled = retrieve_state_from_ontology(ag, state) #state_filled = state #state_filled.individuals = {'DtCube': ["cube_BGTG", "cube_GBTG", "cube_GGTB"], 'DtBox': ["box_C1", "box_C2", 'box_C3']} #state_filled.isInContainer = {'cube_BGTG': ["box_C1"], 'cube_GBTG': ['box_C2'], 'cube_GGTB': ['box_C3']} #state_filled.isHeldBy = {'cube_BGTG': [], 'cube_GBTG': [], 'cube_GGTB': []} # TODO: remove state_filled.individuals["DtReceiver"] = ["human_0"] state_filled.isHolding = {next(iter(unctrl_agent)): []} state_filled.isReachableBy = { c: [next(iter(unctrl_agent))] for c in state_filled.individuals["DtCube"] } hatpehda.declare_operators(ag, *operators) for me in methods: hatpehda.declare_methods(ag, *me) hatpehda.set_state(ag, state_filled) hatpehda.add_tasks(ag, [(t[0], *t[1]) for t in tasks])
return agents def robot_stack(agents, self_state, self_name): if self_state.isCarrying[self_name] is not None: c = self_state.isCarrying[self_name] for a in agents.values(): a.state.isOnStack[c] = True a.state.onStack.append(c) a.state.isCarrying[self_name] = None return agents else: return False hatpehda.declare_operators("human", human_pick, human_stack, human_wait) hatpehda.declare_operators("robot", robot_pick, robot_stack) ### Methods definitions def moveb_m_human(agents, self_state, self_name, c, goal): """ @param agents: @param self_state: @param self_name: @param c: @param goal: @return: @ontology_type c: Cube @ontology_type goal: Goal
return agents def robot_take_from_human(agents, self_state, self_name, human): if self_state.isCarrying[ self_name] == [] and self_state.isCarrying[human] != []: if agents[human].plan[-1].name == "human_give_to_robot" or agents[ human].plan[-1].name == "human_wait_for_give": for a in agents.values(): a.state.isCarrying[self_name] = a.state.isCarrying[human] a.state.isCarrying[human] = [] return agents return False hatpehda.declare_operators("human", human_pick, human_assemble, human_give_to_robot, human_wait_for_give) hatpehda.declare_operators("robot", robot_pick, robot_assemble, robot_ask_human_to_give_leg, robot_ask_to_help_in_assembly, robot_take_from_human, robot_wait_for_take) ### Methods definitions def assemble_tables_human(agents, self_state, self_name, goal): for top in goal.assembly: return [("assemble_table", top, goal)] def assemble_table_human(agents, self_state, self_name, top, goal): for leg in goal.assembly[top]:
return agents else: return False def switch_off(agents, self_state, self_name): if self_state.redLightOn and self_state.isCarrying[self_name] is None: for a in agents.values(): a.state.redLightCounter = 0 a.state.redLightOn = False return agents else: return False hatpehda.declare_operators("human", human_pick, human_stack, switch_off_while_holding, switch_off) hatpehda.declare_operators("robot", robot_pick, robot_stack) ### Methods definitions def moveb_m_human(agents, self_state, self_name, c, goal): """ This method implements the following block-stacking algorithm: If there's a block that can be moved to its final position, then do so and call move_blocks recursively. Otherwise, if there's a block that needs to be moved and can be moved to the table, then do so and call move_blocks recursively. Otherwise, no blocks need to be moved. """ if self_name in self_state.isReachableBy[
'Mug': ["mug_{}".format(i) for i in range(n_mug)] } state_filled.isHeldBy = {m: [] for m in state_filled.individuals["Mug"]} state_filled.isHolding = {"human": [], "robot": []} state_filled.isOwnedBy = {} state_filled.isNotOwnedBy = {} state_filled.isReachableBy = { m: ["human", "robot"] for m in state_filled.individuals["Mug"] } state_filled.agentsInRoom = {"office": ["human", "robot"]} state_filled.isInRoom = {"human": ["office"], "robot": ["office"]} #state_filled.notReferrableMugs = {"mug_0": ["mug_1"], "mug_1": ["mug_0"]} state_filled.notReferrableMugs = {} hatpehda.declare_operators("robot", *ctrl_operators) for me in ctrl_methods: hatpehda.declare_methods("robot", *me) hatpehda.declare_triggers("robot", *ctrl_triggers) hatpehda.declare_operators("human", *unctrl_operators) for me in unctrl_methods: hatpehda.declare_methods("human", *me) hatpehda.declare_triggers("human", *unctrl_triggers) #robot_goal = hatpehda.Goal("robot_goal") #robot_goal.isInContainer = {"cube_BGTG": ["throw_box_green"], "cube_GBTG": ["throw_box_green"]} hatpehda.set_state("robot", state_filled) hatpehda.add_tasks("robot", [("robot_get_right_mug", "human"), ("robot_go_to_coffee_machine", )])
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. """ hatpehda.declare_operators(pickup, unstack, putdown, stack)
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 hatpehda.declare_operators(walk, call_taxi, ride_taxi, pay_driver) print('') hatpehda.print_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
return False, 0 def go_to_parking(agents, self_state, agent_name): for a in agents.values(): a.state.is_at_parking[agent_name] = True return agents, cost_find_parking() def come_from_parking(agents, self_state, agent_name): for a in agents.values(): a.state.is_at_parking[agent_name] = False return agents, cost_get_back_from_parking() hatpehda.declare_operators("robot", walk, drive, go_to_parking, come_from_parking) print('') hatpehda.print_operators() def travel_by_foot(agents, self_state, agent_name, x, y): return [('walk', x, y)] def travel_by_car(agents, self_state, agent_name, x, y): return [('go_to_parking', ), ("drive", x, y), ("come_from_parking", )] hatpehda.declare_methods("robot", "travel", travel_by_foot, travel_by_car) hatpehda.print_methods()
return agents, 1. return False def human_drop_cube(agents, self_state, self_name): if len(self_state.isHolding[self_name]) == 1: for a in agents.values(): cube = a.state.isHolding[self_name][0] a.state.isIn[cube] = [] # TODO: Add the goal box a.state.isHolding[self_name] = [] a.state.isHeldBy[cube] = [] return agents, 1. return False hatpehda.declare_operators("robot", robot_tell_human_to_tidy, robot_wait_for_human_to_tidy) hatpehda.declare_operators("human", human_pick_cube, human_drop_cube) def robot_tidy_one(angents, self_state, self_name, cube): """ @param angents: @param self_state: @param self_name: @param cube: @return: @ontology_type cube: Cube """ return [("robot_tell_human_to_tidy", "human", cube), ("robot_wait_for_human_to_tidy", "human", cube)]
def human_make_coffee(agents: Dict[str, hatpehda.Agent], self_state, self_name, mug): if self_state.isCarrying[self_name] == mug: if self_state.contains[mug] == "coffee": return agents elif self_state.contains[mug] is None: for a in agentsInSameRoomAs(self_state, self_name, agents): a.state.contains[mug] = "coffee" return agents else: return False else: return False hatpehda.declare_operators("human", human_handover_give, human_handover_end, human_handover_take, human_make_coffee) hatpehda.declare_operators("robot", robot_pick_mug, robot_navigate, robot_ask_to_human_do_task, robot_handover_give, robot_handover_end, robot_handover_take, robot_wait) ### Methods definitions def robot_get_coffee(agents, self_state, self_name, goal, mug): coffee_machine_loc = self_state.isIn["coffee_machine"] if self_state.contains[mug] == "coffee": return [] human = next(a for a in self_state.agentsInRoom[coffee_machine_loc] if "human" in a) if "human" not in self_state.agentsInRoom[coffee_machine_loc]: return False