def test_vision_sensor():
download_assets()
download_demo_data()
config_filename = os.path.join(gibson2.root_path,
'../test/test_house.yaml')
env = iGibsonEnv(config_file=config_filename, mode='headless')
vision_modalities = ['rgb', 'depth', 'pc', 'normal', 'seg']
vision_sensor = VisionSensor(env, vision_modalities)
vision_obs = vision_sensor.get_obs(env)
assert vision_obs['rgb'].shape == (env.image_height, env.image_width, 3)
assert np.all(0 <= vision_obs['rgb']) and np.all(vision_obs['rgb'] <= 1.0)
assert vision_obs['depth'].shape == (env.image_height, env.image_width, 1)
assert np.all(0 <= vision_obs['depth']) and np.all(
vision_obs['depth'] <= 1.0)
assert vision_obs['pc'].shape == (env.image_height, env.image_width, 3)
assert vision_obs['normal'].shape == (env.image_height, env.image_width, 3)
normal_norm = np.linalg.norm(vision_obs['normal'] * 2 - 1, axis=2)
assert np.sum(np.abs(normal_norm - 1) > 0.1) / \
(env.image_height * env.image_width) < 0.05
assert vision_obs['seg'].shape == (env.image_height, env.image_width, 1)
assert np.all(0 <= vision_obs['seg']) and np.all(vision_obs['seg'] <= 1.0)
def test_env_reset():
download_assets()
download_demo_data()
config_filename = os.path.join(gibson2.root_path,
'../test/test_house.yaml')
env = iGibsonEnv(config_file=config_filename, mode='headless')
class DummyTask(object):
def __init__(self):
self.reset_scene_called = False
self.reset_agent_called = False
self.get_task_obs_called = False
def get_task_obs(self, env):
self.get_task_obs_called = True
def reset_scene(self, env):
self.reset_scene_called = True
def reset_agent(self, env):
self.reset_agent_called = True
env.task = DummyTask()
env.reset()
assert env.task.reset_scene_called
assert env.task.reset_agent_called
assert env.task.get_task_obs_called
def test_velodyne():
download_assets()
download_demo_data()
config_filename = os.path.join(gibson2.root_path,
'../test/test_house.yaml')
env = iGibsonEnv(config_file=config_filename, mode='headless')
velodyne_sensor = VelodyneSensor(env)
velodyne_obs = velodyne_sensor.get_obs(env)
assert (velodyne_obs.shape[1] == 3)
def test_scan_sensor():
download_assets()
download_demo_data()
config_filename = os.path.join(gibson2.root_path,
'../test/test_house.yaml')
env = iGibsonEnv(config_file=config_filename, mode='headless')
scan_sensor = ScanSensor(env, ['scan'])
scan_obs = scan_sensor.get_obs(env)['scan']
assert scan_obs.shape == (scan_sensor.n_horizontal_rays,
scan_sensor.n_vertical_beams)
assert np.all(0 <= scan_obs) and np.all(scan_obs <= 1.0)
def run_example(args):
nav_env = iGibsonEnv(config_file=args.config,
mode=args.mode,
action_timestep=1.0 / 120.0,
physics_timestep=1.0 / 120.0)
motion_planner = MotionPlanningWrapper(nav_env)
state = nav_env.reset()
while True:
action = np.zeros(nav_env.action_space.shape)
state, reward, done, _ = nav_env.step(action)
def main():
config_filename = os.path.join(os.path.dirname(gibson2.__file__),
'../examples/configs/turtlebot_demo.yaml')
env = iGibsonEnv(config_file=config_filename, mode='gui')
for j in range(10):
env.reset()
for i in range(100):
with Profiler('Environment action step'):
action = env.action_space.sample()
state, reward, done, info = env.step(action)
if done:
logging.info(
"Episode finished after {} timesteps".format(i + 1))
break
env.close()
def test_env():
download_assets()
download_demo_data()
config_filename = os.path.join(gibson2.root_path,
'../test/test_house.yaml')
env = iGibsonEnv(config_file=config_filename, mode='headless')
try:
for j in range(2):
env.reset()
for i in range(300): # 300 steps, 30s world time
s = time()
action = env.action_space.sample()
ts = env.step(action)
print('ts', 1 / (time() - s))
if ts[2]:
print("Episode finished after {} timesteps".format(i + 1))
break
finally:
env.close()
def test_occupancy_grid():
print("Test env")
download_assets()
download_demo_data()
config_filename = os.path.join(gibson2.root_path,
'../test/test_house_occupancy_grid.yaml')
nav_env = iGibsonEnv(config_file=config_filename, mode='headless')
nav_env.reset()
nav_env.robots[0].set_position_orientation([0, 0, 0], [0, 0, 0, 1])
nav_env.simulator.step()
action = nav_env.action_space.sample()
ts = nav_env.step(action)
assert np.sum(ts[0]['occupancy_grid'] == 0) > 0
assert np.sum(ts[0]['occupancy_grid'] == 1) > 0
plt.imshow(ts[0]['occupancy_grid'][:, :, 0])
plt.colorbar()
plt.savefig('occupancy_grid.png')
nav_env.clean()
def test_base_planning():
print("Test env")
download_assets()
download_demo_data()
config_filename = os.path.join(gibson2.root_path,
'../test/test_house_occupancy_grid.yaml')
nav_env = iGibsonEnv(config_file=config_filename, mode='headless')
motion_planner = MotionPlanningWrapper(nav_env)
state = nav_env.reset()
nav_env.robots[0].set_position_orientation([0, 0, 0], [0, 0, 0, 1])
nav_env.simulator.step()
plan = None
itr = 0
while plan is None and itr < 10:
plan = motion_planner.plan_base_motion([0.5, 0, 0])
print(plan)
itr += 1
motion_planner.dry_run_base_plan(plan)
assert len(plan) > 0
nav_env.clean()
H, W = trav_map.shape
kernel = np.ones((5, 5),np.uint8)
trav_map = cv2.erode(trav_map, kernel, iterations=1)
x = np.linspace(0, W-1, W)
y = np.linspace(0, H-1, H)
xv, yv = np.meshgrid(x, y)
# map resolution is 1pixel per 0.1m
x_coord = ((xv - W/2)*0.01)
y_coord = ((yv - H/2)*0.01)
assert x_coord.shape[0] == y_coord.shape[0]
# generate observations
mode = 'headless'
config = '/home/yimeng/Datasets/iGibson/my_code/configs/my_config.yaml'
nav_env = iGibsonEnv(config_file=config,
mode=mode,
action_timestep=1.0 / 120.0,
physics_timestep=1.0 / 120.0)
nav_env.reset()
poses = []
count = 0
# 30 means 30*0.01m = 0.3m
for i in range(0, x_coord.shape[0], 30):
for j in range(0, x_coord.shape[1], 30):
print('i = {}, j = {}'.format(i, j))
camera_pose = np.array([x_coord[i][j], y_coord[i][j], 0.0])
#flag_free = nav_env.test_valid_position(nav_env.robots[0], camera_pose)
flag_free = (trav_map[i][j] > 0)
###################################################################################################################
###################################################PRINT GRID######################################################
###################################################################################################################
def printGrid(grid):
for i in range(0, len(grid)):
print(grid[i])
#################################################################################################
#################################Environment Definitions#########################################
#################################################################################################
config_filename = parse_config(
os.path.join(gibson2.example_config_path, 'fetch_motion_planning.yaml'))
nav_env = iGibsonEnv(config_file=config_filename,
mode='iggui',
action_timestep=1.0 / 120.0,
physics_timestep=1.0 / 120.0)
motion_planner = MotionPlanningWrapper(nav_env)
goal = [1, -0.2, 0]
n = 21
grid = [[0] * n for _ in range(0, n)]
printGrid(grid)
m = 10
XY = []
for i in range(0, 21):
o = (i - m) / 5
XY.append(o)
print(XY)
initial_index = [15, 10]
def run_example(args):
nav_env = iGibsonEnv(config_file=args.config,
mode=args.mode,
action_timestep=1.0 / 120.0,
physics_timestep=1.0 / 120.0)
def load_env():
return iGibsonEnv(config_file=config_filename, mode='headless')
