def on_new_plan_req(agents):
hatpehda.reset_agents_tasks()
hatpehda.set_state("robot", state1_r)
hatpehda.set_state("human", state1_h)
for ag, tasks in agents.items():
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(sol)
rosnode.send_plan(sol)
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))
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)
hatpehda.set_state("human", state1_h)
hatpehda.add_tasks("human", [('stack', goal1_h)])
hatpehda.set_state("robot", state1_r)
hatpehda.add_tasks("robot", [('stack', goal1_r)])
hatpehda.print_state(hatpehda.agents["human"].state)
sol = []
plans = hatpehda.seek_plan_robot(hatpehda.agents, "robot", sol)
rosnode = None
generate_standard_domain("plop.xml", "simple_cube_pile")
def on_new_plan_req(agents):
hatpehda.reset_agents_tasks()
#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", )])
human_state = deepcopy(state_filled)
human_state.__name__ = "human_init"
hatpehda.set_state("human", human_state)
sols = []
fails = []
start_explore = time.time()
hatpehda.seek_plan_robot(hatpehda.agents, "robot", sols, "human", fails)
end_explore = time.time()
print(len(sols))
"park": 1.5,
"beach": 50
},
"park": {
"home": 0.5,
"work": 1.5,
"beach": 51
},
"beach": {
"home": 50,
"work": 50,
"park": 51
}
}
hatpehda.set_state("robot", state1_h)
hatpehda.add_tasks("robot", [('travel', "home", "work")])
# plans = hatpehda.multi_agent_planning()
# print(len(hatpehda.ma_solutions), " plans found !")
# for ags in hatpehda.ma_solutions:
# print("Plan :", ags["robot"].global_plan, "with cost:", ags["robot"].global_plan_cost)
sol = []
plans = hatpehda.seek_plan_robot(hatpehda.agents, "robot", sol)
print(plans)
print(len(sol), "solutions found")
for agents in sol:
for name, a in agents.items():
print(name, "plan:", a.plan)
