Exemple #1
0
    def poll(self, poll_time, on_enter=None, on_exit=None):
        print('starting polling')
        history = RingBuffer(capacity=20, dtype=np.float)
        entered = False
        while True:
            dist = self.distance()
            history.append(dist)

            if len(history) < 10:
                continue

            avg = np.median(history)
            if DEBUG:
                sys.stdout.write('\rdist: {:06.10f} avg: {:06.10f}'.format(
                    dist, avg))

            if not entered and avg < THRESHOLD:
                entered = True
                if on_enter:
                    on_enter()
            elif entered and avg > THRESHOLD:
                entered = False
                if on_exit:
                    on_exit()
            time.sleep(poll_time)
Exemple #2
0
class RingBufferTests(unittest.TestCase):
    def setUp(self):
        self.buffer = RingBuffer(5)
        self.buffer_2 = RingBuffer(5)

    def test_ring_buffer(self):
        self.assertEqual(len(self.buffer.storage), 5)

        self.buffer.append('a')
        self.buffer.append('b')
        self.buffer.append('c')
        self.buffer.append('d')
        self.assertEqual(len(self.buffer.storage), 5)
        self.assertEqual(self.buffer.get(), ['a', 'b', 'c', 'd'])
Exemple #3
0
class XYBuffer(object):
    '''A class for holding X and Y about the sensor

    Basically a ring buffer, which serves as the data input for plotting
    sensor values as a moving windows in pyqtgraph
    '''
    def __init__(self, update_interval_secs, total_secs=3600, data_type=float):
        '''XYBuffer constructor

        Args:
            update_interval_secs (int): How often is the pyqtgraph updated
            total_secs (int): What's the maximum length of the moving window.
                Default size 3600 seconds - i.e. 1 hour
            data_type(): use numpy data types
        '''
        self.buffer_size = total_secs // update_interval_secs
        self.x_buffer = RingBuffer(size_max=self.buffer_size,
                                   default_value=0,
                                   dtype=np.uint32)
        self.y_buffer = RingBuffer(size_max=self.buffer_size,
                                   default_value=0.0,
                                   dtype=data_type)
        self._x_counter = 0

    def append_value(self, y_value):
        '''Appends the y_value to the y_buffer and increments the sample
        number in the x_buffer
        '''
        self.y_buffer.append(y_value)
        self._x_counter += 1
        self.x_buffer.append(self._x_counter)

    def get_actual_filled_values(self):
        '''Gets the values from the buffer, which are already filled
        
        Used for data plotting
        '''
        return self.x_buffer.partial, self.y_buffer.partial
Exemple #4
0
class RingBufferTests(unittest.TestCase):
    def setUp(self):
        self.capacity = 5
        self.buffer = RingBuffer(self.capacity)

    def test_new_buffer_has_appropriate_capacity(self):
        self.assertEqual(self.buffer.capacity, self.capacity)

    def test_adding_one_element_to_buffer(self):
        self.buffer.append("a")
        self.assertEqual(self.buffer.get(), ["a"])

    def test_filling_buffer_to_capacity(self):
        self.buffer.append("a")
        self.buffer.append("b")
        self.buffer.append("c")
        self.buffer.append("d")
        self.buffer.append("e")
        self.assertEqual(self.buffer.get(), ["a", "b", "c", "d", "e"])

    def test_adding_one_element_to_full_buffer(self):
        self.buffer.append("a")
        self.buffer.append("b")
        self.buffer.append("c")
        self.buffer.append("d")
        self.buffer.append("e")
        self.buffer.append("f")
        self.assertEqual(self.buffer.get(), ["f", "b", "c", "d", "e"])

    def test_adding_many_elements_to_full_buffer(self):
        self.buffer.append("a")
        self.buffer.append("b")
        self.buffer.append("c")
        self.buffer.append("d")
        self.buffer.append("e")
        self.buffer.append("f")
        self.buffer.append("g")
        self.buffer.append("h")
        self.buffer.append("i")
        self.assertEqual(self.buffer.get(), ["f", "g", "h", "i", "e"])

    def test_adding_50_elements_to_buffer(self):
        for i in range(50):
            self.buffer.append(i)

        self.assertEqual(self.buffer.get(), [45, 46, 47, 48, 49])
class RingBufferTests(unittest.TestCase):
    def setUp(self):
        self.buffer = RingBuffer(5)
        self.buffer_2 = RingBuffer(5)

    def test_ring_buffer(self):
        self.assertEqual(self.buffer.storage.length, 0)

        self.buffer.append("a")
        self.buffer.append("b")
        self.buffer.append("c")
        self.buffer.append("d")
        self.assertEqual(self.buffer.storage.length, 4)
        self.assertEqual(self.buffer.get(), ["a", "b", "c", "d"])

        self.buffer.append("e")
        self.assertEqual(self.buffer.storage.length, 5)
        self.assertEqual(self.buffer.get(), ["a", "b", "c", "d", "e"])

        self.buffer.append("f")
        self.assertEqual(self.buffer.storage.length, 5)
        self.assertEqual(self.buffer.get(), ["f", "b", "c", "d", "e"])

        self.buffer.append("g")
        self.buffer.append("h")
        self.buffer.append("i")
        self.assertEqual(self.buffer.storage.length, 5)
        self.assertEqual(self.buffer.get(), ["f", "g", "h", "i", "e"])

        self.buffer.append("j")
        self.buffer.append("k")
        self.assertEqual(self.buffer.get(), ["k", "g", "h", "i", "j"])

        for i in range(50):
            self.buffer_2.append(i)
        self.assertEqual(self.buffer_2.get(), [45, 46, 47, 48, 49])
Exemple #6
0
def main():

    env = gym.make('BreakoutDeterministic-v4')
    frame = env.reset()
    env.render()

    frames_per_action = 4
    num_actions = 4
    ATARI_SHAPE_PLUSONE = (105, 80, 5)
    num_games = 10

    this_states = RingBuffer(5)
    this_rewards = RingBuffer(4)

    all_prev_states = []
    all_next_states = []
    all_actions = []
    all_rewards = []
    all_isterminal = []

    # print('a')
    prev_frame = preprocess(frame)
    for this_game in range(0, num_games):
        iter_count = 0
        is_done = False
        while not is_done:
            this_action = env.action_space.sample()
            # print('b')
            this_action_onehot = action_to_onehot(this_action)
            this_states.append(prev_frame)
            for action_count in range(0, frames_per_action):
                # print('c')
                frame, reward, is_done, _ = env.step(this_action)
                this_states.append(preprocess(frame))
                this_rewards.append(transform_reward(reward))
                if not is_done:
                    env.render()
                else:
                    frame = env.reset()
                    env.render()
                    break
            prev_frame = frame
            if (iter_count > 0):
                all_prev_states.append(this_states.clip_from_end(1))
                all_next_states.append(this_states.clip_from_start(1))
                all_rewards.append(this_rewards)
                all_actions.append(this_action)
                all_isterminal.append(int(is_done))
                # is_done = False
            iter_count += 1
            # input()
    np_prev_states = np.asarray(all_prev_states)
    # print('prev states: ',np.shape(np_prev_states))
    np_next_states = np.asarray(all_next_states)
    # print('next states: ',np.shape(np_next_states))
    np_rewards = np.asarray(all_rewards)
    # np_rewards = np_rewards[:-1,:]
    # print('rewards: ',np.shape(np_rewards))
    np_actions = np.asarray(all_actions)
    # np_actions = np_actions[:-1]
    # print('actions: ',np.shape(np_actions))
    np_isterminal = np.asarray(all_isterminal)
    # np_isterminal = np_isterminal[:-1]
    # print('isterminal: ',np.shape(np_isterminal))

    np_num_objects = np.size(np_isterminal)
    # print('num_objects:',np_num_objects)

    t_model = atari_model(num_actions)
Exemple #7
0
class RingBufferTests(unittest.TestCase):
    def setUp(self):
        self.capacity = 5
        self.buffer = RingBuffer(self.capacity)

    def test_new_buffer_has_appropriate_capacity(self):
        self.assertEqual(self.buffer.capacity, self.capacity)

    def test_adding_one_element_to_buffer(self):
        self.buffer.append('a')
        self.assertEqual(self.buffer.get(), ['a'])

    def test_filling_buffer_to_capacity(self):
        self.buffer.append('a')
        self.buffer.append('b')
        self.buffer.append('c')
        self.buffer.append('d')
        self.buffer.append('e')
        self.assertEqual(self.buffer.get(), ['a', 'b', 'c', 'd', 'e'])

    def test_adding_one_element_to_full_buffer(self):
        self.buffer.append('a')
        self.buffer.append('b')
        self.buffer.append('c')
        self.buffer.append('d')
        self.buffer.append('e')
        self.buffer.append('f')
        self.assertEqual(self.buffer.get(), ['f', 'b', 'c', 'd', 'e'])

    def test_adding_many_elements_to_full_buffer(self):
        self.buffer.append('a')
        self.buffer.append('b')
        self.buffer.append('c')
        self.buffer.append('d')
        self.buffer.append('e')
        self.buffer.append('f')
        self.buffer.append('g')
        self.buffer.append('h')
        self.buffer.append('i')
        self.assertEqual(self.buffer.get(), ['f', 'g', 'h', 'i', 'e'])

    def test_adding_50_elements_to_buffer(self):

        for i in range(50):
            self.buffer.append(i)

        self.assertEqual(self.buffer.get(), [45, 46, 47, 48, 49])
