class PrintStatus(LivePlugin):
    
    def __init__(self, interval_print):
        self.tracker = InAWhile(interval_print)

    def init(self, data):
        pass

    def starting_stream(self, stream):
        """ A new stream is being read """
        self.cur_stream_observations = 0
        self.stream = stream
    
    def update(self, up):
        self.cur_stream_observations += 1
        progress = up['progress']
        state = up['state']
        
        if not self.tracker.its_time():
            return

        perc_obs = 100 * (float(progress.obs.done) / progress.obs.target)
        perc_obs_log = 100 * (float(self.cur_stream_observations) / self.stream.get_num_observations())
        msg = ('overall %.2f%% (log %3d%%) (eps: %4d/%d, obs: %4d/%d);'
               ' %5.1f fps' % 
               (perc_obs, perc_obs_log, len(state.id_episodes),
                 progress.eps.target, state.num_observations, progress.obs.target,
                self.tracker.fps()))
        logger.info(msg)
Example #2
0
    def __init__(self, data_central, id_robot, id_agent, num_episodes,
                 cumulative=True, interval_print=5):
        self.data_central = data_central
        self.id_robot = id_robot
        self.cumulative = cumulative

        if self.cumulative:
            log_index = data_central.get_log_index()
            self.done_before = log_index.get_episodes_for_robot(id_robot,
                                                                id_agent)
            self.num_episodes_done_before = len(self.done_before)
            self.num_episodes_todo = (num_episodes - 
                                      self.num_episodes_done_before)
            logger.info('Preparing to do %d episodes (already done %d).' % 
                        (self.num_episodes_todo,
                         self.num_episodes_done_before))
        else:
            self.num_episodes_todo = num_episodes
            logger.info('Preparing to do %d episodes.' % 
                        self.num_episodes_todo)
        self.num_episodes_done = 0
        self.num_observations = 0
        self.num_observations_episode = 0
        self.observations_per_episode = []

        self.interval_print = interval_print
        self.tracker = InAWhile(interval_print)
        self.id_episodes = set()

        try:
            from compmake import progress
            progress('Simulating episodes', (0, self.num_episodes_todo))
        except ImportError:
            pass
Example #3
0
class Bookkeeping:
    """ Simple class to keep track of how many we have to simulate. """

    def __init__(self, data_central, id_robot, num_episodes, cumulative=True, interval_print=5):
        self.data_central = data_central
        self.id_robot = id_robot
        self.cumulative = cumulative

        if self.cumulative:
            log_index = data_central.get_log_index()
            if log_index.has_streams_for_robot(id_robot):
                self.done_before = log_index.get_episodes_for_robot(id_robot)
                self.num_episodes_done_before = len(self.done_before)
            else:
                self.done_before = set()
                self.num_episodes_done_before = 0
            self.num_episodes_todo = num_episodes - self.num_episodes_done_before
            logger.info(
                "Preparing to do %d episodes (already done %d)."
                % (self.num_episodes_todo, self.num_episodes_done_before)
            )
        else:
            self.num_episodes_todo = num_episodes
            logger.info("Preparing to do %d episodes." % self.num_episodes_todo)
        self.num_episodes_done = 0
        self.num_observations = 0
        self.num_observations_episode = 0
        self.observations_per_episode = []

        self.interval_print = interval_print
        self.tracker = InAWhile(interval_print)
        self.id_episodes = set()

        try:
            from compmake import progress

            progress("Simulating episodes", (0, self.num_episodes_todo))
        except ImportError:
            pass

    def observations(self, observations):
        self.id_episodes.add(observations["id_episode"].item())

        self.num_observations_episode += 1
        self.num_observations += 1
        if self.tracker.its_time():
            msg = "simulating %d/%d episodes obs %d (%5.1f fps)" % (
                self.num_episodes_done,
                self.num_episodes_todo,
                self.num_observations,
                self.tracker.fps(),
            )
            if self.num_episodes_done > 0:
                msg += " (mean obs/ep: %.1f)" % (np.mean(self.observations_per_episode))
            logger.info(msg)

    def get_id_episodes(self):
        """ Returns the list of episodes simulated. """
        return natsorted(self.id_episodes)

    def get_all_episodes(self):
        """ Returns the list of all episodes, both the already present
            and the simulated. """
        eps = []
        eps.extend(self.id_episodes)
        eps.extend(self.done_before)
        return natsorted(set(eps))

    def episode_done(self):
        self.num_episodes_done += 1
        self.observations_per_episode.append(self.num_observations_episode)
        self.num_observations_episode = 0

        try:
            from compmake import progress

            progress("Simulating episodes", (self.num_episodes_done, self.num_episodes_todo))
        except ImportError:
            pass

    def another_episode_todo(self):
        return self.num_episodes_done < self.num_episodes_todo
 def __init__(self, interval_print):
     self.tracker = InAWhile(interval_print)
