def robot_ask_mug_to_take(agents, self_state, self_name, human):
"""
Asks the 'human' which one is their mug by adding to their agenda that they will answer the question.
Note that we could have checked if the communication was feasible
"""
hatpehda.add_tasks(human, [("human_answer_mug_a", self_name)], agents)
return agents
def robot_tell_human_to_tidy(agents, self_state, self_name, human, cube, box):
"""
@param agents:
@param self_state:
@param self_name:
@param human:
@param cube:
@return:
@semantic_name: ask_to_put_pickable_in_container
@ontology_type human: Human
@ontology_type cube: Cube
@ontology_type box: Box
"""
if human in self_state.isReachableBy[cube]:
ctx = [("?0", "isAbove", "table_1")]
symbols = {"?0": cube}
reg = regHandler.get_re(human, agents[human].state, ctx, symbols, cube)
if not reg.success:
return False
cost = len(reg.sparqlResult)
cost = 1
print("Cube", cube, "costs", cost, "to disambiguate")
hatpehda.add_tasks(human, [("tidy", cube, box)], agents)
return agents
else:
print("cube", cube, "is not reachable by", human)
return False
def robot_yell_human_to_tidy(agents, self_state, self_name, human, cube, box):
if human in self_state.isReachableBy[cube]:
ctx = [("?0", "isAbove", "table_1")]
symbols = {"?0": cube}
hatpehda.add_tasks(human, [("human_surprise", ), ("tidy", cube, box)],
agents)
return agents
else:
print("cube", cube, "is not reachable by", human)
return False
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 robot_tell_human_to_tidy(agents, self_state, self_name, human, cube, box):
"""
@param agents:
@param self_state:
@param self_name:
@param human:
@param cube:
@return:
@semantic_name: ask_to_put_pickable_in_container
@ontology_type human: Human
@ontology_type cube: Cube
@ontology_type box: Box
"""
if human in self_state.isReachableBy[cube]:
ctx = [("?0", "isAbove", "table_1")]
symbols = {"?0": cube}
hatpehda.add_tasks(human, [("tidy", cube, box)], agents)
return agents
else:
print("cube", cube, "is not reachable by", human)
return False
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))
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()
hatpehda.set_state("robot", state1_r)
def robot_ask_to_help_in_assembly(agents, self_state, self_name, human, top,
legs):
goal = hatpehda.Goal("human_goal")
goal.assembly = {top: legs}
hatpehda.add_tasks(human, [("assemble_table", top, goal)], agents)
return agents
def robot_ask_human_to_give_leg(agents, self_state, self_name, human, leg):
hatpehda.add_tasks(human, [("take_give_to_robot", leg, self_name)], agents)
print(agents[human].tasks)
return agents
}
make_reachable_by(state1_r, state1_r.individuals["leg"], ["human"])
make_reachable_by(state1_r, ["leg3", "leg4"], ["robot"])
state1_r.isCarrying = {"human": [], "robot": []}
state1_r.assembly = {"top1": []}
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)
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))
#gui.show_plan(sols, "robot", "human")
#rosnode = ros.RosNode.start_ros_node("planner", lambda x: print("plop"))
hatpehda.declare_operators("robot", *ctrl_operators)
for me in ctrl_methods:
hatpehda.declare_methods("robot", *me)
hatpehda.declare_operators("human", *unctrl_operators)
for me in unctrl_methods:
hatpehda.declare_methods("human", *me)
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", [("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)
"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)
print("######")
"mug3": ["human"]
}
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)