diff --git a/habitat/config/default.py b/habitat/config/default.py
index 8bd9189b2213f82e4f49213aaf88609d02247cf4..aaad2860ccaa43d019c2c29c5c32109261f059dd 100644
--- a/habitat/config/default.py
+++ b/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()
diff --git a/habitat/core/simulator.py b/habitat/core/simulator.py
index fbeaac1e96f9f72c9dbec1026b02ca849d95f05d..b2acb18f9e0dfce2fab2716db9444e4a03ecd46e 100644
--- a/habitat/core/simulator.py
+++ b/habitat/core/simulator.py
@@ -29,6 +29,7 @@ class SensorTypes(Enum):
TEXT = 9
MEASUREMENT = 10
HEADING = 11
+ TACTILE = 12
class Sensor:
diff --git a/habitat/sims/habitat_simulator.py b/habitat/sims/habitat_simulator.py
index 3a5311454fa39a4531e845f75549fda79c2f717d..97385c5deb10de28e13d862f36b11de313277a13 100644
--- a/habitat/sims/habitat_simulator.py
+++ b/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
+ )
diff --git a/habitat/tasks/nav/nav_task.py b/habitat/tasks/nav/nav_task.py
index bf44de91943dd1fd3ea149c00ee4cf22061faac1..3f703a7df82802296f83664a6e49d1b87196fa6b 100644
--- a/habitat/tasks/nav/nav_task.py
+++ b/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)
diff --git a/habitat/utils/visualizations/maps.py b/habitat/utils/visualizations/maps.py
index f845cbf2ccbde8f130e9d4acbc38462bf9ea9db5..b8f226df936e3fbbc9c9d522a09e103c09c9da68 100644
--- a/habitat/utils/visualizations/maps.py
+++ b/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:
diff --git a/test/data/habitat-sim_trajectory_data.json b/test/data/habitat-sim_trajectory_data.json
index 79e09a270743dda5dc67cbd97ca9f103b8b2fb5d..0af0fd9072db05ef86106b475b6ca2d1f44f2f98 100644
--- a/test/data/habitat-sim_trajectory_data.json
+++ b/test/data/habitat-sim_trajectory_data.json
@@ -1 +1 @@
-{"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
diff --git a/test/test_sensors.py b/test/test_sensors.py
index d6f8d1b503245f2c4a5802f664f8e2fc8723c59a..788bb72968e87c47ea7e9da1ccf6d00c0a12c673 100644
--- a/test/test_sensors.py
+++ b/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()
diff --git a/test/test_trajectory_sim.py b/test/test_trajectory_sim.py
index 9123cbf11666ab828c1b66897be0b2d90b972183..db448bc9dd4b9d710821a112b86b156aec1c9c19 100644
--- a/test/test_trajectory_sim.py
+++ b/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)