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
