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)))