Added collision and proximity measurements; Use is_navigable for a more robust...

Added collision and proximity measurements; Use is_navigable for a more robust check for top-down map

habitat/config/default.py

@@ -42,12 +42,23 @@ _C.TASK.POINTGOAL_SENSOR.GOAL_FORMAT = "POLAR"
 _C.TASK.HEADING_SENSOR = CN()
 _C.TASK.HEADING_SENSOR.TYPE = "HeadingSensor"
 # -----------------------------------------------------------------------------
+# # PROXIMITY SENSOR
+# -----------------------------------------------------------------------------
+_C.TASK.PROXIMITY_SENSOR = CN()
+_C.TASK.PROXIMITY_SENSOR.TYPE = "ProximitySensor"
+_C.TASK.PROXIMITY_SENSOR.MAX_DETECTION_RADIUS = 2.0
+# -----------------------------------------------------------------------------
 # # SPL MEASUREMENT
 # -----------------------------------------------------------------------------
 _C.TASK.SPL = CN()
 _C.TASK.SPL.TYPE = "SPL"
 _C.TASK.SPL.SUCCESS_DISTANCE = 0.2
 # -----------------------------------------------------------------------------
+# # COLLISIONS MEASUREMENT
+# -----------------------------------------------------------------------------
+_C.TASK.COLLISIONS = CN()
+_C.TASK.COLLISIONS.TYPE = "Collisions"
+# -----------------------------------------------------------------------------
 # SIMULATOR
 # -----------------------------------------------------------------------------
 _C.SIMULATOR = CN()

habitat/core/simulator.py

@@ -29,6 +29,7 @@ class SensorTypes(Enum):
     TEXT = 9
     MEASUREMENT = 10
     HEADING = 11
+    TACTILE = 12
 
 
 class Sensor:

habitat/sims/habitat_simulator.py

@@ -399,6 +399,10 @@ class HabitatSim(habitat.Simulator):
     def index_stop_action(self):
         return SIM_NAME_TO_ACTION[SimulatorActions.STOP.value]
 
+    @property
+    def index_forward_action(self):
+        return SIM_NAME_TO_ACTION[SimulatorActions.FORWARD.value]
+
     def _get_agent_config(self, agent_id: Optional[int] = None) -> Any:
         if agent_id is None:
             agent_id = self.config.DEFAULT_AGENT_ID
@@ -442,3 +446,8 @@ class HabitatSim(habitat.Simulator):
     def _check_agent_position(self, position, agent_id=0):
         if not np.allclose(position, self.get_agent_state(agent_id).position):
             logger.info("Agent state diverges from configured start position.")
+
+    def distance_to_closest_obstacle(self, position, max_search_radius=2.0):
+        return self._sim.pathfinder.distance_to_closest_obstacle(
+            position, max_search_radius
+        )

habitat/tasks/nav/nav_task.py

@@ -20,6 +20,8 @@ from habitat.core.simulator import (
 )
 from habitat.tasks.utils import quaternion_to_rotation, cartesian_to_polar
 
