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 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))
c] and not self_state.isOnStack[c]:
if c == next(x for x in goal.onStack if x not in self_state.onStack
and self_name in self_state.isReachableBy[x]):
return [("move_one", c, goal), ("stack", goal)]
return False
hatpehda.declare_methods("human", "stack_cube", stack_cube_human)
hatpehda.declare_methods("human", "move_one", moveb_m_human)
hatpehda.declare_methods("robot", "move_one", moveb_m_robot)
hatpehda.declare_methods("human", "stack", stack_human)
hatpehda.declare_methods("robot", "stack", stack_robot)
hatpehda.print_operators()
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")