def step(self, action, object_id=None, receptacleObjectId=None, epsd_collector=None):
assert object_id is not None
object_id = PlanParser.map_legal_object_name_back(object_id)
if action in ["PickupObject", "OpenObject", "DropObject"]:
super().step(action=action, objectId=object_id)
elif action == "PutObjectIntoReceptacle":
assert receptacleObjectId is not None
receptacleObjectId = PlanParser.map_legal_object_name_back(receptacleObjectId)
super().step(action="PutObject", objectId=object_id, receptacleObjectId=receptacleObjectId)
if epsd_collector:
epsd_collector.add_experience(self.env.step_output, action)
def __init__(self, config):
self.face_to_front = False
self.object_facing = None
self.object_in_hand = None
self.knowledge = GameKnowlege()
self.agent_loc_info = {
self.AGENT_NAME:
(config['performerStart']['position']['x'], self.AGENT_Y_INIT,
config['performerStart']['position']['z'])
}
self.object_loc_info = {}
self.object_open_close_info = {}
for obj in config['objects']:
object_id = PlanParser.create_legal_object_name(obj['uuid'])
if 'agent_position' not in obj:
self.object_loc_info[object_id] = (obj['position']['x'],
obj['position']['y'],
obj['position']['z'])
else:
self.object_loc_info[object_id] = (obj['position']['x'],
obj['position']['y'],
obj['position']['z'],
obj['agent_position'])
#self.object_loc_info[object_id] =
if "opened" in obj:
self.object_open_close_info[object_id] = obj["opened"]
self.object_containment_info = defaultdict(lambda: [])
self.object_knowledge_info = {}
self.goal_predicate_list = None
self.goal_category = None
self.creat_goal(config)
def __init__(self, config):
self.face_to_front = False
self.object_facing = None
self.object_in_hand = None
self.knowledge = GameKnowlege()
self.agent_loc_info = {
self.AGENT_NAME:
(config['performerStart']['position']['x'], self.AGENT_Y_INIT,
config['performerStart']['position']['z'])
}
self.object_loc_info = {}
self.receptacle_info = defaultdict(lambda: [])
self.object_open_close_info = {}
for obj in config['objects']:
for obj_2 in config['objects']:
if obj['uuid'] == obj_2['uuid']:
continue
for key, value_list in self.RECEPTACLE_RELATIONSHIP.items():
if key in obj['uuid']:
for one_value in value_list:
if one_value in obj_2['uuid']:
self.receptacle_info[
PlanParser.create_legal_object_name(
obj['uuid'])].append(
PlanParser.
create_legal_object_name(
obj_2['uuid']))
object_id = PlanParser.create_legal_object_name(obj['uuid'])
self.object_loc_info[object_id] = (
obj['position']['x'],
obj['position']['y'],
obj['position']['z'],
)
receptacle_object_id = PlanParser.create_legal_object_name(
obj['id'])
if "box" in obj['id']:
self.object_open_close_info[receptacle_object_id] = False
else:
self.object_open_close_info[receptacle_object_id] = None
self.object_containment_info = defaultdict(lambda: [])
self.goal_predicate_list = None
self.goal_category = None
self.creat_goal(config)
def step(self,
action,
object_id=None,
receptacleObjectId=None,
from_playroom=None):
assert object_id is not None
object_id = PlanParser.map_legal_object_name_back(
object_id, from_playroom)
if action in [
"PickupObject", "OpenObject", "DropObject", "CloseObject"
]:
super().step(action=action, objectId=object_id)
elif action == "PutObjectIntoReceptacle":
assert receptacleObjectId is not None
receptacleObjectId = PlanParser.map_legal_object_name_back(
receptacleObjectId, from_playroom)
super().step(action="PutObject",
objectId=object_id,
receptacleObjectId=receptacleObjectId)
print(plan)
break
if result_plan[0]['action'] == "LookForObjectInReceptacle":
new_config = deepcopy(metaController.env.scene_config)
new_config['performerStart']['position'] = {
"x": metaController.env.step_output.position['x'],
"y": 0,
"z": metaController.env.step_output.position['z']
}
new_config['performerStart']['rotation'] = {
"y": metaController.env.step_output.rotation
}
new_config['goal'][
'category'] = 'searchObjectInReceptacleTraining'
new_config['goal']['metadata'][
'targetReceptacleId'] = PlanParser.map_legal_object_name_back(
result_plan[0]['receptacleId'])
file_name = new_config['name'].split(".")[0]
file_path = os.path.join(
'interaction_scenes', 'searchObjectInReceptacleTraining',
"{}_{}.json".format(file_name, search_cnt))
with open(file_path, 'w') as fp:
json.dump(new_config, fp, indent=4)
search_cnt += 1
success = metaController.step(result_plan[0])
if not success:
break
if result_plan[0]['action'] == "End":
break
meta_stage += 1
# time.sleep(2)
@staticmethod
def preprocess(img):
img = img.resize((50, 50), Image.ANTIALIAS)
return np.transpose(np.array(img), (2, 0, 1)) / 255
if __name__ == "__main__":
import time
env = McsEnv()
domain_file = "planner/domains/Playroom_domain.pddl"
facts_file = "planner/sample_problems/playroom_facts.pddl"
parser = PlanParser()
replay_buffer = []
metaController = MetaController(env)
episode = 0
while episode < 100:
print("Episode: {}".format(episode))
env.reset()
PlanParser.scene_config_to_pddl(env.scene_config, random_pick_up(env.scene_config), facts_file)
result_plan = parser.get_plan_from_file(domain_file, facts_file)
epsd_collector = Episode_collector()
for action in result_plan:
print(action)
metaController.step(action, epsd_collector)
if len(epsd_collector) < 10:
continue
def step(self, action_dict, epsd_collector=None, frame_collector=None):
assert 'action' in action_dict
if action_dict['action'] == "GotoLocation":
goal = get_goal(action_dict['location'])
current_object_id = None
for objectId, object_data in self.plannerState.object_loc_info.items(
):
#object_data == goal:
flag = 0
for k in range(len(object_data)):
if round(object_data[k], 2) != goal[k]:
print(round(object_data[k], 2), goal[k])
flag = 1
break
if flag == 0:
current_object_id = objectId
break
current_object_loc = self.plannerState.object_loc_info[
current_object_id]
if len(current_object_loc) == 3:
final_goal = goal[:]
success_distance = machine_common_sense.mcs_controller_ai2thor.MAX_REACH_DISTANCE - 0.7
else:
final_goal = (float(current_object_loc[3]['x']),
float(current_object_loc[3]['z']),
float(current_object_loc[3]['z']))
success_distance = 0
success = self.nav.go_to_goal(self.nav_env, final_goal,
success_distance)
if not success:
print("Navigation Fail")
return False
self.plannerState.agent_loc_info[
self.plannerState.AGENT_NAME] = goal
self.plannerState.object_facing = None
elif action_dict['action'] == "FaceToFront":
FaceTurnerResNet.look_to_front(self.face_env)
self.plannerState.face_to_front = True
self.plannerState.object_facing = None
elif action_dict['action'] == "FaceToObject":
goal = get_goal(action_dict['location'])
FaceTurnerResNet.look_to_direction(self.face_env, goal,
epsd_collector)
object_in_view = [
PlanParser.create_legal_object_name(obj.uuid)
for obj in self.env.step_output.object_list if not obj.held
]
if action_dict['objectId'] in object_in_view:
self.plannerState.object_facing = action_dict['objectId']
else:
del self.plannerState.object_loc_info[action_dict['objectId']]
for k in self.plannerState.object_containment_info:
if action_dict[
'objectId'] in self.plannerState.object_containment_info[
k]:
self.plannerState.object_containment_info[k].remove(
action_dict['objectId'])
if action_dict[
'objectId'] in self.plannerState.object_open_close_info:
del self.plannerState.object_open_close_info[
action_dict['objectId']]
print("Object {} not at {}".format(action_dict['objectId'],
action_dict['location']))
self.plannerState.face_to_front = False
elif action_dict['action'] == "PickupObject":
self.obj_env.step("PickupObject",
object_id=action_dict['objectId'],
epsd_collector=epsd_collector)
if self.env.step_output.return_status == "SUCCESSFUL":
self.plannerState.object_in_hand = action_dict['objectId']
else:
print("Pickup {} fail!".format(action_dict['objectId']))
return False
elif action_dict['action'] == "LookForObjectInReceptacle":
found_object = False
self.env.step(action="MoveAhead", amount=0.8)
goal = self.plannerState.object_loc_info[
action_dict['receptacleId']]
FaceTurnerResNet.look_to_direction(self.face_env, goal,
epsd_collector)
self.env.step(action="RotateLook", rotation=-45)
for _ in range(9):
if action_dict['objectId'] not in [
PlanParser.create_legal_object_name(obj.uuid)
for obj in self.env.step_output.object_list
]:
self.face_env.step(action="RotateRight")
else:
self.plannerState.object_facing = action_dict['objectId']
for obj in self.env.step_output.object_list:
if PlanParser.create_legal_object_name(
obj.uuid) == action_dict['objectId']:
goal = (obj.position['x'], obj.position['y'],
obj.position['z'])
FaceTurnerResNet.look_to_direction(
self.face_env, goal, epsd_collector)
found_object = True
break
if not found_object:
print("{} not in {}".format(action_dict['objectId'],
action_dict['receptacleId']))
self.plannerState.object_containment_info[
action_dict['objectId']].remove(
action_dict['receptacleId'])
elif action_dict['action'] == "PutObjectIntoReceptacle":
self.obj_env.step("PutObjectIntoReceptacle",
object_id=action_dict['objectId'],
receptacleObjectId=action_dict['receptacleId'],
epsd_collector=epsd_collector)
if self.env.step_output.return_status == "SUCCESSFUL":
self.plannerState.object_in_hand = None
self.plannerState.object_containment_info[
action_dict['objectId']] = action_dict['receptacleId']
else:
self.plannerState.knowledge.canNotPutin.add(
(action_dict['objectId'], action_dict['receptacleId']))
elif action_dict['action'] == "OpenObject":
self.obj_env.step("OpenObject",
object_id=action_dict['objectId'],
epsd_collector=epsd_collector)
if self.env.step_output.return_status == "SUCCESSFUL":
self.plannerState.object_open_close_info[
action_dict['objectId']] = True
else:
print("Open {} fail".format(action_dict['objectId']))
return False
elif action_dict['action'] == "DropObjectNextTo":
FaceTurnerResNet.look_to_front(self.face_env)
self.env.step(action="RotateLook", horizon=9.536743e-06)
self.obj_env.step("DropObject",
object_id=action_dict['objectId'],
epsd_collector=epsd_collector)
if self.env.step_output.return_status == "SUCCESSFUL":
self.plannerState.knowledge.objectNextTo[
action_dict['objectId']] = action_dict['goal_objectId']
self.plannerState.object_in_hand = None
print(
"Drop Successful, if reward 0 then it should be out of range."
)
'''
for _ in range(10):
print("Head_tilt before drop object {}".format(self.env.step_output.head_tilt))
self.obj_env.step("DropObject", object_id=action_dict['objectId'], epsd_collector=epsd_collector)
if self.env.step_output.return_status == "SUCCESSFUL":
self.plannerState.knowledge.objectNextTo[action_dict['objectId']] = action_dict['goal_objectId']
self.plannerState.object_in_hand = None
print("Drop Successful, if reward 0 then it should be out of range.")
break
self.env.step(action="MoveBack", amount=0.05)
'''
if self.plannerState.object_in_hand:
print("DropObject fail")
return False
elif action_dict['action'] == "DropObjectOnTopOf":
self.obj_env.step("PutObjectIntoReceptacle",
object_id=action_dict['objectId'],
receptacleObjectId=action_dict['goal_objectId'],
epsd_collector=epsd_collector
) # use magical action at current point
if self.env.step_output.return_status == "SUCCESSFUL":
self.plannerState.knowledge.objectOnTopOf[
action_dict['objectId']] = action_dict['goal_objectId']
self.plannerState.object_in_hand = None
print("PutObject succeeded! If reward 0 then it is a bug!")
# assert self.env.step_output.reward == 1
else:
print("PutObjectIntoReceptacle {}".format(
self.env.step_output.return_status))
return False
return True
def plan_on_current_state(self):
planner = PlanParser(self.plannerState)
planner.planner_state_to_pddl(self.plannerState)
return planner.get_plan()
def creat_goal(self, config):
if 'goal' in config:
self.goal_category = config['goal']['category']
self.goal_predicate_list = []
if self.goal_category == "traversal":
self.goal_object_id = PlanParser.create_legal_object_name(
config['goal']['metadata']['target']['id'])
agent_final_loc = PlanParser.replace_digital_number(
"loc|{:.2f}|{:.2f}|{:.2f}".format(
self.object_loc_info[self.goal_object_id][0],
self.object_loc_info[self.goal_object_id][1],
self.object_loc_info[self.goal_object_id][2]))
self.goal_predicate_list.append(
"(agentAtLocation {} {})".format(self.AGENT_NAME,
agent_final_loc))
elif self.goal_category == "retrieval":
self.goal_object_id = PlanParser.create_legal_object_name(
config['goal']['metadata']['target']['id'])
self.goal_predicate_list.append("(held {} {})".format(
self.AGENT_NAME, self.goal_object_id))
elif self.goal_category == "transferral":
self.transfer_object_id = PlanParser.create_legal_object_name(
config['goal']['metadata']['target_1']['id'])
self.target_object_id = PlanParser.create_legal_object_name(
config['goal']['metadata']['target_2']['id'])
if config['goal']['metadata']['relationship'][1] == "next to":
self.goal_predicate_list.append(
"(objectNextTo {} {})".format(self.transfer_object_id,
self.target_object_id))
elif config['goal']['metadata']['relationship'][
1] == "on top of":
self.goal_predicate_list.append(
"(objectOnTopOf {} {})".format(self.transfer_object_id,
self.target_object_id))
elif self.goal_category == "playroom":
self.transfer_object_id = PlanParser.create_legal_object_name(
"box_b")
self.target_object_id = PlanParser.create_legal_object_name(
"chair_b")
for obj in config['objects']:
if "openable" in obj and obj["openable"] == True:
receptacle_object_id = PlanParser.create_legal_object_name(
obj['id'])
if "opened" in obj:
self.object_open_close_info[
receptacle_object_id] = True
else:
self.object_open_close_info[
receptacle_object_id] = False
self.apple_a = PlanParser.create_legal_object_name("apple_a")
self.ball_a = PlanParser.create_legal_object_name("ball_a")
self.ball_b = PlanParser.create_legal_object_name("ball_b")
self.bowl_a = PlanParser.create_legal_object_name("bowl_a")
self.goal_predicate_list.extend([
"(objectOnTopOf {} {})".format(self.apple_a,
self.target_object_id),
"(objectOnTopOf {} {})".format(self.ball_a,
self.target_object_id),
"(objectOnTopOf {} {})".format(self.ball_b,
self.target_object_id),
# "(objectOnTopOf {} {})".format(self.bowl_a, self.target_object_id),
# "(objectOnTopOf {} {})".format(self.transfer_object_id, self.target_object_id),
# "(inreceptacle {} {})".format(self.apple_a, self.bowl_a),
# "(inreceptacle {} {})".format(self.ball_a, self.transfer_object_id),
# "(inreceptacle {} {})".format(self.ball_b, self.transfer_object_id),
# "(inreceptacle {} {})".format(self.bowl_a, self.transfer_object_id),
])
def step(self, action_dict):
assert 'action' in action_dict
if action_dict['action'] == "GotoLocation":
goal = get_goal(action_dict['location'])
success_distance = machine_common_sense.mcs_controller_ai2thor.MAX_REACH_DISTANCE - 0.2
success = self.nav.go_to_goal(self.nav_env, goal, success_distance)
if success:
self.plannerState.agent_loc_info[
self.plannerState.AGENT_NAME] = goal
else:
print("Navigation Fail")
return False
elif action_dict['action'] == "FaceToFront":
FaceTurnerResNet.look_to_front(self.face_env)
self.plannerState.face_to_front = True
self.plannerState.object_facing = None
elif action_dict['action'] == "FaceToObject":
goal = get_goal(action_dict['location'])
FaceTurnerResNet.look_to_direction(self.face_env, goal)
object_in_view = [
PlanParser.create_legal_object_name(obj.uuid)
for obj in self.env.step_output.object_list if not obj.held
]
if action_dict['objectId'] in object_in_view:
self.plannerState.object_facing = action_dict['objectId']
self.plannerState.face_to_front = False
else:
del self.plannerState.object_loc_info[action_dict['objectId']]
print("Object {} not at {}".format(action_dict['objectId'],
action_dict['location']))
self.plannerState.face_to_front = False
elif action_dict['action'] == "PickupObject":
self.obj_env.step(
"PickupObject",
object_id=action_dict['objectId'],
from_playroom=self.plannerState.goal_category == "playroom")
if self.env.step_output.return_status == "SUCCESSFUL":
self.plannerState.object_in_hand = action_dict['objectId']
self.plannerState.object_facing = None
del self.plannerState.object_loc_info[action_dict['objectId']]
for key, value_list in self.plannerState.object_containment_info.items(
):
for value in value_list:
if value == action_dict['objectId']:
self.plannerState.object_containment_info[
key].remove(value)
else:
print("Pickup {} fail!".format(action_dict['objectId']))
return False
elif action_dict['action'] == "PutObjectIntoReceptacle":
self.obj_env.step(
"PutObjectIntoReceptacle",
object_id=action_dict['objectId'],
receptacleObjectId=action_dict['receptacleId'],
from_playroom=self.plannerState.goal_category == "playroom")
if self.env.step_output.return_status == "SUCCESSFUL":
self.plannerState.object_in_hand = None
for _ in range(5):
self.env.step(action="Pass")
self.plannerState.object_containment_info[
action_dict['receptacleId']].append(
action_dict['objectId'])
self.plannerState.object_loc_info[action_dict[
'objectId']] = self.plannerState.object_loc_info[
action_dict['receptacleId']]
for obj in self.plannerState.object_containment_info[
action_dict['objectId']]:
self.plannerState.object_containment_info[
action_dict['receptacleId']].append(obj)
self.plannerState.object_loc_info[
obj] = self.plannerState.object_loc_info[
action_dict['receptacleId']]
else:
print("Put object into receptacle fail")
exit(0)
# self.plannerState.knowledge.canNotPutin.add((action_dict['objectId'], action_dict['receptacleId']))
elif action_dict['action'] == "OpenObject":
self.obj_env.step(
"OpenObject",
object_id=action_dict['objectId'],
from_playroom=self.plannerState.goal_category == "playroom")
if self.env.step_output.return_status == "SUCCESSFUL":
self.plannerState.object_open_close_info[
action_dict['objectId']] = True
else:
print("Open {} fail".format(action_dict['objectId']))
return False
elif action_dict['action'] == "CloseObject":
self.obj_env.step(
"CloseObject",
object_id=action_dict['objectId'],
from_playroom=self.plannerState.goal_category == "playroom")
if self.env.step_output.return_status == "SUCCESSFUL":
self.plannerState.object_open_close_info[
action_dict['objectId']] = False
for _ in range(5):
self.env.step(action="Pass")
else:
print("Close {} fail".format(action_dict['objectId']))
return False
elif action_dict['action'] == "DropObjectNextTo":
FaceTurnerResNet.look_to_front(self.face_env)
print("Head_tilt before drop object {}".format(
self.env.step_output.head_tilt))
self.obj_env.step(
"DropObject",
object_id=action_dict['objectId'],
from_playroom=self.plannerState.goal_category == "playroom")
if self.env.step_output.return_status == "SUCCESSFUL":
self.plannerState.knowledge.objectNextTo[
action_dict['goal_objectId']].append(
action_dict['objectId'])
self.plannerState.object_in_hand = None
print(
"Drop Successful, if reward 0 then it should be out of range."
)
if self.plannerState.object_in_hand:
print("DropObject fail")
return False
elif action_dict['action'] == "DropObjectOnTopOf":
self.obj_env.step(
"PutObjectIntoReceptacle",
object_id=action_dict['objectId'],
receptacleObjectId=action_dict['goal_objectId'],
from_playroom=self.plannerState.goal_category ==
"playroom") # use magical action at current point
if self.env.step_output.return_status == "SUCCESSFUL":
self.plannerState.knowledge.objectOnTopOf[
action_dict['goal_objectId']].append(
action_dict['objectId'])
self.plannerState.object_containment_info[
action_dict['goal_objectId']].append(
action_dict['objectId'])
self.plannerState.object_loc_info[action_dict[
'objectId']] = self.plannerState.object_loc_info[
action_dict['goal_objectId']]
for obj in self.plannerState.object_containment_info[
action_dict['objectId']]:
self.plannerState.knowledge.objectOnTopOf[
action_dict['goal_objectId']].append(obj)
self.plannerState.object_containment_info[
action_dict['goal_objectId']].append(obj)
self.plannerState.object_loc_info[
obj] = self.plannerState.object_loc_info[
action_dict['goal_objectId']]
self.plannerState.object_in_hand = None
print("PutObject succeeded! If reward 0 then it is a bug!")
# assert self.env.step_output.reward == 1
else:
print("PutObjectIntoReceptacle {}".format(
self.env.step_output.return_status))
return False
return True
def creat_goal(self, config):
if 'goal' in config:
self.goal_category = config['goal']['category']
self.goal_predicate_list = []
if self.goal_category == "traversal":
self.goal_object_id = PlanParser.create_legal_object_name(
config['goal']['metadata']['target']['id'])
if self.goal_object_id not in self.object_loc_info:
return
agent_final_loc = PlanParser.replace_digital_number(
"loc|{:.2f}|{:.2f}|{:.2f}".format(
self.object_loc_info[self.goal_object_id][0],
self.object_loc_info[self.goal_object_id][1],
self.object_loc_info[self.goal_object_id][2]))
self.goal_predicate_list.append(
"(agentAtLocation {} {})".format(self.AGENT_NAME,
agent_final_loc))
elif self.goal_category == "retrieval":
self.goal_object_id = PlanParser.create_legal_object_name(
config['goal']['metadata']['target']['id'])
for obj in config['objects']:
if obj['id'] == self.goal_object_id:
continue
if "openable" in obj and obj["openable"] == True:
receptacle_object_id = PlanParser.create_legal_object_name(
obj['id'])
if "opened" in obj:
self.object_open_close_info[
receptacle_object_id] = True
else:
self.object_open_close_info[
receptacle_object_id] = False
self.goal_predicate_list.append("(held {} {})".format(
self.AGENT_NAME, self.goal_object_id))
elif self.goal_category == "transferral":
self.transfer_object_id = PlanParser.create_legal_object_name(
config['goal']['metadata']['target_1']['id'])
if config['goal_in_hand'] == True:
self.object_in_hand = PlanParser.create_legal_object_name(
config['id_goal_in_hand'])
self.target_object_id = PlanParser.create_legal_object_name(
config['goal']['metadata']['target_2']['id'])
for obj in config['objects']:
if obj['id'] == self.transfer_object_id:
continue
if "openable" in obj and obj["openable"] == True:
receptacle_object_id = PlanParser.create_legal_object_name(
obj['id'])
if "opened" in obj:
self.object_open_close_info[
receptacle_object_id] = True
else:
self.object_open_close_info[
receptacle_object_id] = False
if config['goal']['metadata']['relationship'][1] == "next to":
self.goal_predicate_list.append(
"(objectNextTo {} {})".format(self.transfer_object_id,
self.target_object_id))
elif config['goal']['metadata']['relationship'][
1] == "on top of":
self.goal_predicate_list.append(
"(objectOnTopOf {} {})".format(self.transfer_object_id,
self.target_object_id))
import time
from planner.ff_planner_handler import PlanParser
parser = PlanParser()
# Playroom Demo Plan
t_start = time.time()
result_plan = parser.get_plan_from_file(
"planner/domains/Playroom_domain.pddl", "planner/sample_problems/playroom_facts.pddl"
)
print("Playroom Plan")
print("Plan took %.2f seconds" % (time.time() - t_start))
for plan in result_plan:
print(plan)