+COLLISION_PROXIMITY_TOLERANCE: float = 1e-3
+
 
 def merge_sim_episode_config(
     sim_config: Config, episode: Type[Episode]
@@ -240,6 +242,44 @@ class HeadingSensor(habitat.Sensor):
         return np.array(phi)
 
 
+class ProximitySensor(habitat.Sensor):
+    """
+       Sensor for observing the distance to the closest obstacle
+
+       Args:
+           sim: reference to the simulator for calculating task observations.
+           config: config for the sensor.
+       """
+
+    def __init__(self, sim, config):
+        self._sim = sim
+        self._max_detection_radius = getattr(
+            config, "MAX_DETECTION_RADIUS", 2.0
+        )
+        super().__init__(config=config)
+
+    def _get_uuid(self, *args: Any, **kwargs: Any):
+        return "proximity"
+
+    def _get_sensor_type(self, *args: Any, **kwargs: Any):
+        return SensorTypes.TACTILE
+
+    def _get_observation_space(self, *args: Any, **kwargs: Any):
+        return spaces.Box(
+            low=0.0,
+            high=self._max_detection_radius,
+            shape=(1,),
+            dtype=np.float,
+        )
+
+    def get_observation(self, observations, episode):
+        current_position = self._sim.get_agent_state().position
+
+        return self._sim.distance_to_closest_obstacle(
+            current_position, self._max_detection_radius
+        )
+
+
 class SPL(habitat.Measure):
     """SPL (Success weighted by Path Length)
 
@@ -298,6 +338,33 @@ class SPL(habitat.Measure):
         )
 
 
+class Collisions(habitat.Measure):
+    def __init__(self, sim, config):
+        self._sim = sim
+        self._config = config
+        self._metric = None
+
+        super().__init__()
+
+    def _get_uuid(self, *args: Any, **kwargs: Any):
+        return "collisions"
+
+    def reset_metric(self, episode):
+        self._metric = None
+
+    def update_metric(self, episode, action):
+        if self._metric is None:
+            self._metric = 0
+
+        current_position = self._sim.get_agent_state().position
+        if (
+            action == self._sim.index_forward_action
+            and self._sim.distance_to_closest_obstacle(current_position)
+            < COLLISION_PROXIMITY_TOLERANCE
+        ):
+            self._metric += 1
+
+
 class NavigationTask(habitat.EmbodiedTask):
     def __init__(
         self,
@@ -310,16 +377,16 @@ class NavigationTask(habitat.EmbodiedTask):
         for measurement_name in task_config.MEASUREMENTS:
             measurement_cfg = getattr(task_config, measurement_name)
             is_valid_measurement = hasattr(
-                habitat.tasks.nav.nav_task,  # type: ignore
-                measurement_cfg.TYPE,
+                habitat.tasks.nav.nav_task,
+                measurement_cfg.TYPE,  # type: ignore
             )
             assert is_valid_measurement, "invalid measurement type {}".format(
                 measurement_cfg.TYPE
             )
             task_measurements.append(
                 getattr(
-                    habitat.tasks.nav.nav_task,  # type: ignore
-                    measurement_cfg.TYPE,
+                    habitat.tasks.nav.nav_task,
+                    measurement_cfg.TYPE,  # type: ignore
                 )(sim, measurement_cfg)
             )
         self.measurements = Measurements(task_measurements)
@@ -335,8 +402,10 @@ class NavigationTask(habitat.EmbodiedTask):
             )
             task_sensors.append(
                 getattr(
-                    habitat.tasks.nav.nav_task, sensor_cfg.TYPE  # type: ignore
-                )(sim, sensor_cfg)
+                    habitat.tasks.nav.nav_task, sensor_cfg.TYPE
+                )(  # type: ignore
+                    sim, sensor_cfg
+                )
             )
 
         self.sensor_suite = SensorSuite(task_sensors)

habitat/utils/visualizations/maps.py

@@ -200,15 +200,6 @@ def _from_grid(
     return realworld_x, realworld_y
 
 
-def _check_valid_nav_point(sim: Simulator, point: List[float]) -> bool:
-    """Checks if a given point is inside a wall or other object or not."""
-    return (
-        0.01
-        < sim.geodesic_distance(point, [point[0], point[1] + 0.1, point[2]])
-        < 0.11
-    )
-
-
 def _outline_border(top_down_map):
     left_right_block_nav = (top_down_map[:, :-1] == 1) & (
         top_down_map[:, :-1] != top_down_map[:, 1:]
@@ -282,17 +273,17 @@ def get_topdown_map(
         min(range_y[-1] + padding + 1, top_down_map.shape[1]),
     )
     top_down_map[:] = 0
+
     # Search over grid for valid points.
     for ii in range(range_x[0], range_x[1]):
         for jj in range(range_y[0], range_y[1]):
             realworld_x, realworld_y = _from_grid(
                 ii, jj, COORDINATE_MIN, COORDINATE_MAX, map_resolution
             )
-            valid_point = _check_valid_nav_point(
-                sim, [realworld_x, start_height + 0.5, realworld_y]
+            valid_point = sim._sim.pathfinder.is_navigable(
+                [realworld_x, start_height, realworld_y]
             )
-            if valid_point:
-                top_down_map[ii, jj] = 1
+            top_down_map[ii, jj] = 1 if valid_point else 0
 
     # Draw border if necessary
     if draw_border:

test/data/habitat-sim_trajectory_data.json

-{"positions": [[2.9085702896118164, 0.07244700193405151, 0.13949552178382874], [2.692063808441162, 0.17669875919818878, 0.014495700597763062], [2.475557327270508, 0.17669875919818878, -0.11050412058830261], [2.2590508460998535, 0.17669875919818878, -0.12059974670410156], [2.042544364929199, 0.17669875919818878, -0.12059974670410156], [1.826037883758545, 0.17669875919818878, -0.12059974670410156], [1.6095314025878906, 0.17669875919818878, -0.12059974670410156], [1.3930249214172363, 0.17669875919818878, -0.12059974670410156], [1.176518440246582, 0.17669875919818878, -0.12059974670410156], [1.176518440246582, 0.17669875919818878, -0.12059974670410156], [0.941595196723938, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156]], "rotations": [[0.0, 0.5000002980232239, 0.0, 0.8660252094268799], [0.0, 0.5000003576278687, 0.0, 0.8660252094268799], [0.0, 0.5000002980232239, 0.0, 0.8660252094268799], [0.0, 0.5000002980232239, 0.0, 0.8660252094268799], [0.0, 0.5000002980232239, 0.0, 0.8660252094268799], [0.0, 0.5000002980232239, 0.0, 0.8660252094268799], [0.0, 0.5000002980232239, 0.0, 0.8660252094268799], [0.0, 0.5000002980232239, 0.0, 0.8660252094268799], [0.0, 0.5000002980232239, 0.0, 0.8660252094268799], [0.0, 0.573576807975769, 0.0, 0.8191518187522888], [0.0, 0.5735767483711243, 0.0, 0.8191518187522888], [0.0, 0.573576807975769, 0.0, 0.8191518187522888], [0.0, 0.5735767483711243, 0.0, 0.8191518187522888], [0.0, 0.573576807975769, 0.0, 0.8191518187522888], [0.0, 0.5735767483711243, 0.0, 0.8191518187522888], [0.0, 0.573576807975769, 0.0, 0.8191518187522888], [0.0, 0.5735767483711243, 0.0, 0.8191518187522888], [0.0, 0.573576807975769, 0.0, 0.8191518187522888], [0.0, 0.5735767483711243, 0.0, 0.8191518187522888], [0.0, 0.573576807975769, 0.0, 0.8191518187522888], [0.0, 0.5735767483711243, 0.0, 0.8191518187522888], [0.0, 0.573576807975769, 0.0, 0.8191518187522888], [0.0, 0.5735767483711243, 0.0, 0.8191518187522888], [0.0, 0.573576807975769, 0.0, 0.8191518187522888], [0.0, 0.5735767483711243, 0.0, 0.8191518187522888], [0.0, 0.573576807975769, 0.0, 0.8191518187522888], [0.0, 0.5735767483711243, 0.0, 0.8191518187522888], [0.0, 0.573576807975769, 0.0, 0.8191518187522888], [0.0, 0.6427879929542542, 0.0, 0.7660441398620605], [0.0, 0.6427879929542542, 0.0, 0.7660441398620605], [0.0, 0.6427879929542542, 0.0, 0.7660441398620605], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7660449147224426, 0.0, 0.6427870988845825], [0.0, 0.7660449743270874, 0.0, 0.6427870392799377], [0.0, 0.7660449743270874, 0.0, 0.642786979675293], [0.0, 0.8191525340080261, 0.0, 0.5735757350921631], [0.0, 0.8191525340080261, 0.0, 0.5735757350921631], [0.0, 0.8191525340080261, 0.0, 0.5735757350921631], [0.0, 0.8191525340080261, 0.0, 0.5735757350921631], [0.0, 0.8660258650779724, 0.0, 0.4999992549419403], [0.0, 0.8660258650779724, 0.0, 0.4999992549419403], [0.0, 0.8660258650779724, 0.0, 0.4999992549419403], [0.0, 0.8660258650779724, 0.0, 0.4999992549419403], [0.0, 0.8660258650779724, 0.0, 0.4999992549419403], [0.0, 0.8660258650779724, 0.0, 0.4999992549419403]], "actions": [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 3]}
+{"positions": [[2.9085702896118164, 0.07244700193405151, 0.13949552178382874], [2.692063808441162, 0.17669875919818878, 0.014495700597763062], [2.475557327270508, 0.17669875919818878, -0.11050412058830261], [2.2590508460998535, 0.17669875919818878, -0.12059974670410156], [2.042544364929199, 0.17669875919818878, -0.12059974670410156], [1.826037883758545, 0.17669875919818878, -0.12059974670410156], [1.6095314025878906, 0.17669875919818878, -0.12059974670410156], [1.3930249214172363, 0.17669875919818878, -0.12059974670410156], [1.176518440246582, 0.17669875919818878, -0.12059974670410156], [1.176518440246582, 0.17669875919818878, -0.12059974670410156], [0.941595196723938, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156], [0.8741309642791748, 0.17669875919818878, -0.12059974670410156]], "rotations": [[0.0, 0.5000002980232239, 0.0, 0.8660252094268799], [0.0, 0.5000003576278687, 0.0, 0.8660252094268799], [0.0, 0.5000002980232239, 0.0, 0.8660252094268799], [0.0, 0.5000002980232239, 0.0, 0.8660252094268799], [0.0, 0.5000002980232239, 0.0, 0.8660252094268799], [0.0, 0.5000002980232239, 0.0, 0.8660252094268799], [0.0, 0.5000002980232239, 0.0, 0.8660252094268799], [0.0, 0.5000002980232239, 0.0, 0.8660252094268799], [0.0, 0.5000002980232239, 0.0, 0.8660252094268799], [0.0, 0.573576807975769, 0.0, 0.8191518187522888], [0.0, 0.5735767483711243, 0.0, 0.8191518187522888], [0.0, 0.573576807975769, 0.0, 0.8191518187522888], [0.0, 0.5735767483711243, 0.0, 0.8191518187522888], [0.0, 0.573576807975769, 0.0, 0.8191518187522888], [0.0, 0.5735767483711243, 0.0, 0.8191518187522888], [0.0, 0.573576807975769, 0.0, 0.8191518187522888], [0.0, 0.5735767483711243, 0.0, 0.8191518187522888], [0.0, 0.573576807975769, 0.0, 0.8191518187522888], [0.0, 0.5735767483711243, 0.0, 0.8191518187522888], [0.0, 0.573576807975769, 0.0, 0.8191518187522888], [0.0, 0.5735767483711243, 0.0, 0.8191518187522888], [0.0, 0.573576807975769, 0.0, 0.8191518187522888], [0.0, 0.5735767483711243, 0.0, 0.8191518187522888], [0.0, 0.573576807975769, 0.0, 0.8191518187522888], [0.0, 0.5735767483711243, 0.0, 0.8191518187522888], [0.0, 0.573576807975769, 0.0, 0.8191518187522888], [0.0, 0.5735767483711243, 0.0, 0.8191518187522888], [0.0, 0.573576807975769, 0.0, 0.8191518187522888], [0.0, 0.6427879929542542, 0.0, 0.7660441398620605], [0.0, 0.6427879929542542, 0.0, 0.7660441398620605], [0.0, 0.6427879929542542, 0.0, 0.7660441398620605], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7071071863174438, 0.0, 0.707106351852417], [0.0, 0.7660449147224426, 0.0, 0.6427870988845825], [0.0, 0.7660449743270874, 0.0, 0.6427870392799377], [0.0, 0.7660449743270874, 0.0, 0.642786979675293], [0.0, 0.8191525340080261, 0.0, 0.5735757350921631], [0.0, 0.8191525340080261, 0.0, 0.5735757350921631], [0.0, 0.8191525340080261, 0.0, 0.5735757350921631], [0.0, 0.8191525340080261, 0.0, 0.5735757350921631], [0.0, 0.8660258650779724, 0.0, 0.4999992549419403], [0.0, 0.8660258650779724, 0.0, 0.4999992549419403], [0.0, 0.8660258650779724, 0.0, 0.4999992549419403], [0.0, 0.8660258650779724, 0.0, 0.4999992549419403], [0.0, 0.8660258650779724, 0.0, 0.4999992549419403], [0.0, 0.8660258650779724, 0.0, 0.4999992549419403]], "actions": [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 3], "distances_to_obstacles": [null, 0.2564087510108948, 0.010195869952440262, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}
\ No newline at end of file

test/test_sensors.py

@@ -11,11 +11,35 @@ import random
 
 import habitat
 from habitat.config.default import get_config
-from habitat.tasks.nav.nav_task import NavigationEpisode
+from habitat.tasks.nav.nav_task import (
+    NavigationEpisode,
+    COLLISION_PROXIMITY_TOLERANCE,
+)
+from habitat.sims.habitat_simulator import SimulatorActions, SIM_NAME_TO_ACTION
 
 CFG_TEST = "test/habitat_all_sensors_test.yaml"
 
 
+def _random_episode(env, config):
+    random_location = env._sim.sample_navigable_point()
+    random_heading = np.random.uniform(-np.pi, np.pi)
+    random_rotation = [
+        0,
+        np.sin(random_heading / 2),
+        0,
+        np.cos(random_heading / 2),
+    ]
+    env.episodes = [
+        NavigationEpisode(
+            episode_id="0",
+            scene_id=config.SIMULATOR.SCENE,
+            start_position=random_location,
+            start_rotation=random_rotation,
+            goals=[],
+        )
+    ]
+
+
 def test_heading_sensor():
     config = get_config(CFG_TEST)
     if not os.path.exists(config.SIMULATOR.SCENE):
@@ -26,7 +50,8 @@ def test_heading_sensor():
     config.freeze()
     env = habitat.Env(config=config, dataset=None)
     env.reset()
-    random.seed(1234)
+    random.seed(123)
+    np.random.seed(123)
 
     for _ in range(100):
         random_heading = np.random.uniform(-np.pi, np.pi)
@@ -51,3 +76,83 @@ def test_heading_sensor():
         assert np.allclose(heading, random_heading)
 
     env.close()
+
+
+def test_tactile():
+    config = get_config(CFG_TEST)
+    if not os.path.exists(config.SIMULATOR.SCENE):
+        pytest.skip("Please download Habitat test data to data folder.")
+    config = get_config()
+    config.defrost()
+    config.TASK.SENSORS = ["PROXIMITY_SENSOR"]
+    config.TASK.MEASUREMENTS = ["COLLISIONS"]
+    config.freeze()
+    env = habitat.Env(config=config, dataset=None)
+    env.reset()
+    random.seed(1234)
+
+    for _ in range(20):
+        _random_episode(env, config)
+        env.reset()
+        assert env.get_metrics()["collisions"] is None
+
+        my_collisions_count = 0
+        action = env._sim.index_forward_action
+        for _ in range(10):
+            obs = env.step(action)
+            collisions = env.get_metrics()["collisions"]
+            proximity = obs["proximity"]
+            if proximity < COLLISION_PROXIMITY_TOLERANCE:
+                my_collisions_count += 1
+
+            assert my_collisions_count == collisions
+
+    env.close()
+
+
+def test_collisions():
+    config = get_config(CFG_TEST)
+    if not os.path.exists(config.SIMULATOR.SCENE):
+        pytest.skip("Please download Habitat test data to data folder.")
+    config = get_config()
+    config.defrost()
+    config.TASK.MEASUREMENTS = ["COLLISIONS"]
+    config.freeze()
+    env = habitat.Env(config=config, dataset=None)
+    env.reset()
+    random.seed(123)
+    np.random.seed(123)
+
+    actions = [
+        SIM_NAME_TO_ACTION[SimulatorActions.FORWARD.value],
+        SIM_NAME_TO_ACTION[SimulatorActions.LEFT.value],
+        SIM_NAME_TO_ACTION[SimulatorActions.RIGHT.value],
+    ]
+
+    for _ in range(20):
+        _random_episode(env, config)
+
+        env.reset()
+        assert env.get_metrics()["collisions"] is None
+
+        prev_collisions = 0
+        prev_loc = env.sim.get_agent_state().position
+        for _ in range(50):
+            action = np.random.choice(actions)
+            env.step(action)
+            collisions = env.get_metrics()["collisions"]
+
+            loc = env.sim.get_agent_state().position
+            if (
+                np.linalg.norm(loc - prev_loc)
+                < 0.9 * config.SIMULATOR.FORWARD_STEP_SIZE
+                and action == actions[0]
+            ):
+                # Check to see if the new method of doing collisions catches all the same
+                # collisions as the old method
+                assert collisions == prev_collisions + 1
+
+            prev_loc = loc
+            prev_collisions = collisions
+
+    env.close()

test/test_trajectory_sim.py

@@ -54,6 +54,15 @@ def test_sim_trajectory():
                 )
                 is True
             ), "mismatch in rotation " "at step {}".format(i)
+
+            max_search_radius = 2.0
+            dist_to_obs = sim.distance_to_closest_obstacle(
+                state.position, max_search_radius
+            )
+            assert np.isclose(
+                dist_to_obs, test_trajectory["distances_to_obstacles"][i]
+            )
+
         assert sim.action_space.contains(action)
 
         sim.step(action)