def process_frame(self, changed_object_id=None): self.event = self.env.last_event self.pose = game_util.get_pose(self.event) self.s_t_orig = self.event.frame self.s_t = game_util.imresize(self.event.frame, (constants.SCREEN_HEIGHT, constants.SCREEN_WIDTH), rescale=False) self.s_t_depth = game_util.imresize(self.event.depth_frame, (constants.SCREEN_HEIGHT, constants.SCREEN_WIDTH), rescale=False)
def explore_3d_scene(self, event, config_filename=None): number_actions = 0 success_distance = 0.3 self.scene_name = 'transferral_data' #print('New episode. Scene %s' % self.scene_name) self.agent.reset(self.scene_name, config_filename=config_filename, event=event) self.graph = graph_2d() #self.graph.reset() self.event = self.game_state.event #self.agent.nav.add_obstacle_from_step_output(self.event) #plan, path = self.agent.gt_graph.get_shortest_path( # pose, self.game_state.end_point) #print ("optimal path planning done", path, plan) num_iters = 0 exploration_routine = [] exploration_routine = cover_floor.flood_fill( 0, 0, cover_floor.check_validity) #print (exploration_routine, len(exploration_routine)) pose = game_util.get_pose(self.game_state.event)[:3] self.graph.update_seen(self.event.position['x'], self.event.position['z'], self.event.rotation, 100, self.event.camera_field_of_view, self.agent.nav.scene_obstacles_dict) unexplored = self.graph.get_unseen() print("before explore point ", len(unexplored)) unexplored = self.graph.get_unseen() print("after explore point", len(unexplored)) self.event = self.agent.game_state.event pose = game_util.get_pose(self.game_state.event)[:3] while (len(unexplored) > 35): start_time = time.time() if self.agent.game_state.goals_found: return print("before next best point calculation") max_visible = 0 max_visible_position = [] processed_points = {} start_time = time.time() print(exploration_routine) min_distance = 20 while (len(max_visible_position) == 0): for elem in exploration_routine: #number_visible_points = points_visible_from_position(exploration_routine[1][0],exploration_routine[1][1], self.event.camera_field_of_view,self.event.camera_clipping_planes[1] ) #number_visible_points = points_visible_from_position(self.event.position['x'],self.event.position['z'],self.event.camera_field_of_view,100,self.nav.scene_obstacles_dict,self.graph.graph ) distance_to_point = math.sqrt((pose[0] - elem[0])**2 + (pose[1] - elem[1])**2) if distance_to_point > min_distance and elem not in processed_points: new_visible_pts = self.graph.points_visible_from_position( elem[0] * constants.AGENT_STEP_SIZE, elem[1] * constants.AGENT_STEP_SIZE, self.event.camera_field_of_view, 100, self.agent.nav.scene_obstacles_dict) processed_points[elem] = new_visible_pts #if max_visible < number_visible_points/math.sqrt((pose[0]-elem[0])**2 + (pose[1]-elem[1])**2): if max_visible < new_visible_pts: #and abs(max_visible_points[-1][0] - elem[0]) > 2 and : max_visible_position.append(elem) max_visible = new_visible_pts min_distance = min_distance / 2 #points_visible(elem) max_visible_position.append((7, -7)) end_time = time.time() print(max_visible_position) print("time taken to select next position", end_time - start_time) if len(max_visible_position) == 0: return number_actions new_end_point = [0] * 3 new_end_point[0] = max_visible_position[-1][0] new_end_point[1] = max_visible_position[-1][1] new_end_point[2] = pose[2] exploration_routine.remove(max_visible_position[-1]) print("New goal selected : ", new_end_point) number_actions = self.agent.nav.go_to_goal(new_end_point, self.agent, success_distance, self.graph, True) if self.agent.game_state.goals_found: return self.graph.explore_point(self.agent.game_state.event.position['x'], self.agent.game_state.event.position['z'], self.agent, 42.5, self.agent.nav.scene_obstacles_dict) if self.agent.game_state.goals_found: return ''' flag = 0 object_id_to_search = "" for key,value in self.agent.game_state.discovered_explored.items(): for key_2,value_2 in value.items(): if key_2 == 0:# and key not in self.unexplored_objects: #self.unexplored_objects[key] = value graph_pos_x = math.floor(value_2['x']/constants.AGENT_STEP_SIZE) graph_pos_z = math.floor(value_2['z']/constants.AGENT_STEP_SIZE) if math.sqrt((abs(pose[0] -graph_pos_x))**2+(abs(pose[1]-graph_pos_z))**2) < 10: print ("objects nearby to explore ") flag = 1 optimal_plan, optimal_path = self.agent.gt_graph.get_shortest_path( pose, (graph_pos_x,graph_pos_z,pose[2])) object_id_to_search = key plan = optimal_plan path = optimal_path break if flag == 1 : break while len(plan) > 0: action = plan[0] self.agent.step(action) number_actions += 1 self.event = self.game_state.event plan = plan[1:] path.pop() pose = game_util.get_pose(self.game_state.event)[:3] #print ("pose_reached while going to object=" , pose) if flag == 1: action = {"action":"OpenObject", "objectId":object_id_to_search} #action = "OpenObject, objectId=%s" % object_id_to_search self.agent.step(action) number_actions += 1 self.event = self.game_state.event print ("return status of open object aciton:",self.agent.game_state.event.return_status) print ("agent pose : ", pose, ", object location", graph_pos_x,graph_pos_z) #while (self.agent.event.return_status != "SUCCESSFUL" ) or trials < 10 : # trials += 1 ''' unexplored = self.graph.get_unseen() print(len(unexplored)) end_time = time.time() print("Time taken for 1 loop run = ", end_time - start_time) return number_actions
def explore_scene_view(self, event, config_filename=None): number_actions = 0 success_distance = 0.3 self.scene_name = 'transferral_data' # print('New episode. Scene %s' % self.scene_name) self.agent.reset(self.scene_name, config_filename=config_filename, event=event) self.event = self.agent.game_state.event #rotation = self.agent.game_state.event.rotation / 180 * math.pi cover_floor.update_seen(self.event.position['x'], self.event.position['z'], self.agent.game_state, self.event.rotation, self.event.camera_field_of_view, self.agent.nav.scene_obstacles_dict.values()) cover_floor.explore_initial_point( self.event.position['x'], self.event.position['z'], self.agent, self.agent.nav.scene_obstacles_dict.values()) exploration_routine = cover_floor.flood_fill( 0, 0, cover_floor.check_validity) if self.agent.game_state.goals_found: return pose = game_util.get_pose(self.game_state.event)[:3] x_list, y_list = [], [] for key, value in self.agent.nav.scene_obstacles_dict.items(): x_list.append(min(value.x_list)) x_list.append(max(value.x_list)) y_list.append(min(value.y_list)) y_list.append(max(value.y_list)) #print ("bounds", min(x_list),max(x_list),min(y_list),max(y_list)) x_min = min(x_list) x_max = max(x_list) y_min = min(y_list) y_max = max(y_list) #outer_poly = Polygon(zip([x_min,x_min,x_max,x_max],[y_min,y_max,y_min,y_max])) #outer_poly_new = outer_poly.difference(self.agent.game_state.world_poly) #print (value.x_list,value.y_list) #print (outer_poly_new.area) overall_area = abs(x_max - x_min) * abs(y_max - y_min) print(self.agent.game_state.world_poly.area) while overall_area * 0.65 > self.agent.game_state.world_poly.area or len( self.agent.game_state.discovered_objects) == 0: points_checked = 0 #z+=1 max_visible_position = [] processed_points = {} start_time = time.time() #print(exploration_routine) min_distance = 20 while (len(max_visible_position) == 0): max_visible = 0 for elem in exploration_routine: distance_to_point = math.sqrt((pose[0] - elem[0])**2 + (pose[1] - elem[1])**2) if distance_to_point > min_distance and elem not in processed_points: points_checked += 1 #for obstacle_key, obstacle in self.agent.nav.scene_obstacles_dict.items(): # if obstacle.contains_goal(elem): # continue new_visible_area = cover_floor.get_point_all_new_coverage( elem[0] * constants.AGENT_STEP_SIZE, elem[1] * constants.AGENT_STEP_SIZE, self.agent.game_state, self.agent.game_state.event.rotation, self.agent.nav.scene_obstacles_dict.values()) processed_points[elem] = new_visible_area #if max_visible < number_visible_points/math.sqrt((pose[0]-elem[0])**2 + (pose[1]-elem[1])**2): if max_visible < new_visible_area: #and abs(max_visible_points[-1][0] - elem[0]) > 2 and : max_visible_position.append(elem) max_visible = new_visible_area min_distance = min_distance / 2 if min_distance < 1: break #points_visible(elem) end_time = time.time() #max_visible_position = [(7,-7)] #print(max_visible_position) time_taken = end_time - start_time #print("time taken to select next position", end_time - start_time) #print ("points searched with area overlap", points_checked) if len(max_visible_position) == 0: return new_end_point = [0] * 3 new_end_point[ 0] = max_visible_position[-1][0] * constants.AGENT_STEP_SIZE new_end_point[ 1] = max_visible_position[-1][1] * constants.AGENT_STEP_SIZE new_end_point[2] = pose[2] #print("New goal selected : ", new_end_point) nav_success = self.agent.nav.go_to_goal(new_end_point, self.agent, success_distance) exploration_routine.remove(max_visible_position[-1]) if nav_success == False: continue self.event = self.agent.game_state.event if self.agent.game_state.goals_found: return cover_floor.explore_point( self.event.position['x'], self.event.position['z'], self.agent, self.agent.nav.scene_obstacles_dict.values()) if self.agent.game_state.goals_found: return if self.agent.game_state.number_actions > constants.MAX_STEPS: print("Too many actions performed") return if len(exploration_routine) == 0: print("explored a lot of points but objects not found") return all_explored = False while (all_explored == False): current_pos = self.agent.game_state.event.position min_distance = math.inf flag = 0 for object in self.agent.game_state.discovered_objects: #distance_to_object = #if object['explored'] == 0 and object['locationParent'] == None and len(object['dimensions']) >0: if object[ 'explored'] == 0: #and len(object['dimensions']) > 0:# and object['locationParent'] == None flag = 1 distance_to_object = math.sqrt( (current_pos['x'] - object['position']['x'])**2 + (current_pos['z'] - object['position']['z'])**2) else: continue if distance_to_object < min_distance: min_distance_obj_id = object['uuid'] min_distance = distance_to_object if flag == 0: all_explored = True break self.explore_object(min_distance_obj_id) action = { 'action': 'RotateLook', 'horizon': -(self.agent.game_state.event.head_tilt) } self.agent.step(action) if self.agent.game_state.goals_found: return if self.agent.game_state.number_actions > constants.MAX_STEPS: print("Too many actions performed") return
def step(self, action_or_ind): self.reward = -0.01 action, teleport_failure, should_fail = self.get_action(action_or_ind) t_start = time.time() if should_fail or teleport_failure: self.event.metadata['lastActionSuccess'] = False else: if action[ 'action'] != 'Teleport' or not constants.USE_NAVIGATION_AGENT: self.event = self.env.step(action) else: # Action is teleport and I should do low level navigation. pass new_pose = game_util.get_pose(self.event) point_dists = np.sum(np.abs(self.graph.points - np.array(new_pose)[:2]), axis=1) if np.min(point_dists) > 0.0001: print('Point teleport failure') closest_point = self.graph.points[np.argmin(point_dists)] self.event = self.env.step({ 'action': 'Teleport', 'x': closest_point[0] * constants.AGENT_STEP_SIZE, 'y': self.agent_height, 'z': closest_point[1] * constants.AGENT_STEP_SIZE, 'rotateOnTeleport': True, 'rotation': self.pose[2] * 90, }) else: closest_point = np.argmin(point_dists) if closest_point not in self.visited_locations: self.visited_locations.add(closest_point) self.times[2, 0] += time.time() - t_start self.times[2, 1] += 1 if self.times[2, 1] % 100 == 0: print('env step time %.3f' % (self.times[2, 0] / self.times[2, 1])) if self.event.metadata['lastActionSuccess']: self.process_frame() if action['action'] == 'OpenObject': if self.question_type_ind == 2 and action['objectId'].split( '|')[0] != constants.OBJECTS[self.question_target[1]]: self.reward -= 1.0 elif action['objectId'] not in self.opened_receptacles: if self.question_type_ind == 2 and action[ 'objectId'].split('|')[0] == constants.OBJECTS[ self.question_target[1]]: self.reward += 5.0 else: self.reward += 0.1 self.opened_receptacles.add(action['objectId']) elif action[ 'action'] == 'CloseObject' and self.question_type_ind != 2: if action['objectId'] not in self.closed_receptacles: self.reward += 0.1 self.closed_receptacles.add(action['objectId']) # Update seen objects related to question objs = game_util.get_objects_of_type( constants.OBJECTS[self.object_target], self.event.metadata) objs = [ obj for obj in objs if (obj['objectId'] in self.event.instance_detections2D and game_util.check_object_size( self.event.instance_detections2D[obj['objectId']])) ] for obj in objs: self.seen_obj1.add(obj['objectId']) if self.question_type_ind in {2, 3}: objs = game_util.get_objects_of_type( constants.OBJECTS[self.question_target[1]], self.event.metadata) objs = [ obj for obj in objs if (obj['objectId'] in self.event.instance_detections2D and game_util.check_object_size( self.event.instance_detections2D[obj['objectId']])) ] for obj in objs: self.seen_obj2.add(obj['objectId']) if not self.can_end: if self.question_type_ind == 0: self.can_end = len(self.seen_obj1) > 0 elif self.question_type_ind == 1: self.can_end = len(self.seen_obj1) == self.answer elif self.question_type_ind == 2: objs = game_util.get_objects_of_type( constants.OBJECTS[self.question_target[1]], self.event.metadata) if not self.answer: if objs[0]['openable']: if all([ obj['objectId'] in self.opened_receptacles for obj in objs ]): self.can_end = True else: if all([ obj['objectId'] in self.seen_obj2 for obj in objs ]): self.can_end = True else: objs = [ obj for obj in objs if (obj['objectId'] in self.event.instance_detections2D and game_util.check_object_size( self.event.instance_detections2D[ obj['objectId']])) ] for obj in objs: for contained_obj in obj['pivotSimObjs']: if contained_obj['objectId'] in self.seen_obj1: self.can_end = True else: self.reward -= 0.05
def process_frame(self, run_object_detection=False): self.im_count += 1 #print ("pose b4 manipulation", self.event.pose) self.pose = game_util.get_pose(self.event) #self.pose,pose_2 = game_util.get_pose(self.event) #print ("pose after manipulation", self.pose) #print ("pose after own manipulation", pose_2) #print () #for key,items in self.event.metadata.items(): #print (len(self.event.events)) i = 0 #for key,value in self.event.__dict__.items(): # if key == "frame" : # print (key,type(value),len(value))#value) # break # i += 1 return #print ("++++++ B$ function call") self.s_t_orig = self.event.frame self.s_t = game_util.imresize( self.event.frame, (constants.SCREEN_HEIGHT, constants.SCREEN_WIDTH), rescale=False) #print ("========== after function call") #print ("size of s_t", len(self.s_t)) #print ("type of s_t", type(self.s_t)) #return# #print ("predict depth , drawing ", constants.PREDICT_DEPTH,constants.DRAWING) if constants.DRAWING: self.detection_image = self.s_t_orig.copy() if constants.PREDICT_DEPTH: print("in predict depth") t_start = time.time() self.s_t_depth = self.depth_estimator.get_depth(self.s_t) self.times[0, 0] += time.time() - t_start self.times[0, 1] += 1 if self.times[0, 1] % 100 == 0: print('depth time %.3f' % (self.times[0, 0] / self.times[0, 1])) elif constants.RENDER_DEPTH_IMAGE: self.s_t_depth = game_util.imresize( self.event.depth_frame, (constants.SCREEN_HEIGHT, constants.SCREEN_WIDTH), rescale=False) if (constants.GT_OBJECT_DETECTION or constants.OBJECT_DETECTION or (constants.END_TO_END_BASELINE and constants.USE_OBJECT_DETECTION_AS_INPUT) and not run_object_detection): if constants.OBJECT_DETECTION and not run_object_detection: # Get detections. t_start = time.time() boxes, scores, class_names = self.object_detector.detect( game_util.imresize(self.event.frame, (608, 608), rescale=False)) self.times[1, 0] += time.time() - t_start self.times[1, 1] += 1 if self.times[1, 1] % 100 == 0: print('detection time %.3f' % (self.times[1, 0] / self.times[1, 1])) mask_dict = {} used_inds = [] inds = list(range(len(boxes))) for (ii, box, score, class_name) in zip(inds, boxes, scores, class_names): if class_name in constants.OBJECT_CLASS_TO_ID: if class_name not in mask_dict: mask_dict[class_name] = np.zeros( (constants.SCREEN_HEIGHT, constants.SCREEN_WIDTH), dtype=np.float32) mask_dict[class_name][box[1]:box[3] + 1, box[0]:box[2] + 1] += score used_inds.append(ii) mask_dict = {k: np.minimum(v, 1) for k, v in mask_dict.items()} used_inds = np.array(used_inds) if len(used_inds) > 0: boxes = boxes[used_inds] scores = scores[used_inds] class_names = class_names[used_inds] else: boxes = np.zeros((0, 4)) scores = np.zeros(0) class_names = np.zeros(0) masks = [mask_dict[class_name] for class_name in class_names] if constants.END_TO_END_BASELINE: self.detection_mask_image = np.zeros( (constants.SCREEN_HEIGHT, constants.SCREEN_WIDTH, len(constants.OBJECTS)), dtype=np.float32) for cls in constants.OBJECTS: if cls not in mask_dict: continue self.detection_mask_image[:, :, constants.OBJECT_CLASS_TO_ID[ cls]] = mask_dict[cls] else: scores = [] class_names = [] masks = [] for (k, v) in self.event.class_masks.items(): if k in constants.OBJECT_CLASS_TO_ID and len(v) > 0: scores.append(1) class_names.append(k) masks.append(v) if constants.END_TO_END_BASELINE: self.detection_mask_image = np.zeros( (constants.SCREEN_HEIGHT, constants.SCREEN_WIDTH, constants.NUM_CLASSES), dtype=np.uint8) for cls in constants.OBJECTS: if cls not in self.event.class_detections2D: continue for box in self.event.class_detections2D[cls]: self.detection_mask_image[ box[1]:box[3] + 1, box[0]:box[2] + 1, constants.OBJECT_CLASS_TO_ID[cls]] = 1 if constants.RENDER_DEPTH_IMAGE or constants.PREDICT_DEPTH: xzy = game_util.depth_to_world_coordinates( self.s_t_depth, self.pose, self.camera_height / constants.AGENT_STEP_SIZE) max_depth_mask = self.s_t_depth >= constants.MAX_DEPTH for ii in range(len(masks)): mask = masks[ii] mask_locs = (mask > 0) locations = xzy[mask_locs, :2] max_depth_locs = max_depth_mask[mask_locs] depth_locs = np.logical_not(max_depth_locs) locations = locations[depth_locs] score = mask[mask_locs] score = score[depth_locs] # remove outliers: locations = locations.reshape(-1, 2) locations = np.round(locations).astype(np.int32) locations -= np.array(self.bounds)[[0, 1]] locations[:, 0] = np.clip(locations[:, 0], 0, self.bounds[2] - 1) locations[:, 1] = np.clip(locations[:, 1], 0, self.bounds[3] - 1) locations, unique_inds = game_util.unique_rows( locations, return_index=True) score = score[unique_inds] curr_score = self.graph.memory[ locations[:, 1], locations[:, 0], constants.OBJECT_CLASS_TO_ID[class_names[ii]] + 1] avg_locs = np.logical_and(curr_score > 0, curr_score < 1) curr_score[avg_locs] = curr_score[avg_locs] * .5 + score[ avg_locs] * .5 curr_score[curr_score == 0] = score[curr_score == 0] self.graph.memory[ locations[:, 1], locations[:, 0], constants.OBJECT_CLASS_TO_ID[class_names[ii]] + 1] = curr_score # inverse marked as empty locations = xzy[np.logical_not(mask_locs), :2] max_depth_locs = max_depth_mask[np.logical_not(mask_locs)] depth_locs = np.logical_not(max_depth_locs) locations = locations[depth_locs] locations = locations.reshape(-1, 2) locations = np.round(locations).astype(np.int32) locations[:, 0] = np.clip(locations[:, 0], self.bounds[0], self.bounds[0] + self.bounds[2] - 1) locations[:, 1] = np.clip(locations[:, 1], self.bounds[1], self.bounds[1] + self.bounds[3] - 1) locations = game_util.unique_rows(locations) locations -= np.array(self.bounds)[[0, 1]] curr_score = self.graph.memory[ locations[:, 1], locations[:, 0], constants.OBJECT_CLASS_TO_ID[class_names[ii]] + 1] replace_locs = np.logical_and(curr_score > 0, curr_score < 1) curr_score[replace_locs] = curr_score[replace_locs] * .8 self.graph.memory[ locations[:, 1], locations[:, 0], constants.OBJECT_CLASS_TO_ID[class_names[ii]] + 1] = curr_score if constants.DRAWING: if constants.GT_OBJECT_DETECTION: boxes = [] scores = [] class_names = [] for k, v in self.event.class_detections2D.items(): if k in constants.OBJECT_CLASS_TO_ID and len(v) > 0: boxes.extend(v) scores.extend([1] * len(v)) class_names.extend([k] * len(v)) boxes = np.array(boxes) scores = np.array(scores) self.detection_image = detector.visualize_detections( self.event.frame, boxes, class_names, scores)
def step(self, action_or_ind, process_frame=True): if self.event is None: self.event = self.env.last_event self.current_frame_count += 1 self.total_frame_count += 1 action, should_fail = self.get_action(action_or_ind) if should_fail: self.env.last_event.metadata['lastActionSuccess'] = False else: t_start = time.time() start_pose = game_util.get_pose(self.event) if 'action' not in action or action['action'] is None or action['action'] == 'None': self.env.last_event.metadata['lastActionSuccess'] = True else: if constants.RECORD_VIDEO_IMAGES: im_ind = len(glob.glob(constants.save_path + '/*.png')) if 'Teleport' in action['action']: position = self.env.last_event.metadata['agent']['position'] rotation = self.env.last_event.metadata['agent']['rotation'] start_horizon = self.env.last_event.metadata['agent']['cameraHorizon'] start_rotation = rotation['y'] if (np.abs(action['x'] - position['x']) > 0.001 or np.abs(action['z'] - position['z']) > 0.001): # Movement for xx in np.arange(.1, 1, .1): new_action = copy.deepcopy(action) new_action['x'] = np.round(position['x'] * (1 - xx) + action['x'] * xx, 5) new_action['z'] = np.round(position['z'] * (1 - xx) + action['z'] * xx, 5) new_action['rotation'] = start_rotation new_action['horizon'] = start_horizon self.event = self.env.step(new_action) cv2.imwrite(constants.save_path + '/%09d.png' % im_ind, self.event.frame[:, :, ::-1]) depth_image = self.event.depth_frame depth_image = depth_image * (255 / MAX_DEPTH) depth_image = depth_image.astype(np.uint8) cv2.imwrite(constants.save_depth_path + '/%09d.png' % im_ind, depth_image) game_util.store_image_name('%09d.png' % im_ind) # eww... seriously need to clean this up im_ind += 1 if np.abs(action['horizon'] - self.env.last_event.metadata['agent']['cameraHorizon']) > 0.001: end_horizon = action['horizon'] for xx in np.arange(.1, 1, .1): new_action = copy.deepcopy(action) new_action['horizon'] = np.round(start_horizon * (1 - xx) + end_horizon * xx, 3) new_action['rotation'] = start_rotation self.event = self.env.step(new_action) cv2.imwrite(constants.save_path + '/%09d.png' % im_ind, self.event.frame[:, :, ::-1]) depth_image = self.event.depth_frame depth_image = depth_image * (255 / MAX_DEPTH) depth_image = depth_image.astype(np.uint8) cv2.imwrite(constants.save_depth_path + '/%09d.png' % im_ind, depth_image) game_util.store_image_name('%09d.png' % im_ind) im_ind += 1 if np.abs(action['rotation'] - rotation['y']) > 0.001: end_rotation = action['rotation'] for xx in np.arange(.1, 1, .1): new_action = copy.deepcopy(action) new_action['rotation'] = np.round(start_rotation * (1 - xx) + end_rotation * xx, 3) self.event = self.env.step(new_action) cv2.imwrite(constants.save_path + '/%09d.png' % im_ind, self.event.frame[:, :, ::-1]) depth_image = self.event.depth_frame depth_image = depth_image * (255 / MAX_DEPTH) depth_image = depth_image.astype(np.uint8) cv2.imwrite(constants.save_depth_path + '/%09d.png' % im_ind, depth_image) game_util.store_image_name('%09d.png' % im_ind) im_ind += 1 self.event = self.env.step(action) elif 'MoveAhead' in action['action']: self.store_ll_action(action) self.save_image(1) events = self.env.smooth_move_ahead(action) for event in events: im_ind = len(glob.glob(constants.save_path + '/*.png')) cv2.imwrite(constants.save_path + '/%09d.png' % im_ind, event.frame[:, :, ::-1]) depth_image = event.depth_frame depth_image = depth_image * (255 / MAX_DEPTH) depth_image = depth_image.astype(np.uint8) cv2.imwrite(constants.save_depth_path + '/%09d.png' % im_ind, depth_image) game_util.store_image_name('%09d.png' % im_ind) elif 'Rotate' in action['action']: self.store_ll_action(action) self.save_image(1) events = self.env.smooth_rotate(action) for event in events: im_ind = len(glob.glob(constants.save_path + '/*.png')) cv2.imwrite(constants.save_path + '/%09d.png' % im_ind, event.frame[:, :, ::-1]) depth_image = event.depth_frame depth_image = depth_image * (255 / MAX_DEPTH) depth_image = depth_image.astype(np.uint8) cv2.imwrite(constants.save_depth_path + '/%09d.png' % im_ind, depth_image) game_util.store_image_name('%09d.png' % im_ind) elif 'Look' in action['action']: self.store_ll_action(action) self.save_image(1) events = self.env.smooth_look(action) for event in events: im_ind = len(glob.glob(constants.save_path + '/*.png')) cv2.imwrite(constants.save_path + '/%09d.png' % im_ind, event.frame[:, :, ::-1]) depth_image = event.depth_frame depth_image = depth_image * (255 / MAX_DEPTH) depth_image = depth_image.astype(np.uint8) cv2.imwrite(constants.save_depth_path + '/%09d.png' % im_ind, depth_image) game_util.store_image_name('%09d.png' % im_ind) elif 'OpenObject' in action['action']: open_action = dict(action=action['action'], objectId=action['objectId'], moveMagnitude=1.0) self.store_ll_action(open_action) self.save_act_image(open_action, dir=constants.BEFORE) self.event = self.env.step(open_action) self.save_act_image(open_action, dir=constants.AFTER) self.check_action_success(self.event) elif 'CloseObject' in action['action']: close_action = dict(action=action['action'], objectId=action['objectId']) self.store_ll_action(close_action) self.save_act_image(close_action, dir=constants.BEFORE) self.event = self.env.step(close_action) self.save_act_image(close_action, dir=constants.AFTER) self.check_action_success(self.event) elif 'PickupObject' in action['action']: # [hack] correct object ids of slices action['objectId'] = self.correct_slice_id(action['objectId']) # open the receptacle if needed parent_recep = self.get_parent_receps(action['objectId']) if parent_recep is not None and parent_recep['objectType'] in constants.OPENABLE_CLASS_SET: self.open_recep(parent_recep) # stores LL action # close/open the object if needed pickup_obj = game_util.get_object(action['objectId'], self.env.last_event.metadata) if pickup_obj['objectType'] in constants.FORCED_OPEN_STATE_ON_PICKUP: if pickup_obj['isOpen'] != constants.FORCED_OPEN_STATE_ON_PICKUP[pickup_obj['objectType']]: if pickup_obj['isOpen']: self.close_recep(pickup_obj) # stores LL action else: self.open_recep(pickup_obj) # stores LL action # pick up the object self.check_obj_visibility(action, min_pixels=10) pickup_action = dict(action=action['action'], objectId=action['objectId']) self.store_ll_action(pickup_action) self.save_act_image(pickup_action, dir=constants.BEFORE) self.event = self.env.step(pickup_action) self.save_act_image(pickup_action, dir=constants.AFTER) self.check_action_success(self.event) # close the receptacle if needed if parent_recep is not None: parent_recep = game_util.get_object(parent_recep['objectId'], self.env.last_event.metadata) self.close_recep(parent_recep) # stores LL action elif 'PutObject' in action['action']: if len(self.env.last_event.metadata['inventoryObjects']) > 0: inv_obj = self.env.last_event.metadata['inventoryObjects'][0]['objectId'] else: raise RuntimeError("Taking 'PutObject' action with no held inventory object") action['objectId'] = inv_obj # open the receptacle if needed parent_recep = game_util.get_object(action['receptacleObjectId'], self.env.last_event.metadata) if parent_recep is not None and parent_recep['objectType'] in constants.OPENABLE_CLASS_SET: self.open_recep(parent_recep) # stores LL action # Open the parent receptacle of the movable receptacle target. elif parent_recep['objectType'] in constants.MOVABLE_RECEPTACLES_SET: movable_parent_recep_ids = parent_recep['parentReceptacles'] if movable_parent_recep_ids is not None and len(movable_parent_recep_ids) > 0: print(parent_recep['objectId'], movable_parent_recep_ids) # DEBUG movable_parent_recep = game_util.get_object(movable_parent_recep_ids[0], self.env.last_event.metadata) if movable_parent_recep['objectType'] in constants.OPENABLE_CLASS_SET: self.open_recep(movable_parent_recep) # stores LL action # put the object put_action = dict(action=action['action'], objectId=action['receptacleObjectId'], forceAction=True, placeStationary=True) self.store_ll_action(put_action) self.save_act_image(put_action, dir=constants.BEFORE) self.event = self.env.step(put_action) self.save_act_image(put_action, dir=constants.AFTER) self.check_obj_visibility(action) self.check_action_success(self.event) # close the receptacle if needed if parent_recep is not None: parent_recep = game_util.get_object(parent_recep['objectId'], self.env.last_event.metadata) self.close_recep(parent_recep) # stores LL action elif 'CleanObject' in action['action']: # put the object in the sink sink_obj_id = self.get_some_visible_obj_of_name('SinkBasin')['objectId'] inv_obj = self.env.last_event.metadata['inventoryObjects'][0] put_action = dict(action='PutObject', objectId=sink_obj_id, forceAction=True, placeStationary=True) self.store_ll_action(put_action) self.save_act_image(put_action, dir=constants.BEFORE) self.event = self.env.step(put_action) self.save_act_image(put_action, dir=constants.AFTER) self.check_obj_visibility(put_action) self.check_action_success(self.event) # turn on the tap clean_action = copy.deepcopy(action) clean_action['action'] = 'ToggleObjectOn' clean_action['objectId'] = game_util.get_obj_of_type_closest_to_obj( "Faucet", inv_obj['objectId'], self.env.last_event.metadata)['objectId'] self.store_ll_action(clean_action) self.save_act_image(clean_action, dir=constants.BEFORE) self.event = self.env.step({k: clean_action[k] for k in ['action', 'objectId']}) self.save_act_image(clean_action, dir=constants.AFTER) self.check_action_success(self.event) # Need to delay one frame to let `isDirty` update on stream-affected. self.env.noop() # Call built-in 'CleanObject' THOR action on every object in the SinkBasin. # This means we clean everything in the sink, rather than just the things that happen to touch # the water stream, which is the default simulator behavior but looks weird for our purposes. sink_basin_obj = game_util.get_obj_of_type_closest_to_obj( "SinkBasin", clean_action['objectId'], self.env.last_event.metadata) for in_sink_obj_id in sink_basin_obj['receptacleObjectIds']: if (game_util.get_object(in_sink_obj_id, self.env.last_event.metadata)['dirtyable'] and game_util.get_object(in_sink_obj_id, self.env.last_event.metadata)['isDirty']): self.event = self.env.step({'action': 'CleanObject', 'objectId': in_sink_obj_id}) # turn off the tap close_action = copy.deepcopy(clean_action) close_action['action'] = 'ToggleObjectOff' self.store_ll_action(close_action) self.save_act_image(close_action, dir=constants.BEFORE) self.event = self.env.step({k: close_action[k] for k in ['action', 'objectId']}) self.save_act_image(action, dir=constants.AFTER) self.check_action_success(self.event) # pick up the object from the sink pickup_action = dict(action='PickupObject', objectId=inv_obj['objectId']) self.store_ll_action(pickup_action) self.save_act_image(pickup_action, dir=constants.BEFORE) self.event = self.env.step(pickup_action) self.save_act_image(pickup_action, dir=constants.AFTER) self.check_obj_visibility(pickup_action) self.check_action_success(self.event) elif 'HeatObject' in action['action']: # open the microwave microwave_obj_id = self.get_some_visible_obj_of_name('Microwave')['objectId'] microwave_obj = game_util.get_object(microwave_obj_id, self.env.last_event.metadata) self.open_recep(microwave_obj) # put the object in the microwave inv_obj = self.env.last_event.metadata['inventoryObjects'][0] put_action = dict(action='PutObject', objectId=microwave_obj_id, forceAction=True, placeStationary=True) self.store_ll_action(put_action) self.save_act_image(put_action, dir=constants.BEFORE) self.event = self.env.step(put_action) self.save_act_image(put_action, dir=constants.AFTER) self.check_obj_visibility(put_action) self.check_action_success(self.event) # close the microwave microwave_obj = game_util.get_object(microwave_obj_id, self.env.last_event.metadata) self.close_recep(microwave_obj) # turn on the microwave heat_action = copy.deepcopy(action) heat_action['action'] = 'ToggleObjectOn' heat_action['objectId'] = microwave_obj_id self.store_ll_action(heat_action) self.save_act_image(heat_action, dir=constants.BEFORE) self.event = self.env.step({k: heat_action[k] for k in ['action', 'objectId']}) self.save_act_image(heat_action, dir=constants.AFTER) # turn off the microwave stop_action = copy.deepcopy(heat_action) stop_action['action'] = 'ToggleObjectOff' self.store_ll_action(stop_action) self.save_act_image(stop_action, dir=constants.BEFORE) self.event = self.env.step({k: stop_action[k] for k in ['action', 'objectId']}) self.save_act_image(stop_action, dir=constants.AFTER) # open the microwave microwave_obj = game_util.get_object(microwave_obj_id, self.env.last_event.metadata) self.open_recep(microwave_obj) # pick up the object from the microwave pickup_action = dict(action='PickupObject', objectId=inv_obj['objectId']) self.store_ll_action(pickup_action) self.save_act_image(pickup_action, dir=constants.BEFORE) self.event = self.env.step(pickup_action) self.save_act_image(pickup_action, dir=constants.AFTER) self.check_obj_visibility(pickup_action) self.check_action_success(self.event) # close the microwave again microwave_obj = game_util.get_object(microwave_obj_id, self.env.last_event.metadata) self.close_recep(microwave_obj) elif 'CoolObject' in action['action']: # open the fridge fridge_obj_id = self.get_some_visible_obj_of_name('Fridge')['objectId'] fridge_obj = game_util.get_object(fridge_obj_id, self.env.last_event.metadata) self.open_recep(fridge_obj) # put the object in the fridge inv_obj = self.env.last_event.metadata['inventoryObjects'][0] put_action = dict(action='PutObject', objectId=fridge_obj_id, forceAction=True, placeStationary=True) self.store_ll_action(put_action) self.save_act_image(put_action, dir=constants.BEFORE) self.event = self.env.step(put_action) self.save_act_image(put_action, dir=constants.AFTER) self.check_obj_visibility(put_action) self.check_action_success(self.event) # close and cool the object inside the frige cool_action = dict(action='CloseObject', objectId=action['receptacleObjectId']) self.store_ll_action(cool_action) self.save_act_image(action, dir=constants.BEFORE) self.event = self.env.step(cool_action) self.save_act_image(action, dir=constants.MIDDLE) self.save_act_image(action, dir=constants.AFTER) # open the fridge again fridge_obj = game_util.get_object(fridge_obj_id, self.env.last_event.metadata) self.open_recep(fridge_obj) # pick up the object from the fridge pickup_action = dict(action='PickupObject', objectId=inv_obj['objectId']) self.store_ll_action(pickup_action) self.save_act_image(pickup_action, dir=constants.BEFORE) self.event = self.env.step(pickup_action) self.save_act_image(pickup_action, dir=constants.AFTER) self.check_obj_visibility(pickup_action) self.check_action_success(self.event) # close the fridge again fridge_obj = game_util.get_object(fridge_obj_id, self.env.last_event.metadata) self.close_recep(fridge_obj) elif 'ToggleObject' in action['action']: on_action = dict(action=action['action'], objectId=action['objectId']) toggle_obj = game_util.get_object(action['objectId'], self.env.last_event.metadata) on_action['action'] = 'ToggleObjectOff' if toggle_obj['isToggled'] else 'ToggleObjectOn' self.store_ll_action(on_action) self.save_act_image(on_action, dir=constants.BEFORE) self.event = self.env.step(on_action) self.save_act_image(on_action, dir=constants.AFTER) self.check_action_success(self.event) elif 'SliceObject' in action['action']: # open the receptacle if needed parent_recep = self.get_parent_receps(action['objectId']) if parent_recep is not None and parent_recep['objectType'] in constants.OPENABLE_CLASS_SET: self.open_recep(parent_recep) # slice the object slice_action = dict(action=action['action'], objectId=action['objectId']) self.store_ll_action(slice_action) self.save_act_image(slice_action, dir=constants.BEFORE) self.event = self.env.step(slice_action) self.save_act_image(action, dir=constants.AFTER) self.check_action_success(self.event) # close the receptacle if needed if parent_recep is not None: parent_recep = game_util.get_object(parent_recep['objectId'], self.env.last_event.metadata) self.close_recep(parent_recep) # stores LL action else: # check that the object to pick is visible in the camera frame if action['action'] == 'PickupObject': self.check_obj_visibility(action) self.store_ll_action(action) self.save_act_image(action, dir=constants.BEFORE) self.event = self.env.step(action) self.save_act_image(action, dir=constants.AFTER) if action['action'] == 'PutObject': self.check_obj_visibility(action) else: self.event = self.env.step(action) new_pose = game_util.get_pose(self.event) point_dists = np.sum(np.abs(self.gt_graph.points - np.array(new_pose)[:2]), axis=1) if np.min(point_dists) > 0.0001: print('Point teleport failure') self.event = self.env.step({ 'action': 'Teleport', 'x': start_pose[0] * constants.AGENT_STEP_SIZE, 'y': self.agent_height, 'z': start_pose[1] * constants.AGENT_STEP_SIZE, 'rotateOnTeleport': True, 'rotation': new_pose[2] * 90, }) self.env.last_event.metadata['lastActionSuccess'] = False self.timers[0, 0] += time.time() - t_start self.timers[0, 1] += 1 if self.timers[0, 1] % 100 == 0: print('env step time %.3f' % (self.timers[0, 0] / self.timers[0, 1])) self.timers[0, :] = 0 if self.env.last_event.metadata['lastActionSuccess']: if action['action'] == 'OpenObject': self.currently_opened_object_ids.add(action['objectId']) elif action['action'] == 'CloseObject': self.currently_opened_object_ids.remove(action['objectId']) elif action['action'] == 'PickupObject': self.inventory_ids.add(action['objectId']) elif action['action'] == 'PutObject': self.inventory_ids.remove(action['objectId']) if self.env.last_event.metadata['lastActionSuccess'] and process_frame: self.process_frame()
def reset(self, scene_num=None, use_gt=True, seed=None, max_num_repeats=constants.MAX_NUM_OF_OBJ_INSTANCES, remove_prob=None, scene=None, objs=None): # Reset should be called only when all information from a scene should be cleared. self.current_frame_count = 0 self.timers = np.zeros((2, 2)) self.board = None self.original_board = None self.currently_opened_object_ids = SetWithGet() self.cleaned_object_ids = SetWithGet() self.hot_object_ids = SetWithGet() self.cool_object_ids = SetWithGet() self.on_object_ids = set() self.toggleable_object_ids = set() self.sliced_object_ids = set() self.inventory_ids = SetWithGet() self.scene_name = None self.bounds = None self.start_point = None self.event = None self.s_t = None self.s_t_orig = None self.s_t_depth = None self.pose = None self.agent_height = None self.camera_height = None new_scene = (self.gt_graph is None or self.gt_graph.scene_id is None or self.gt_graph.scene_id == scene_num) self.scene_num = scene_num if self.scene_num is None: self.scene_num = self.local_random.choice(constants.SCENE_NUMBERS) self.scene_num = self.scene_num if scene is not None: self.scene_num = scene['scene_num'] seed = scene['random_seed'] % 1000000 self.scene_name = 'FloorPlan%d' % self.scene_num self.event = self.env.reset(self.scene_name) if max_num_repeats is None: self.event = self.env.random_initialize(seed) else: self.env.step(dict( action='Initialize', gridSize=constants.AGENT_STEP_SIZE / constants.RECORD_SMOOTHING_FACTOR, cameraY=constants.CAMERA_HEIGHT_OFFSET, renderImage=constants.RENDER_IMAGE, renderDepthImage=constants.RENDER_DEPTH_IMAGE, renderClassImage=constants.RENDER_CLASS_IMAGE, renderObjectImage=constants.RENDER_OBJECT_IMAGE, visibility_distance=constants.VISIBILITY_DISTANCE, makeAgentsVisible=False, )) free_per_receptacle = [] if objs is not None: if 'sparse' in objs: for o, c in objs['sparse']: free_per_receptacle.append({'objectType': o, 'count': c}) if 'empty' in objs: for o, c in objs['empty']: free_per_receptacle.append({'objectType': o, 'count': c}) self.env.step(dict(action='InitialRandomSpawn', randomSeed=seed, forceVisible=True, numDuplicatesOfType=[{'objectType': o, 'count': c} for o, c in objs['repeat']] if objs is not None and 'repeat' in objs else None, excludedReceptacles=[obj['objectType'] for obj in free_per_receptacle] )) # if 'clean' action, make everything dirty and empty out fillable things if constants.pddl_goal_type == "pick_clean_then_place_in_recep": # need to create a dictionary that contains all the object's type and state change. for o in objs['repeat']: self.env.step(dict(action='SetObjectStates', SetObjectStates={'objectType': o[0], 'stateChange': 'dirtyable', 'isDirty': True})) self.env.step(dict(action='SetObjectStates', SetObjectStates={'objectType': o[0], 'stateChange': 'canFillWithLiquid', 'isFilledWithLiquid': False})) if objs is not None and ('seton' in objs and len(objs['seton']) > 0): for o, v in objs['seton']: self.env.step(dict(action='SetObjectStates', SetObjectStates={'objectType': o, 'stateChange': 'toggleable', 'isToggled': v})) self.gt_graph = graph_obj.Graph(use_gt=True, construct_graph=True, scene_id=self.scene_num) if seed is not None: self.local_random.seed(seed) print('set seed in game_state_base reset', seed) self.bounds = np.array([self.gt_graph.xMin, self.gt_graph.yMin, self.gt_graph.xMax - self.gt_graph.xMin + 1, self.gt_graph.yMax - self.gt_graph.yMin + 1]) self.agent_height = self.env.last_event.metadata['agent']['position']['y'] self.camera_height = self.agent_height + constants.CAMERA_HEIGHT_OFFSET start_point = self.local_random.randint(0, self.gt_graph.points.shape[0] - 1) start_point = self.gt_graph.points[start_point, :].copy() self.start_point = (start_point[0], start_point[1], self.local_random.randint(0, 3)) action = {'action': 'TeleportFull', 'x': self.start_point[0] * constants.AGENT_STEP_SIZE, 'y': self.agent_height, 'z': self.start_point[1] * constants.AGENT_STEP_SIZE, 'rotateOnTeleport': True, 'horizon': 30, 'rotation': self.start_point[2] * 90, } self.event = self.env.step(action) constants.data_dict['scene']['scene_num'] = self.scene_num constants.data_dict['scene']['init_action'] = action constants.data_dict['scene']['floor_plan'] = self.scene_name constants.data_dict['scene']['random_seed'] = seed self.pose = game_util.get_pose(self.event) # Manually populate parentReceptacles data based on existing ReceptableObjectIds. objects = self.env.last_event.metadata['objects'] for idx in range(len(objects)): if objects[idx]['parentReceptacles'] is None: objects[idx]['parentReceptacles'] = [] for jdx in range(len(objects)): if (objects[jdx]['receptacleObjectIds'] is not None and objects[idx]['objectId'] in objects[jdx]['receptacleObjectIds']): objects[idx]['parentReceptacles'].append(objects[jdx]['objectId'])
def state_to_pddl(self, traj_data): object_dict = game_util.get_object_dict(self.env.last_event.metadata) domain = 'alfred' problem_id = traj_data['task_id'] pddl_params = traj_data['pddl_params'] points_source = 'gen/layouts/%s-openable.json' % traj_data['scene'][ 'floor_plan'] with open(points_source, 'r') as f: openable_object_to_point = json.load(f) self.openable_object_to_point = openable_object_to_point receptacle_types = copy.deepcopy(constants.RECEPTACLES) - set( constants.MOVABLE_RECEPTACLES) objects = copy.deepcopy(constants.OBJECTS_SET) - receptacle_types object_str = '\n '.join([obj + ' # object' for obj in objects]) self.knife_obj = {'ButterKnife', 'Knife' } if pddl_params['object_sliced'] else {} otype_str = '\n '.join( [obj + 'Type # otype' for obj in objects]) rtype_str = '\n '.join( [obj + 'Type # rtype' for obj in receptacle_types]) pddl_goal = get_goal_pddl(traj_data) pddl_start = ''' (define (problem plan_%s) (:domain %s) (:objects agent1 # agent %s %s %s ''' % ( problem_id, domain, object_str, otype_str, rtype_str, ) pddl_init = ''' (:init ''' pddl_start = fix_pddl_str_chars(pddl_start) pddl_init = fix_pddl_str_chars(pddl_init) pddl_goal = fix_pddl_str_chars(pddl_goal) # pddl_mid section pose = game_util.get_pose(self.env.last_event) agent_location = 'loc|%d|%d|%d|%d' % (pose[0], pose[1], pose[2], pose[3]) #agent_location = "Middle of the room." agent_location_str = '\n (atLocation agent1 %s)' % agent_location opened_receptacle_str = '\n '.join( ['(opened %s)' % obj for obj in list()]) movable_recep_cls_with_knife = [] in_receptacle_strs = [] was_in_receptacle_strs = [] for key, val in self.in_receptacle_ids.items(): if len(val) == 0: continue key_cls = object_dict[key]['objectType'] if key_cls in constants.MOVABLE_RECEPTACLES_SET: recep_str = 'inReceptacleObject' else: recep_str = 'inReceptacle' for vv in val: vv_cls = object_dict[vv]['objectType'] if (vv_cls == pddl_params['object_target'] or (pddl_params['mrecep_target'] is not None and vv_cls == pddl_params['mrecep_target']) or (len(self.knife_obj) > 0 and vv_cls in self.knife_obj)): # if knife is inside a movable receptacle, make sure to add it to the object list if recep_str == 'inReceptacleObject': movable_recep_cls_with_knife.append(key_cls) in_receptacle_strs.append('(%s %s %s)' % (recep_str, vv, key)) # if key_cls not in constants.MOVABLE_RECEPTACLES_SET and vv_cls == pddl_params['object_target']: # was_in_receptacle_strs.append('(wasInReceptacle %s %s)' % (vv, key)) in_receptacle_str = '\n '.join(in_receptacle_strs) was_in_receptacle_str = '\n '.join(was_in_receptacle_strs) # Note which openable receptacles we can safely open (precomputed). openable_objects = self.openable_object_to_point.keys() metadata_objects = self.env.last_event.metadata['objects'] receptacles = set({ obj['objectId'] for obj in metadata_objects if obj['objectType'] in constants.RECEPTACLES and obj['objectType'] not in constants.MOVABLE_RECEPTACLES_SET }) objects = set({ obj['objectId'] for obj in metadata_objects if (obj['objectType'] == pddl_params['object_target'] or obj['objectType'] in constants.MOVABLE_RECEPTACLES_SET or ( pddl_params['mrecep_target'] is not None and obj['objectType'] == pddl_params['mrecep_target']) or ((pddl_params['toggle_target'] is not None and obj['objectType'] == pddl_params['toggle_target']) or ( (len(self.knife_obj) > 0 and (obj['objectType'] in self.knife_obj or obj['objectType'] in movable_recep_cls_with_knife))))) }) objects = set(obj['objectId'] for obj in metadata_objects if obj['objectId'] not in receptacles) #from ipdb import set_trace; set_trace() if len(self.inventory_ids) > 0: objects = objects | self.inventory_ids if len(self.placed_items) > 0: objects = objects | self.placed_items receptacle_str = '\n '.join( sorted( [receptacle + ' # receptacle' for receptacle in receptacles])) object_str = '\n '.join( sorted([obj + ' # object' for obj in objects])) locations = set() for key, val in self.receptacle_to_point.items(): key_cls = object_dict[key]['objectType'] if key_cls not in constants.MOVABLE_RECEPTACLES_SET: locations.add(tuple(val.tolist())) # locations.add(key + "|loc") for obj, loc in self.object_to_point.items(): obj_cls = object_dict[obj]['objectType'] # if (obj_cls == pddl_params['object_target'] or # (pddl_params['toggle_target'] is not None and obj_cls == pddl_params['toggle_target']) or # (len(self.knife_obj) > 0 and obj_cls in self.knife_obj) or # (obj_cls in constants.MOVABLE_RECEPTACLES_SET)): if obj_cls in constants.OBJECTS: locations.add(tuple(loc)) location_str = ('\n '.join( ['loc|%d|%d|%d|%d # location' % (*loc, ) for loc in locations]) + '\n %s # location' % agent_location) #location_str = ('\n '.join(['loc|%d|%d|%d|%d # location' % (*loc,) # location_str = ('\n '.join(['{loc} # location'.format(loc=loc) for loc in locations]) + # '\n %s # location' % agent_location) # location_str = '\n %s # location' % agent_location if constants.PRUNE_UNREACHABLE_POINTS: # don't flag problematic receptacleTypes for the planner. receptacle_type_str = '\n '.join([ '(receptacleType %s %sType)' % (receptacle, object_dict[receptacle]['objectType']) for receptacle in receptacles if object_dict[receptacle]['objectType'] not in constants.OPENABLE_CLASS_SET or receptacle in openable_objects ]) else: receptacle_type_str = '\n '.join([ '(receptacleType %s %sType)' % (receptacle, object_dict[receptacle]['objectType']) for receptacle in receptacles ]) object_type_str = '\n '.join([ '(objectType %s %sType)' % (obj, object_dict[obj]['objectType']) for obj in objects if object_dict[obj]['objectType'] in constants.OBJECTS ]) receptacle_objects_str = '\n '.join([ '(isReceptacleObject %s)' % (obj) for obj in objects if object_dict[obj]['objectType'] in constants.MOVABLE_RECEPTACLES_SET ]) if constants.PRUNE_UNREACHABLE_POINTS: openable_str = '\n '.join([ '(openable %s)' % receptacle for receptacle in receptacles if object_dict[receptacle]['objectType'] in constants.OPENABLE_CLASS_SET ]) else: # don't flag problematic open objects as openable for the planner. openable_str = '\n '.join([ '(openable %s)' % receptacle for receptacle in receptacles if object_dict[receptacle]['objectType'] in constants.OPENABLE_CLASS_SET and receptacle in openable_objects ]) # dists = [] # dist_points = list(locations | {(pose[0], pose[1], pose[2], pose[3])}) # for dd, l_start in enumerate(dist_points[:-1]): # for l_end in dist_points[dd + 1:]: # actions, path = self.gt_graph.get_shortest_path_unweighted(l_start, l_end) # # Should cost one more for the trouble of going there at all. Discourages waypoints. # dist = len(actions) + 1 # dists.append('(= (distance loc|%d|%d|%d|%d loc|%d|%d|%d|%d) %d)' % ( # l_start[0], l_start[1], l_start[2], l_start[3], # l_end[0], l_end[1], l_end[2], l_end[3], dist)) # dists.append('(= (distance loc|%d|%d|%d|%d loc|%d|%d|%d|%d) %d)' % ( # l_end[0], l_end[1], l_end[2], l_end[3], # l_start[0], l_start[1], l_start[2], l_start[3], dist)) # location_distance_str = '\n '.join(dists) location_distance_str = "" # pickupable objects pickupable_str = '\n '.join([ '(pickupable %s)' % obj_id for obj_id in objects if object_dict[obj_id]["pickupable"] ]) # clean objects cleanable_str = '\n '.join([ '(cleanable %s)' % obj for obj in objects if object_dict[obj] ['objectType'] in constants.VAL_ACTION_OBJECTS['Cleanable'] ]) is_clean_str = '\n '.join(([ '(isClean %s)' % obj for obj in self.cleaned_object_ids if object_dict[obj]['objectType'] == pddl_params['object_target'] ])) # heat objects heatable_str = '\n '.join([ '(heatable %s)' % obj for obj in objects if object_dict[obj] ['objectType'] in constants.VAL_ACTION_OBJECTS['Heatable'] ]) is_hot_str = '\n '.join(([ '(isHot %s)' % obj for obj in self.hot_object_ids if object_dict[obj]['objectType'] == pddl_params['object_target'] ])) # cool objects coolable_str = '\n '.join([ '(coolable %s)' % obj for obj in objects if object_dict[obj] ['objectType'] in constants.VAL_ACTION_OBJECTS['Coolable'] ]) # toggleable objects toggleable_str = '\n '.join([ '(toggleable %s)' % obj for obj in objects if object_dict[obj] ['objectType'] in constants.VAL_ACTION_OBJECTS['Toggleable'] ]) is_on_str = '\n '.join([ '(isOn %s)' % obj for obj in self.on_object_ids if (pddl_params['toggle_target'] is not None and object_dict[obj] ['objectType'] == pddl_params['toggle_target']) ]) # sliceable objects sliceable_str = '\n '.join([ '(sliceable %s)' % obj for obj in objects if (object_dict[obj]['objectType'] in constants.VAL_ACTION_OBJECTS['Sliceable']) ]) # sliced objects # TODO cleanup: sliced_object_ids is never added to. Does that matter? is_sliced_str = '\n '.join(([ '(isSliced %s)' % obj for obj in self.sliced_object_ids if object_dict[obj]['objectType'] == pddl_params['object_target'] ])) # look for objects that are already cool for (key, val) in self.was_in_receptacle_ids.items(): if 'Fridge' in key: for vv in val: self.cool_object_ids.add(vv) is_cool_str = '\n '.join(([ '(isCool %s)' % obj for obj in self.cool_object_ids if object_dict[obj]['objectType'] == pddl_params['object_target'] ])) # Receptacle Objects recep_obj_str = '\n '.join([ '(isReceptacleObject %s)' % obj for obj in receptacles if (object_dict[obj]['objectType'] in constants.MOVABLE_RECEPTACLES_SET and (pddl_params['mrecep_target'] is not None and object_dict[obj] ['objectType'] == pddl_params['mrecep_target'])) ]) receptacle_nearest_point_strs = sorted( # ['(receptacleAtLocation {obj} {loc})'.format(obj=obj_id, loc=obj_id + "|loc") [ '(receptacleAtLocation %s loc|%d|%d|%d|%d)' % (obj_id, *point) for obj_id, point in self.receptacle_to_point.items() if (object_dict[obj_id]['objectType'] in constants.RECEPTACLES and object_dict[obj_id]['objectType'] not in constants.MOVABLE_RECEPTACLES_SET) ]) receptacle_at_location_str = '\n '.join( receptacle_nearest_point_strs) receptacle_can_holds = [] obj_types_in_scene = [ obj['objectType'] for obj in object_dict.values() ] for recep in receptacles: recep_type = object_dict[recep]['objectType'] for obj in constants.VAL_RECEPTACLE_OBJECTS[recep_type]: if obj in obj_types_in_scene: can_hold_str = '(canContain %sType %sType)' % (recep_type, obj) receptacle_can_holds.append(can_hold_str) receptacle_can_holds_str = '\n '.join(receptacle_can_holds) extra_facts = self.get_extra_facts() # salient_mat_str = '\n '.join(['(salientMaterials %s %s)' % (obj, ','.join(object_dict[obj]['salientMaterials'])) # for obj in objects if object_dict[obj]['salientMaterials'] is not None]) # mass_props_str = '\n '.join(['(mass %s %s)' % (obj, str(object_dict[obj]['mass'])) # for obj in objects]) # temp_props_str = '\n '.join(['(temp %s %s)' % (obj, object_dict[obj]['ObjectTemperature']) # for obj in objects]) pddl_mid_start = ''' %s %s %s ) ''' % ( object_str, receptacle_str, location_str, ) pddl_mid_init = ''' %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s %s ) ''' % ( receptacle_type_str, object_type_str, receptacle_can_holds_str, # salient_mat_str, # mass_props_str, # temp_props_str, pickupable_str, receptacle_objects_str, openable_str, agent_location_str, opened_receptacle_str, cleanable_str, is_clean_str, heatable_str, coolable_str, is_hot_str, is_cool_str, toggleable_str, is_on_str, recep_obj_str, sliceable_str, is_sliced_str, in_receptacle_str, was_in_receptacle_str, location_distance_str, receptacle_at_location_str, extra_facts, ) pddl_mid_start = fix_pddl_str_chars(pddl_mid_start) pddl_mid_init = fix_pddl_str_chars(pddl_mid_init) pddl_str = (pddl_start + '\n' + pddl_mid_start + '\n' + pddl_init + '\n' + pddl_mid_init + '\n' + pddl_goal) return pddl_str
def explore_scene(self, config_filename): self.scene_name = 'transferral_data' #print('New episode. Scene %s' % self.scene_name) self.agent.reset(self.scene_name, config_filename=config_filename) self.states = [] planning = random.random() < self.planner_prob #print ("reset done in sequence generator") label = self.agent.get_label() pose = game_util.get_pose(self.game_state.event)[:3] optimal_plan, optimal_path = self.agent.gt_graph.get_shortest_path( pose, self.game_state.end_point) plan = optimal_plan path = optimal_path print("optimal path planning done", path, plan) num_iters = 0 seen_terminal = False exploration_routine = [] exploration_routine = cover_floor.flood_fill( 0, 0, cover_floor.check_validity) print(exploration_routine, len(exploration_routine)) while ((not seen_terminal) and len(plan) != 0 and self.agent.is_possible >= constants.POSSIBLE_THRESH): num_iters += 1 #print ("In the while loop of executing the plan") action_vec = np.zeros(self.action_util.num_actions) while len(plan) > 0: action = plan[0] #print ("action to take" , action) self.event = self.agent.step(action) action_vec[self.action_util.action_dict_to_ind(action)] = 1 #plan.pop() #print(plan) plan = plan[1:] path.pop() pose = game_util.get_pose(self.game_state.event)[:3] print("pose_reached =", pose) #explore_objects() flag = 0 object_id_to_search = "" for key, value in self.agent.game_state.discovered_explored.items( ): for key_2, value_2 in value.items(): if key_2 == 0: # and key not in self.unexplored_objects: #self.unexplored_objects[key] = value graph_pos_x = math.floor(value_2['x'] / constants.AGENT_STEP_SIZE) graph_pos_z = math.floor(value_2['z'] / constants.AGENT_STEP_SIZE) if math.sqrt((abs(pose[0] - graph_pos_x))**2 + (abs(pose[1] - graph_pos_z))**2) < 10: print("objects nearby to explore ") flag = 1 optimal_plan, optimal_path = self.agent.gt_graph.get_shortest_path( pose, (graph_pos_x, graph_pos_z, pose[2])) object_id_to_search = key plan = optimal_plan path = optimal_path break if flag == 1: break while len(plan) > 0: action = plan[0] #print ("action to take" , action) self.event = self.agent.step(action) action_vec[self.action_util.action_dict_to_ind(action)] = 1 #plan.pop() #print(plan) plan = plan[1:] path.pop() pose = game_util.get_pose(self.game_state.event)[:3] print("pose_reached while going to object=", pose) if flag == 1: #action = {"action":"OpenObject", "objectId":object_id_to_search} action = "OpenObject, objectId:%s" % object_id_to_search self.event = self.agent.step(action) print("return status of open object aciton:", self.agent.game_state.event.return_status) ''' calculate_object_graph_position() for key,value in self.unexplored_objects.items(): for key_2,value_2 in value.items(): graph_pos_x = math.floor(value['x']/constants.AGENT_STEP_SIZE) graph_pos_z = math.floor(value['z']/constants.AGENT_STEP_SIZE) if math.sqrt((abs(pose[0] -graph_pos_x))**2+(abs(pose[1]-graph_pos_z))**2): optimal_plan, optimal_path = self.agent.gt_graph.get_shortest_path( pose, tuple(graph_pos_x,graph_pos,pose[2])) ''' #print ("done going to first end point") #print (exploration_routine, len(exploration_routine)) new_end_point = [0] * 3 if len(exploration_routine) > 0: new_end_point_data = exploration_routine.pop() else: break #newend_point[0] = new_end_point[0] #self.game_state.end_point[1] = new_end_point[1] #self.game_state.end_point[2] = self.game_state[2] new_end_point[0] = new_end_point_data[0] new_end_point[1] = new_end_point_data[1] new_end_point[2] = self.game_state.end_point[2] #print ("new starting point = ", pose) #print ("new ending point = ", tuple(new_end_point)) optimal_plan, optimal_path = self.agent.gt_graph.get_shortest_path( #pose, self.game_state.end_point) pose, tuple(new_end_point)) #if planning: # print ("new plannning") # plan, path = self.agent.get_plan() #else: # print ("not new plannning") plan = optimal_plan path = optimal_path print("optimal path planning done", path, plan) label = self.agent.get_label() #print ("self.states ", self.states) #seen_terminal = seen_terminal or int(len(optimal_plan) == 0) #self.agent.game_state.env.end_scene('', 0.0) self.bounds = [ self.game_state.graph.xMin, self.game_state.graph.yMin, self.game_state.graph.xMax - self.game_state.graph.xMin + 1, self.game_state.graph.yMax - self.game_state.graph.yMin + 1 ] goal_pose = np.array([ self.game_state.end_point[0] - self.game_state.graph.xMin, self.game_state.end_point[1] - self.game_state.graph.yMin ], dtype=np.int32)[:2] return (self.states, self.bounds, goal_pose)
def generate_episode(self): self.count += 1 self.states = [] self.debug_images = [] planning = random.random() < self.planner_prob while len(self.states) == 0: if self.count % 50000 == 0: #self.scene_name = 'FloorPlan%d' % random.choice(constants.SCENE_NUMBERS) #self.scene_name = 'FloorPlan%d' % 6#random.choice(constants.SCENE_NUMBERS) self.scene_name = 'transferral_data' print('New episode. Scene %s' % self.scene_name) self.agent.reset(self.scene_name) else: self.agent.reset() #self.agent.reset() print("reset done in sequence generator") label = self.agent.get_label() pose = game_util.get_pose(self.game_state.event)[:3] if constants.DRAWING: patch = self.game_state.graph.get_graph_patch(pose)[0] self.debug_images.append({ 'color': self.game_state.s_t, 'label': np.flipud(label), 'patch': np.flipud(patch), 'label_memory': np.minimum(np.flipud(self.agent.gt_graph.memory.copy()), 2), 'state_image': self.game_state.draw_state().copy(), 'memory_map': np.minimum(np.flipud(self.game_state.graph.memory.copy()), 10), 'pose_indicator': np.flipud(self.agent.pose_indicator), 'detections': self.game_state.detection_image if constants.OBJECT_DETECTION else None, 'weight': 1, 'possible_label': (1 if self.game_state.graph.memory[ self.game_state.end_point[1] - self.game_state.graph.yMin, self.game_state.end_point[0] - self.game_state.graph.xMin, 0] == 1 else self.game_state.is_possible_end_point), 'possible_pred': self.agent.is_possible, }) self.states.append({ 'color': self.game_state.s_t, 'pose': self.agent.gt_graph.get_shifted_pose(self.agent.pose)[:3], 'label': label, 'action': np.zeros(self.action_util.num_actions), 'pose_indicator': self.agent.pose_indicator, 'weight': 1, #'possible_label': (1 if self.game_state.graph.memory[ # self.game_state.end_point[1] - self.game_state.graph.yMin, # self.game_state.end_point[0] - self.game_state.graph.xMin, 0] == 1 # else self.game_state.is_possible_end_point), }) optimal_plan, optimal_path = self.agent.gt_graph.get_shortest_path( pose, self.game_state.end_point) if planning: plan, path = self.agent.get_plan() else: plan = optimal_plan path = optimal_path #print ("optimal path planning done", path, plan) num_iters = 0 seen_terminal = False while ((not seen_terminal) and len(plan) != 0 and self.agent.is_possible >= constants.POSSIBLE_THRESH): num_iters += 1 print("In the while loop of executing the plan") if constants.DEBUG: print('num iters', num_iters, 'max', constants.MAX_EPISODE_LENGTH) if num_iters > constants.MAX_EPISODE_LENGTH: print('Path length too long in scene', self.scene_name, 'goal_position', self.game_state.end_point, 'pose', pose, 'plan', plan) plan = [] break action_vec = np.zeros(self.action_util.num_actions) if len(plan) > 0: action = plan[0] #print ("action to take" , action) self.agent.step(action) action_vec[self.action_util.action_dict_to_ind(action)] = 1 pose = game_util.get_pose(self.game_state.event)[:3] optimal_plan, optimal_path = self.agent.gt_graph.get_shortest_path( pose, self.game_state.end_point) if planning: plan, path = self.agent.get_plan() else: plan = optimal_plan path = optimal_path label = self.agent.get_label() self.states.append({ 'color': self.game_state.s_t, 'pose': self.agent.gt_graph.get_shifted_pose(self.agent.pose)[:3], 'label': label, 'action': action_vec, 'pose_indicator': self.agent.pose_indicator, 'weight': 1, 'possible_label': (1 if self.game_state.graph.memory[ self.game_state.end_point[1] - self.game_state.graph.yMin, self.game_state.end_point[0] - self.game_state.graph.xMin, 0] == 1 else self.game_state.is_possible_end_point), }) #print ("self.states ", self.states) seen_terminal = seen_terminal or int(len(optimal_plan) == 0) if self.states[-1]['label'].shape != (constants.STEPS_AHEAD, constants.STEPS_AHEAD): self.states = [] print('Label is wrong size scene', self.scene_name, 'pose', pose) break if constants.DRAWING: patch = self.game_state.graph.get_graph_patch(pose)[0] self.debug_images.append({ 'color': self.game_state.s_t, 'label': np.flipud(label), 'patch': np.flipud(patch), 'label_memory': np.minimum( np.flipud(self.agent.gt_graph.memory.copy()), 2), 'state_image': self.game_state.draw_state().copy(), 'pose_indicator': np.flipud(self.agent.pose_indicator), 'detections': self.game_state.detection_image if constants.OBJECT_DETECTION else None, 'memory_map': np.minimum( np.flipud(self.game_state.graph.memory.copy()), 10), 'possible_label': (1 if self.game_state.graph.memory[ self.game_state.end_point[1] - self.game_state.graph.yMin, self.game_state.end_point[0] - self.game_state.graph.xMin, 0] == 1 else self.game_state.is_possible_end_point), 'possible_pred': self.agent.is_possible, }) break self.bounds = [ self.game_state.graph.xMin, self.game_state.graph.yMin, self.game_state.graph.xMax - self.game_state.graph.xMin + 1, self.game_state.graph.yMax - self.game_state.graph.yMin + 1 ] goal_pose = np.array([ self.game_state.end_point[0] - self.game_state.graph.xMin, self.game_state.end_point[1] - self.game_state.graph.yMin ], dtype=np.int32)[:2] return (self.states, self.bounds, goal_pose)
def go_to_goal(self, goal_pose, agent, success_distance, visibility_graph, SHOW_ANIMATION): #print ("current pose in go to goal", current_pose[1]) goal_pose[0], goal_pose[ 1] = goal_pose[0] * constants.AGENT_STEP_SIZE, goal_pose[ 1] * constants.AGENT_STEP_SIZE #goal_pose[0],goal_pose[1] = goal_pose[0]*5,goal_pose[1]*5 gx, gy = goal_pose[0], goal_pose[1] sx, sy = agent.game_state.event.position[ 'x'], agent.game_state.event.position['z'] self.agentX, self.agentY = sx, sy self.agentH = agent.game_state.event.rotation / 360 * (2 * math.pi) self.epsilon = success_distance actions = 0 print("beginning of go to goal", gx, gy, self.agentX, self.agentY, sx, sy) while True: dis_to_goal = math.sqrt((self.agentX - gx)**2 + (self.agentY - gy)**2) if dis_to_goal < self.epsilon: break roadmap = IncrementalVisibilityRoadMap(self.radius, do_plot=False) #roadmap = VisibilityRoadMap(self.radius, do_plot=False) for obstacle_key, obstacle in self.scene_obstacles_dict.items(): roadmap.addObstacle(obstacle) #if not obstacle.contains_goal((gx, gy)): # roadmap.addObstacle(obstacle) #if not obstacle.contains_goal((gx, gy)): # roadmap.addObstacle(obstacle) #fov = FieldOfView([sx, sy, 0], 42.5 / 180.0 * math.pi, self.scene_obstacles_dict.values()) fov = FieldOfView([self.agentX, self.agentY, 0], 42.5 / 180.0 * math.pi, self.scene_obstacles_dict.values()) fov.agentX = self.agentX fov.agentY = self.agentY fov.agentH = self.agentH poly = fov.getFoVPolygon(100) if SHOW_ANIMATION: plt.cla() plt.xlim((-7, 7)) plt.ylim((-7, 7)) plt.gca().set_xlim((-7, 7)) plt.gca().set_ylim((-7, 7)) plt.plot(self.agentX, self.agentY, "or") plt.plot(gx, gy, "ob") poly.plot("-r") for obstacle in self.scene_obstacles_dict.values(): obstacle.plot("-g") plt.axis("equal") plt.pause(0.1) stepSize, heading = self.get_one_step_move([gx, gy], roadmap) # needs to be replaced with turning the agent to the appropriate heading in the simulator, then stepping. # the resulting agent position / heading should be used to set plan.agent* values. rotation_degree = heading / ( 2 * math.pi) * 360 - agent.game_state.event.rotation print("heading , rotation", heading, rotation_degree) #print("all objects found until now and stored" ,self.scene_obstacles_dict ) #nav_env.env.step(action="RotateLook", rotation=rotation_degree) action = {'action': "RotateLook", 'rotation': rotation_degree} agent.step( action) #={'action':"RotateLook",rotation=rotation_degree} self.add_obstacle_from_step_output(agent.game_state.event) if agent.game_state.event.return_status == "SUCCESSFUL": actions += 1 #rotation = agent.game_state.event.rotation / 360 * (2 * math.pi) rotation = agent.game_state.event.rotation pose = game_util.get_pose(agent.game_state.event)[:3] update_seen(visibility_graph, pose[0], pose[1], rotation, 100, 42.5, self.scene_obstacles_dict) print( "Visible points", get_visible_points(self.agentX, self.agentY, rotation, 42.5, 25, self.scene_obstacles_dict, visibility_graph)) else: continue agentX_exp = self.agentX + stepSize * math.sin(self.agentH) agentY_exp = self.agentY + stepSize * math.cos(self.agentH) # if abs(agentX_exp - nav_env.step_output.position['x']) > 1e-2: # print("Collision happened, re-update agent's position") # elif abs(agentY_exp - nav_env.step_output.position['z']) > 1e-2: # print("Collision happened, re-update agent's position") self.agentX = agent.game_state.event.position['x'] self.agentY = agent.game_state.event.position['z'] self.agentH = agent.game_state.event.rotation / 360 * (2 * math.pi) #nav_env.env.step(action="MoveAhead", amount=0.5) if abs(rotation_degree) > 1: if 360 - abs(rotation_degree) > 1: #print ("in the continue", abs(rotation_degree), 360 - abs(rotation_degree)) continue action = {'action': "MoveAhead", 'amount': stepSize} agent.step( action) #={'action':"RotateLook",rotation=rotation_degree} if agent.game_state.event.return_status == "SUCCESSFUL": actions += 1 pose = game_util.get_pose(agent.game_state.event)[:3] rotation = agent.game_state.event.rotation update_seen(visibility_graph, pose[0], pose[1], rotation, 100, 42.5, self.scene_obstacles_dict) #update_seen(visibility_graph,pose[0],pose[1],pose[2] ,agent.game_state.event) else: continue self.agentX = agent.game_state.event.position['x'] self.agentY = agent.game_state.event.position['z'] self.agentH = agent.game_state.event.rotation / 360 * (2 * math.pi) self.add_obstacle_from_step_output(agent.game_state.event) #nav_env.env.step(action="MoveAhead", amount=0.5) # # any new obstacles that were observed during the step should be added to the planner # for i in range(len(obstacles)): # if not visible[i] and obstacles[i].minDistanceToVertex(self.agentX, self.agentY) < 30: # self.addObstacle(obstacles[i]) # visible[i] = True return actions
def process_frame(self, run_object_detection=False): self.im_count += 1 self.pose = game_util.get_pose(self.event) i = 0 return