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)
print(plans)
#rosnode = RosNode.start_ros_node("planner", on_new_plan_req)
#rosnode.wait_for_request()
print(len(sol), "solutions found")
from hatpehda import gui
gui.show_plan(sol)
for agents in sol:
reconstituted_plan = [None] * (2 * len(agents["robot"].plan))
reconstituted_plan[::2] = agents["robot"].plan
reconstituted_plan[1::2] = agents["human"].plan
for name, a in agents.items():
print(name, "plan:", [("{} {}".format(o.why.id, o.why.name) if o.why
is not None else None, o.name, *o.parameters)
for o in a.plan])
print("complete plan:", reconstituted_plan)
print("######")
state1_h = deepcopy(state1_r)
goal1_r = hatpehda.Goal("goal1_r")
goal1_r.assembly = {"top1": ["leg1", "leg2", "leg3", "leg4"]}
#goal1_h = deepcopy(goal1_r)
hatpehda.set_state("human", state1_h)
#hatpehda.add_tasks("human", [('stack', goal1_h)])
hatpehda.set_state("robot", state1_r)
hatpehda.add_tasks("robot", [('assemble_tables', goal1_r)])
#hatpehda.print_state(hatpehda.agents["human"].state)
sol = []
fails = []
plans = hatpehda.seek_plan_robot(hatpehda.agents,
"robot",
sol,
uncontrollable_agent_name="human",
fails=fails)
gui.show_plan(sol + fails, "robot", "human")
print(fails)
print(len(sol), "solutions found")
for agents in sol:
for name, a in agents.items():
print(name, "plan:", a.plan)
print("######")
hatpehda.set_state("robot", state_filled)
hatpehda.add_tasks("robot", [("tidy_cubes", robot_goal)])
human_state = deepcopy(state_filled)
human_state.__name__ = "human_init"
hatpehda.set_state("human", human_state)
sols = []
fails = []
hatpehda.seek_plan_robot(hatpehda.agents, "robot", sols, "human", fails)
end = time.time()
print(len(sols))
gui.show_plan(sols, "robot", "human")
#rosnode = ros.RosNode.start_ros_node("planner", lambda x: print("plop"))
#time.sleep(5)
#rosnode.send_plan(sols, "robot", "human")
input()
first_action = get_first_action(sols[0])
cost, action = select_policies(first_action)
gui.show_plan(get_last_actions(action), "robot", "human")
print("policy cost", cost)
# print(len(hatpehda.ma_solutions))
# for ags in hatpehda.ma_solutions:
# print("Plan :", ags["robot"].global_plan, "with cost:", ags["robot"].global_plan_cost)
# print("Took", end - start, "seconds")
# regHandler.export_log("robot_planning")
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))
#gui.show_plan(sols, "robot", "human")
#rosnode = ros.RosNode.start_ros_node("planner", lambda x: print("plop"))
#time.sleep(5)
#rosnode.send_plan(sols, "robot", "human")
cost, root_action = hatpehda.select_conditional_plan(
sols, "robot", "human", cost_dict)
gui.show_plan(hatpehda.get_last_actions(root_action), "robot", "human")
print("policy cost", cost)
#print("n mug: {}\texplore duration: {}sec\tselect duration: {}sec\tnumber of valid branches: {}".format(n_mug, end_explore - start_explore, end_select - start_select, len(sols)))
# print(len(hatpehda.ma_solutions))
# for ags in hatpehda.ma_solutions:
# print("Plan :", ags["robot"].global_plan, "with cost:", ags["robot"].global_plan_cost)
# print("Took", end - start, "seconds")
# regHandler.export_log("robot_planning")
# regHandler.cleanup()
def on_new_plan_req(ctrl_agents, unctrl_agent):
hatpehda.reset_planner()
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])
update_agents(ctrl_agents, ctrl_operators, ctrl_methods)
update_agents(unctrl_agent, unctrl_operators, unctrl_methods)
# hatpehda.print_state(hatpehda.agents["robot"].state)
# print(hatpehda.agents["robot"].tasks)
# hatpehda.print_methods("robot")
# hatpehda.print_methods("human")
sol = []
# For now it only works with one controllable and one uncontrollable agents...
plans = hatpehda.seek_plan_robot(hatpehda.agents,
next(iter(ctrl_agents)),
sol,
uncontrollable_agent_name=next(
iter(unctrl_agent)))
print(len(sol))
#regHandler.export_log("human_planning")
gui.show_plan(sol, next(iter(ctrl_agents)), next(iter(unctrl_agent)))
print(sol)
rosnode.send_plan(sol, next(iter(ctrl_agents)), next(iter(unctrl_agent)))