class Follower(BehaviorBase):
    def __init__(self, g, zeta,
                 leader, trajectory_delay=2.0,
                 update_delta_t=0.01,
                 orig_leader=None, orig_leader_delay=None,
                 noise_sigma=0.0, log_file=None,
                 visibility_fov=config.FOV_ANGLE, visibility_radius=None,
                 id=None,
                 dt=0.02):

        """
        Construct a follower Behavior.

        leader is the Bot to be followed
        g > 0 is a tuning parameter
        zeta in (0, 1) is a damping coefficient
        trajectory_delay is the time interval between leader and follower
        """
        super(Follower, self).__init__()

        assert(isinstance(leader, Bot))
        assert(isinstance(orig_leader, Bot))

        self.radius = config.BOT_RADIUS
        self.leader = leader
        self.orig_leader = orig_leader
        self.trajectory_delay = trajectory_delay

        assert g > 0, "Follower: g parameter must be positive"
        self.g = g
        assert 0 < zeta < 1, "Follower: zeta parameter must be in (0, 1)"
        self.zeta = zeta

        # leader_positions stores config.POSITIONS_BUFFER_SIZE tuples;
        # tuple's first field is time, second is the
        # corresponding position of the leader
        self.leader_positions = RingBuffer(config.POSITIONS_BUFFER_SIZE)
        self.noisy_leader_positions = RingBuffer(config.POSITIONS_BUFFER_SIZE)

        self.leader_is_visible = False

        # precise_orig_leader_states stores the first leader's precise state;
        # used to calculate "real" errors (as opposed to calculations
        # w.r.t. the approximation curve)
        self.precise_orig_leader_states = []
        # precise_leader_states is used for the same purpose, but with the bot's own leader
        self.precise_leader_states = []

        self.update_delta_t = update_delta_t
        needed_buffer_size = config.SAMPLE_COUNT // 2 + self.trajectory_delay / self.update_delta_t
        if needed_buffer_size > config.POSITIONS_BUFFER_SIZE:
            sys.stderr.write("leader_positions buffer is too small for update_delta_t = {}".format(self.update_delta_t) +
                             " (current = {}, required = {})".format(config.POSITIONS_BUFFER_SIZE, needed_buffer_size))
            raise RuntimeError, "leader_positions buffer is too small"
        self.last_update_time = 0.0

        self.noise_sigma = noise_sigma

        self.log_file = log_file

        self.visibility_fov = visibility_fov
        if visibility_radius is None:
            visibility_radius = 2.0 * trajectory_delay * config.BOT_VEL_CAP
        self.visibility_radius = visibility_radius

        self.id = id;

        if orig_leader_delay is None:
            orig_leader_delay = trajectory_delay
        self.orig_leader_delay = orig_leader_delay


        if self.log_file is not None:
            log_dict = {"id": self.id,
                         "g": self.g,
                         "zeta": self.zeta,
                         "noise_sigma": self.noise_sigma,
                         "reference_points_cnt": config.SAMPLE_COUNT,
                         "trajectory_delay": trajectory_delay}
            print >> self.log_file, log_dict

        q = 0.01   # std of process
        r = 0.05   # std of measurement

        self.delta_time = dt
        if not config.DISABLE_UKF:
            points = MerweScaledSigmaPoints(n=6, alpha=.1, beta=2., kappa=-3)
            self.ukf = UKF(dim_x=6, dim_z=2, fx=f, hx=h, dt=dt, points=points)

            self.ukf_x_initialized = 0
            #self.ukf.x = np.array([0, 0, 1, pi/4, 0, 0])
            self.ukf.R = np.diag([r, r]);
            self.ukf.Q = np.diag([0, 0, 0, 0, q, q])


    def point_in_fov(self, p):
        if length(p - self.pos) > self.visibility_radius:
            return False
        d = p - self.pos
        ang = atan2(cross(self.real_dir, d), dot(self.real_dir, d))
        return -0.5 * self.visibility_fov < ang < 0.5 * self.visibility_fov


    def point_is_visible(self, p, obstacles):
        if not self.point_in_fov(p):
            return False
        try:
            ray = Ray(self.pos, p - self.pos)
        except NormalizationError:
            ray = Ray(self.pos, Vector(1.0, 0.0))
        i = first_intersection(ray, obstacles)
        return (i is None) or (length(i - self.pos) > length(p - self.pos))


    def store_leaders_state(self, engine, obstacles):
        if self.point_is_visible(self.leader.real.pos, obstacles):
            self.leader_is_visible = True

            noisy_pos = self.leader.real.pos
            noisy_pos += Vector(gauss(0.0, self.noise_sigma),
                                gauss(0.0, self.noise_sigma))
            if config.DISABLE_UKF:
                filtered_pos = noisy_pos
            else:
                if (self.ukf_x_initialized == 0):
                    self.ukf_x1 = noisy_pos
                    self.ukf_x_initialized += 1
                    filtered_pos = noisy_pos
                    self.ukf.x = np.array([noisy_pos.x, noisy_pos.y,
                                           0, pi/2, 0, 0])
                #elif (self.ukf_x_initialized == 1):
                #    self.ukf_x2 = noisy_pos
                #    self.ukf_x_initialized += 1
                #    self.ukf.x = np.array([noisy_pos.x, noisy_pos.y,
                #                           length((noisy_pos - self.ukf_x1) / self.delta_time),
                #                           atan2(noisy_pos.y - self.ukf_x1.y,
                #                                 noisy_pos.x - self.ukf_x1.x),
                #                           0, 0])
                #    filtered_pos = noisy_pos
                else: # UKF is initialized
                    self.ukf.predict()
                    self.ukf.update(np.array(noisy_pos))
                    filtered_pos = Point(self.ukf.x[0], self.ukf.x[1])
                                      
            self.leader_noisy_pos = noisy_pos
            self.noisy_leader_positions.append(TimedPosition(engine.time, noisy_pos))
            self.leader_positions.append(TimedPosition(engine.time, filtered_pos))
            self.last_update_time = engine.time
        else:
            self.leader_is_visible = False
            if not config.DISABLE_UKF:
                self.ukf.predict()
                #TODO: do magic with UKF?

        orig_leader_theta = atan2(self.orig_leader.real.dir.y,
                                  self.orig_leader.real.dir.x)
        self.precise_orig_leader_states.append(TimedState(time=engine.time,
                                                   pos=self.orig_leader.real.pos,
                                                   theta=orig_leader_theta))
        leader_theta = atan2(self.leader.real.dir.y,
                             self.leader.real.dir.x)
        self.precise_leader_states.append(TimedState(time=engine.time,
                                             pos=self.leader.real.pos,
                                             theta=leader_theta))


    def polyfit_trajectory(self, pos_data, t):
        x_pos = np.array([el.pos.x for el in pos_data])
        y_pos = np.array([el.pos.y for el in pos_data])
        times = np.array([el.time   for el in pos_data])

        # needed for trajectory rendering
        self.t_st = times[0]
        self.t_fn = max(times[-1], t)

        # calculate quadratic approximation of the reference trajectory
        x_poly = np.polyfit(times, x_pos, deg=2)
        y_poly = np.polyfit(times, y_pos, deg=2)
        known = Trajectory.from_poly(x_poly, y_poly)
        return known, x_pos[-1], y_pos[-1], times[-1]


    def extend_trajectory(self, known, last_x, last_y, last_t):
        # now adding a circle to the end of known trajectory
        # k is signed curvature of the trajectry at t_fn
        try:
            k = known.curvature(last_t)
        except (ValueError, ZeroDivisionError):
            k = config.MIN_CIRCLE_CURVATURE
        if abs(k) < config.MIN_CIRCLE_CURVATURE:
            k = copysign(config.MIN_CIRCLE_CURVATURE, k)

        if abs(k) > config.MAX_CURVATURE:
            k = copysign(config.MAX_CURVATURE, k)

        radius = abs(1.0/k)
        # trajectory direction at time t_fn
        try:
            d = normalize(Vector(known.dx(last_t), known.dy(last_t)))
        except NormalizationError:
            d = self.real_dir

        r = Vector(-d.y, d.x) / k
        center = Point(last_x, last_y) + r
        phase = atan2(-r.y, -r.x)
        #freq = known.dx(last_t) / r.y
        freq = k
        self.x_approx = lambda time: known.x(time) if time < last_t else \
                                     center.x + radius * cos(freq * (time - last_t) + phase)
        self.y_approx = lambda time: known.y(time) if time < last_t else \
                                     center.y + radius * sin(freq * (time - last_t) + phase)
        dx = lambda time: known.dx(time) if time < last_t else \
                          -radius * freq * sin(freq * (time - last_t) + phase)
        dy = lambda time: known.dy(time) if time < last_t else \
                          radius * freq * cos(freq * (time - last_t) + phase)
        ddx = lambda time: known.ddx(time) if time < last_t else \
                          -radius * freq * freq * cos(freq * (time - last_t) + phase)
        ddy = lambda time: known.ddy(time) if time < last_t else \
                          -radius * freq * freq * sin(freq * (time - last_t) + phase)
        # FIXME: don't use division by y if y == 0
        try:
            if isnan(self.x_approx(last_t + 1)):
                return known
        except Exception:
            return known
        return Trajectory(x=self.x_approx, y=self.y_approx,
                          dx=dx, dy=dy, ddx=ddx, ddy=ddy)



    def generate_trajectory(self, leader_positions, t):
        arr = get_interval(self.leader_positions, t, config.SAMPLE_COUNT)
        self.traj_interval = arr
        if len(arr) == 0:
            return None
        known, last_x, last_y, last_t = self.polyfit_trajectory(arr, t)
        if config.DISABLE_CIRCLES:
            return known
        else:
            return self.extend_trajectory(known, last_x, last_y, last_t)


    def calc_desired_velocity(self, bots, obstacles, targets, engine):
        # update trajectory
        if engine.time - self.last_update_time > self.update_delta_t:
            self.store_leaders_state(engine, obstacles)

        # reduce random movements at the start
        # TODO: this causes oscillations that smash bots into each other.
        # Change to something smoother. PID control?
        #if self.leader_is_visible and length(self.pos - self.leader_noisy_pos) < MIN_DISTANCE_TO_LEADER:
        #    return Instr(0.0, 0.0)

        t = engine.time - self.trajectory_delay
        self.trajectory = self.generate_trajectory(self.leader_positions, t)
        if self.trajectory is None:
            return Instr(0.0, 0.0)

        dx = self.trajectory.dx
        dy = self.trajectory.dy
        ddx = self.trajectory.ddx
        ddy = self.trajectory.ddy

        # calculate the feed-forward velocities
        v_fun = lambda time: sqrt(dx(time)**2 + dy(time)**2)
        #omega_fun = lambda time: (dx(time) * 2 * y_poly[0] - dy(time) * 2 * x_poly[0]) / (dx(time)**2 + dy(time)**2)
        omega_fun = lambda time: (dx(time) * ddy(time) - dy(time) * ddx(time)) / (dx(time)**2 + dy(time)**2)
        v_ff = v_fun(t)
        omega_ff = omega_fun(t)

        # x_r, y_r and theta_r denote the reference robot's state
        r = State(x=self.trajectory.x(t),
                  y=self.trajectory.y(t),
                  theta=atan2(self.trajectory.dy(t),
                              self.trajectory.dx(t)))
        self.target_point = Point(r.x, r.y)

        if isnan(v_ff):
            v_ff = 0.0
        if isnan(omega_ff):
            omega_ff = 0.0

        # cur is the current state
        cur = State(x=self.pos.x,
                    y=self.pos.y,
                    theta=atan2(self.real_dir.y, self.real_dir.x))

        # error in the global (fixed) reference frame
        delta = State(x=r.x - cur.x,
                      y=r.y - cur.y,
                      theta=normalize_angle(r.theta - cur.theta))

        # translate error into the follower's reference frame
        e = State(x= cos(cur.theta) * delta.x + sin(cur.theta) * delta.y,
                  y=-sin(cur.theta) * delta.x + cos(cur.theta) * delta.y,
                  theta=delta.theta)

        # calculate gains k_x, k_y, k_theta
        # these are just parameters, not a state in any sense!
        omega_n = sqrt(omega_ff**2 + self.g * v_ff**2)
        k_x = 2 * self.zeta * omega_n
        k_y = self.g
        k_theta = 2 * self.zeta * omega_n

        # calculate control velocities
        v = v_ff * cos(e.theta) + k_x * e.x
        se = sin(e.theta) / e.theta if e.theta != 0 else 1.0
        omega = omega_ff + k_y * v_ff * se * e.y + k_theta * e.theta

        if config.DISABLE_FEEDBACK:
            v = v_ff
            omega = omega_ff

        real_e = State(0.0, 0.0, 0.0)
        try:
            orig_t = engine.time - self.orig_leader_delay
            orig_leader_state = lerp_precise_states(orig_t, self.precise_orig_leader_states)
            real_e = State(x=orig_leader_state.x - cur.x,
                               y=orig_leader_state.y - cur.y,
                               theta=normalize_angle(orig_leader_state.theta - cur.theta))
        except LerpError:
            # not enough data is yet available to calculate error
            pass

        try:
            leader_t = engine.time - self.trajectory_delay
            leader_state = lerp_precise_states(leader_t, self.precise_leader_states)
            approx_e = State(x=leader_state.x - cur.x,
                             y=leader_state.y - cur.y,
                             theta=normalize_angle(leader_state.theta - cur.theta))
        except LerpError:
            # same as above
            pass

        if self.log_file is not None:
            log_dict = {"id": self.id,
                         "time": engine.time,
                         "delta_time": engine.time_since_last_bot_update,
                         "x": cur.x,
                         "y": cur.y,
                         "theta": cur.theta,
                         "v": v,
                         "omega": omega,
                         "v_ff": v_ff,
                         "omega_ff": omega_ff,
                         "e_x": e.x,
                         "e_y": e.y,
                         "e_theta": e.theta,
                         "real_e_x": real_e.x,
                         "real_e_y": real_e.y,
                         "real_e_theta": real_e.theta,
                         "approx_e_x": approx_e.x,
                         "approx_e_y": approx_e.y,
                         "approx_e_theta": approx_e.theta,
                         "leader_is_visible": self.leader_is_visible
                        }
            print >> self.log_file, log_dict

        return Instr(v,  omega)


    def draw(self, screen, field):
        if config.DISPLAYED_POINTS_COUNT > 0:
            k = config.POSITIONS_BUFFER_SIZE / config.DISPLAYED_POINTS_COUNT
            if k == 0:
                k = 1
            for index, (time, point) in enumerate(self.leader_positions):
                if index % k == 0:
                    draw_circle(screen, field, config.TRAJECTORY_POINTS_COLOR, point, 0.03, 1)
            for index, (time, point) in enumerate(self.noisy_leader_positions):
                if index % k == 0:
                    draw_circle(screen, field, config.NOISY_TRAJECTORY_POINTS_COLOR, point, 0.03, 1)

        if config.DISPLAYED_USED_POINTS_COUNT > 0:
            k = len(self.traj_interval) / config.DISPLAYED_USED_POINTS_COUNT
            if k == 0:
                k = 1
            for index, (time, point) in enumerate(self.traj_interval):
                if index % k == 0:
                    draw_circle(screen, field, config.USED_TRAJECTORY_POINTS_COLOR, point, 0.03, 1)

        if config.DRAW_DELAYED_LEADER_POS:
            try:
                orig_leader_dir = Vector(cos(self.orig_leader_theta),
                                         sin(self.orig_leader_theta))
                draw_directed_circle(screen, field, (0, 255, 0),
                                     self.orig_leader_pos, 0.2,
                                     orig_leader_dir, 1)
            except AttributeError:
                pass

        if config.DRAW_SENSOR_RANGE:
            ang = atan2(self.real_dir.y, self.real_dir.x)
            draw_arc(screen, field, config.SENSOR_COLOR, self.pos, self.visibility_radius,
                                    ang - 0.5 * self.visibility_fov,
                                    ang + 0.5 * self.visibility_fov,
                                    1)
            draw_line(screen, field, config.SENSOR_COLOR,
                      self.pos,
                      self.pos + rotate(self.real_dir * self.visibility_radius,
                                        0.5 * self.visibility_fov),
                      1)
            draw_line(screen, field, config.SENSOR_COLOR,
                      self.pos,
                      self.pos + rotate(self.real_dir * self.visibility_radius,
                                        -0.5 * self.visibility_fov),
                      1)

        try:
            if config.DRAW_VISIBILITY and self.leader_is_visible:
                draw_circle(screen, field, config.config.VISIBILITY_COLOR, self.leader.real.pos,
                            0.5 * config.BOT_RADIUS)
        except AttributeError:
            pass

        try:
            if config.DRAW_REFERENCE_POS:
                draw_circle(screen, field, config.TARGET_POINT_COLOR, self.target_point, 0.2)

            if config.DRAW_APPROX_TRAJECTORY and self.trajectory is not None:
                #if len(self.traj_interval) > 1:
                #    p2 = Point(self.traj_interval[0].pos.x,
                #               self.traj_interval[0].pos.y)
                #    for t, p in self.traj_interval:
                #        draw_line(screen, field, config.APPROX_TRAJECTORY_COLOR, p, p2)
                #        p2 = p

                step = (self.t_fn - self.t_st) / config.TRAJECTORY_SEGMENT_COUNT
                for t in (self.t_st + k * step for k in xrange(config.TRAJECTORY_SEGMENT_COUNT)):
                    p = Point(self.trajectory.x(t),
                              self.trajectory.y(t))
                    p2 = Point(self.trajectory.x(t + step),
                               self.trajectory.y(t + step))
                    draw_line(screen, field, config.APPROX_TRAJECTORY_COLOR, p, p2)

                p_st = Point(self.trajectory.x(self.t_st), self.trajectory.y(self.t_st))
                p_fn = Point(self.trajectory.x(self.t_fn), self.trajectory.y(self.t_fn))

                step = 0.5 / config.TRAJECTORY_SEGMENT_COUNT
                p = p_fn
                p2 = p
                t = self.t_fn
                it = 0
                while it < config.TRAJECTORY_SEGMENT_COUNT and min(dist(p, p_fn), dist(p, p_st)) < 1.0:
                    it += 1
                    t += step
                    p2 = p
                    p = Point(self.trajectory.x(t), self.trajectory.y(t))
                    draw_line(screen, field, config.APPROX_TRAJECTORY_COLOR, p, p2)

        except AttributeError as e: # approximation hasn't been calculated yet
            pass
