def _quat_to_xy_heading(self, quat):
direction_vector = np.array([0, 0, -1])
heading_vector = quaternion_rotate_vector(quat, direction_vector)
phi = cartesian_to_polar(-heading_vector[2], heading_vector[0])[1]
return np.array(phi)
def _compute_pointgoal(self, source_position, source_rotation,
goal_position):
fov = self.fov
direction_vector = goal_position - source_position
direction_vector_agent = quaternion_rotate_vector(
source_rotation.inverse(), direction_vector)
_, phi = cartesian_to_polar(-direction_vector_agent[2],
direction_vector_agent[0])
theta = np.arcsin(direction_vector_agent[1] /
np.linalg.norm(direction_vector_agent))
rho = np.linalg.norm(direction_vector_agent)
dx = phi
dy = -1 * theta
fov_min = -1 * fov / 2
fov_max = 1 * fov / 2
xpx = dx / fov
ypx = dy / fov
if fov_max < dx < fov_min:
xpx = -1
if fov_max < dy < fov_min:
ypx = -1
return np.array([rho, -phi, theta, xpx, ypx], dtype=np.float32)
def _compute_pointgoal(self, source_position, source_rotation,
goal_position):
direction_vector = goal_position - source_position
direction_vector_agent = quaternion_rotate_vector(
source_rotation.inverse(), direction_vector)
if self._goal_format == "POLAR":
if self._dimensionality == 2:
rho, phi = cartesian_to_polar(-direction_vector_agent[2],
direction_vector_agent[0])
return np.array([rho, -phi], dtype=np.float32)
else:
_, phi = cartesian_to_polar(-direction_vector_agent[2],
direction_vector_agent[0])
theta = np.arccos(direction_vector_agent[1] /
np.linalg.norm(direction_vector_agent))
rho = np.linalg.norm(direction_vector_agent)
return np.array([rho, -phi, theta], dtype=np.float32)
else:
if self._dimensionality == 2:
return np.array(
[-direction_vector_agent[2], direction_vector_agent[0]],
dtype=np.float32,
)
else:
return direction_vector_agent
def get_rel_heading(self, posA, rotA, posB):
direction_vector = np.array([0, 0, -1])
quat = quaternion_from_coeff(rotA).inverse()
# The heading vector and heading angle are in arctan2 format
heading_vector = quaternion_rotate_vector(quat, direction_vector)
heading = cartesian_to_polar(-heading_vector[2], heading_vector[0])[1]
heading = self.normalize_angle(heading)
adjusted_heading = np.pi / 2 - heading
camera_horizon_vec = [
np.cos(adjusted_heading),
np.sin(adjusted_heading), 0
]
# This vectors are in habitat format we need to rotate them.
rotated_posB = [posB[0], -posB[2], posB[1]]
rotated_posA = [posA[0], -posA[2], posA[1]]
target_vector = np.array(rotated_posB) - np.array(rotated_posA)
y = target_vector[0] * camera_horizon_vec[1] - \
target_vector[1] * camera_horizon_vec[0]
x = target_vector[0] * camera_horizon_vec[0] + \
target_vector[1] * camera_horizon_vec[1]
return np.arctan2(y, x)
def test_pointgoal_with_gps_compass_sensor():
config = get_config()
if not os.path.exists(config.SIMULATOR.SCENE):
pytest.skip("Please download Habitat test data to data folder.")
config.defrost()
config.TASK.SENSORS = [
"POINTGOAL_WITH_GPS_COMPASS_SENSOR",
"COMPASS_SENSOR",
"GPS_SENSOR",
"POINTGOAL_SENSOR",
]
config.TASK.POINTGOAL_WITH_GPS_COMPASS_SENSOR.DIMENSIONALITY = 3
config.TASK.POINTGOAL_WITH_GPS_COMPASS_SENSOR.GOAL_FORMAT = "CARTESIAN"
config.TASK.POINTGOAL_SENSOR.DIMENSIONALITY = 3
config.TASK.POINTGOAL_SENSOR.GOAL_FORMAT = "CARTESIAN"
config.TASK.GPS_SENSOR.DIMENSIONALITY = 3
config.freeze()
with habitat.Env(config=config, dataset=None) as env:
# start position is checked for validity for the specific test scene
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]
env.episode_iterator = iter(
[
NavigationEpisode(
episode_id="0",
scene_id=config.SIMULATOR.SCENE,
start_position=valid_start_position,
start_rotation=start_rotation,
goals=[NavigationGoal(position=goal_position)],
)
]
)
env.reset()
for _ in range(100):
obs = env.step(sample_non_stop_action(env.action_space))
pointgoal = obs["pointgoal"]
pointgoal_with_gps_compass = obs["pointgoal_with_gps_compass"]
compass = float(obs["compass"][0])
gps = obs["gps"]
# check to see if taking non-stop actions will affect static point_goal
assert np.allclose(
pointgoal_with_gps_compass,
quaternion_rotate_vector(
quaternion.from_rotation_vector(
compass * np.array([0, 1, 0])
).inverse(),
pointgoal - gps,
),
atol=1e-5,
)
def get_observation(self, *args: Any, observations, episode,
**kwargs: Any):
agent_state = self._sim.get_agent_state()
origin = np.array(episode.start_position, dtype=np.float32)
rotation_world_start = quaternion_from_coeff(episode.start_rotation)
agent_position = agent_state.position
agent_position = quaternion_rotate_vector(
rotation_world_start.inverse(), agent_position - origin)
if self._dimensionality == 3:
rel_start_pos = np.array([-agent_position[2], agent_position[0]],
dtype=np.float32)
else:
rel_start_pos = agent_position.astype(np.float32)
rotation_world_agent = agent_state.rotation
rotation_world_start = quaternion_from_coeff(episode.start_rotation)
relative_start_heading = np.array([
self._quat_to_xy_heading(rotation_world_agent.inverse() *
rotation_world_start)
])
return np.concatenate([rel_start_pos, relative_start_heading])
def get_rel_elevation(self, posA, rotA, cameraA, posB):
direction_vector = np.array([0, 0, -1])
quat = quaternion_from_coeff(rotA)
rot_vector = quaternion_rotate_vector(quat.inverse(), direction_vector)
camera_quat = quaternion_from_coeff(cameraA)
camera_vector = quaternion_rotate_vector(camera_quat.inverse(),
direction_vector)
elevation_angle = dir_angle_between_quaternions(quat, camera_quat)
rotated_posB = [posB[0], -posB[2], posB[1]]
rotated_posA = [posA[0], -posA[2], posA[1]]
target_vector = np.array(rotated_posB) - np.array(rotated_posA)
target_z = target_vector[2]
target_length = np.linalg.norm([target_vector[0], target_vector[1]])
rel_elevation = np.arctan2(target_z, target_length)
return rel_elevation - elevation_angle
def get_observation(self, observations, episode):
agent_state = self._sim.get_agent_state()
rotation_world_agent = agent_state.rotation
direction_vector = np.array([0, 0, -1])
heading_vector = quaternion_rotate_vector(
rotation_world_agent.inverse(), direction_vector)
phi = cartesian_to_polar(-heading_vector[2], heading_vector[0])[1]
return np.array(phi)
def get_polar_angle(self):
agent_state = self._sim.get_agent_state()
# quaternion is in x, y, z, w format
ref_rotation = agent_state.rotation
heading_vector = quaternion_rotate_vector(ref_rotation.inverse(),
np.array([0, 0, -1]))
phi = cartesian_to_polar(-heading_vector[2], heading_vector[0])[1]
x_y_flip = -np.pi / 2
return np.array(phi) + x_y_flip
def get_observation(self, observations, episode):
agent_state = self._sim.get_agent_state()
origin = np.array(episode.start_position, dtype=np.float32)
rotation_world_start = quaternion_from_coeff(episode.start_rotation)
agent_position = agent_state.position
agent_position = quaternion_rotate_vector(
rotation_world_start.inverse(), agent_position - origin)
if self._dimensionality == 2:
return np.array([-agent_position[2], agent_position[0]],
dtype=np.float32)
else:
return agent_position.astype(np.float32)
def get_observation(self, observations, episode):
agent_state = self._sim.get_agent_state()
ref_position = agent_state.position
rotation_world_agent = agent_state.rotation
direction_vector = (
np.array(episode.goals[0].position, dtype=np.float32) -
ref_position)
direction_vector_agent = quaternion_rotate_vector(
rotation_world_agent.inverse(), direction_vector)
if self._goal_format == "POLAR":
rho, phi = cartesian_to_polar(-direction_vector_agent[2],
direction_vector_agent[0])
direction_vector_agent = np.array([rho, -phi], dtype=np.float32)
return direction_vector_agent
def convert_position_to_relative(self, c_data, prev_pos, prev_heading):
agent_position = c_data['position'][[0, 2, 1]]
agent_rotation = quaternion.from_euler_angles(
np.roll(c_data['rotation'], 1))
relative_pos = quaternion_rotate_vector(agent_rotation,
agent_position - prev_pos)
heading = np.array([_quat_to_xy_heading(agent_rotation)])
relative_heading = heading - prev_heading
if np.abs(relative_heading) > np.pi:
relative_heading = np.mod(relative_heading,
2 * np.pi * -np.sign(relative_heading))
pos = np.array([relative_pos[0], relative_pos[2]], dtype=np.float32)
prev_pos = agent_position
prev_heading = heading
return np.concatenate([pos, relative_heading]), prev_pos, prev_heading
def get_observation(self, observations, episode):
episode_id = (episode.episode_id, episode.scene_id)
if self.current_episode_id != episode_id:
# Only compute the direction vector when a new episode is started.
self.current_episode_id = episode_id
agent_state = self._sim.get_agent_state()
ref_position = agent_state.position
rotation_world_agent = agent_state.rotation
direction_vector = (
np.array(episode.goals[0].position, dtype=np.float32) -
ref_position)
direction_vector_agent = quaternion_rotate_vector(
rotation_world_agent.inverse(), direction_vector)
if self._goal_format == "POLAR":
rho, phi = cartesian_to_polar(-direction_vector_agent[2],
direction_vector_agent[0])
direction_vector_agent = np.array([rho, -phi],
dtype=np.float32)
self._initial_vector = direction_vector_agent
return self._initial_vector
def get_observation(self, *args: Any, observations, episode,
**kwargs: Any):
env_reset = "action" not in kwargs
agent_state = self._sim.get_agent_state()
if env_reset:
self._prev_rotation.clear()
self._prev_pos.clear()
self._prev_heading.clear()
self._prev_rotation.append(agent_state.rotation)
self._prev_pos.append(agent_state.position)
self._prev_heading.append(
np.array([self._quat_to_xy_heading(agent_state.rotation)]))
agent_position = agent_state.position
relative_pos = quaternion_rotate_vector(
self._prev_rotation[0].inverse(),
agent_position - self._prev_pos[0])
heading = np.array([self._quat_to_xy_heading(agent_state.rotation)])
relative_heading = heading - self._prev_heading[0]
if np.abs(relative_heading) > np.pi:
relative_heading = np.mod(relative_heading,
2 * np.pi * -np.sign(relative_heading))
if self._dimensionality == 3:
pos = np.array([-relative_pos[2], relative_pos[0]],
dtype=np.float32)
else:
pos = relative_pos.astype(np.float32)
self._prev_pos.append(agent_position)
self._prev_heading.append(heading)
self._prev_rotation.append(agent_state.rotation)
return np.concatenate([pos, relative_heading])
locs = []
for ep_num, ep in enumerate(data):
info = ep['info']
episode_info, start_rot, actions, weights, gps, heading = info.values()
print(episode_info.keys())
_, _, start_pos, start_rot, more_ep_info, goals, start_room, shortest_path = episode_info.values()
# if not in_goal_box(goals[0]["position"]):
# continue
for i, reading in enumerate(gps):
reading_pos = [reading[1], start_pos[1], -reading[0]]
# reading_pos = [reading[0], start_pos[1], reading[1]]
head_phi = heading[i] # this is already global
rotation_world_start = quaternion_from_coeff(start_rot)
global_offset = quaternion_rotate_vector(
rotation_world_start, reading_pos
)
global_pos = global_offset + start_pos
cur_x, _, cur_y = global_pos
loc_info = [cur_x, cur_y, head_phi, actions[i]]
loc_info.extend(weights[i])
locs.append(loc_info)
df = pd.DataFrame(locs, columns=cols, index=[i for i in range(len(locs))])
for task in aux_tasks:
df[task] = pd.to_numeric(df[task])
df['head_cat'] = pd.cut(df['heading'], [-1 * np.pi, -1 * np.pi / 2, 0, np.pi/2, np.pi],# bins=dist_bins,
labels=['N', 'W', 'S', 'E'])
df['head_cat'].describe()
#%%