Example #5
0
class BookkeepingServo():
    ''' Simple class to keep track of how many we have to simulate. '''
    @contract(interval_print='None|>=0')
    def __init__(self, data_central, id_robot, id_agent, num_episodes,
                 cumulative=True, interval_print=5):
        self.data_central = data_central
        self.id_robot = id_robot
        self.cumulative = cumulative

        if self.cumulative:
            log_index = data_central.get_log_index()
            self.done_before = log_index.get_episodes_for_robot(id_robot,
                                                                id_agent)
            self.num_episodes_done_before = len(self.done_before)
            self.num_episodes_todo = (num_episodes - 
                                      self.num_episodes_done_before)
            logger.info('Preparing to do %d episodes (already done %d).' % 
                        (self.num_episodes_todo,
                         self.num_episodes_done_before))
        else:
            self.num_episodes_todo = num_episodes
            logger.info('Preparing to do %d episodes.' % 
                        self.num_episodes_todo)
        self.num_episodes_done = 0
        self.num_observations = 0
        self.num_observations_episode = 0
        self.observations_per_episode = []

        self.interval_print = interval_print
        self.tracker = InAWhile(interval_print)
        self.id_episodes = set()

        try:
            from compmake import progress
            progress('Simulating episodes', (0, self.num_episodes_todo))
        except ImportError:
            pass

    def observations(self, observations):
        self.id_episodes.add(observations['id_episode'].item())

        self.num_observations_episode += 1
        self.num_observations += 1
        if self.tracker.its_time():
            msg = ('simulating %d/%d episodes obs %d (%5.1f fps)' % 
                   (self.num_episodes_done,
                    self.num_episodes_todo,
                    self.num_observations, self.tracker.fps()))
            if self.num_episodes_done > 0:
                msg += (' (mean obs/ep: %.1f)' % 
                        (np.mean(self.observations_per_episode)))
            logger.info(msg)

    def get_id_episodes(self):
        ''' Returns the list of episodes simulated. '''
        return natsorted(self.id_episodes)

    def get_all_episodes(self):
        ''' Returns the list of all episodes, both the already present
            and the simulated. '''
        eps = []
        eps.extend(self.id_episodes)
        eps.extend(self.done_before)
        return natsorted(set(eps))

    def episode_done(self):
        self.num_episodes_done += 1
        self.observations_per_episode.append(self.num_observations_episode)
        self.num_observations_episode = 0

        try:
            from compmake import progress
            progress('servoing', (self.num_episodes_done,
                                 self.num_episodes_todo))
        except ImportError:
            pass

    def another_episode_todo(self):
        return self.num_episodes_done < self.num_episodes_todo
