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])
def on_new_plan_req(agents):
for ag, tasks in agents.items():
state = hatpehda.State(ag + "_init")
state.types = {
"Agent": ["isHolding"],
"Cube": ["isIn", "isHeldBy"],
"Box": [],
"ReachableDtBox": [],
"ReceiverReachableDtBox": [],
"VisibleDtBox": [],
"ReceiverVisibleDtBox": [],
"DirectorVisibleDtBox": [],
"DirectorReachableDtBox": [],
"DtDirector": [],
"DtReceiver": []
}
# state_r = retrieve_state_from_ontology("robot", state_r)
state.individuals = {}
state.isHolding = {}
state.individuals["DtReceiver"] = ["human"]
state.isHolding["human"] = []
state_h = deepcopy(state) # TODO: Retrieve it from the ontology
generate_tidy_all_orders(
["cube_GBTG", "cube_BBTG", "cube_GBCG", "cube_GBTB"])
hatpehda.set_state(ag, state)
hatpehda.add_tasks(ag, [(t[0], *t[1]) for t in tasks])
hatpehda.print_state(hatpehda.agents["robot"].state)
print(hatpehda.agents["robot"].tasks)
hatpehda.print_methods("robot")
hatpehda.print_methods("human")
sol = []
plans = hatpehda.seek_plan_robot(hatpehda.agents, "robot", sol)
print(len(hatpehda.ma_solutions))
hatpehda.print_methods()
def make_reachable_by(state, cubes, agent):
if not hasattr(state, "isReachableBy"):
state.isReachableBy = {}
state.isReachableBy.update({c: agent for c in cubes})
def put_on_stack(state, cubes, is_stacked):
if not hasattr(state, "isOnStack"):
state.isOnStack = {}
state.isOnStack.update({c: is_stacked for c in cubes})
state1_h = hatpehda.State("state1_h")
state1_h.cubes = ["cube1", "cube2", "cube3", "cube4", "cube5", "cube6"]
make_reachable_by(state1_h, state1_h.cubes[:3], ["human"])
make_reachable_by(state1_h, state1_h.cubes[3:], ["robot"])
make_reachable_by(state1_h, state1_h.cubes[5:6], ["human", "robot"])
put_on_stack(state1_h, state1_h.cubes, False)
state1_h.isCarrying = {"human": None, "robot": None}
state1_h.onStack = []
state1_r = deepcopy(state1_h)
goal1_h = hatpehda.Goal("goal1_h")
goal1_h.isOnStack = {"cube4": True, "cube5": True, "cube6": True}
goal1_h.onStack = ["cube4", "cube5", "cube6"]
goal1_r = deepcopy(goal1_h)
def make_reachable_by(state, indivs, agents):
if not hasattr(state, "isReachableBy"):
state.isReachableBy = {}
if not hasattr(state, "canReach"):
state.canReach = {}
for c in indivs:
if c not in state.isReachableBy:
state.isReachableBy[c] = []
for ag in agents:
if ag not in state.canReach:
state.canReach[ag] = []
state.isReachableBy[c].append(ag)
state.canReach[ag].append(c)
state1_r = hatpehda.State("state1_r")
state1_r.individuals = {
"leg": ["leg1", "leg2", "leg3", "leg4"],
"top": ["top1"]
}
make_reachable_by(state1_r, state1_r.individuals["leg"], ["human"])
make_reachable_by(state1_r, ["leg3", "leg4"], ["robot"])
state1_r.isCarrying = {"human": [], "robot": []}
state1_r.assembly = {"top1": []}
state1_h = deepcopy(state1_r)
goal1_r = hatpehda.Goal("goal1_r")
goal1_r.assembly = {"top1": ["leg1", "leg2", "leg3", "leg4"]}
#goal1_h = deepcopy(goal1_r)
if heldMug in self_state.isNotOwnedBy and "human" == self_state.isNotOwnedBy[
heldMug]:
return [("robot_drop_mug", ), ("robot_get_right_mug", "human")]
return False
ctrl_triggers = [robot_check_wrong_mug]
unctrl_triggers = [human_check_mug]
if __name__ == "__main__":
regHandler = REGHandler()
n_mug = 2
start = time.time()
state = hatpehda.State("robot_init")
state.types = {
"Agent": ["isHolding"],
"Mug": ["isHeldBy", "isReachableBy"]
}
state_filled = state
state_filled.individuals = {
'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"]
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
hatpehda.declare_methods('travel', travel_by_foot, travel_by_taxi)
print('')
hatpehda.print_methods()
state1 = hatpehda.State('state1')
state1.loc = {'me':'home'}
state1.cash = {'me':20}
state1.owe = {'me':0}
state1.dist = {'home':{'park':8}, 'park':{'home':8}}
print("""
********************************************************************************
Call hatpehda.hatpehda(state1,[('travel','me','home','park')]) with different verbosity levels
********************************************************************************
""")
print("- If verbose=0 (the default), Pyhop returns the solution but prints nothing.\n")
hatpehda.pyhop(state1, [('travel', 'me', 'home', 'park')])
print('- If verbose=1, Pyhop prints the problem and solution, and returns the solution:')
