def update_metric(self, *args: Any, episode, action, task: EmbodiedTask,
**kwargs: Any):
current_position = self._sim.get_agent_state().position.tolist()
discrete = getattr(task, "is_discrete")
if discrete:
curr_viewpoint_id = episode.curr_viewpoint.image_id
goal = episode.goals[-1].image_id
scan = episode.scan
distance_to_target = \
task.get_distance_to_target(scan, curr_viewpoint_id, goal)
else:
current_position = self._sim.get_agent_state().position.tolist()
distance_to_target = self._sim.geodesic_distance(
current_position, episode.goals[-1].get_position())
if np.isinf(distance_to_target):
print("WARNING: The Oracle distance might be compromised " +
"The geodesic_distance failed " +
"looking for a snap_point instead")
new_position = np.array(current_position, dtype='f')
new_position = self._sim._sim.pathfinder.snap_point(
new_position)
if np.isnan(new_position[0]):
print("ERROR: The Oracle distance is compromised " +
"The geodesic_distance failed " + "Cannot find path")
else:
current_position = new_position
distance_to_target = self._sim.geodesic_distance(
current_position, episode.goals[-1].get_position())
self._metric = distance_to_target
def step_physics(self, task: EmbodiedTask, id_agent_obj, *args: Any, **kwargs: Any):
r"""Update ``_metric``, this method is called from ``Env`` on each
``step``. Step with physics enabled.
"""
# convert forward command to linear velocity
agent_vel_control = self._sim.get_object_velocity_control(id_agent_obj)
agent_vel_control.controlling_lin_vel = True
agent_vel_control.controlling_ang_vel = True
agent_vel_control.lin_vel_is_local = True
agent_vel_control.ang_vel_is_local = True
agent_vel_control.linear_velocity = np.float32([0, 0, 0])
agent_vel_control.angular_velocity = np.float32([0, 0, 0])
task.is_stop_called = True
return self._sim.get_observations_at()
def update_metric(self, *args: Any, episode, action, task: EmbodiedTask,
**kwargs: Any):
ep_success = 0
current_position = self._sim.get_agent_state().position.tolist()
discrete = getattr(task, "is_discrete")
if discrete:
curr_viewpoint_id = episode.curr_viewpoint.image_id
goal = episode.goals[-1].image_id
scan = episode.scan
distance_to_target = \
task.get_distance_to_target(scan, curr_viewpoint_id, goal)
else:
distance_to_target = self._sim.geodesic_distance(
current_position, episode.goals[-1].get_position())
if np.isinf(distance_to_target):
print("WARNING: The Success metric might be compromised " +
"The geodesic_distance failed " +
"looking for a snap_point instead")
new_position = np.array(current_position, dtype='f')
new_position = self._sim._sim.pathfinder.snap_point(
new_position)
if np.isnan(new_position[0]):
print("ERROR: The Success metric is compromised " +
"The geodesic_distance failed " + "Cannot find path")
else:
current_position = new_position
distance_to_target = self._sim.geodesic_distance(
current_position, episode.goals[-1].get_position())
if (hasattr(task, "is_stop_called") and task.is_stop_called
and distance_to_target < self._config.SUCCESS_DISTANCE):
ep_success = 1
self._agent_episode_distance += self._euclidean_distance(
current_position, self._previous_position)
self._previous_position = current_position
self._metric = ep_success * (self._start_end_episode_distance / max(
self._start_end_episode_distance, self._agent_episode_distance))
# removing rounding error of 0.0000025 meters
if self._metric >= 0.9999975:
self._metric = 1.0
def update_metric(self, *args: Any, episode, task: EmbodiedTask,
**kwargs: Any):
ep_success = 0
discrete = getattr(task, "is_discrete")
if discrete:
curr_viewpoint_id = episode.curr_viewpoint.image_id
goal = episode.goals[-1].image_id
scan = episode.scan
distance_to_target = \
task.get_distance_to_target(scan, curr_viewpoint_id, goal)
else:
current_position = self._sim.get_agent_state().position.tolist()
distance_to_target = self._sim.geodesic_distance(
current_position, episode.goals[-1].get_position())
if np.isinf(distance_to_target):
print("WARNING: The Oracle distance might be compromised " +
"The geodesic_distance failed " +
"looking for a snap_point instead")
new_position = np.array(current_position, dtype='f')
new_position = self._sim._sim.pathfinder.snap_point(
new_position)
if np.isnan(new_position[0]):
print("ERROR: The Oracle distance is compromised " +
"The geodesic_distance failed " + "Cannot find path")
else:
current_position = new_position
distance_to_target = self._sim.geodesic_distance(
current_position, episode.goals[-1].get_position())
if (self._nearest_distance == -1
or distance_to_target < self._nearest_distance):
self._nearest_distance = distance_to_target
if (hasattr(task, "is_stop_called") and task.is_stop_called and
self._nearest_distance < self._config.ORACLE_SUCCESS_DISTANCE):
ep_success = 1
self._metric = ep_success
def step(self, task: EmbodiedTask, *args: Any, **kwargs: Any):
r"""Update ``_metric``, this method is called from ``Env`` on each
``step``.
"""
task.is_stop_called = True # type: ignore
return self._sim.get_observations_at() # type: ignore
def reset(self, task: EmbodiedTask, *args: Any, **kwargs: Any):
task.is_stop_called = False # type: ignore
def step(
self,
*args: Any,
task: EmbodiedTask,
linear_velocity: float,
angular_velocity: float,
time_step: Optional[float] = None,
allow_sliding: Optional[bool] = None,
**kwargs: Any,
):
r"""Moves the agent with a provided linear and angular velocity for the
provided amount of time
Args:
linear_velocity: between [-1,1], scaled according to
config.LIN_VEL_RANGE
angular_velocity: between [-1,1], scaled according to
config.ANG_VEL_RANGE
time_step: amount of time to move the agent for
allow_sliding: whether the agent will slide on collision
"""
if allow_sliding is None:
allow_sliding = self._sim.config.sim_cfg.allow_sliding # type: ignore
if time_step is None:
time_step = self.time_step
# Convert from [-1, 1] to [0, 1] range
linear_velocity = (linear_velocity + 1.0) / 2.0
angular_velocity = (angular_velocity + 1.0) / 2.0
# Scale actions
linear_velocity = self.min_lin_vel + linear_velocity * (
self.max_lin_vel - self.min_lin_vel)
angular_velocity = self.min_ang_vel + angular_velocity * (
self.max_ang_vel - self.min_ang_vel)
# Stop is called if both linear/angular speed are below their threshold
if (abs(linear_velocity) < self.min_abs_lin_speed
and abs(angular_velocity) < self.min_abs_ang_speed):
task.is_stop_called = True # type: ignore
return self._sim.get_observations_at(position=None, rotation=None)
angular_velocity = np.deg2rad(angular_velocity)
self.vel_control.linear_velocity = np.array(
[0.0, 0.0, -linear_velocity])
self.vel_control.angular_velocity = np.array(
[0.0, angular_velocity, 0.0])
agent_state = self._sim.get_agent_state()
# Convert from np.quaternion to mn.Quaternion
normalized_quaternion = agent_state.rotation
agent_mn_quat = mn.Quaternion(normalized_quaternion.imag,
normalized_quaternion.real)
current_rigid_state = RigidState(
agent_mn_quat,
agent_state.position,
)
# manually integrate the rigid state
goal_rigid_state = self.vel_control.integrate_transform(
time_step, current_rigid_state)
# snap rigid state to navmesh and set state to object/agent
if allow_sliding:
step_fn = self._sim.pathfinder.try_step # type: ignore
else:
step_fn = self._sim.pathfinder.try_step_no_sliding # type: ignore
final_position = step_fn(agent_state.position,
goal_rigid_state.translation)
final_rotation = [
*goal_rigid_state.rotation.vector,
goal_rigid_state.rotation.scalar,
]
# Check if a collision occured
dist_moved_before_filter = (goal_rigid_state.translation -
agent_state.position).dot()
dist_moved_after_filter = (final_position - agent_state.position).dot()
# NB: There are some cases where ||filter_end - end_pos|| > 0 when a
# collision _didn't_ happen. One such case is going up stairs. Instead,
# we check to see if the the amount moved after the application of the
# filter is _less_ than the amount moved before the application of the
# filter.
EPS = 1e-5
collided = (dist_moved_after_filter + EPS) < dist_moved_before_filter
agent_observations = self._sim.get_observations_at(
position=final_position,
rotation=final_rotation,
keep_agent_at_new_pose=True,
)
# TODO: Make a better way to flag collisions
self._sim._prev_sim_obs["collided"] = collided # type: ignore
return agent_observations