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
