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)