def get_box_containing_cube(state, cube):
box = None
if cube in state.isInContainer and state.isInContainer[cube] != []:
for container in state.isInContainer[cube]:
hatpehda.print_state(state)
if container in state.individuals["DtBox"]:
box = container
break
return box
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 retrieve_state_from_ontology(agent_name, state):
ontos = OntologiesManipulator()
ontos.waitInit()
ontos.add(agent_name)
onto = ontos.get(agent_name)
#onto = OntologyManipulator()
onto.close()
for t, rels in state.types.items():
indivs = onto.individuals.getType(t)
if not hasattr(state, "individuals"):
state.individuals = {}
state.individuals[t] = indivs
for rel in rels:
if not hasattr(state, rel):
setattr(state, rel, {})
for indiv in indivs:
related = onto.individuals.getOn(indiv, rel)
getattr(state, rel)[indiv] = related
hatpehda.print_state(state)
return state
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.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()
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])
state1_r.isCarrying = {"robot": None, "human": None}
state1_r.connectedTo = {
"kitchen": ["experimentation_room"],
"experimentation_room": ["kitchen"]
}
state1_h = deepcopy(state1_r)
goal1_r = hatpehda.Goal("goal1_r")
goal1_h = deepcopy(goal1_r)
hatpehda.set_state("human", state1_h)
hatpehda.set_state("robot", state1_r)
hatpehda.add_tasks("robot", [("robot_get_coffee", goal1_r, "mug1")])
hatpehda.print_state(hatpehda.agents["robot"].state)
# plans = hatpehda.multi_agent_planning(verbose=0)
#
# for ags in hatpehda.ma_solutions:
# print("Plan :", ags["robot"].global_plan, "with cost:", ags["robot"].global_plan_cost)
#
# print(plans)
sol = []
plans = hatpehda.seek_plan_robot(hatpehda.agents, "robot", sol)
print(plans)
print(len(sol), "solutions found")
for agents in sol: