def example_get_topdown_map():
config = habitat.get_config(config_file="tasks/pointnav.yaml")
dataset = habitat.make_dataset(
id_dataset=config.DATASET.TYPE, config=config.DATASET
)
env = habitat.Env(config=config, dataset=dataset)
env.reset()
top_down_map = maps.get_topdown_map(env.sim, map_resolution=(5000, 5000))
recolor_map = np.array(
[[255, 255, 255], [128, 128, 128], [0, 0, 0]], dtype=np.uint8
)
range_x = np.where(np.any(top_down_map, axis=1))[0]
range_y = np.where(np.any(top_down_map, axis=0))[0]
padding = int(np.ceil(top_down_map.shape[0] / 125))
range_x = (
max(range_x[0] - padding, 0),
min(range_x[-1] + padding + 1, top_down_map.shape[0]),
)
range_y = (
max(range_y[0] - padding, 0),
min(range_y[-1] + padding + 1, top_down_map.shape[1]),
)
top_down_map = top_down_map[
range_x[0] : range_x[1], range_y[0] : range_y[1]
]
top_down_map = recolor_map[top_down_map]
imageio.imsave(os.path.join(IMAGE_DIR, "top_down_map.png"), top_down_map)
def generate_map_frame(env, obs_dim):
"""Generates a map that displays the state of the agent in the given environment,
for the current frame.
Args:
env: sim environment.
obs_dim: dimension of observations.
Returns:
the height x width x 1 map
"""
# draw map
top_down_map = maps.get_topdown_map(sim=env.sim,
map_resolution=(1250, 1250, 1),
num_samples=200)
# draw agent
# TODO: figure out how to convert quaternion to angles to get agent_rotation
top_down_map = maps.draw_agent(
image=top_down_map,
agent_center_coord=(1, 1),
agent_rotation=0.0,
agent_radius_px=8,
)
# scale top down map to align with rgb view
old_h, old_w, _ = top_down_map.shape
top_down_height = obs_dim[0]
top_down_width = int(float(top_down_height) / old_h * old_w)
# cv2 resize (dsize is width first)
top_down_map = cv2.resize(
top_down_map,
(top_down_width, top_down_height),
interpolation=cv2.INTER_CUBIC,
)
top_down_map = np.expand_dims(top_down_map, axis=2)
return top_down_map
def get_topdown_map(self,
map_resolution=(1250, 1250),
num_samples=20000,
draw_border=True):
assert np.abs(self.start_height - self.position[1]) <= 1.5
house_aabb = self.get_house_dimensions()
min_x = house_aabb.center[0] - house_aabb.sizes[0] / 2
max_x = house_aabb.center[0] + house_aabb.sizes[0] / 2
min_z = house_aabb.center[2] - house_aabb.sizes[2] / 2
max_z = house_aabb.center[2] + house_aabb.sizes[2] / 2
min_coord = min(min_x, min_z)
max_coord = max(max_x, max_z)
maps.COORDINATE_MIN = min_coord
maps.COORDINATE_MAX = max_coord
map = maps.get_topdown_map(self.sim,
map_resolution=map_resolution,
num_samples=num_samples,
draw_border=draw_border)
return map
def get_topdown_map(config_paths, map_name):
config = habitat.get_config(config_paths=config_paths)
dataset = habitat.make_dataset(id_dataset=config.DATASET.TYPE,
config=config.DATASET)
env = habitat.Env(config=config, dataset=dataset)
env.reset()
square_map_resolution = 5000
top_down_map = maps.get_topdown_map(env.sim,
map_resolution=(square_map_resolution,
square_map_resolution))
# Image containing 0 if occupied, 1 if unoccupied, and 2 if border (if
# the flag is set)
top_down_map[np.where(top_down_map == 0)] = 125
top_down_map[np.where(top_down_map == 1)] = 255
top_down_map[np.where(top_down_map == 2)] = 0
imageio.imsave(os.path.join(MAP_DIR, map_name + ".pgm"), top_down_map)
complete_name = os.path.join(MAP_DIR, map_name + ".yaml")
f = open(complete_name, "w+")
f.write("image: " + map_name + ".pgm\n")
f.write("resolution: " +
str((COORDINATE_MAX - COORDINATE_MIN) / square_map_resolution) +
"\n")
f.write("origin: [" + str(-1) + "," + str(-1) + ", 0.000000]\n")
f.write("negate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196")
f.close()
def get_original_map(self):
top_down_map = maps.get_topdown_map(
self._sim,
self._map_resolution,
self._num_samples,
False,
)
return top_down_map
def get_map(env, map_res):
# top down map
top_down_map = maps.get_topdown_map(env.sim,
num_samples=1000000,
map_resolution=(map_res, map_res))
top_down_map, rmin, rmax, cmin, cmax = crop_map(top_down_map)
# target/goal
target_position = env.current_episode.goals[0].position
target_position_grid = maps.to_grid(target_position[0], target_position[2],
maps.COORDINATE_MIN,
maps.COORDINATE_MAX,
(map_res, map_res))
# agent's current position # to_grid converts real world (x,y) to pixel (x,y)
agent_position = env.sim.get_agent_state().position
agent_position_grid = maps.to_grid(agent_position[0], agent_position[2],
maps.COORDINATE_MIN,
maps.COORDINATE_MAX, (map_res, map_res))
# grid offset for map
agent_position_grid = (agent_position_grid[0] - rmin,
agent_position_grid[1] - cmin)
target_position_grid = (target_position_grid[0] - rmin,
target_position_grid[1] - cmin)
tdm = top_down_map.copy()
# agent pos
m1 = 4
tdm[agent_position_grid[0] - m1:agent_position_grid[0] + m1,
agent_position_grid[1] - m1:agent_position_grid[1] + m1] = 0
# target pos
m2 = 7
tdm[target_position_grid[0] - m1:target_position_grid[0] + m1,
target_position_grid[1] - m2:target_position_grid[1] + m2] = 0
tdm[target_position_grid[0] - m2:target_position_grid[0] + m2,
target_position_grid[1] - m1:target_position_grid[1] + m1] = 0
assert agent_position_grid[0] > 0 and agent_position_grid[
0] < top_down_map.shape[0], print('assert1 ', agent_position_grid,
top_down_map.shape)
assert target_position_grid[0] > 0 and target_position_grid[
0] < top_down_map.shape[0], print('assert2 ', target_position_grid,
top_down_map.shape)
assert agent_position_grid[1] > 0 and agent_position_grid[
1] < top_down_map.shape[1], print('assert3 ', agent_position_grid,
top_down_map.shape)
assert target_position_grid[1] > 0 and target_position_grid[
1] < top_down_map.shape[1], print('assert4 ', target_position_grid,
top_down_map.shape)
return tdm, agent_position_grid, target_position_grid
def topdown_map(env, recolor=False):
top_down_map = maps.get_topdown_map(env.sim, map_resolution=(5000, 5000))
padding = int(np.ceil(top_down_map.shape[0] / 125))
top_down_map = crop(top_down_map, padding)
# background is 0
# border is 1,
# interior is 2
# recolor
if recolor:
map_colors = np.array([[255, 255, 255], [128, 128, 128], [0, 0, 0]],
dtype=np.uint8)
top_down_map = map_colors[top_down_map]
return top_down_map
def get_original_map(self):
top_down_map = maps.get_topdown_map(
self._sim,
self._map_resolution,
self._num_samples,
self._config.DRAW_BORDER,
)
range_x = np.where(np.any(top_down_map, axis=1))[0]
range_y = np.where(np.any(top_down_map, axis=0))[0]
self._ind_x_min = range_x[0]
self._ind_x_max = range_x[-1]
self._ind_y_min = range_y[0]
self._ind_y_max = range_y[-1]
return top_down_map
def get_original_map(self, episode):
top_down_map = maps.get_topdown_map(
self._sim,
self._map_resolution,
self._num_samples,
self._config.DRAW_BORDER,
)
range_x = np.where(np.any(top_down_map, axis=1))[0]
range_y = np.where(np.any(top_down_map, axis=0))[0]
self._ind_x_min = range_x[0]
self._ind_x_max = range_x[-1]
self._ind_y_min = range_y[0]
self._ind_y_max = range_y[-1]
if self._config.DRAW_SOURCE_AND_TARGET:
# mark source point
s_x, s_y = maps.to_grid(
episode.start_position[0],
episode.start_position[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
point_padding = 2 * int(
np.ceil(self._map_resolution[0] / MAP_THICKNESS_SCALAR))
top_down_map[s_x - point_padding:s_x + point_padding + 1,
s_y - point_padding:s_y + point_padding +
1, ] = maps.MAP_SOURCE_POINT_INDICATOR
# mark target point
t_x, t_y = maps.to_grid(
episode.goals[0].position[0],
episode.goals[0].position[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
top_down_map[t_x - point_padding:t_x + point_padding + 1,
t_y - point_padding:t_y + point_padding +
1, ] = maps.MAP_TARGET_POINT_INDICATOR
return top_down_map
def get_original_map(self):
top_down_map = maps.get_topdown_map(
self._sim,
self._map_resolution,
self._num_samples,
self._config.DRAW_BORDER,
)
range_x = np.where(np.any(top_down_map, axis=1))[0]
range_y = np.where(np.any(top_down_map, axis=0))[0]
self._ind_x_min = range_x[0]
self._ind_x_max = range_x[-1]
self._ind_y_min = range_y[0]
self._ind_y_max = range_y[-1]
if self._config.FOG_OF_WAR.DRAW:
self._fog_of_war_mask = np.zeros_like(top_down_map)
return top_down_map
def _get_navigable_coord(self):
# get the navigable coord
coordinate_max = maps.COORDINATE_MAX
coordinate_min = maps.COORDINATE_MIN
# to low the memory requirement
self.top_down_grid_size = self._rl_config.ANS.MAPPER.map_scale*2.5
resolution = (coordinate_max - coordinate_min) / self.top_down_grid_size
grid_resolution = (int(resolution), int(resolution))
top_down_map = maps.get_topdown_map(
self.habitat_env.sim, grid_resolution, 20000, draw_border=False,
)
map_w, map_h = top_down_map.shape
intervals = (max(int(1 / self.top_down_grid_size), 1), max(int(0.5 / self.top_down_grid_size), 1))
x_vals = np.arange(0, map_w, intervals[0], dtype=int)
y_vals = np.arange(0, map_h, intervals[1], dtype=int)
coors = np.stack(np.meshgrid(x_vals, y_vals), axis=2) # (H, W, 2)
coors = coors.reshape(-1, 2) # (H*W, 2)
map_vals = top_down_map[coors[:, 0], coors[:, 1]]
self.valid_coors = coors[map_vals > 0] * self.top_down_grid_size
def act(self, observations):
# print (observations.keys())
# import ipdb; ipdb.set_trace()
goal_pos = self.env.current_episode.goals[0].position
best_action = self.follower.get_next_action(goal_pos)
if self.episode_i == 0:
cv2.imwrite(
'./temp/ep%d-step%d.png' % (self.episode_i, self.step_i),
observations['rgb'])
if self.step_i == 0:
top_down_map = maps.get_topdown_map(self.env.sim,
map_resolution=(5000, 5000))
import matplotlib.pyplot as plt
plt.imshow(top_down_map)
plt.show()
plt.waitforbuttonpress(0.001)
# global_map = observations['top_down_map']
# cv2.imwrite('./temp/map-%d.png'%self.episode_i, global_map)
self.step_i += 1
return {"action": best_action} # 0: stop, forward, left, right
print("The NavMesh bounds are: " + str(sim.pathfinder.get_bounds()))
if not custom_height:
# get bounding box minumum elevation for automatic height
height = sim.pathfinder.get_bounds()[0][1]
if not sim.pathfinder.is_loaded:
print("Pathfinder not initialized, aborting.")
else:
# @markdown You can get the topdown map directly from the Habitat-sim API with *PathFinder.get_topdown_view*.
# This map is a 2D boolean array
sim_topdown_map = sim.pathfinder.get_topdown_view(meters_per_pixel, height)
if display:
# @markdown Alternatively, you can process the map using the Habitat-Lab [maps module](https://github.com/facebookresearch/habitat-api/blob/master/habitat/utils/visualizations/maps.py)
hablab_topdown_map = maps.get_topdown_map(
sim.pathfinder, height, meters_per_pixel=meters_per_pixel
)
recolor_map = np.array(
[[255, 255, 255], [128, 128, 128], [0, 0, 0]], dtype=np.uint8
)
hablab_topdown_map = recolor_map[hablab_topdown_map]
print("Displaying the raw map from get_topdown_view:")
display_map(sim_topdown_map)
print("Displaying the map from the Habitat-Lab maps module:")
display_map(hablab_topdown_map)
# easily save a map to file:
map_filename = os.path.join(output_path, "top_down_map.png")
imageio.imsave(map_filename, hablab_topdown_map)
panorama=config.PANORAMA,
num_floors=house.num_floors)
env.reset(floor)
locs = house.object_locations_for_habitat_dest
goals = [
gibson_info.relevant_locations(env.agent_state()[0], locs[k])
for k in sorted(locs.keys())
]
ims = []
corrs = []
positions = []
top_down_map = maps.get_topdown_map(env.env.sim,
map_resolution=(map_resolution,
map_resolution),
draw_border=False)
points = np.argwhere(top_down_map == 1)
hposes = poses[(house_name, floor)]
if rerender:
pos,rot = hposes[ind]
env.set_agent_state(pos, rot)
im, cor = vis_panorama(env, 12, model, goals)
im.savefig(f'vis/pano/final/{num}.pdf', bbox_inches='tight', pad_inches=0.05)
num += 1
env.close()
continue
for pos, rot in tqdm(hposes):
env.set_agent_state(pos, rot)
def get_eval_map(env:ExpRLEnv, trainer:SemAntExpTrainer, M:int,depth_projection_net:DepthProjectionNet, step:int, objectName:str):
"""Given the environment and the configuration, compute the global
top-down wall and seen area maps by sampling maps for individual locations
along a uniform grid in the environment, and registering them.
step (m): the length between two measure points
"""
# Initialize a global map for the episode
scene_name = env.habitat_env.current_episode.scene_id.split('/')[-1].split('.')[0]
dirPath = './data/debug/data/' + objectName + "/" + scene_name + '_' + env.habitat_env.current_episode.episode_id + '/' + "record/"
safe_mkdir(dirPath)
mapper = trainer.mapper
config = trainer.config.TASK_CONFIG
device = trainer.device
scene = env.habitat_env.sim.semantic_scene
obj_pos = {}
obj_pos = {obj.id : [obj.aabb.center,obj.aabb.sizes]
for obj in scene.objects
if objectName == obj.category.name()}
with open(dirPath + 'obj_pos.pkl', 'wb') as f: pickle.dump(obj_pos, f)
objectIndexes = [int(key.split('_')[-1]) for key in obj_pos.keys()]
global_wall_map = torch.zeros(1, 2, M, M).to(device)
global_seen_map = torch.zeros(1, 2, M, M).to(device)
global_object_map = torch.zeros(1, 2, M, M).to(device)
global_se_map = torch.zeros(1, 2, M, M).to(device)
global_se_filtered_map = torch.zeros(1, 2, M, M).to(device)
global_se_seen_map = torch.zeros(1, 2, M, M).to(device)
global_se_map_mit = torch.zeros(1, 2, M, M).to(device)
global_se_map_gt = torch.zeros(1, 2, M, M).to(device)
grid_size = config.TASK.GT_EGO_MAP.MAP_SCALE
coordinate_max = maps.COORDINATE_MAX
coordinate_min = maps.COORDINATE_MIN
resolution = (coordinate_max - coordinate_min) / grid_size
grid_resolution = (int(resolution), int(resolution))
top_down_map = maps.get_topdown_map(
env.habitat_env.sim, grid_resolution, 20000, draw_border=False,
)
map_w, map_h = top_down_map.shape
intervals = (max(int(step / grid_size), 1), max(int(step / grid_size), 1))
x_vals = np.arange(0, map_w, intervals[0], dtype=int)
y_vals = np.arange(0, map_h, intervals[1], dtype=int)
coors = np.stack(np.meshgrid(x_vals, y_vals), axis=2) # (H, W, 2)
coors = coors.reshape(-1, 2) # (H*W, 2)
map_vals = top_down_map[coors[:, 0], coors[:, 1]]
valid_coors = coors[map_vals > 0]
real_x_vals = coordinate_max - valid_coors[:, 0] * grid_size
real_z_vals = coordinate_min + valid_coors[:, 1] * grid_size
start_y = env.habitat_env.sim.get_agent_state().position[1]
index = 0
records = []
totalOffsetX = np.random.uniform(-1.5, 1.5)
totalOffsetY = np.random.uniform(-1.5, 1.5)
for j in tqdm.tqdm(range(real_x_vals.shape[0]),desc='occupacy map', position=2):
for theta in np.arange(-np.pi, np.pi, np.pi ):
index +=1
randomAngle = np.random.uniform(-np.pi,np.pi)
randomX = np.random.uniform(-0.5, 0.5)
randomY = np.random.uniform(-0.5, 0.5)
position = [
real_x_vals[j].item() + randomX + totalOffsetX,
start_y.item(),
real_z_vals[j].item() + randomY + totalOffsetY,
]
rotation = [
0.0,
np.sin((theta + randomAngle) / 2).item(),
0.0,
np.cos((theta + randomAngle) / 2).item(),
]
sim_obs = env.habitat_env.sim.get_observations_at(
position, rotation, keep_agent_at_new_pose=True,
)
episode = env.habitat_env.current_episode
obs = obs, _, _, _ = env.step(action={'action':1})
batch = batch_obs([obs], device=trainer.device)
semantic = batch["semantic"]
mask = torch.zeros_like(semantic,dtype=bool)
for objectIndex in objectIndexes:
mask ^= (semantic == objectIndex)
mask = rearrange(mask, "b h w -> b h w ()")
batch["object_mask_gt"] = mask
ego_map_gt_b = depth_projection_net(
rearrange(batch["depth"], "b h w c -> b c h w")
)
object_depth = batch["depth"]*(mask > 0) + (mask == 0)*100
ego_map_gt_object = depth_projection_net(rearrange(object_depth, "b h w c -> b c h w"))
batch["ego_map_gt_object"] = ego_map_gt_object
#begin use MIT Seg
img_data = pil_to_tensor(rearrange(batch["rgb"], "b h w c -> b c h w")[0] / 255)
singleton_batch = {'img_data': img_data[None]}
output_size = img_data.shape[1:]
with torch.no_grad():
scores = segmentation_module(singleton_batch, segSize=output_size)
_, pred = torch.max(scores, dim=1)
# visualize_result(pred)
#end use MIT Seg
batch["mit"] = pred
#mask for chair
if objectName == "chair":
mask = (pred == 19)^(pred == 30)^(pred == 75)
elif objectName == "bed":
mask = (pred == 7)
elif objectName == "table":
mask = (pred == 15)
else:
mask = pred > 0
mask = rearrange(mask, "b h w -> b h w ()")
batch["object_mask_mit"] = mask
object_depth2 = batch["depth"]*(mask > 0) + (mask == 0)*100
ego_map_gt_object2 = depth_projection_net(rearrange(object_depth2, "b h w c -> b c h w"))
batch["ego_map_mit_object"] = ego_map_gt_object2
ego_map_gt_anti = transpose_image(batch["ego_map_gt_anticipated"]).to(trainer.device)
pu_inputs_t = {
"rgb": process_image(batch["rgb"], trainer.mapper.img_mean_t, trainer.mapper.img_std_t).to(trainer.device),
"depth": transpose_image(batch["depth"]).to(trainer.device),
"ego_map_gt": transpose_image(
rearrange(ego_map_gt_b, "b c h w -> b h w c")
).to(trainer.device),
"ego_map_gt_anticipated": ego_map_gt_anti,
}
pu_inputs = trainer.mapper._safe_cat(pu_inputs_t, pu_inputs_t)
torch.save(batch, dirPath + 'pu_inputs_t_'+str(index)+'.pt')
pu_output = trainer.mapper.projection_unit(pu_inputs)
se_map = asnumpy(pu_output['sem_estimate'])[0,:,:,:]
ego_map_gt_b_np = asnumpy(ego_map_gt_b[0,...])
se_seen = ego_map_gt_b_np*se_map
dilation_mask = np.ones((100, 100))
current_mask = cv2.dilate(
se_seen.astype(np.float32), dilation_mask, iterations=1,
).astype(np.float32)
se_filtered_map = se_map
se_seen_map = torch.Tensor(se_seen).to(device)
se_seen_map = rearrange(se_seen_map, "c h w -> () c h w")
se_seen_map = (se_seen_map[:,0])[None,:]
se_map = torch.Tensor(se_map).to(device)
se_map = rearrange(se_map, "c h w -> () c h w")
se_filtered_map = torch.Tensor(se_filtered_map).to(device)
se_filtered_map = rearrange(se_filtered_map, "c h w -> () c h w")
torch.save(se_filtered_map, dirPath + 'se_filtered_map_'+str(index)+'.pt')
ego_map_gt = torch.Tensor(obs["ego_map_gt"]).to(device)
ego_map_gt = rearrange(ego_map_gt, "h w c -> () c h w")
ego_wall_map_gt = torch.Tensor(obs["ego_wall_map_gt"]).to(device)
ego_wall_map_gt = rearrange(ego_wall_map_gt, "h w c -> () c h w")
pose_gt = torch.Tensor(obs["pose_gt"]).unsqueeze(0).to(device)
global_se_seen_map = mapper.ext_register_map(
global_se_seen_map, se_seen_map, pose_gt
)
global_se_map_gt = mapper.ext_register_map(
global_se_map_gt, ego_map_gt_anti, pose_gt
)
global_seen_map = mapper.ext_register_map(
global_seen_map, ego_map_gt_b, pose_gt
)
global_object_map = mapper.ext_register_map(
global_object_map, ego_map_gt_object, pose_gt
)
global_wall_map = mapper.ext_register_map(
global_wall_map, ego_wall_map_gt, pose_gt
)
global_se_map = mapper.ext_register_map(
global_se_map, se_map, pose_gt
)
global_se_filtered_map = mapper.ext_register_map(
global_se_filtered_map, se_filtered_map, pose_gt
)
global_se_map_mit = mapper.ext_register_map(
global_se_map_mit, ego_map_gt_object2, pose_gt
)
se_filtered_map = None
batch = None
se_map = None
ego_map_gt = None
ego_wall_map_gt = None
obs = None
pu_inputs_t = None
pu_output=None
_ = None
pu_inputs=None
gc.collect()
torch.cuda.empty_cache()
mask = dilate_tensor(global_seen_map*(1 - global_wall_map > 0)*global_se_map,(51, 51))
global_se_filtered_map = global_se_filtered_map*mask
global_se_seen_map = global_se_seen_map*mask
mask = dilate_tensor(global_seen_map*(1 - global_wall_map > 0)*global_object_map,(51, 51))
global_se_map_gt = global_se_map_gt*mask
global_wall_map_np = asnumpy(rearrange(global_wall_map, "b c h w -> b h w c")[0])
global_seen_map_np = asnumpy(rearrange(global_seen_map, "b c h w -> b h w c")[0])
global_se_map_np = asnumpy(rearrange(global_se_map, "b c h w -> b h w c")[0])
global_se_global_se_filtered_map_np= asnumpy(rearrange(global_se_filtered_map, "b c h w -> b h w c")[0])
global_se_map_mit_np= asnumpy(rearrange(global_se_map_mit, "b c h w -> b h w c")[0])
global_se_map_gt_np= asnumpy(rearrange(global_se_map_gt, "b c h w -> b h w c")[0])
global_se_seen_map_np = asnumpy(rearrange(global_se_seen_map, "b c h w -> b h w c")[0])
global_object_map_np = asnumpy(rearrange(global_object_map, "b c h w -> b h w c")[0])
return global_seen_map_np, global_wall_map_np, global_se_map_np,global_se_global_se_filtered_map_np, global_se_map_mit_np,global_se_map_gt_np,global_se_seen_map_np,global_object_map_np
def get_map_with_path(env, map_res, agent_path):
# top down map
top_down_map = maps.get_topdown_map(env.sim,
num_samples=1000000,
map_resolution=(map_res, map_res))
top_down_map, rmin, rmax, cmin, cmax = crop_map(top_down_map)
tdm = top_down_map.copy()
# target/goal
target_position = env.current_episode.goals[0].position
target_position_grid = maps.to_grid(target_position[0], target_position[2],
maps.COORDINATE_MIN,
maps.COORDINATE_MAX,
(map_res, map_res))
for k in range(len(agent_path)):
pos = agent_path[k]
# agent's position # to_grid converts real world (x,y) to pixel (x,y)
agent_position = pos #env.sim.get_agent_state().position
agent_position_grid = maps.to_grid(agent_position[0],
agent_position[2],
maps.COORDINATE_MIN,
maps.COORDINATE_MAX,
(map_res, map_res))
# grid offset for map
agent_position_grid = (agent_position_grid[0] - rmin,
agent_position_grid[1] - cmin)
# agent pos
m1 = 1
color = [255, 0, 0]
if (k == 0):
m1 = 3
color = [0, 255, 0]
tdm[agent_position_grid[0] - m1:agent_position_grid[0] + m1,
agent_position_grid[1] - m1:agent_position_grid[1] + m1, :] = color
assert agent_position_grid[0] > 0 and agent_position_grid[
0] < top_down_map.shape[0], print('assert1 ', agent_position_grid,
top_down_map.shape)
assert agent_position_grid[1] > 0 and agent_position_grid[
1] < top_down_map.shape[1], print('assert3 ', agent_position_grid,
top_down_map.shape)
target_position_grid = (target_position_grid[0] - rmin,
target_position_grid[1] - cmin)
# target pos
m2 = 3
color = [0, 0, 255]
tdm[target_position_grid[0] - m2:target_position_grid[0] + m2,
target_position_grid[1] - m2:target_position_grid[1] + m2, :] = color
#tdm[target_position_grid[0]-m1:target_position_grid[0]+m1,target_position_grid[1]-m2:target_position_grid[1]+m2,:] = color
#tdm[target_position_grid[0]-m2:target_position_grid[0]+m2,target_position_grid[1]-m1:target_position_grid[1]+m1,:] = color
assert target_position_grid[0] > 0 and target_position_grid[
0] < top_down_map.shape[0], print('assert2 ', target_position_grid,
top_down_map.shape)
assert target_position_grid[1] > 0 and target_position_grid[
1] < top_down_map.shape[1], print('assert4 ', target_position_grid,
top_down_map.shape)
return tdm
def main():
parser = argparse.ArgumentParser()
parser.add_argument("--task-config", type=str, default="configs/tasks/pointnav.yaml")
parser.add_argument("--publish-odom", type=bool, default=True)
parser.add_argument("--create-map", type=bool, default=False)
parser.add_argument("--save-observations", type=bool, default=False)
parser.add_argument("--preset-trajectory", type=bool, default=False)
args = parser.parse_args()
# Now define the config for the sensor
config = habitat.get_config(args.task_config)
config.defrost()
config.TASK.AGENT_POSITION_SENSOR = habitat.Config()
# Use the custom name
config.TASK.AGENT_POSITION_SENSOR.TYPE = "position_sensor"
config.TASK.AGENT_POSITION_SENSOR.ANSWER_TO_LIFE = 42
# Add the sensor to the list of sensors in use
config.TASK.SENSORS.append("AGENT_POSITION_SENSOR")
config.freeze()
max_depth = config.SIMULATOR.DEPTH_SENSOR.MAX_DEPTH
print(args.create_map)
agent = KeyboardAgent(args.save_observations)
env = habitat.Env(config=config)
if args.create_map:
print('create map')
top_down_map = maps.get_topdown_map(env.sim, map_resolution=(5000, 5000))
print(top_down_map.min(), top_down_map.mean(), top_down_map.max())
recolor_map = np.array([[0, 0, 0], [128, 128, 128], [255, 255, 255]], dtype=np.uint8)
range_x = np.where(np.any(top_down_map, axis=1))[0]
range_y = np.where(np.any(top_down_map, axis=0))[0]
padding = int(np.ceil(top_down_map.shape[0] / 125))
range_x = (
max(range_x[0] - padding, 0),
min(range_x[-1] + padding + 1, top_down_map.shape[0]),
)
range_y = (
max(range_y[0] - padding, 0),
min(range_y[-1] + padding + 1, top_down_map.shape[1]),
)
top_down_map = top_down_map[
range_x[0] : range_x[1], range_y[0] : range_y[1]
]
top_down_map = recolor_map[top_down_map]
imsave('top_down_map.png', top_down_map)
observations = env.reset()
if not args.preset_trajectory:
while not keyboard.is_pressed('q'):
action = agent.act(observations)
observations = env.step(action)
else:
fin = open('actions.txt', 'r')
actions = [int(x) for x in fin.readlines()]
for action in actions:
agent.act(observations)
observations = env.step(action)
fin.close()
if args.save_observations:
with h5py.File('observations.hdf5', 'w') as f:
f.create_dataset("points", data=np.array(agent.points))
f.create_dataset("true_positions", data=np.array(agent.true_positions))
f.create_dataset("true_rotations", data=np.array(agent.true_rotations))
f.create_dataset("predicted_positions", data=np.array(agent.predicted_positions))
f.create_dataset("predicted_rotations", data=np.array(agent.predicted_rotations))
f.create_dataset("rgb", data=np.array(agent.rgbs))
f.create_dataset("depth", data=np.array(agent.depths))
f.create_dataset("true_timestamps", data=np.array(agent.true_timestamps))
f.create_dataset("predicted_timestamps", data=np.array(agent.predicted_timestamps))
print(np.array(agent.points).shape, np.array(agent.rgbs).shape)
fout = open('actions.txt', 'w')
for action in agent.actions:
print(action, file=fout)
fout.close()
def train_rl():
print('create env')
env = habitat.Env(config)
# save a copy of the map
print('map res: ', map_res)
top_down_map = maps.get_topdown_map(env.sim, num_samples=1000000, map_resolution=(map_res, map_res))
top_down_map, rmin, rmax, cmin, cmax = crop_map(top_down_map)
np.save('TDM_maps/tdm', top_down_map)
print('create tf session')
sess = tf.Session()
Policy = Policy_net('policy', sess, env, S_DIM, A_DIM)
Old_Policy = Policy_net('old_policy', sess, env, S_DIM, A_DIM)
PPO = PPOTrain(Policy, Old_Policy, sess, gamma=gamma)
ppo_vars = Policy.get_trainable_variables() + Old_Policy.get_trainable_variables()
saver_ppo = tf.train.Saver(ppo_vars)
rec_loss_rgb = 'l2'
lat_loss_rgb = 'kld'
beta_rgb = 1.0
rec_loss_depth = 'l2'
lat_loss_depth = 'kld'
beta_depth = 1.0
vae_rgb = VAE(sess, lr=learning_rate, batch_size=batch_size, n_z=n_z, img_size=img_size, nchannels=3, recon_loss=rec_loss_rgb, lat_loss=lat_loss_rgb, scope='vae_rgb')
vae_rgb_vars = vae_rgb.get_vae_vars()
vae_depth = VAE(sess, lr=learning_rate, batch_size=batch_size, n_z=n_z, img_size=img_size, nchannels=1, recon_loss=rec_loss_depth, lat_loss=lat_loss_depth, scope='vae_depth')
vae_depth_vars = vae_depth.get_vae_vars()
saver_vae_rgb = tf.train.Saver(vae_rgb_vars)
saver_vae_depth = tf.train.Saver(vae_depth_vars)
sess.run(tf.global_variables_initializer())
if (load_ckpt):
saver_ppo.restore(sess, 'ckpt/ppo/model')
saver_vae_rgb.restore(sess, 'ckpt/vae_rgb/model')
saver_vae_depth.restore(sess, 'ckpt/vae_depth/model')
print("total number of trainable params: ", Policy.num_train_params())
for ep in range(ep_start, nepisodes_max):
#if (ep < 175000):
epsilon_ppo = EPSILON_CLIP
stochastic = True
c_2 = c_2_ENTROPY
#else:
# epsilon_ppo = EPSILON_CLIP * (1.0 - float(ep-175000)/float(100000))
# stochastic = False
# c_2 = c_2_ENTROPY * (1.0 - float(ep-175000)/float(100000))
epsilon_ppo = max(epsilon_ppo, 0.02)
c_2 = max(c_2, 0.001)
print('epsilon: ', epsilon_ppo, ' | c_2: ', c_2)
s = env.reset()
observations, actions, rewards, v_preds = [], [], [], []
init_agent_position = env.sim.get_agent_state().position
dist = get_dist(env)
init_geodesic_dist = dist
rnn_state = Policy.state_init
done = False
success = False
nsteps = 0
ep_rewards = 0.0
dist_traveled_by_agent = 0.0
old_position = init_agent_position
ncollisions = 0
agent_path = []
while (not done) and (not env.episode_over) and (not success):
agent_path.append(env.sim.get_agent_state().position)
rgb = s["rgb"]
depth = s["depth"]
pointgoal = s["pointgoal"]; pointgoal = [pointgoal[0], pointgoal[2]]
heading = s["heading"]
# perception_embedding is the costly operation in the entire one time step (~ 3-4X slower than env.step/Policy.act)
obs, z_rgb_t, z_depth_t = perception_embedding(vae_rgb, vae_depth, rgb, depth, pointgoal, heading, S_DIM, n_z, beta_rgb, beta_depth)
act, v_pred, rnn_state = Policy.act(obs=obs, rnn_state=rnn_state, stochastic=stochastic)
act = np.asscalar(act)
v_pred = np.asscalar(v_pred)
s2 = env.step(allowed_actions[act])
nsteps += 1
new_dist = get_dist(env)
reward, done, success = get_reward(dist, new_dist, nsteps, env)
#-----------------------------
# collision penalty
if (act == 0):
if (was_there_a_collision(env, old_position, config, act)):
ncollisions += 1
# reward -= 0.025 # 0.1
#-----------------------------
new_position = env.sim.get_agent_state().position
if (new_dist <= 0.2):
s2 = env.step(SimulatorActions.STOP)
new_dist = get_dist(env)
a_t = np.zeros((1,1),dtype=np.int32); a_t[0,0] = act
pg_t = np.zeros((1,2),dtype=np.float32); pg_t[0,:] = pointgoal
h_t = np.zeros((1,1),dtype=np.float32); h_t[0,0] = heading
dist_traveled_by_agent += abs(new_dist - dist)
#-----------------------------
# SPL reward for success (SPL bonus reward)
#if (success):
# spl = float(success)*init_geodesic_dist/max(dist_traveled_by_agent, init_geodesic_dist)
# reward += spl * 10.0
#-----------------------------
ep_rewards += reward
observations.append(obs)
actions.append(act)
rewards.append(reward)
v_preds.append(v_pred)
if done or success:
rgb = s2["rgb"]
depth = s2["depth"]
pointgoal = s2["pointgoal"]; pointgoal = [pointgoal[0], pointgoal[2]]
heading = s2["heading"]
next_obs, _, _ = perception_embedding(vae_rgb, vae_depth, rgb, depth, pointgoal, heading, S_DIM, n_z, beta_rgb, beta_depth)
_, v_last, rnn_state = Policy.act(obs=next_obs, rnn_state=rnn_state, stochastic=stochastic)
v_preds_next = v_preds[1:] + [v_last]
break
else:
s = s2
dist = new_dist
old_position = new_position
if (ep % 50 == 0):
saver_ppo.save(sess, 'ckpt/ppo/model')
gaes = PPO.get_gaes(rewards=rewards, v_preds=v_preds, v_preds_next=v_preds_next)
observations = np.array(observations).astype(dtype=np.float32)
observations = np.squeeze(observations, 1)
actions = np.array(actions).astype(dtype=np.int32)
rewards = np.array(rewards).astype(dtype=np.float32)
v_preds_next = np.array(v_preds_next).astype(dtype=np.float32)
gaes = np.array(gaes).astype(dtype=np.float32)
gaes = (gaes - gaes.mean()) / gaes.std()
gaes = np.squeeze(gaes,2)
gaes = np.squeeze(gaes,1)
PPO.assign_policy_parameters()
inp = [observations, actions, rewards, v_preds_next, gaes]
# train
sample_indices = np.arange(observations.shape[0])
sampled_inp = [np.take(a=a, indices=sample_indices, axis=0) for a in inp]
for epoch in range(5):
PPO.train(obs=sampled_inp[0], actions=sampled_inp[1], rewards=sampled_inp[2], v_preds_next=sampled_inp[3], gaes=sampled_inp[4], epsilon_ppo=epsilon_ppo, c_2=c_2)
final_agent_position = env.sim.get_agent_state().position
target_position = env.current_episode.goals[0].position
geo_dist_traveled_by_agent = env.sim.geodesic_distance(init_agent_position, final_agent_position)
SPL = float(success)*init_geodesic_dist/max(dist_traveled_by_agent, init_geodesic_dist)
final_dist_to_target = env.sim.geodesic_distance(final_agent_position, target_position)
# final position of agent
agent_path.append(env.sim.get_agent_state().position)
# TDM
if (ep % 10 == 0):
tdm_with_path = get_map_with_path(env, map_res, agent_path)
plt.imshow(tdm_with_path); plt.tight_layout(); plt.axis('off')
fname = 'TDM_maps/tdm_' + str(ep) + '_' + str(int(SPL*100.0)) + '.png'; plt.savefig(fname)
print(' ')
print('episode: ', ep, ' | ep_rewards: ', ep_rewards, ' | ep_steps: ', nsteps, ' | SPL: ', SPL)
print('init geodesic dist to target: ', init_geodesic_dist)
print('final agent pos: ', final_agent_position)
print('target pos: ', target_position)
print(' ')
print('total dist traveled by agent: ', dist_traveled_by_agent)
print('geo dist traveled by agent: ', geo_dist_traveled_by_agent)
print('final dist to target: ', final_dist_to_target)
print('ncollisions: ', ncollisions)
print('-'*50)
f = open("performance/performance_ppo.txt", "a+")
f.write(str(ep) + " " + str(ep_rewards) + " " + str(nsteps) + " " + str(SPL) + " " + " " + str(init_geodesic_dist) + " " + str(final_dist_to_target) + '\n')
f.close()
def get_episode_map(env:habitat.RLEnv, mapper:Mapper, M:int, config:CN, device:torch.device):
"""Given the environment and the configuration, compute the global
top-down wall and seen area maps by sampling maps for individual locations
along a uniform grid in the environment, and registering them.
"""
# Initialize a global map for the episode
global_wall_map = torch.zeros(1, 2, M, M).to(device)
global_seen_map = torch.zeros(1, 2, M, M).to(device)
grid_size = config.TASK.GT_EGO_MAP.MAP_SCALE
coordinate_max = maps.COORDINATE_MAX
coordinate_min = maps.COORDINATE_MIN
resolution = (coordinate_max - coordinate_min) / grid_size
grid_resolution = (int(resolution), int(resolution))
top_down_map = maps.get_topdown_map(
env.habitat_env.sim, grid_resolution, 20000, draw_border=False,
)
map_w, map_h = top_down_map.shape
intervals = (max(int(0.5 / grid_size), 1), max(int(0.5 / grid_size), 1))
x_vals = np.arange(0, map_w, intervals[0], dtype=int)
y_vals = np.arange(0, map_h, intervals[1], dtype=int)
coors = np.stack(np.meshgrid(x_vals, y_vals), axis=2) # (H, W, 2)
coors = coors.reshape(-1, 2) # (H*W, 2)
map_vals = top_down_map[coors[:, 0], coors[:, 1]]
valid_coors = coors[map_vals > 0]
real_x_vals = coordinate_max - valid_coors[:, 0] * grid_size
real_z_vals = coordinate_min + valid_coors[:, 1] * grid_size
start_y = env.habitat_env.sim.get_agent_state().position[1]
for j in tqdm.tqdm(range(real_x_vals.shape[0]),desc='occupacy map', position=2):
for theta in np.arange(-np.pi, np.pi, np.pi / 3.0):
position = [
real_x_vals[j].item(),
start_y.item(),
real_z_vals[j].item(),
]
rotation = [
0.0,
np.sin(theta / 2).item(),
0.0,
np.cos(theta / 2).item(),
]
sim_obs = env.habitat_env.sim.get_observations_at(
position, rotation, keep_agent_at_new_pose=True,
)
episode = env.habitat_env.current_episode
obs = env.habitat_env.task.sensor_suite.get_observations(
observations=sim_obs, episode=episode, task=env.habitat_env.task
)
ego_map_gt = torch.Tensor(obs["ego_map_gt"]).to(device)
ego_map_gt = rearrange(ego_map_gt, "h w c -> () c h w")
ego_wall_map_gt = torch.Tensor(obs["ego_wall_map_gt"]).to(device)
ego_wall_map_gt = rearrange(ego_wall_map_gt, "h w c -> () c h w")
pose_gt = torch.Tensor(obs["pose_gt"]).unsqueeze(0).to(device)
global_seen_map = mapper.ext_register_map(
global_seen_map, ego_map_gt, pose_gt
)
global_wall_map = mapper.ext_register_map(
global_wall_map, ego_wall_map_gt, pose_gt
)
global_wall_map_np = asnumpy(rearrange(global_wall_map, "b c h w -> b h w c")[0])
global_seen_map_np = asnumpy(rearrange(global_seen_map, "b c h w -> b h w c")[0])
return global_seen_map_np, global_wall_map_np
"scene": test_scene, # Scene path
"default_agent": 0,
"sensor_height": 1.5, # Height of sensors in meters
"color_sensor": rgb_sensor, # RGB sensor
"depth_sensor": depth_sensor, # Depth sensor
"semantic_sensor": semantic_sensor, # Semantic sensor
"seed": 1, # used in the random navigation
"enable_physics": False, # kinematics only
}
cfg = make_cfg(sim_settings)
sim = habitat_sim.Simulator(cfg)
#height = sim.pathfinder.get_bounds()[0][1]
height = 0.1
meters_per_pixel = 0.01
hablab_topdown_map = maps.get_topdown_map(
sim.pathfinder, height, meters_per_pixel=meters_per_pixel)
first_list = read_file_list(first_file)
second_list = read_file_list(second_file)
matches = first_list.keys()
first_xyz = [[float(value) for value in first_list[a][0:3]]
for a in matches]
second_xyz = [[float(value) * float(1.0) for value in second_list[b][0:3]]
for b in matches]
first_rot = [[float(value) for value in first_list[a][3:]]
for a in matches]
grid_dimensions = (hablab_topdown_map.shape[0],
hablab_topdown_map.shape[1])
trajectory = [
maps.to_grid(
-(path_point[2]) + 19.25,
def ours_evaluate(config, env, ep, house, epind, model, visualize,
model_config):
hn, floor, class_label, goal_dist, pos, rot = ep
if config.SCORE == 'detector' or config.COMBINE_DETECTOR:
predictor = get_predictor()
predictor_class_index = predictor.metadata.thing_classes.index(
class_label)
rng = random.Random()
rng.seed(config.SEED)
if goal_dist == float('inf'):
return np.array([]) if config.STOP else 0
class_index = class_labels.index(class_label)
def model_score(ims):
torch_ims = util.torch.to_imgnet(torch.tensor(ims).to('cuda'))
with torch.no_grad():
return model(torch_ims.unsqueeze(0))[0,
class_index, :].max().item()
# compute the frame score combined with detector
def score(ims):
sc = model_score(ims['rgb'])
if config.COMBINE_DETECTOR:
size = ims['rgb'].shape[1]
left_lim, right_lim = int(size / 3), int(size * 2 / 3)
im = ims['rgb'][0] if len(ims['rgb']) == 4 else ims['rgb']
boxes, scores = get_scores(predictor, im, predictor_class_index)
if len(scores) > 0 and scores.max() > config.CONFIDENCE_THRESHOLD:
box = boxes[scores.argmax()]
if box[0] <= right_lim or box[2] >= left_lim:
if len(ims['rgb']) == 4:
ims['rgb'][0] = draw_box(ims['rgb'][0], box,
scores.max().item())
else:
ims['rgb'] = draw_box(ims['rgb'], box,
scores.max().item())
sc += (scores.max().item() + 1)
return sc
def output():
print(f"SPL: {spl}: {goal_dist}/{dist_traveled}")
if config.SLAM and visualize:
planner.write_combined(
f'%04d_{class_label}-%dm-spl%.2f-steps%d' %
(epind, int(goal_dist), spl, agent_steps_taken))
# np.save(f'data_dump/good_trajectory{epind}',np.array(log))
return np.array(log) if config.STOP else spl
locs = house.object_locations_for_habitat_dest
all_goals = [locs[k] for k in sorted(locs.keys())]
from habitat.utils.visualizations import maps
top_down_map = maps.get_topdown_map(env.env.sim,
map_resolution=(map_resolution,
map_resolution))
rrange, crange = util.habitat.crop_range(top_down_map)
point_min = util.habitat.from_grid([rrange[0], crange[0]], map_resolution,
0)
point_max = util.habitat.from_grid([rrange[1], crange[1]], map_resolution,
0)
max_dim = np.abs(point_max - point_min).max()
out_dir = f'{config.VIDEO_LOCATION}/{name_from_config(config)}'
util.ensure_folders(out_dir, True)
planner = DepthMapperAndPlanner(dt=30,
out_dir=out_dir,
map_size_cm=max_dim * 230,
mark_locs=True,
close_small_openings=True,
log_visualization=visualize)
polygons = relevant_objects(env.pos, house.objects[class_label])
planner._reset(goal_dist,
global_goals=polygons,
start_pos=env.pos,
start_ang=env.angle)
openlist = []
visited = []
dist_traveled = 0
log = []
spl = 0
agent_steps_taken = 0
full_log = []
episode_ims = [env.get_observation()]
no_move = False
def semantic_reasoning():
# log for visualizations
planner.log_reasoning()
images = []
display_values = []
for _ in range(NUM_ROTATIONS):
ims, _, _, _ = env.step(1)
loc = [*planner.pos_to_loc(env.pos), env.angle]
planner.add_observation(ims['depth'] * 1000, loc)
dest = check_movement(config,
env,
env.angle,
planner=planner,
rng=rng)
sc = score(ims)
# for visualizations
images.append(ims)
display_values.append(sc)
if dest is not None:
openlist.append((sc, dest))
if visualize and config.SLAM:
ims_to_render = [
e['rgb'][0] if len(e['rgb'].shape) == 4 else e['rgb']
for e in images
]
current_pan = join_images(
ims_to_render,
-np.array(display_values),
bl_text='Predicted Values',
br_text=f'Object Class: {class_label.title()}')
planner.set_current_pan(current_pan)
macro_steps = 50 if config.SLAM else 30
print("goals ", env.goals)
# initial steps to scan env and choose dest
semantic_reasoning()
agent_steps_taken += NUM_ROTATIONS
for macro_step_num in range(macro_steps):
print(agent_steps_taken)
if config.BACKTRACK_REJECTION and len(visited) > 0:
vis_stack = np.stack(visited)
def reject(point):
dists = np.linalg.norm((vis_stack - point)[:, [0, 2]], axis=1)
return (dists < (success_distance - 0.1)).sum() > 0
openlist = [e for e in openlist if not reject(e[1])]
def maxfunc(x):
s, d = x
dist = np.linalg.norm(env.pos - d)
return s + config.CONSISTENCY_WEIGHT * max(10 - dist, 0) / 10
if len(openlist) == 0:
print("openlist exhausted")
if visualize: planner.write_combined()
return output()
ind, (sc, next_pos), _ = util.argmax(openlist, maxfunc)
openlist.pop(ind)
original_openlist = openlist.copy()
# remove points which we cannot move toward, with an exception
# for the first step due to the initilization process
dist_est = planner.fmmDistance(next_pos)
while not planner.action_toward(next_pos):
if len(openlist) == 0:
print("openlist exhausted fmm")
if visualize: planner.write_combined()
return output()
ind, (sc, next_pos), _ = util.argmax(openlist, maxfunc)
openlist.pop(ind)
dist_est = planner.fmmDistance(next_pos)
print('score of', sc)
if visualize and config.SLAM:
planner.set_current_open(openlist)
obs = env.get_observation()
planner.set_goal(next_pos)
goal_reached = False
# 6 for initial rotation, and 2x distance*4 to
# account for 1 rotation per forward step on average
step_estimate = math.ceil(2 * (dist_est / 0.25) + 6)
cur_dist_est = dist_est
for step in range(step_estimate):
new_dist_est = planner.fmmDistance(next_pos)
# replan if the estimated distance jumps up too much
if new_dist_est > cur_dist_est + 0.1:
print('replan')
break
cur_dist_est = new_dist_est
action = planner.get_action_toward(next_pos)
print('action: ', action)
if action == 3:
print('subgoal reached')
break
before_pos = env.pos
obs, _, _, _ = env.step(action)
if action == 0:
dist_traveled += 0.25
planner.pos_to_loc(env.pos)
planner.log_act(obs, env.pos, env.angle, action)
episode_ims.append(obs)
visited.append(env.pos)
log.append([
env.pos, env.rot, dist_traveled,
env.distance_to_goal(), step == 0
])
agent_steps_taken += 1
if env._dist_to_goal(
env.pos) < success_distance and not config.STOP:
spl = min(goal_dist / (dist_traveled + 1e-5), 1)
return output()
if agent_steps_taken >= MAX_STEPS: return output()
semantic_reasoning()
agent_steps_taken += NUM_ROTATIONS
if agent_steps_taken >= MAX_STEPS: return output()
return output()
