def generate_tidy_all_orders(cubes_to_tidy):
fns = []
for order in permutations(cubes_to_tidy):
fn = partial(robot_tidy, order=order)
update_wrapper(fn, robot_tidy)
fns.append(fn)
hatpehda.declare_methods("robot", "tidy", *fns)
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])
@return:
@ontology_type goal: Goal
"""
if goal.onStack == self_state.onStack:
return []
for c in self_state.cubes:
if self_name in self_state.isReachableBy[
c] and c in goal.isOnStack and goal.isOnStack[
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})
return [("robot_pick", leg), ("robot_assemble", top)]
else:
return False
def handover_take_robot(agents, self_state, self_name, human):
if agents[human].plan[-1].name == "human_give_to_robot" or agents[
human].plan[-1].name == "human_wait_for_give":
return [("robot_take_from_human", human)]
elif same_last_tasks(agents[self_name].plan, 3, "robot_wait_for_take"):
return False
else:
return [("robot_wait_for_take", human), ("handover_take", human)]
hatpehda.declare_methods("human", "assemble_table", assemble_table_human)
hatpehda.declare_methods("human", "handle_leg", take_give_leg_human,
take_assemble_human)
hatpehda.declare_methods("human", "take_give_to_robot",
take_give_leg_to_robot_human)
hatpehda.declare_methods("human", "give_to_robot", handover_give_human)
hatpehda.declare_methods("robot", "assemble_tables", assemble_tables_robot,
assemble_tables_with_help_robot)
hatpehda.declare_methods("robot", "assemble_table", assemble_table_robot)
hatpehda.declare_methods("robot", "handle_leg", handover_assemble_robot,
take_assemble_robot)
hatpehda.declare_methods("robot", "handover_take", handover_take_robot)
hatpehda.print_operators()
hatpehda.print_methods()
else:
return False
return []
def switch_light_off_human(agents, self_state, self_name):
if self_state.redLightOn:
if self_state.isCarrying[self_name] is not None:
return [("switch_off_while_holding", )]
else:
return [("switch_off", )]
else:
return []
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.declare_methods("human", "switch_light_off", switch_light_off_human)
### Triggers
def on_red_light(agents, self_state, self_name):
if self_state.redLightOn:
return [("switch_light_off", )]
return False
hatpehda.declare_trigger("human", on_red_light)
state_filled.isHeldBy = {m: [] for m in state_filled.individuals["Mug"]}
state_filled.isHolding = {"human": [], "robot": []}
state_filled.isOwnedBy = {}
state_filled.isNotOwnedBy = {}
state_filled.isReachableBy = {
m: ["human", "robot"]
for m in state_filled.individuals["Mug"]
}
state_filled.agentsInRoom = {"office": ["human", "robot"]}
state_filled.isInRoom = {"human": ["office"], "robot": ["office"]}
#state_filled.notReferrableMugs = {"mug_0": ["mug_1"], "mug_1": ["mug_0"]}
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"
print('')
hatpehda.print_operators()
def travel_by_foot(state,a,x,y):
if state.dist[x][y] <= 2:
return [('walk',a,x,y)]
return False
def travel_by_taxi(state,a,x,y):
if state.cash[a] >= taxi_rate(state.dist[x][y]):
return [('call_taxi',a,x), ('ride_taxi',a,x,y), ('pay_driver',a)]
return False
hatpehda.declare_methods('travel', travel_by_foot, travel_by_taxi)
print('')
hatpehda.print_methods()
state1 = hatpehda.State('state1')
state1.loc = {'me':'home'}
state1.cash = {'me':20}
state1.owe = {'me':0}
state1.dist = {'home':{'park':8}, 'park':{'home':8}}
print("""
********************************************************************************
Call hatpehda.hatpehda(state1,[('travel','me','home','park')]) with different verbosity levels
********************************************************************************
""")
hatpehda.declare_operators("robot", walk, drive, go_to_parking,
come_from_parking)
print('')
hatpehda.print_operators()
def travel_by_foot(agents, self_state, agent_name, x, y):
return [('walk', x, y)]
def travel_by_car(agents, self_state, agent_name, x, y):
return [('go_to_parking', ), ("drive", x, y), ("come_from_parking", )]
hatpehda.declare_methods("robot", "travel", travel_by_foot, travel_by_car)
hatpehda.print_methods()
state1_h = hatpehda.State("state1_h")
state1_h.places = ["home", "park", "work", "beach"]
state1_h.loc = {"robot": "home", "car": "home"}
state1_h.is_at_parking = {"human": False}
state1_h.dist = {
"home": {
"work": 1,
"park": 0.5,
"beach": 50
},
"work": {
"home": 1,
"park": 1.5,
@return:
@ontology_type cube: Cube
"""
return [("human_pick_cube", cube), ("human_drop_cube", )]
def generate_tidy_all_orders(cubes_to_tidy):
fns = []
for order in permutations(cubes_to_tidy):
fn = partial(robot_tidy, order=order)
update_wrapper(fn, robot_tidy)
fns.append(fn)
hatpehda.declare_methods("robot", "tidy", *fns)
hatpehda.declare_methods("robot", "tidy_one", robot_tidy_one)
hatpehda.declare_methods("human", "tidy", human_tidy)
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": [],
continue
#
# if we get here, no blocks can be moved to their final locations
b1 = hatpehda.find_if(lambda x: status(x, state, goal) == 'waiting',
all_blocks(state))
if b1 != None:
return [('move_one', b1, 'table'), ('move_blocks', goal)]
#
# if we get here, there are no blocks that need moving
return []
"""
declare_methods must be called once for each taskname. Below, 'declare_methods('get',get_m)' tells Pyhop that 'get' has one method, get_m. Notice that 'get' is a quoted string, and get_m is the actual function.
"""
hatpehda.declare_methods('move_blocks', moveb_m)
### methods for "move_one"
def move1(state, b1, dest):
"""
Generate subtasks to get b1 and put it at dest.
"""
return [('get', b1), ('put', b1, dest)]
hatpehda.declare_methods('move_one', move1)
### methods for "get"
def robot_wait_for_human(agents, self_state, self_name, human):
print(agents[human].plan)
if agents[human].plan[-1][0] != "human_handover_give":
return [("robot_wait", ), ("robot_wait_for_human", human)]
else:
return []
def human_get_mug_make_coffee(agents, self_state, self_name, goal, giver, mug):
return [("human_handover_take", giver), ("human_make_coffee", mug),
("human_handover_give", giver), ("human_handover_end", )]
hatpehda.declare_methods("human", "human_get_mug_make_coffee",
human_get_mug_make_coffee)
hatpehda.declare_methods("robot", "robot_get_coffee", robot_get_coffee)
hatpehda.declare_methods("robot", "robot_ask_to_fill_cup",
robot_ask_to_fill_cup)
hatpehda.declare_methods("robot", "robot_wait_for_human", robot_wait_for_human)
hatpehda.print_operators()
hatpehda.print_methods()
state1_r = hatpehda.State("state1_r")
state1_r.isIn = {
"coffee_machine": "kitchen",
"mug1": "experimentation_room",
"mug2": "experimentation_room",
"mug3": "kitchen"
def stack_robot(agents, self_state, self_name, goal):
if goal.onStack == self_state.onStack:
return []
for c in self_state.cubes:
if self_name in self_state.isReachableBy[
c] and c in goal.isOnStack and goal.isOnStack[
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", "move_one", moveb_m_human)
hatpehda.declare_methods("robot", "move_one", moveb_m_robot)
hatpehda.declare_methods("human", "stack", wait_uncoop_human, 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})
continue
#
# if we get here, no blocks can be moved to their final locations
b1 = hatpehda.find_if(lambda x: status(x, state, goal) == 'waiting',
all_blocks(state))
if b1 != None:
return [('move_one', b1, 'table'), ('move_blocks', goal)]
#
# if we get here, there are no blocks that need moving
return []
"""
declare_methods must be called once for each taskname. Below, 'declare_methods('get',get_m)' tells Pyhop that 'get' has one method, get_m. Notice that 'get' is a quoted string, and get_m is the actual function.
"""
hatpehda.declare_methods('move_blocks', moveb_m)
### methods for "move_one"
def move1(state, b1, dest):
"""
Generate subtasks to get b1 and put it at dest.
"""
return [('get', b1), ('put', b1, dest)]
hatpehda.declare_methods('move_one', move1)
### methods for "get"