Example #6
0
def servonav_episode(
    id_robot,
    robot,
    id_servo_agent,
    servo_agent,
    writer,
    id_episode,
    max_episode_len,
    save_robot_state,
    interval_write=1,
    interval_print=5,
    resolution=0.5,  # grid resolution
    delta_t_threshold=0.2,  # when to switch
    MIN_PATH_LENGTH=8,
    MAX_TIME_FOR_SWITCH=20.0,
    fail_if_not_working=False,
    max_tries=10000,
):
    """
    
        :arg:displacement: Time in seconds to displace the robot.
    """
    from geometry import SE2_from_SE3, translation_from_SE2, angle_from_SE2, SE3

    stats_write = InAWhile(interval_print)

    # Access the vehicleSimulation interface
    vsim = get_vsim_from_robot(robot)

    for _ in xrange(max_tries):
        # iterate until we can do this correctly
        episode = robot.new_episode()
        locations = get_grid(robot=robot, vsim=vsim, resolution=resolution)

        if len(locations) < MIN_PATH_LENGTH:
            logger.info("Path too short, trying again.")
        else:
            break

    else:
        msg = "Could not find path in %d tries." % max_tries
        raise Exception(msg)

    locations_yaml = convert_to_yaml(locations)

    vsim.vehicle.set_pose(locations[0]["pose"])

    current_goal = 1
    current_goal_obs = locations[current_goal]["observations"]
    servo_agent.set_goal_observations(current_goal_obs)

    counter = 0
    time_last_switch = None

    num_written = 0
    for robot_observations, boot_observations in run_simulation_servonav(
        id_robot,
        robot,
        id_servo_agent,
        servo_agent,
        100000,
        max_episode_len,
        id_episode=id_episode,
        id_environment=episode.id_environment,
        raise_error_on_collision=fail_if_not_working,
    ):

        current_time = boot_observations["timestamp"].item()
        if time_last_switch is None:
            time_last_switch = current_time

        time_since_last_switch = float(current_time - time_last_switch)

        def obs_distance(obs1, obs2):
            return float(np.linalg.norm(obs1.flatten() - obs2.flatten()))

        curr_pose = robot_observations.robot_pose
        curr_obs = boot_observations["observations"]
        curr_goal = locations[current_goal]["observations"]
        prev_goal = locations[current_goal - 1]["observations"]
        curr_err = obs_distance(curr_goal, curr_obs)
        prev_err = obs_distance(prev_goal, curr_obs)
        current_goal_pose = locations[current_goal]["pose"]
        current_goal_obs = locations[current_goal]["observations"]

        delta = SE2_from_SE3(SE3.multiply(SE3.inverse(curr_pose), current_goal_pose))
        delta_t = np.linalg.norm(translation_from_SE2(delta))
        delta_th = np.abs(angle_from_SE2(delta))

        if stats_write.its_time():
            msg = "  deltaT: %.2fm  deltaTh: %.1fdeg" % (delta_t, np.rad2deg(delta_th))
            logger.debug(msg)

        # If at the final goal, go closer
        is_final_goal = current_goal == len(locations) - 1
        if is_final_goal:
            delta_t_threshold *= 0.3

        # TODO: should we care also about delta_th?
        time_to_switch = (delta_t < delta_t_threshold) or (time_since_last_switch > MAX_TIME_FOR_SWITCH)
        # does not work: curr_err < SWITCH_THRESHOLD * prev_err:

        if time_to_switch:
            current_goal += 1
            logger.info("Switched to goal %d." % current_goal)

            time_last_switch = current_time
            if current_goal >= len(locations):
                # finished
                logger.info("Finished :-)")
                break

        threshold_lost_m = 3
        if delta_t > threshold_lost_m:
            msg = "Breaking because too far away."
            if not (fail_if_not_working):
                logger.error(msg)
                break
            else:
                raise Exception(msg)

        servo_agent.set_goal_observations(current_goal_obs)

        extra = {}
        extra["servoing_base"] = dict(goal=curr_goal.tolist(), current=curr_obs.tolist())

        extra["servoing_poses"] = dict(goal=SE3.to_yaml(current_goal_pose), current=SE3.to_yaml(curr_pose))

        extra["servonav"] = dict(
            poseK=SE3.to_yaml(curr_pose),
            obsK=boot_observations["observations"].tolist(),
            pose1=SE3.to_yaml(current_goal_pose),
            locations=locations_yaml,
            current_goal=current_goal,
            curr_err=curr_err,
            prev_err=prev_err,
            time_last_switch=time_last_switch,
            time_since_last_switch=time_since_last_switch,
        )

        if counter % interval_write == 0:
            if save_robot_state:
                extra["robot_state"] = robot.get_state()

            writer.push_observations(observations=boot_observations, extra=extra)
            num_written += 1
        counter += 1

    if num_written == 0:
        msg = "This log was too short to be written (%d observations)" % counter
        raise Exception(msg)