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)
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'])
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
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])
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)
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
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()
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'])
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
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 })
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']
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__,
)
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'])
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...")
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])
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