state.isOnStack = {}
state.isOnStack.update({c: is_stacked for c in cubes})
state1_h = hatpehda.State("state1_h")
state1_h.cubes = ["cube1", "cube2", "cube3", "cube4", "cube5", "cube6"]
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
state.canReach[ag].append(c)
state1_r = hatpehda.State("state1_r")
state1_r.individuals = {
"leg": ["leg1", "leg2", "leg3", "leg4"],
"top": ["top1"]
}
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,
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
state_filled.isHeldBy = {'cube_BGTG': [], 'cube_GBTG': [], 'cube_GGTB': []}
state_filled.individuals["DtReceiver"] = ["human_0"]
state_filled.isHolding = {"human": []}
state_filled.isReachableBy = {
c: ["human"]
for c in state_filled.individuals["DtCube"]
}
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)