Exemple #9
0
def main():
    model = load_model('./tmp/1521265089/model-99.h5')
    _, _, _, int_to_label = files_and_labels('./data')

    ring_buffer = RingBuffer(SAMPLE_RATE)

    audio = pyaudio.PyAudio()

    stream = audio.open(format=FORMAT,
                        channels=CHANNELS,
                        rate=SAMPLE_RATE,
                        input=True,
                        frames_per_buffer=CHUNK)

    output = audio.open(format=FORMAT,
                        channels=CHANNELS,
                        rate=SAMPLE_RATE,
                        frames_per_buffer=CHUNK,
                        output=True)

    stream.start_stream()
    output.start_stream()

    silence = chr(0) * CHUNK * CHANNELS * 2

    audio_data = tf.placeholder(tf.float32, [SAMPLE_RATE, 1])
    mfcc = encode_data(audio_data, SAMPLE_RATE)

    while True:
        available = stream.get_read_available()
        if available > 0:
            for _ in range(int(available / CHUNK)):
                data = stream.read(CHUNK)
                ring_buffer.append(data)
                # output.write(data)
        else:
            output.write(silence)

            raw_audio_data = np.array(ring_buffer.get())

            to_audio_data = raw_audio_data.reshape([SAMPLE_RATE, 1])

            feed_dict = {
                audio_data: to_audio_data
            }

            with tf.Session() as sess:
                mfcc_result = sess.run(mfcc, feed_dict)

            x = mfcc_result.reshape(-1, 98, 40, 1)
            predictions = model.predict(x)

            classes = np.argmax(predictions, axis=1)
            for prediction in classes:
              label = int_to_label[prediction]
              print(label)

    stream.stop_stream()
    stream.close()

    audio.terminate()
