Exemplo n.º 1
0
    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
Exemplo n.º 2
0
 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()
Exemplo n.º 3
0
    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
Exemplo n.º 4
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
Exemplo n.º 5
0
 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
Exemplo n.º 6
0
 def reset(self, task: EmbodiedTask, *args: Any, **kwargs: Any):
     task.is_stop_called = False  # type: ignore
Exemplo n.º 7
0
    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