Integrated RGB, Depth, PyRobot actuation noises, initial camera orien…

…tation. (facebookresearch#305)

* Integrated RGB noises
* Integrated Depth noise
* Added PyRobot noisy actuations action space
* Integrated initial camera orientation
* Updated objectnav_mp3d.yaml config
* Improved overwrite_config to decrease code duplication
* Added fix to respect CONTENT_SCENES for habitat baselines training

configs/tasks/objectnav_mp3d.yaml

@@ -7,9 +7,10 @@ SIMULATOR:
   AGENT_0:
     SENSORS: ['RGB_SENSOR', 'DEPTH_SENSOR']
     HEIGHT: 0.88
-    RADIUS: 0.2
+    RADIUS: 0.18
   HABITAT_SIM_V0:
     GPU_DEVICE_ID: 0
+    ALLOW_SLIDING: False
   SEMANTIC_SENSOR:
     WIDTH: 640
     HEIGHT: 480
@@ -39,13 +40,12 @@ TASK:
   SPL:
     TYPE: SPL
     DISTANCE_TO: VIEW_POINTS
-    SUCCESS_DISTANCE: 0.2
+    SUCCESS_DISTANCE: 0.1
   DISTANCE_TO_GOAL:
     DISTANCE_TO: VIEW_POINTS
 
 DATASET:
   TYPE: ObjectNav-v1
   SPLIT: val
-  CONTENT_SCENES: []
   DATA_PATH: "data/datasets/objectnav/mp3d/v0/{split}/{split}.json.gz"
   SCENES_DIR: "data/scene_datasets/"

habitat/config/default.py

@@ -198,7 +198,7 @@ def __init__(self, *args, **kwargs):
 _C.SIMULATOR.ACTION_SPACE_CONFIG = "v0"
 _C.SIMULATOR.FORWARD_STEP_SIZE = 0.25  # in metres
 _C.SIMULATOR.SCENE = (
-    "data/scene_datasets/habitat-test-scenes/" "van-gogh-room.glb"
+    "data/scene_datasets/habitat-test-scenes/van-gogh-room.glb"
 )
 _C.SIMULATOR.SEED = _C.SEED
 _C.SIMULATOR.TURN_ANGLE = 10  # angle to rotate left or right in degrees
@@ -212,6 +212,7 @@ def __init__(self, *args, **kwargs):
 SIMULATOR_SENSOR.WIDTH = 640
 SIMULATOR_SENSOR.HFOV = 90  # horizontal field of view in degrees
 SIMULATOR_SENSOR.POSITION = [0, 1.25, 0]
+SIMULATOR_SENSOR.ORIENTATION = [0.0, 0.0, 0.0]  # Euler's angles
 # -----------------------------------------------------------------------------
 # RGB SENSOR
 # -----------------------------------------------------------------------------

habitat/sims/habitat_simulator/actions.py

@@ -131,3 +131,46 @@ def get(self):
         config.update(new_config)
 
         return config
+
+
+@registry.register_action_space_configuration(name="pyrobotnoisy")
+class HabitatSimPyRobotActionSpaceConfiguration(ActionSpaceConfiguration):
+    def get(self):
+        return {
+            HabitatSimActions.STOP: habitat_sim.ActionSpec("stop"),
+            HabitatSimActions.MOVE_FORWARD: habitat_sim.ActionSpec(
+                "pyrobot_noisy_move_forward",
+                habitat_sim.PyRobotNoisyActuationSpec(
+                    amount=self.config.FORWARD_STEP_SIZE,
+                    robot=self.config.NOISE_MODEL.ROBOT,
+                    controller=self.config.NOISE_MODEL.CONTROLLER,
+                    noise_multiplier=self.config.NOISE_MODEL.NOISE_MULTIPLIER,
+                ),
+            ),
+            HabitatSimActions.TURN_LEFT: habitat_sim.ActionSpec(
+                "pyrobot_noisy_turn_left",
+                habitat_sim.PyRobotNoisyActuationSpec(
+                    amount=self.config.TURN_ANGLE,
+                    robot=self.config.NOISE_MODEL.ROBOT,
+                    controller=self.config.NOISE_MODEL.CONTROLLER,
+                    noise_multiplier=self.config.NOISE_MODEL.NOISE_MULTIPLIER,
+                ),
+            ),
+            HabitatSimActions.TURN_RIGHT: habitat_sim.ActionSpec(
+                "pyrobot_noisy_turn_right",
+                habitat_sim.PyRobotNoisyActuationSpec(
+                    amount=self.config.TURN_ANGLE,
+                    robot=self.config.NOISE_MODEL.ROBOT,
+                    controller=self.config.NOISE_MODEL.CONTROLLER,
+                    noise_multiplier=self.config.NOISE_MODEL.NOISE_MULTIPLIER,
+                ),
+            ),
+            HabitatSimActions.LOOK_UP: habitat_sim.ActionSpec(
+                "look_up",
+                habitat_sim.ActuationSpec(amount=self.config.TILT_ANGLE),
+            ),
+            HabitatSimActions.LOOK_DOWN: habitat_sim.ActionSpec(
+                "look_down",
+                habitat_sim.ActuationSpec(amount=self.config.TILT_ANGLE),
+            ),
+        }

habitat/sims/habitat_simulator/habitat_simulator.py

@@ -29,9 +29,24 @@
 
 
 def overwrite_config(config_from: Config, config_to: Any) -> None:
+    r"""Takes Habitat-API config and Habitat-Sim config structures. Overwrites
+     Habitat-Sim config with Habitat-API values, where a field name is present
+     in lowercase. Mostly used to avoid `sim_cfg.field = hapi_cfg.FIELD` code.
+
+    Args:
+        config_from: Habitat-API config node.
+        config_to: Habitat-Sim config structure.
+    """
+
+    def if_config_to_lower(config):
+        if isinstance(config, Config):
+            return {key.lower(): val for key, val in config.items()}
+        else:
+            return config
+
     for attr, value in config_from.items():
         if hasattr(config_to, attr.lower()):
-            setattr(config_to, attr.lower(), value)
+            setattr(config_to, attr.lower(), if_config_to_lower(value))
 
 
 def check_sim_obs(obs, sensor):
@@ -175,13 +190,10 @@ def create_sim_config(
         self, _sensor_suite: SensorSuite
     ) -> habitat_sim.Configuration:
         sim_config = habitat_sim.SimulatorConfiguration()
-        sim_config.scene.id = self.config.SCENE
-        sim_config.gpu_device_id = self.config.HABITAT_SIM_V0.GPU_DEVICE_ID
-        sim_config.allow_sliding = self.config.HABITAT_SIM_V0.ALLOW_SLIDING
-        sim_config.enable_physics = self.config.HABITAT_SIM_V0.ENABLE_PHYSICS
-        sim_config.physics_config_file = (
-            self.config.HABITAT_SIM_V0.PHYSICS_CONFIG_FILE
+        overwrite_config(
+            config_from=self.config.HABITAT_SIM_V0, config_to=sim_config
         )
+        sim_config.scene.id = self.config.SCENE
         agent_config = habitat_sim.AgentConfiguration()
         overwrite_config(
             config_from=self._get_agent_config(), config_to=agent_config
@@ -190,12 +202,15 @@ def create_sim_config(
         sensor_specifications = []
         for sensor in _sensor_suite.sensors.values():
             sim_sensor_cfg = habitat_sim.SensorSpec()
+            overwrite_config(
+                config_from=sensor.config, config_to=sim_sensor_cfg
+            )
             sim_sensor_cfg.uuid = sensor.uuid
             sim_sensor_cfg.resolution = list(
                 sensor.observation_space.shape[:2]
             )
             sim_sensor_cfg.parameters["hfov"] = str(sensor.config.HFOV)
-            sim_sensor_cfg.position = sensor.config.POSITION
+
             # TODO(maksymets): Add configure method to Sensor API to avoid
             # accessing child attributes through parent interface
             sim_sensor_cfg.sensor_type = sensor.sim_sensor_type  # type: ignore

habitat_baselines/common/env_utils.py

@@ -54,7 +54,9 @@ def construct_envs(
     configs = []
     env_classes = [env_class for _ in range(num_processes)]
     dataset = make_dataset(config.TASK_CONFIG.DATASET.TYPE)
-    scenes = dataset.get_scenes_to_load(config.TASK_CONFIG.DATASET)
+    scenes = config.TASK_CONFIG.DATASET.CONTENT_SCENES
+    if "*" in config.TASK_CONFIG.DATASET.CONTENT_SCENES:
+        scenes = dataset.get_scenes_to_load(config.TASK_CONFIG.DATASET)
 
     if len(scenes) > 0:
         random.shuffle(scenes)

test/test_sensors.py

@@ -19,7 +19,12 @@
     NavigationGoal,
 )
 from habitat.tasks.utils import quaternion_rotate_vector
+from habitat.utils.geometry_utils import angle_between_quaternions
 from habitat.utils.test_utils import sample_non_stop_action
+from habitat.utils.visualizations.utils import (
+    images_to_video,
+    observations_to_image,
+)
 
 
 def _random_episode(env, config):
@@ -319,3 +324,115 @@ def test_get_observations_at():
     assert np.allclose(agent_state.position, start_state.position)
     assert np.allclose(agent_state.rotation, start_state.rotation)
     env.close()
+
+
+def test_noise_models_rgbd():
+    DEMO_MODE = False
+    N_STEPS = 100
+
+    config = get_config()
+    config.defrost()
+    config.SIMULATOR.SCENE = (
+        "data/scene_datasets/habitat-test-scenes/skokloster-castle.glb"
+    )
+    config.SIMULATOR.AGENT_0.SENSORS = ["RGB_SENSOR", "DEPTH_SENSOR"]
+    config.freeze()
+    if not os.path.exists(config.SIMULATOR.SCENE):
+        pytest.skip("Please download Habitat test data to data folder.")
+
+    valid_start_position = [-1.3731, 0.08431, 8.60692]
+
+    expected_pointgoal = [0.1, 0.2, 0.3]
+    goal_position = np.add(valid_start_position, expected_pointgoal)
+
+    # starting quaternion is rotated 180 degree along z-axis, which
+    # corresponds to simulator using z-negative as forward action
+    start_rotation = [0, 0, 0, 1]
+    test_episode = NavigationEpisode(
+        episode_id="0",
+        scene_id=config.SIMULATOR.SCENE,
+        start_position=valid_start_position,
+        start_rotation=start_rotation,
+        goals=[NavigationGoal(position=goal_position)],
+    )
+
+    print(f"{test_episode}")
+    env = habitat.Env(config=config, dataset=None)
+
+    env.episode_iterator = iter([test_episode])
+    no_noise_obs = [env.reset()]
+    no_noise_states = [env.sim.get_agent_state()]
+
+    actions = [
+        sample_non_stop_action(env.action_space) for _ in range(N_STEPS)
+    ]
+    for action in actions:
+        no_noise_obs.append(env.step(action))
+        no_noise_states.append(env.sim.get_agent_state())
+    env.close()
+
+    config.defrost()
+
+    config.SIMULATOR.RGB_SENSOR.NOISE_MODEL = "GaussianNoiseModel"
+    config.SIMULATOR.RGB_SENSOR.NOISE_MODEL_KWARGS = habitat.Config()
+    config.SIMULATOR.RGB_SENSOR.NOISE_MODEL_KWARGS.INTENSITY_CONSTANT = 0.5
+    config.SIMULATOR.DEPTH_SENSOR.NOISE_MODEL = "RedwoodDepthNoiseModel"
+
+    config.SIMULATOR.ACTION_SPACE_CONFIG = "pyrobotnoisy"
+    config.SIMULATOR.NOISE_MODEL = habitat.Config()
+    config.SIMULATOR.NOISE_MODEL.ROBOT = "LoCoBot"
+    config.SIMULATOR.NOISE_MODEL.CONTROLLER = "Proportional"
+    config.SIMULATOR.NOISE_MODEL.NOISE_MULTIPLIER = 0.5
+
+    config.freeze()
+
+    env = habitat.Env(config=config, dataset=None)
+
+    env.episode_iterator = iter([test_episode])
+
+    obs = env.reset()
+    assert np.linalg.norm(
+        obs["rgb"].astype(np.float) - no_noise_obs[0]["rgb"].astype(np.float)
+    ) > 1.5e-2 * np.linalg.norm(
+        no_noise_obs[0]["rgb"].astype(np.float)
+    ), f"No RGB noise detected."
+
+    assert np.linalg.norm(
+        obs["depth"].astype(np.float)
+        - no_noise_obs[0]["depth"].astype(np.float)
+    ) > 1.5e-2 * np.linalg.norm(
+        no_noise_obs[0]["depth"].astype(np.float)
+    ), f"No Depth noise detected."
+
+    images = []
+    state = env.sim.get_agent_state()
+    angle_diffs = []
+    pos_diffs = []
+    for step_id, action in enumerate(actions):
+        prev_state = state
+        obs = env.step(action)
+        state = env.sim.get_agent_state()
+        position_change = np.linalg.norm(
+            np.array(state.position) - np.array(prev_state.position), ord=2
+        )
+
+        if action["action"][:5] == "TURN_":
+            angle_diff = abs(
+                angle_between_quaternions(state.rotation, prev_state.rotation)
+                - np.deg2rad(config.SIMULATOR.TURN_ANGLE)
+            )
+            angle_diffs.append(angle_diff)
+        else:
+            pos_diffs.append(
+                abs(position_change - config.SIMULATOR.FORWARD_STEP_SIZE)
+            )
+
+        if DEMO_MODE:
+            images.append(observations_to_image(obs, {}))
+
+    if DEMO_MODE:
+        images_to_video(images, "data/video/test_noise", "test_noise")
+
+    assert sum(angle_diffs) > 2.8, "No turn action actuation noise detected."
+    assert sum(pos_diffs) > 1.1, "No forward action actuation noise detected."
+    env.close()