def get_moving_avg(vehicle_name, show=False):
path = 'Vehicles/' + vehicle_name + "/training_rewards.csv"
smoothpath = 'Vehicles/' + vehicle_name + f"/TrainingData.csv"
rewards = []
with open(path, 'r') as csvfile:
csvFile = csv.reader(csvfile, quoting=csv.QUOTE_NONNUMERIC)
for lines in csvFile:
rewards.append(lines)
rewards = np.array(rewards)[:, 1]
smooth_rewards = lib.get_moving_average(50, rewards)
new_rewards = []
l = 10
N = int(len(smooth_rewards) / l)
for i in range(N):
avg = np.mean(smooth_rewards[i*l:(i+1)*l])
new_rewards.append(avg)
smooth_rewards = np.array(new_rewards)
save_csv_data(smooth_rewards, smoothpath)
if show:
lib.plot_no_avg(rewards, figure_n=1)
lib.plot_no_avg(smooth_rewards, figure_n=2)
plt.show()
def print_update(self):
mean = np.mean(self.rewards)
score = self.rewards[-1]
print(
f"Run: {self.t_counter} --> Score: {score:.2f} --> Mean: {mean:.2f} --> "
)
lib.plot(self.rewards, figure_n=2)
def convert_pts_s_th(pts):
N = len(pts)
s_i = np.zeros(N - 1)
th_i = np.zeros(N - 1)
for i in range(N - 1):
s_i[i] = lib.get_distance(pts[i], pts[i + 1])
th_i[i] = lib.get_bearing(pts[i], pts[i + 1])
return s_i, th_i
def load_center_pts(self):
track_data = []
filename = 'maps/' + self.map_name + '_std.csv'
try:
with open(filename, 'r') as csvfile:
csvFile = csv.reader(csvfile, quoting=csv.QUOTE_NONNUMERIC)
for lines in csvFile:
track_data.append(lines)
except FileNotFoundError:
raise FileNotFoundError("No map file center pts")
track = np.array(track_data)
print(f"Track Loaded: {filename} in reward")
N = len(track)
self.wpts = track[:, 0:2]
ss = np.array([
lib.get_distance(self.wpts[i], self.wpts[i + 1])
for i in range(N - 1)
])
ss = np.cumsum(ss)
self.ss = np.insert(ss, 0, 0)
self.total_s = self.ss[-1]
self.diffs = self.wpts[1:, :] - self.wpts[:-1, :]
self.l2s = self.diffs[:, 0]**2 + self.diffs[:, 1]**2
def __call__(self, s, a, s_p, r, dev):
if r == -1:
return r
else:
pt_i, pt_ii, d_i, d_ii = find_closest_pt(s_p[0:2], self.wpts)
d = lib.get_distance(pt_i, pt_ii)
d_c = get_tiangle_h(d_i, d_ii, d) / self.dis_scale
th_ref = lib.get_bearing(pt_i, pt_ii)
th = s_p[2]
d_th = abs(lib.sub_angles_complex(th_ref, th))
v_scale = s_p[3] / self.max_v
new_r = self.mh * np.cos(d_th) * v_scale - self.md * d_c
return new_r + r
def min_render(self, wait=False):
fig = plt.figure(4)
plt.clf()
ret_map = self.env_map.scan_map
plt.imshow(ret_map)
# plt.xlim([0, self.env_map.width])
# plt.ylim([0, self.env_map.height])
s_x, s_y = self.env_map.convert_to_plot(self.env_map.start)
plt.plot(s_x, s_y, '*', markersize=12)
c_x, c_y = self.env_map.convert_to_plot([self.car.x, self.car.y])
plt.plot(c_x, c_y, '+', markersize=16)
for i in range(self.scan_sim.number_of_beams):
angle = i * self.scan_sim.dth + self.car.theta - self.scan_sim.fov / 2
fs = self.scan_sim.scan_output[
i] * self.scan_sim.n_searches * self.scan_sim.step_size
dx = [np.sin(angle) * fs, np.cos(angle) * fs]
range_val = lib.add_locations([self.car.x, self.car.y], dx)
r_x, r_y = self.env_map.convert_to_plot(range_val)
x = [c_x, r_x]
y = [c_y, r_y]
plt.plot(x, y)
for pos in self.action_memory:
p_x, p_y = self.env_map.convert_to_plot(pos)
plt.plot(p_x, p_y, 'x', markersize=6)
text_start = self.env_map.width + 10
spacing = int(self.env_map.height / 10)
s = f"Reward: [{self.reward:.1f}]"
plt.text(text_start, spacing * 1, s)
s = f"Action: [{self.action[0]:.2f}, {self.action[1]:.2f}]"
plt.text(text_start, spacing * 2, s)
s = f"Done: {self.done}"
plt.text(text_start, spacing * 3, s)
s = f"Pos: [{self.car.x:.2f}, {self.car.y:.2f}]"
plt.text(text_start, spacing * 4, s)
s = f"Vel: [{self.car.velocity:.2f}]"
plt.text(text_start, spacing * 5, s)
s = f"Theta: [{(self.car.theta * 180 / np.pi):.2f}]"
plt.text(text_start, spacing * 6, s)
s = f"Delta x100: [{(self.car.steering*100):.2f}]"
plt.text(text_start, spacing * 7, s)
s = f"Theta Dot: [{(self.car.th_dot):.2f}]"
plt.text(text_start, spacing * 8, s)
s = f"Steps: {self.steps}"
plt.text(100, spacing * 9, s)
plt.pause(0.0001)
if wait:
plt.show()
def trace_ray(self, x, y, theta, noise=True):
# obs_res = 10
for j in range(self.n_searches): # number of search points
fs = self.step_size * (j + 1
) # search from 1 step away from the point
dx = [np.sin(theta) * fs, np.cos(theta) * fs]
search_val = lib.add_locations([x, y], dx)
if self._check_location(search_val):
break
ray = (
j) / self.n_searches #* (1 + np.random.normal(0, self.std_noise))
return ray
def update_kinematic_state(self, a, d_dot, dt):
self.x = self.x + self.velocity * np.sin(self.theta) * dt
self.y = self.y + self.velocity * np.cos(self.theta) * dt
theta_dot = self.velocity / self.wheelbase * np.tan(self.steering)
self.th_dot = theta_dot
dth = theta_dot * dt
self.theta = lib.add_angles_complex(self.theta, dth)
a = np.clip(a, -self.max_a, self.max_a)
d_dot = np.clip(d_dot, -self.max_d_dot, self.max_d_dot)
self.steering = self.steering + d_dot * dt
self.velocity = self.velocity + a * dt
self.steering = np.clip(self.steering, -self.max_steer, self.max_steer)
self.velocity = np.clip(self.velocity, -self.max_v, self.max_v)
def find_true_widths(pts, nvecs, ws, check_scan_location):
tx = pts[:, 0]
ty = pts[:, 1]
onws = ws[:, 0]
opws = ws[:, 1]
stp_sze = 0.1
sf = 0.5 # safety factor
N = len(pts)
nws, pws = [], []
for i in range(N):
pt = [tx[i], ty[i]]
nvec = nvecs[i]
if not check_scan_location(pt):
j = stp_sze
s_pt = lib.add_locations(pt, nvec, j)
while not check_scan_location(s_pt) and j < opws[i]:
j += stp_sze
s_pt = lib.add_locations(pt, nvec, j)
pws.append(j * sf)
j = stp_sze
s_pt = lib.sub_locations(pt, nvec, j)
while not check_scan_location(s_pt) and j < onws[i]:
j += stp_sze
s_pt = lib.sub_locations(pt, nvec, j)
nws.append(j * sf)
else:
print(f"Obs in way of pt: {i}")
for j in np.linspace(0, onws[i], 10):
p_pt = lib.add_locations(pt, nvec, j)
n_pt = lib.sub_locations(pt, nvec, j)
if not check_scan_location(p_pt):
nws.append(-j * (1 + sf))
pws.append(opws[i])
print(f"PosPt NewW: [{-j*(1+sf)}, {opws[i]}]")
break
elif not check_scan_location(n_pt):
pws.append(-j * (1 + sf))
nws.append(onws[i])
print(f"PosPt NewW: [{-j*(1+sf)}, {onws[i]}]")
break
if j == onws[i]:
print(f"Problem - no space found")
nws, pws = np.array(nws), np.array(pws)
ws = np.concatenate([nws[:, None], pws[:, None]], axis=-1)
return ws
def find_closest_pt(pt, wpts):
"""
Returns the two closes points in order along wpts
"""
dists = [lib.get_distance(pt, wpt) for wpt in wpts]
min_i = np.argmin(dists)
d_i = dists[min_i]
if min_i == len(dists) - 1:
min_i -= 1
if dists[max(min_i - 1, 0)] > dists[min_i + 1]:
p_i = wpts[min_i]
p_ii = wpts[min_i + 1]
d_i = dists[min_i]
d_ii = dists[min_i + 1]
else:
p_i = wpts[min_i - 1]
p_ii = wpts[min_i]
d_i = dists[min_i - 1]
d_ii = dists[min_i]
return p_i, p_ii, d_i, d_ii
def check_done_reward_track_train(self):
self.reward = 0 # normal
if self.env_map.check_scan_location([self.car.x, self.car.y]):
self.done = True
self.reward = -1
self.done_reason = f"Crash obstacle: [{self.car.x:.2f}, {self.car.y:.2f}]"
# horizontal_force = self.car.mass * self.car.th_dot * self.car.velocity
# self.y_forces.append(horizontal_force)
# if horizontal_force > self.car.max_friction_force:
# self.done = True
# self.reward = -1
# self.done_reason = f"Friction limit reached: {horizontal_force} > {self.car.max_friction_force}"
if self.steps > 500:
self.done = True
self.done_reason = f"Max steps"
car = [self.car.x, self.car.y]
if lib.get_distance(car, self.env_map.start) < 0.6 and self.steps > 50:
self.done = True
self.reward = 1
self.done_reason = f"Lap complete"
return self.done
def render(self,
wait=False,
scan_sim=None,
save=False,
pts1=None,
pts2=None):
self.env_map.render_map(4)
fig = plt.figure(4)
if scan_sim is not None:
for i in range(scan_sim.number_of_beams):
angle = i * scan_sim.dth + self.car.theta - scan_sim.fov / 2
fs = scan_sim.scan_output[
i] * scan_sim.n_searches * scan_sim.step_size
dx = [np.sin(angle) * fs, np.cos(angle) * fs]
range_val = lib.add_locations([self.car.x, self.car.y], dx)
cx, cy = self.env_map.convert_position(
[self.car.x, self.car.y])
rx, ry = self.env_map.convert_position(range_val)
x = [cx, rx]
y = [cy, ry]
plt.plot(x, y)
xs, ys = [], []
for pos in self.action_memory:
x, y = self.env_map.xy_to_row_column(pos)
xs.append(x)
ys.append(y)
plt.plot(xs, ys, 'r', linewidth=3)
plt.plot(xs, ys, '+', markersize=12)
x, y = self.env_map.xy_to_row_column([self.car.x, self.car.y])
plt.plot(x, y, 'x', markersize=20)
if pts1 is not None:
for i, pt in enumerate(pts1):
x, y = self.env_map.convert_position(pt)
plt.text(x, y, f"{i}")
plt.plot(x, y, 'x', markersize=10)
if pts2 is not None:
for pt in pts2:
x, y = self.env_map.convert_position(pt)
plt.plot(x, y, 'o', markersize=6)
text_x = self.env_map.obs_img.shape[0] + 10
text_y = self.env_map.obs_img.shape[1] / 10
s = f"Reward: [{self.reward:.1f}]"
plt.text(text_x, text_y * 1, s)
s = f"Action: [{self.action[0]:.2f}, {self.action[1]:.2f}]"
plt.text(text_x, text_y * 2, s)
s = f"Done: {self.done}"
plt.text(text_x, text_y * 3, s)
s = f"Pos: [{self.car.x:.2f}, {self.car.y:.2f}]"
plt.text(text_x, text_y * 4, s)
s = f"Vel: [{self.car.velocity:.2f}]"
plt.text(text_x, text_y * 5, s)
s = f"Theta: [{(self.car.theta * 180 / np.pi):.2f}]"
plt.text(text_x, text_y * 6, s)
s = f"Delta x100: [{(self.car.steering*100):.2f}]"
plt.text(text_x, text_y * 7, s)
s = f"Done reason: {self.done_reason}"
plt.text(text_x, text_y * 8, s)
s = f"Steps: {self.steps}"
plt.text(text_x, text_y * 9, s)
plt.pause(0.0001)
if wait:
plt.show()
if save and self.eps % 2 == 0:
plt.savefig(f'TrainingFigs/t{self.eps}.png')
def distance_potential(s, s_p, end, beta=0.2, scale=0.5):
prev_dist = lib.get_distance(s[0:2], end)
cur_dist = lib.get_distance(s_p[0:2], end)
d_dis = (prev_dist - cur_dist) / scale
return d_dis * beta
def MinCurvatureTrajectory(pts, nvecs, ws):
"""
This function uses optimisation to minimise the curvature of the path
"""
w_min = -ws[:, 0] * 0.9
w_max = ws[:, 1] * 0.9
th_ns = [lib.get_bearing([0, 0], nvecs[i, 0:2]) for i in range(len(nvecs))]
N = len(pts)
n_f_a = ca.MX.sym('n_f', N)
n_f = ca.MX.sym('n_f', N - 1)
th_f = ca.MX.sym('n_f', N - 1)
x0_f = ca.MX.sym('x0_f', N - 1)
x1_f = ca.MX.sym('x1_f', N - 1)
y0_f = ca.MX.sym('y0_f', N - 1)
y1_f = ca.MX.sym('y1_f', N - 1)
th1_f = ca.MX.sym('y1_f', N - 1)
th2_f = ca.MX.sym('y1_f', N - 1)
th1_f1 = ca.MX.sym('y1_f', N - 2)
th2_f1 = ca.MX.sym('y1_f', N - 2)
o_x_s = ca.Function('o_x', [n_f], [pts[:-1, 0] + nvecs[:-1, 0] * n_f])
o_y_s = ca.Function('o_y', [n_f], [pts[:-1, 1] + nvecs[:-1, 1] * n_f])
o_x_e = ca.Function('o_x', [n_f], [pts[1:, 0] + nvecs[1:, 0] * n_f])
o_y_e = ca.Function('o_y', [n_f], [pts[1:, 1] + nvecs[1:, 1] * n_f])
dis = ca.Function('dis', [x0_f, x1_f, y0_f, y1_f],
[ca.sqrt((x1_f - x0_f)**2 + (y1_f - y0_f)**2)])
track_length = ca.Function('length', [n_f_a], [
dis(o_x_s(n_f_a[:-1]), o_x_e(n_f_a[1:]), o_y_s(n_f_a[:-1]),
o_y_e(n_f_a[1:]))
])
real = ca.Function(
'real', [th1_f, th2_f],
[ca.cos(th1_f) * ca.cos(th2_f) + ca.sin(th1_f) * ca.sin(th2_f)])
im = ca.Function(
'im', [th1_f, th2_f],
[-ca.cos(th1_f) * ca.sin(th2_f) + ca.sin(th1_f) * ca.cos(th2_f)])
sub_cmplx = ca.Function('a_cpx', [th1_f, th2_f],
[ca.atan2(im(th1_f, th2_f), real(th1_f, th2_f))])
get_th_n = ca.Function(
'gth', [th_f],
[sub_cmplx(ca.pi * np.ones(N - 1), sub_cmplx(th_f, th_ns[:-1]))])
d_n = ca.Function('d_n', [n_f_a, th_f],
[track_length(n_f_a) / ca.tan(get_th_n(th_f))])
# objective
real1 = ca.Function(
'real1', [th1_f1, th2_f1],
[ca.cos(th1_f1) * ca.cos(th2_f1) + ca.sin(th1_f1) * ca.sin(th2_f1)])
im1 = ca.Function(
'im1', [th1_f1, th2_f1],
[-ca.cos(th1_f1) * ca.sin(th2_f1) + ca.sin(th1_f1) * ca.cos(th2_f1)])
sub_cmplx1 = ca.Function(
'a_cpx1', [th1_f1, th2_f1],
[ca.atan2(im1(th1_f1, th2_f1), real1(th1_f1, th2_f1))])
# define symbols
n = ca.MX.sym('n', N)
th = ca.MX.sym('th', N - 1)
nlp = {\
'x': ca.vertcat(n, th),
'f': ca.sumsqr(sub_cmplx1(th[1:], th[:-1])),
# 'f': ca.sumsqr(track_length(n)),
'g': ca.vertcat(
# dynamic constraints
n[1:] - (n[:-1] + d_n(n, th)),
# boundary constraints
n[0], #th[0],
n[-1], #th[-1],
) \
}
# S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt':{'print_level':5}})
S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt': {'print_level': 0}})
ones = np.ones(N)
n0 = ones * 0
th0 = []
for i in range(N - 1):
th_00 = lib.get_bearing(pts[i, 0:2], pts[i + 1, 0:2])
th0.append(th_00)
th0 = np.array(th0)
x0 = ca.vertcat(n0, th0)
lbx = list(w_min) + [-np.pi] * (N - 1)
ubx = list(w_max) + [np.pi] * (N - 1)
r = S(x0=x0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
x_opt = r['x']
n_set = np.array(x_opt[:N])
# thetas = np.array(x_opt[1*N:2*(N-1)])
return n_set