Exemple #10
0
class RingBufferTests(unittest.TestCase):
    def setUp(self):
        self.capacity = 5
        self.buffer = RingBuffer(self.capacity)

    def test_new_buffer_has_appropriate_capacity(self):
        self.assertEqual(self.buffer.capacity, self.capacity)

    def test_adding_one_element_to_buffer(self):
        self.buffer.append('a')
        self.assertEqual(self.buffer.get(), ['a'])

    def test_filling_buffer_to_capacity(self):
        self.buffer.append('a')
        self.buffer.append('b')
        self.buffer.append('c')
        self.buffer.append('d')
        self.buffer.append('e')
        self.assertEqual(self.buffer.get(), ['a', 'b', 'c', 'd', 'e'])

    def test_adding_one_element_to_full_buffer(self):
        self.buffer.append('a')
        self.buffer.append('b')
        self.buffer.append('c')
        self.buffer.append('d')
        self.buffer.append('e')
        self.buffer.append('f')
        self.assertEqual(self.buffer.get(), ['f', 'b', 'c', 'd', 'e'])
Exemple #11
0
class LaneFitter(object):
    # Define conversions in x and y from pixels space to meters
    ym_per_pix = 30 / 720  # meters per pixel in y dimension
    xm_per_pix = 3.7 / 700  # meters per pixel in x dimension

    def __init__(self, p_matrix, verbose=False):
        self._left = Lane(None, None, None, None)
        self._right: Lane(None, None, None, None)
        self._vebose = verbose
        self._M, self._M_inv = p_matrix
        self._buffer = RingBuffer(5)

    def _compute_average_base(self, bases):
        items = self._buffer.get()
        if len(items) > 0:
            left_base = list(map(lambda x: x[0].base, items))
            right_base = list(map(lambda x: x[1].base, items))

            if bases is not None:
                left_base.append(bases[0])
                right_base.append(bases[1])

            w = np.logspace(0, 1, len(left_base))
            w /= sum(w)

            left_base = int(np.average(left_base, weights=w))
            right_base = int(np.average(right_base, weights=w))

            return left_base, right_base

        return bases

    def _compute_average_fit(self, new_fit):
        items = self._buffer.get()

        left_fit = list(map(lambda x: x[0].fit_params, items))
        right_fit = list(map(lambda x: x[1].fit_params, items))

        if new_fit is not None and new_fit[0] is not None and new_fit[
                1] is not None:
            left_fit.append(new_fit[0])
            right_fit.append(new_fit[1])

        w = np.logspace(0, 1, len(left_fit))
        w /= sum(w)

        left_fit = np.average(np.array(left_fit), axis=0, weights=w)
        right_fit = np.average(np.array(right_fit), axis=0, weights=w)

        return left_fit, right_fit

    def _compute_curvature(self, ploty, fit, fitx) -> Curvature:
        # compute curvature

        # Define y-value where we want radius of curvature
        y_eval = np.max(ploty)

        curve_rad = (
            (1 +
             (2 * fit[0] * y_eval + fit[1])**2)**1.5) / np.absolute(2 * fit[0])

        fit_cr = np.polyfit(ploty * LaneFitter.ym_per_pix,
                            fitx * LaneFitter.xm_per_pix, 2)

        # Calculate the new radii of curvature
        curve_rad_world = (
            (1 +
             (2 * fit_cr[0] * y_eval * LaneFitter.ym_per_pix + fit_cr[1])**2)**
            1.5) / np.absolute(2 * fit_cr[0])

        return Curvature(curve_rad, curve_rad_world)

    def _compute_slope_mse(self, ploty, fit):
        left_fit, right_fit = fit
        left_slope = 2 * ploty * left_fit[0] + left_fit[1]
        right_slope = 2 * ploty * right_fit[0] + right_fit[1]
        mse = np.sum(np.power(left_slope - right_slope, 2)) / len(left_slope)
        return mse

    def fit_polynomial(self, y, x):
        if len(x) > 0 and len(y) > 0:
            return np.polyfit(y, x, 2)

        return None

    def draw_line(self, img, y, x):
        points = np.zeros((len(y), 2), dtype=np.int32)
        points[:, 1] = y.astype(np.int32)
        points[:, 0] = x.astype(np.int32)
        points = points.reshape((-1, 1, 2))

        cv2.polylines(img, points, True, (0, 255, 255), thickness=2)

    def fit(self, image):
        """
        This method find the line pixels on the bird eye view binary image
        :param image:
        :return:
        """
        imshape = image.shape
        out_img = np.dstack([image, image, image]) * 255
        n_windows = 9
        window_height = imshape[0] // n_windows
        margin = 100
        minpix = 50

        hist = np.sum(image[imshape[0] // 2:, :], axis=0)

        midpoint = hist.shape[0] // 2

        current_frame_left_base = np.argmax(hist[:midpoint])
        current_frame_right_base = np.argmax(hist[midpoint:]) + midpoint

        if self._left.fit_params is None or self._right.fit_params is None:
            left_base, right_base = current_frame_left_base, current_frame_right_base

            nonzero = image.nonzero()
            nonzero_y = np.array(nonzero[0])
            nonzero_x = np.array(nonzero[1])

            left_lane_indices, right_lane_indices = [], []
            leftx_current = left_base
            rightx_current = right_base

            for window in range(n_windows):
                window_y_low = imshape[0] - (window + 1) * window_height
                window_y_high = imshape[0] - window * window_height
                window_x_left_low = leftx_current - margin
                window_x_left_high = leftx_current + margin
                window_x_right_low = rightx_current - margin
                window_x_right_high = rightx_current + margin

                left_indices = ((nonzero_y >= window_y_low) &
                                (nonzero_y < window_y_high) &
                                (nonzero_x >= window_x_left_low) &
                                (nonzero_x <= window_x_left_high)).nonzero()[0]
                right_indices = (
                    (nonzero_y >= window_y_low) & (nonzero_y < window_y_high) &
                    (nonzero_x >= window_x_right_low) &
                    (nonzero_x <= window_x_right_high)).nonzero()[0]

                left_lane_indices.append(left_indices)
                right_lane_indices.append(right_indices)

                if len(left_indices) > minpix:
                    leftx_current = np.int(np.mean(nonzero_x[left_indices]))
                if len(right_indices) > minpix:
                    rightx_current = np.int(np.mean(nonzero_x[right_indices]))

            left_lane_indices = np.concatenate(left_lane_indices)
            right_lane_indices = np.concatenate(right_lane_indices)

            leftx = nonzero_x[left_lane_indices]
            lefty = nonzero_y[left_lane_indices]
            rightx = nonzero_x[right_lane_indices]
            righty = nonzero_y[right_lane_indices]
        else:
            left_base, right_base = self._compute_average_base(None)

            nonzero = image.nonzero()
            nonzero_y = nonzero[0]
            nonzero_x = nonzero[1]

            left_fit = self._left.fit_params
            right_fit = self._right.fit_params

            margin = 100
            left_lane_indices = ((nonzero_x > (left_fit[0] * (nonzero_y ** 2) + left_fit[1] * nonzero_y + left_fit[2] - margin)) & \
                (nonzero_x < (left_fit[0] * (nonzero_y ** 2) + left_fit[1] * nonzero_y + left_fit[2] + margin)))
            right_lane_indices = ((nonzero_x >
                                   (right_fit[0] *
                                    (nonzero_y**2) + right_fit[1] * nonzero_y +
                                    right_fit[2] - margin)) &
                                  (nonzero_x <
                                   (right_fit[0] *
                                    (nonzero_y**2) + right_fit[1] * nonzero_y +
                                    right_fit[2] + margin)))

            leftx = nonzero_x[left_lane_indices]
            lefty = nonzero_y[left_lane_indices]
            rightx = nonzero_x[right_lane_indices]
            righty = nonzero_y[right_lane_indices]

        left_fit = self.fit_polynomial(lefty, leftx)
        right_fit = self.fit_polynomial(righty, rightx)

        leftx_current = left_base
        rightx_current = right_base

        for window in range(n_windows):
            window_y_low = imshape[0] - (window + 1) * window_height
            window_y_high = imshape[0] - window * window_height
            window_x_left_low = leftx_current - margin
            window_x_left_high = leftx_current + margin
            window_x_right_low = rightx_current - margin
            window_x_right_high = rightx_current + margin

            cv2.rectangle(out_img, (window_x_left_low, window_y_low),
                          (window_x_left_high, window_y_high), (0, 255, 0), 2)
            cv2.rectangle(out_img, (window_x_right_low, window_y_low),
                          (window_x_right_high, window_y_high), (0, 255, 0), 2)

        ploty = np.linspace(0, image.shape[0] - 1, image.shape[0])

        # do sanity checking, are two lines relatively parallel?
        if left_fit is not None and right_fit is not None:
            slope_diff = self._compute_slope_mse(ploty, (left_fit, right_fit))
            if slope_diff > 0.1:
                print('ignoring fit')
                # ignore this fit
                left_fit, right_fit = None, None

        left_fit, right_fit = self._compute_average_fit((left_fit, right_fit))

        # Generate x and y values for plotting
        left_fitx = left_fit[0] * ploty**2 + left_fit[1] * ploty + left_fit[2]
        right_fitx = right_fit[0] * ploty**2 + right_fit[
            1] * ploty + right_fit[2]

        out_img[nonzero_y[left_lane_indices],
                nonzero_x[left_lane_indices]] = [255, 0, 0]
        out_img[nonzero_y[right_lane_indices],
                nonzero_x[right_lane_indices]] = [0, 0, 255]

        self.draw_line(out_img, ploty, left_fitx)
        self.draw_line(out_img, ploty, right_fitx)

        left_curvature = self._compute_curvature(ploty, left_fit, left_fitx)
        right_curvature = self._compute_curvature(ploty, right_fit, right_fitx)

        offset = LaneFitter.xm_per_pix * (
            (current_frame_left_base +
             (current_frame_right_base - current_frame_left_base) / 2) -
            imshape[1] // 2)

        self._left = Lane(left_base, left_fit, left_fitx, left_curvature)
        self._right = Lane(right_base, right_fit, right_fitx, right_curvature)

        self._buffer.append((self._left, self._right))

        logging.info('Curvature (pixel space): {}'.format(
            (self._left.curvature.pixel, self._right.curvature.pixel)))
        logging.info('Curvature (world space): {}'.format(
            (self._left.curvature.world, self._right.curvature.world)))
        logging.info('Distance from center: {:.2}m'.format(offset))

        return out_img, offset

    def transform(self, image):
        """
        This method transforms the identified lanes back to the original image and draws the lanes on the road
        :param image:
        :return:
        """
        # Create an image to draw the lines on
        imshape = image.shape
        color_warp = np.zeros(imshape, dtype=np.uint8)

        left: Lane = self._left
        right: Lane = self._right

        ploty = np.linspace(0, image.shape[0] - 1, image.shape[0])
        # Recast the x and y points into usable format for cv2.fillPoly()
        pts_left = np.array([np.transpose(np.vstack([left.fit_x, ploty]))])
        pts_right = np.array(
            [np.flipud(np.transpose(np.vstack([right.fit_x, ploty])))])
        pts = np.hstack((pts_left, pts_right))

        # Draw the lane onto the warped blank image
        cv2.fillPoly(color_warp, np.int_([pts]), (0, 255, 0))

        # Warp the blank back to original image space using inverse perspective matrix (Minv)
        new_warp = cv2.warpPerspective(color_warp, self._M_inv,
                                       (imshape[1], imshape[0]))
        # Combine the result with the original image
        result = cv2.addWeighted(image, 1, new_warp, 0.3, 0)

        return result, (left.curvature, right.curvature)

    def fit_transform(self, binary_image, color_image):
        fit_image, offset = self.fit(binary_image)
        final_image, curvatures = self.transform(color_image)
        return final_image, fit_image, curvatures, offset
Exemple #12
0
class DQNAgent:

    def __init__(self, output_size, layers, memory_size=3000, batch_size=32,
                 use_ddqn=False, default_policy=False, model_filename=None, tb_dir="tb_log",
                 epsilon=1, epsilon_lower_bound=0.1, epsilon_decay_function=lambda e: e - (0.9 / 1000000),
                 gamma=0.95, optimizer=RMSprop(0.00025), learn_thresh=50000,
                 update_rate=10000):
        """
        Args:
            output_size: number of actions
            layers: list of Keras layers
            memory_size: size of replay memory
            batch_size: size of batch for replay memory
            use_ddqn: boolean for choosing between DQN/DDQN
            default_policy: boolean for loading a model from a file
            model_filename: name of file to load
            tb_dir: directory for tensorboard logging
            epsilon: annealing function upper bound
            epsilon_lower_bound: annealing function lower bound
            epsilon_decay_function: lambda annealing function 
            gamma: discount factor hyper parameter
            optimizer: Keras optimiser
            learn_thresh: number of steps to perform without learning
            update_rate: number of steps between network-target weights copy
        """
        self.output_size = output_size
        self.memory = RingBuffer(memory_size)
        self.use_ddqn = use_ddqn
        self.default_policy = default_policy
        # Tensorboard parameters
        self.tb_step = 0
        self.tb_gather = 500
        self.tb_dir = tb_dir
        if tb_dir is not None:
            self.tensorboard = TensorBoard(log_dir='./Monitoring/%s' % tb_dir, write_graph=False)
            print("Tensorboard Loaded! (log_dir: %s)" % self.tensorboard.log_dir)
        # Exploration/Exploitation parameters
        self.epsilon = epsilon
        self.epsilon_decay_function = epsilon_decay_function
        self.epsilon_lower_bound = epsilon_lower_bound
        self.total_steps = 0
        # Learning parameters
        self.gamma = gamma
        self.loss = huber_loss #'mean_squared_error'
        self.optimizer = optimizer
        self.batch_size = batch_size
        self.learn_thresh = learn_thresh # Number of steps from which the network starts learning
        self.update_rate = update_rate
        self.discrete_state = False

        if self.default_policy:
            self.evaluate_model = self.load_model(model_filename)
        else:
            self.evaluate_model = self.build_model(layers)

            if self.use_ddqn:
                self.target_model = self.build_model(layers)

    def build_model(self, layers):
        """
        Build a Neural Network.

        Args:
            layers: list of Keras NN layers

        Returns:
            model: compiled model with embedded loss and optimiser
        """
        model = Sequential()
        for l in layers:
            model.add(l)
        model.compile(loss=self.loss, optimizer=self.optimizer)

        return model

    def update_target_model(self):
        """
        Set target net weights to evaluation net weights.
        """
        self.target_model.set_weights(self.evaluate_model.get_weights())

    def replay(self):
        """
        Perform DQN learning phase through experience replay.
        """
        pick = self.random_pick()
        for state, next_action, reward, new_state, end in pick:
        # for state, next_action, reward, frame, end in pick:
            # state = np.float32(state / 255) # for CNN learning
            # frame = np.float32(frame / 255) # for CNN learning
            # new_state = np.append(frame, state[:, :, :, :3], axis=3) # for CNN learning

            # Simple DQN case
            if self.use_ddqn == False:
                if not end:
                    reward = reward + self.gamma * np.amax(self.evaluate_model.predict(new_state)[0])

                new_prediction = self.evaluate_model.predict(state)
                new_prediction[0][next_action] = reward
            else:
                # Double DQN case
                if not end:
                    action = np.argmax(self.evaluate_model.predict(new_state)[0])
                    reward = reward + self.gamma * self.target_model.predict(new_state)[0][action]

                new_prediction = self.target_model.predict(state)
                new_prediction[0][next_action] = reward

            if (self.tb_step % self.tb_gather) == 0 and self.tb_dir is not None:
                self.evaluate_model.fit(state, new_prediction, verbose=0, callbacks=[self.tensorboard])
            else:
                self.evaluate_model.fit(state, new_prediction, verbose=0)
            self.tb_step += 1

    def random_pick(self):
        """
        Pick a random set of elements from replay memory of size self.batch_size.

        Returns:
            set of random elements from memory
        """
        return self.memory.random_pick(self.batch_size)

    def act(self, state, return_prob_dist=False):
        """
        Return the action for current state.

        Args:
            state: current state t
            return_prob_dist: boolean for probability distribution used by ensemblers

        Returns:
            next_action: next action to perform
            prediction: probability distribution
        """
        # Annealing
        if np.random.uniform() > self.epsilon:
            # state = np.float32(state / 255) # for CNN learning
            prediction = self.evaluate_model.predict(state)[0]
            next_action = np.argmax(prediction)
        else:
            prediction = np.random.uniform(0, 1, size=self.output_size)
            next_action = np.argmax(prediction)

        # Start decaying after self.learn_thresh steps
        if self.total_steps > self.learn_thresh:
            self.epsilon = self.epsilon_decay_function(self.epsilon)
            self.epsilon = np.amax([self.epsilon, self.epsilon_lower_bound])

        self.total_steps += 1

        if not return_prob_dist:
            return next_action
        return next_action, prediction

    def memoise(self, t):
        """
        Store tuple to replay memory.

        Args:
            t: element to store
        """
        if not self.default_policy:
            self.memory.append(t)

    def learn(self):
        """
        Perform the learning phase.
        """
        # Start target model update after self.learn_thresh steps
        if (self.total_steps > self.learn_thresh and
            (self.total_steps % self.update_rate) == 0 and not self.default_policy and
            self.use_ddqn == True):
            self.update_target_model()
        # Start learning after self.learn_thresh steps
        if self.total_steps > self.learn_thresh and not self.default_policy and self.total_steps % 4 == 0:   
            self.replay()

    def save_model(self, filename):
        """
        Serialise the model to .h5 file.

        Args:
            filename
        """
        self.evaluate_model.save('%s.h5' % filename)
    
    def load_model(self, filename):
        """
        Load model from .h5 file

        Args:
            filename

        Returns:
            model
        """
        return load_model('%s.h5' % filename, custom_objects={ 'huber_loss': huber_loss })
Exemple #13
0
from ring_buffer import RingBuffer

buffer = RingBuffer(3)

print(buffer.get())  # should return []

buffer.append('a')
buffer.append('b')
buffer.append('c')

print(buffer.get())  # should return ['a', 'b', 'c']

# 'd' overwrites the oldest value in the ring buffer, which is 'a'
buffer.append('d')

print(buffer.get())  # should return ['d', 'b', 'c']

buffer.append('e')
buffer.append('f')

print(buffer.get())  # should return ['d', 'e', 'f']
Exemple #14
0
class JupyterProgress(base.ProgressBase):
    def __init__(self, recent_acceptance_lag=None, verbosity=1):
        self.start_time = None
        self.last_update_time = None
        self.verbosity = verbosity
        self.recent_acceptance_lag = recent_acceptance_lag
        self.last_update_iteration = 0
        self.n_iter = None
        self.__total_errors__ = 0

        self.__iter_time_buffer__ = RingBuffer(100)

        self.__text_field__ = ipywidgets.HTML()
        self.__error_field__ = None
        self.__error_label__ = None

    def initialise(self, n_iter):
        self.start_time = self.last_update_time = time.time()
        self.n_iter = n_iter

        if self.recent_acceptance_lag is None:
            self.recent_acceptance_lag = min(int(n_iter * 1.0 / 100), max_accept_lag)

        self.__text_field__.value = "<i>Waiting for {} iterations to have passed...</i>".format(
            self.recent_acceptance_lag
        )
        # display(self.__progress_field__)
        display(self.__text_field__)

    def report_error(self, iter, error):
        if self.__error_field__ is None:
            self.__initialise_errors__()
        if self.verbosity > 0:
            self.__error_field__.value += "Iteration {}: {}\n".format(iter, error)
        self.__total_errors__ += 1

    def update(self, iteration, acceptances):
        if self.n_iter is None:
            raise Exception("First pass in the number of iterations!")

        recent_acceptance_lag = self.recent_acceptance_lag

        now = time.time()
        toc = now - self.last_update_time
        do_update = toc > update_frequency_seconds and iteration > self.last_update_iteration

        if do_update or iteration == self.n_iter:

            delta_accept = (
                acceptances[-recent_acceptance_lag:].mean() * 100 if iteration > recent_acceptance_lag else np.nan
            )
            tot_accept = acceptances.mean() * 100

            new_iterations = iteration - self.last_update_iteration
            time_per_iter = toc * 1.0 / new_iterations
            self.__iter_time_buffer__.append(time_per_iter)

            # exclude outliers
            all_iter_times = np.array(self.__iter_time_buffer__.data)
            if len(all_iter_times) > 1:
                all_iter_times = all_iter_times[
                    abs(all_iter_times - np.mean(all_iter_times)) < 2 * np.std(all_iter_times)
                ]
                time_per_iter = all_iter_times.mean()

            eta = (self.n_iter - iteration) * time_per_iter

            html = self.__get_text_field__(iteration, self.n_iter, delta_accept, tot_accept, time_per_iter, eta)
            self.__text_field__.value = html

            self.last_update_time = now
            self.last_update_iteration = iteration

    def __initialise_errors__(self):
        if self.verbosity > 0:
            self.__error_label__ = ipywidgets.HTML(value="<span style='color: red'>Errors:</span>")
            self.__error_field__ = ipywidgets.Textarea(disabled=True)
            display(self.__error_label__)
            display(self.__error_field__)

    def __get_text_field__(self, iteration, n_iter, delta_accept, total_accept, time_per_iter, eta):
        template = """
                <div class="progress">
                  <div class="progress-bar" role="progressbar" aria-valuenow="{}"
                  aria-valuemin="0" aria-valuemax="{}" style="width:{:.2f}%">
                    <span class="sr-only">{:.2f}% Complete</span>
                  </div>
                </div>
                <table class="table">
                        <tr>
                                <td>Current iteration</td>
                                <td>{}</td>
                        </tr>
                        <tr>
                                <td>Accept rate (last {})</td>
                                <td>{:.2f}%</td>
                        </tr>
                        <tr>
                                <td>Accept rate (overall)</td>
                                <td>{:.2f}%</td>
                        </tr>
                        <tr>
                                <td>T/iter</td>
                                <td>{:.4f} seconds</td>
                        </tr>
                        <tr>
                                <td>ETA</td>
                                <td>{}</td>
                        </tr>
                        <tr>
                                <td>Errors</td>
                                <td>{}</td>
                        </tr>
                </table>
        """
        return template.format(
            iteration,
            n_iter,
            iteration * 100.0 / n_iter,
            iteration * 100.0 / n_iter,
            iteration,
            self.recent_acceptance_lag,
            delta_accept,
            total_accept,
            time_per_iter,
            pretty_time_delta(eta),
            self.__total_errors__,
        )
Exemple #15
0
def experiment(n_episodes, default_policy=False, policy=None, render=False):
    """
    Run a RL experiment that can be either training or testing

    Args:
        n_episodes: number of train/test episodes
        default_policy: boolean to enable testing/training phase
        policy: numpy tensor with a trained policy
        render: enable OpenAI environment graphical rendering

    Returns:
        Dictionary with:
            cumulative experiments outcomes
            list of steps per episode
            list of cumulative rewards
            trained policy
    """

    with tf.device('/gpu:0'):
        res = [0, 0]  # array of results accumulator: {[0]: Loss, [1]: Victory}
        scores = []  # Cumulative rewards
        steps = []  # Steps per episode

        reward_list = RingBuffer(100)
        env = gym.make('PongDeterministic-v4')

        input_dim = env.observation_space.shape[0]
        output_dim = env.action_space.n

        if default_policy:
            agent = DQNAgent(output_dim,
                             None,
                             use_ddqn=True,
                             default_policy=True,
                             model_filename=policy,
                             epsilon=0.05,
                             epsilon_lower_bound=0.05)
        else:
            layers = [
                Conv2D(32, (8, 8),
                       strides=(4, 4),
                       activation='relu',
                       input_shape=(84, 84, 4),
                       kernel_initializer=VarianceScaling(scale=2.0)),
                Conv2D(64, (4, 4),
                       strides=(2, 2),
                       activation='relu',
                       kernel_initializer=VarianceScaling(scale=2.0)),
                Conv2D(64, (3, 3),
                       strides=(1, 1),
                       activation='relu',
                       kernel_initializer=VarianceScaling(scale=2.0)),
                Flatten(),
                Dense(512, activation='relu'),
                Dense(output_dim)
            ]
            agent = DQNAgent(output_dim,
                             layers,
                             use_ddqn=True,
                             memory_size=700000,
                             gamma=0.99,
                             learn_thresh=50000,
                             epsilon_lower_bound=0.02,
                             epsilon_decay_function=lambda e: e -
                             (0.98 / 950000),
                             update_rate=10000,
                             optimizer=Adam(0.00025))

        gathered_frame = 0
        for episode_number in tqdm(range(n_episodes), desc="Episode"):
            frame = env.reset()
            state = pre_processing(frame)
            empty_state = np.zeros(state.shape, dtype="uint8")
            cumulative_reward = 0

            has_lost_life = True

            t = 0
            while True:
                if has_lost_life:
                    next_action = 1  # [1, 4, 5][ran.randint(0, 2)]

                    stack = np.stack(
                        (empty_state, empty_state, empty_state, empty_state),
                        axis=2)
                    stack = np.reshape([stack], (1, 84, 84, 4))

                    for _ in range(ran.randint(1, 10)):
                        gathered_frame += 1
                        frame, reward, end, _ = env.step(next_action)
                        new_state = np.reshape(pre_processing(frame),
                                               (1, 84, 84, 1))
                        new_stack = np.append(new_state,
                                              stack[:, :, :, :3],
                                              axis=3)
                        stack = new_stack

                        if (render):
                            env.render()

                    has_lost_life = False

                next_action = agent.act(stack)
                new_state, reward, end, _ = env.step(next_action)

                if (render):
                    env.render()
                    time.sleep(0.02)

                reward = np.clip(reward, -1., 1.)

                if reward != 0:
                    has_lost_life = True

                cumulative_reward += reward

                new_state = np.reshape(pre_processing(new_state),
                                       (1, 84, 84, 1))
                new_stack = np.append(new_state, stack[:, :, :, :3], axis=3)
                agent.memoise(
                    (stack, next_action, reward, new_state, has_lost_life))

                stack = new_stack
                gathered_frame += 1

                if end:
                    reward_list.append(cumulative_reward)
                    if cumulative_reward > 0:
                        res[1] += 1
                        print("You Won!, steps:", t, "reward:",
                              reward_list.mean(), "frames:", gathered_frame)
                    else:
                        res[0] += 1
                        print("You Lost!, steps:", t, "reward:",
                              reward_list.mean(), "frames:", gathered_frame)
                    steps.append(t)
                    break

                agent.learn()
                t += 1

            scores.append(cumulative_reward)
            if episode_number >= 50 and episode_number % 10 == 0:
                model_name = "partial_model_pong" + str(episode_number)
                agent.save_model(model_name)

        env.close()
        return {
            "results": np.array(res),
            "steps": np.array(steps),
            "scores": np.array(scores),
            "agent": agent
        }
class JupyterProgress(base.ProgressBase):
    def __init__(self, recent_acceptance_lag=None, verbosity=1):
        self.start_time = None
        self.last_update_time = None
        self.verbosity = verbosity
        self.recent_acceptance_lag = recent_acceptance_lag
        self.last_update_iteration = 0
        self.n_iter = None
        self.__total_errors__ = 0

        self.__iter_time_buffer__ = RingBuffer(100)

        self.__text_field__ = ipywidgets.HTML()
        self.__error_field__ = None
        self.__error_label__ = None

    def initialise(self, n_iter):
        self.start_time = self.last_update_time = time.time()
        self.n_iter = n_iter

        if self.recent_acceptance_lag is None:
            self.recent_acceptance_lag = min(int(n_iter * 1. / 100),
                                             max_accept_lag)

        self.__text_field__.value = '<i>Waiting for {} iterations to have passed...</i>'.format(
            self.recent_acceptance_lag)
        #display(self.__progress_field__)
        display(self.__text_field__)

    def report_error(self, iter, error):
        if self.__error_field__ is None:
            self.__initialise_errors__()
        if self.verbosity > 0:
            self.__error_field__.value += 'Iteration {}: {}\n'.format(
                iter, error)
        self.__total_errors__ += 1

    def update(self, iteration, acceptances):
        if self.n_iter is None:
            raise Exception('First pass in the number of iterations!')

        recent_acceptance_lag = self.recent_acceptance_lag

        now = time.time()
        toc = now - self.last_update_time
        do_update = toc > update_frequency_seconds and iteration > self.last_update_iteration

        if do_update or iteration == self.n_iter:

            delta_accept = acceptances[-recent_acceptance_lag:].mean(
            ) * 100 if iteration > recent_acceptance_lag else np.nan
            tot_accept = acceptances.mean() * 100

            new_iterations = iteration - self.last_update_iteration
            time_per_iter = toc * 1. / new_iterations
            self.__iter_time_buffer__.append(time_per_iter)

            # exclude outliers
            all_iter_times = np.array(self.__iter_time_buffer__.data)
            if len(all_iter_times) > 1:
                all_iter_times = all_iter_times[
                    abs(all_iter_times -
                        np.mean(all_iter_times)) < 2 * np.std(all_iter_times)]
                time_per_iter = all_iter_times.mean()

            eta = (self.n_iter - iteration) * time_per_iter

            html = self.__get_text_field__(iteration, self.n_iter,
                                           delta_accept, tot_accept,
                                           time_per_iter, eta)
            self.__text_field__.value = html

            self.last_update_time = now
            self.last_update_iteration = iteration

    def __initialise_errors__(self):
        if self.verbosity > 0:
            self.__error_label__ = ipywidgets.HTML(
                value="<span style='color: red'>Errors:</span>")
            self.__error_field__ = ipywidgets.Textarea(disabled=True)
            display(self.__error_label__)
            display(self.__error_field__)

    def __get_text_field__(self, iteration, n_iter, delta_accept, total_accept,
                           time_per_iter, eta):
        template = """
                <div class="progress">
                  <div class="progress-bar" role="progressbar" aria-valuenow="{}"
                  aria-valuemin="0" aria-valuemax="{}" style="width:{:.2f}%">
                    <span class="sr-only">{:.2f}% Complete</span>
                  </div>
                </div>
                <table class="table">
                        <tr>
                                <td>Current iteration</td>
                                <td>{}</td>
                        </tr>
                        <tr>
                                <td>Accept rate (last {})</td>
                                <td>{:.2f}%</td>
                        </tr>
                        <tr>
                                <td>Accept rate (overall)</td>
                                <td>{:.2f}%</td>
                        </tr>
                        <tr>
                                <td>T/iter</td>
                                <td>{:.4f} seconds</td>
                        </tr>
                        <tr>
                                <td>ETA</td>
                                <td>{}</td>
                        </tr>
                        <tr>
                                <td>Errors</td>
                                <td>{}</td>
                        </tr>
                </table>
        """
        return template.format(iteration, n_iter, iteration * 100. / n_iter,
                               iteration * 100. / n_iter, iteration,
                               self.recent_acceptance_lag, delta_accept,
                               total_accept, time_per_iter,
                               pretty_time_delta(eta), self.__total_errors__)
class AMGNG:

    def __init__(self, comparison_function, buffer_len, dimensions,
                 prest_gamma, prest_lmbda, prest_theta,
                 pst_gamma, pst_lmbda, pst_theta,
                 prest_alpha=0.5, prest_beta=0.5, prest_delta=0.5,
                 prest_eta=0.9995, prest_e_w=0.05, prest_e_n=0.0006,
                 pst_alpha=0.5, pst_beta=0.75, pst_delta=0.5,
                 pst_eta=0.9995, pst_e_w=0.05, pst_e_n=0.0006,
                 ma_window_len=None, ma_recalc_delay=1, ddof=1):

        values = inspect.getargvalues(inspect.currentframe())[3]
        print('Init parameters: {}'.format(values))
        self.comparison_function = comparison_function
        self.buffer_len = buffer_len
        self.dimensions = dimensions
        self.present = mgng.MGNG(dimensions=dimensions,
                                 gamma=int(prest_gamma),
                                 lmbda=int(prest_lmbda),
                                 theta=int(prest_theta),
                                 alpha=float(prest_alpha),
                                 beta=float(prest_beta),
                                 delta=float(prest_delta),
                                 eta=float(prest_eta),
                                 e_w=float(prest_e_w),
                                 e_n=float(prest_e_n))
        self.past = mgng.MGNG(dimensions=dimensions,
                              gamma=int(pst_gamma),
                              lmbda=int(pst_lmbda),
                              theta=int(pst_theta),
                              alpha=float(pst_alpha),
                              beta=float(pst_beta),
                              delta=float(pst_delta),
                              eta=float(pst_eta),
                              e_w=float(pst_e_w),
                              e_n=float(pst_e_n))
        # self.buffer = deque(maxlen=self.buffer_len)
        self.buffer = RingBuffer([[np.nan]*dimensions]*buffer_len)
        if ma_window_len is None:
            # self.ma_window = deque(maxlen=self.buffer_len)
            self.ma_window = RingBuffer([np.nan]*buffer_len)
        else:
            # self.ma_window = deque(maxlen=ma_window_len)
            self.ma_window = RingBuffer([np.nan]*ma_window_len)
        self.ma_recalc_delay = ma_recalc_delay
        self.ddof = ddof
        self.anomaly_mean = None
        self.anomaly_std = None
        self.t = 0

    def time_step(self, xt):
        xt = np.reshape(xt, newshape=self.dimensions)
        ret_val = 0.
        self.buffer.append(xt)
        self.present.time_step(xt)
        if self.t >= self.buffer_len:
            pst_xt = self.buffer[0]
            self.past.time_step(pst_xt)
            if self.t >= self.present.theta + self.past.theta:
                ret_val = self.comparison_function(self.present, self.past,
                                                   self.present.alpha)
        self.ma_window.append(ret_val)
        if self.t % self.ma_recalc_delay == 0:
            self.anomaly_mean = bn.nanmean(self.ma_window)
            self.anomaly_std = bn.nanstd(self.ma_window, ddof=self.ddof)
        if self.anomaly_std is None or self.t < len(self.ma_window):
            anomaly_density = 0
        else:
            normalized_score = (ret_val - self.anomaly_mean)/self.anomaly_std
            if -4 <= normalized_score <= 4:
                anomaly_density = CDF_TABLE[round(normalized_score, 3)]
            elif normalized_score > 4:
                anomaly_density = 1.
            else:
                anomaly_density = 0.
        self.t += 1
        return ret_val, anomaly_density
class RingBufferTests(unittest.TestCase):
    def setUp(self):
        self.buffer = RingBuffer(5)

    def test_ring_buffer(self):
        self.assertEqual(len(self.buffer.storage), 5)

        self.buffer.append('a')
        self.buffer.append('b')
        self.buffer.append('c')
        self.buffer.append('d')
        print('storage:', self.buffer.storage)
        self.assertEqual(len(self.buffer.storage), 5)
        print('buffer get', self.buffer.get())
        self.assertEqual(self.buffer.get(), ['a', 'b', 'c', 'd'])

        self.buffer.append('e')
        self.assertEqual(len(self.buffer.storage), 5)
        self.assertEqual(self.buffer.get(), ['a', 'b', 'c', 'd', 'e'])

        self.buffer.append('f')
        self.assertEqual(len(self.buffer.storage), 5)
        self.assertEqual(self.buffer.get(), ['f', 'b', 'c', 'd', 'e'])

        self.buffer.append('g')
        self.buffer.append('h')
        self.buffer.append('i')
        self.assertEqual(len(self.buffer.storage), 5)
        self.assertEqual(self.buffer.get(), ['f', 'g', 'h', 'i', 'e'])
Exemple #19
0
class SoundEffectEngine(EffectEngine):
    def __init__(self, queue):
        super().__init__(queue)
        self._name = "SoundEffectEngine"
        self._chunk = 1024
        self._player = pyaudio.PyAudio()
        self._wav_dir = config.SOUNDFILE_DIR
        self._wav_files = {}
        self._cur_wav_file = None
        self._stream = None
        self._dataFile = open("history.csv", "w")
        self._dataWriter = csv.writer(self._dataFile,
                                      delimiter=",",
                                      quotechar='"',
                                      quoting=csv.QUOTE_MINIMAL)
        self._currentBpms = RingBuffer(config.AVG_WINDOW)
        self._heartbeat = config.HEARTBEAT_TIMEOUT

        if config.BPM_RANGE_LOW >= config.BPM_RANGE_HIGH:
            raise ValueError(
                "BPM Ranges are not configured correctly in config")

    def read_wav_files(self):
        self._wav_files = [
            join(self._wav_dir, f) for f in listdir(self._wav_dir)
            if isfile(join(self._wav_dir, f)) and ".wav" in f
        ]
        self._wav_files = natsorted(self._wav_files, key=lambda y: y.lower())
        if len(self._wav_files) < 1:
            raise FileNotFoundError("No wav files found in given directory.")

    def run(self):
        print("Starting " + self._name)
        self.read_wav_files()
        self.effect_loop()

    def shutdown_audio(self):
        self._cur_wav_file.close()
        self._stream.stop_stream()
        try:
            self._player.close(self._stream)
        except ValueError:
            pass

    def idle(self):
        print(self._name + " idling..")
        self.shutdown_audio()
        self._currentBpms.reset()
        while not self._stop_event.is_set():
            try:
                bpm = self._queue.get(timeout=2)
                if bpm > 0:
                    self._currentBpms.append(bpm)
                self.open_wav_file()
                self._heartbeat = config.HEARTBEAT_TIMEOUT
                break
            except queue.Empty:
                pass

    def poll_bpm(self):
        try:
            bpm = self._queue.get_nowait()
            self._heartbeat = config.HEARTBEAT_TIMEOUT
            if bpm > 0:
                self._currentBpms.append(bpm)
                self._bpmHistory.append(bpm)
                # write into csv
                self._dataWriter.writerow([bpm])
                self._dataFile.flush()
            # save data history

        except queue.Empty:
            self._heartbeat = self._heartbeat - 1

    def open_wav_file(self):
        index = self.choose_wav_file()
        self._cur_wav_file = wave.open(self._wav_files[index], "rb")
        self._stream = self._player.open(
            format=self._player.get_format_from_width(
                self._cur_wav_file.getsampwidth()),
            channels=self._cur_wav_file.getnchannels(),
            rate=self._cur_wav_file.getframerate(),
            output=True)

    def choose_wav_file(self):
        if self._currentBpms.get_len() is 0:
            return 0

        if config.SOUND_MODE is config.SoundMode.AVERAGE:
            bpm = self._currentBpms.get_sum() / self._currentBpms.get_len()

            limited_bpm = config.BPM_RANGE_LOW if bpm < config.BPM_RANGE_LOW else \
                config.BPM_RANGE_HIGH if bpm > config.BPM_RANGE_HIGH else bpm
            total_range = config.BPM_RANGE_HIGH - config.BPM_RANGE_LOW
            index = math.floor((limited_bpm - config.BPM_RANGE_LOW) /
                               math.ceil(total_range / len(self._wav_files)))

        elif config.SOUND_MODE is config.SoundMode.DIFFERENCE:
            diff = max(self._currentBpms.get()) - min(self._currentBpms.get())
            if diff > len(self._wav_files) - 1:
                diff = len(self._wav_files) - 1
            index = diff

        print(index)
        return index

    def effect_loop(self):

        self.poll_bpm()
        self.open_wav_file()

        try:
            while True:

                if self._stop_event.is_set():
                    self.shutdown_audio()
                    return

                data = self._cur_wav_file.readframes(self._chunk)
                while data != b'':
                    self._stream.write(data)
                    data = self._cur_wav_file.readframes(self._chunk)

                self.poll_bpm()
                if self._heartbeat is 0:
                    self.idle()
                else:
                    self.shutdown_audio()
                    self.open_wav_file()

                time.sleep(0.1)
        except KeyboardInterrupt:
            print("Got interrupt, crunching Data...")
Exemple #20
0
class RingBufferTests(unittest.TestCase):
    def setUp(self):
        self.buffer = RingBuffer(5)
        self.buffer_2 = RingBuffer(5)

    def test_ring_buffer(self):
        self.assertEqual(self.buffer.storage.length, 0)

        self.buffer.append('a')
        self.buffer.append('b')
        self.buffer.append('c')
        self.buffer.append('d')
        self.assertEqual(self.buffer.storage.length, 4)
        self.assertEqual(self.buffer.get(), ['a', 'b', 'c', 'd'])

        self.buffer.append('e')
        self.assertEqual(self.buffer.storage.length, 5)
        self.assertEqual(self.buffer.get(), ['a', 'b', 'c', 'd', 'e'])

        self.buffer.append('f')
        self.assertEqual(self.buffer.storage.length, 5)
        self.assertEqual(self.buffer.get(), ['f', 'b', 'c', 'd', 'e'])

        self.buffer.append('g')
        self.buffer.append('h')
        self.buffer.append('i')
        self.assertEqual(self.buffer.storage.length, 5)
        self.assertEqual(self.buffer.get(), ['f', 'g', 'h', 'i', 'e'])

        self.buffer.append('j')
        self.buffer.append('k')
        self.assertEqual(self.buffer.get(), ['k', 'g', 'h', 'i', 'j'])

        for i in range(50):
            self.buffer_2.append(i)
        self.assertEqual(self.buffer_2.get(), [45, 46, 47, 48, 49])
Exemple #21
0
class LineTracer():
    def __init__(self):
        self.last5_left = RingBuffer(5)
        self.last5_right = RingBuffer(5)
        self.avg_left_fit = 0
        self.avg_right_fit = 0
        self.avg_left_curv = 0 
        self.avg_right_curv = 0 
        self.avg_left_dist = 0
        self.avg_right_dist = 0

    def getLast(self):
        if len(self.last5_left.get()) > 0 and len(self.last5_right.get()) > 0:
            return [self.last5_left.get()[-1], self.last5_right.get()[-1]]
        else:
            return generate_empty_lines()

    def getAvg(self):
        left_list = self.last5_left.get()
        right_list = self.last5_right.get()
        num_lines = len(left_list)
        #print(num_lines)
        ploty = self.last5_left.get()[-1].fity

        sum_curvl = 0
        sum_curvr = 0
        sum_distl = 0
        sum_distr = 0
        sum_fitl = [0, 0, 0]
        sum_fitr = [0, 0, 0]

        if num_lines > 0:
            for left in left_list:
                sum_curvl += left.curvature
                sum_distl += left.dist2line
                sum_fitl = np.add(sum_fitl, left.fit)
            for right in right_list:
                sum_curvr += right.curvature
                sum_distr += right.dist2line
                sum_fitr = np.add(sum_fitr, right.fit)

        curvl = sum_curvl / num_lines
        curvr = sum_curvr / num_lines
        distl = sum_distl / num_lines
        distr = sum_distr / num_lines
        left_fit = np.divide(sum_fitl, num_lines)
        right_fit = np.divide(sum_fitr, num_lines)
        #print(left_fit)

        # Draw the fit line plain
        left_fitx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2]
        right_fitx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]

        left_fitx = left_fitx.astype(int)
        right_fitx = right_fitx.astype(int)

        leftl = SingleLine(True,left_fit, left_fitx,ploty,curvl,distl,\
            self.last5_left.get()[-1].allx, self.last5_left.get()[-1].ally)
        rightl = SingleLine(True,right_fit, right_fitx,ploty,curvr,distr,\
            self.last5_right.get()[-1].allx, self.last5_right.get()[-1].ally)

        return leftl, rightl

    def newLine(self, new_left, new_right):
        if new_left.detected == False or new_right.detected == False:
            #self.last5_left.append([False])
            #self.last5_right.append([False])
            pass
        else:
            self.last5_left.append(new_left)
            self.last5_right.append(new_right)

        left_list = self.last5_left.get()
        right_list = self.last5_right.get()
        num_lines = len(left_list)

        if num_lines > 0:
            for left in left_list:
                #self.avg_left_fit = np.add(self.avg_left_fit, left.fit)
                self.avg_left_curv += left.curvature
                self.avg_left_dist += left.dist2line
            for right in right_list:
                #self.avg_right_fit = np.add(self.avg_right_fit, right.fit)
                self.avg_right_curv += right.curvature
                self.avg_right_dist += right.dist2line
            
            #self.avg_left_fit = np.divide(self.avg_left_fit, num_lines)
            #self.avg_right_fit = np.divide(self.avg_right_fit, num_lines)
            self.avg_left_curv /= num_lines
            self.avg_right_curv /= num_lines
            self.avg_left_dist /= num_lines
            self.avg_right_dist /= num_lines