def expand_wpts(self):
n = 5 # number of pts per orig pt
dz = 1 / n
o_line = self.wpts
o_ss = self.ss
o_vs = self.vs
new_line = []
new_ss = []
new_vs = []
for i in range(self.N - 1):
dd = lib.sub_locations(o_line[i + 1], o_line[i])
for j in range(n):
pt = lib.add_locations(o_line[i], dd, dz * j)
new_line.append(pt)
ds = o_ss[i + 1] - o_ss[i]
new_ss.append(o_ss[i] + dz * j * ds)
dv = o_vs[i + 1] - o_vs[i]
new_vs.append(o_vs[i] + dv * j * dz)
self.wpts = np.array(new_line)
self.ss = np.array(new_ss)
self.vs = np.array(new_vs)
self.N = len(new_line)
def train(self, batches=5000):
losses = np.zeros(batches)
batch_size = 100
loss = nn.MSELoss()
optimiser = optim.SGD(self.actor.parameters(), lr=0.001)
for i in range(batches):
x, u = self.buffer.sample(batch_size)
state = torch.FloatTensor(x)
action = torch.FloatTensor(u)
optimiser.zero_grad()
outputs = self.actor(state)
actor_loss = loss(outputs[:, 0], action)
actor_loss.backward()
optimiser.step()
losses[i] = actor_loss
if i % 500 == 0:
print(f"Batch: {i}: Loss: {actor_loss}")
lib.plot(losses, 100)
self.save()
return losses
def get_moving_avg(vehicle_name, show=False):
# _{vehicle_name}
path = 'Vehicles/' + vehicle_name + f"/training_data.csv"
smoothpath = 'Vehicles/' + vehicle_name + f"/TrainingDataSmooth.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(100, rewards)
new_rewards = []
l = 30
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, l)
if show:
lib.plot_no_avg(rewards, figure_n=1)
lib.plot_no_avg(smooth_rewards, figure_n=2)
plt.show()
def expand_wpts(self):
n = 5 # number of pts per orig pt
dz = 1 / n
o_line = self.waypoints[:, 0:2]
# o_ss = self.ss
o_vs = self.waypoints[:, 2]
new_line = []
# new_ss = []
new_vs = []
for i in range(len(self.waypoints) - 1):
dd = lib.sub_locations(o_line[i + 1], o_line[i])
for j in range(n):
pt = lib.add_locations(o_line[i], dd, dz * j)
new_line.append(pt)
# ds = o_ss[i+1] - o_ss[i]
# new_ss.append(o_ss[i] + dz*j*ds)
dv = o_vs[i + 1] - o_vs[i]
new_vs.append(o_vs[i] + dv * j * dz)
wpts = np.array(new_line)
# self.ss = np.array(new_ss)
vs = np.array(new_vs)
self.waypoints = np.concatenate([wpts, vs[:, None]], axis=-1)
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 print_update(self, plot_reward=True):
if self.ptr < 5:
return
mean = np.mean(self.rewards[0:self.ptr])
score = self.rewards[-1]
print(f"Run: {self.t_counter} --> Score: {score:.2f} --> Mean: {mean:.2f} ")
if plot_reward:
lib.plot(self.rewards[0:self.ptr], figure_n=2)
def find_nvecs_old(self):
N = self.N
track = self.cline
nvecs = []
# new_track.append(track[0, :])
nvec = lib.theta_to_xy(np.pi/2 + lib.get_bearing(track[0, :], track[1, :]))
nvecs.append(nvec)
for i in range(1, len(track)-1):
pt1 = track[i-1]
pt2 = track[min((i, N)), :]
pt3 = track[min((i+1, N-1)), :]
th1 = lib.get_bearing(pt1, pt2)
th2 = lib.get_bearing(pt2, pt3)
if th1 == th2:
th = th1
else:
dth = lib.sub_angles_complex(th1, th2) / 2
th = lib.add_angles_complex(th2, dth)
new_th = th + np.pi/2
nvec = lib.theta_to_xy(new_th)
nvecs.append(nvec)
nvec = lib.theta_to_xy(np.pi/2 + lib.get_bearing(track[-2, :], track[-1, :]))
nvecs.append(nvec)
self.nvecs = np.array(nvecs)
def cth_reward(self, s_p):
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
r = self.mh * np.cos(d_th) * v_scale - self.md * d_c
return r
def transform_obs(self, obs):
cur_v = [obs[3] / self.max_v]
cur_d = [obs[4] / self.max_d]
th_target = lib.get_bearing(obs[0:2], self.env_map.end)
alpha = lib.sub_angles_complex(th_target, obs[2])
th_scale = [(alpha) * 2 / np.pi]
scan = self.scan_sim.get_scan(obs[0], obs[1], obs[2])
nn_obs = np.concatenate([cur_v, cur_d, th_scale, scan])
return nn_obs
def get_curvature(pos_history):
n = len(pos_history)
ths = [
lib.get_bearing(pos_history[i], pos_history[i + 1])
for i in range(n - 1)
]
dth = [
abs(lib.sub_angles_complex(ths[i], ths[i + 1])) for i in range(n - 2)
]
total_curve = np.sum(dth)
avg_curve = np.mean(dth)
print(f"Total Curvatue: {total_curve}, Avg: {avg_curve}")
return total_curve
def __init__(self, map_name, sim_conf, pp_conf=None) -> None:
if pp_conf is None:
pp_conf = lib.load_conf("mod_conf")
self.name = "Pure Pursuit Path Follower"
self.path_name = None
self.map_name = map_name
mu = sim_conf.mu
g = sim_conf.g
self.m = sim_conf.m
self.wheelbase = sim_conf.l_f + sim_conf.l_r
self.f_max = mu * self.m * g #* safety_f
self.v_gain = pp_conf.v_gain
self.lookahead = pp_conf.lookahead
self.wpts = None
self.vs = None
self.N = None
self.ss = None #TODO: I think this isn't needed and can be removed
self.aim_pts = []
try:
# raise FileNotFoundError
self._load_csv_track()
except FileNotFoundError:
print(f"Problem Loading map - generate map pts")
def run_follow_the_gap_forest():
sim_conf = lib.load_conf("fgm_config")
env = ForestSim(map_name_forest, sim_conf)
vehicle = ForestFGM()
# test_single_vehicle(env, vehicle, True, 10, False, vis=True)
test_single_vehicle(env, vehicle, False, 100, add_obs=True)
def run_forest_gen():
fname = "config_test"
sim_conf = lib.load_conf(fname)
map_name = "forest2"
pre_map = ForestPreMap(map_name, sim_conf)
pre_map.run_generation()
def run_follow_the_gap_track():
sim_conf = lib.load_conf("fgm_config")
env = TrackSim(map_name_track, sim_conf)
vehicle = TrackFGM()
# test_single_vehicle(env, vehicle, True, 10, False)
test_single_vehicle(env, vehicle, True, 100, add_obs=True)
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 train_mod_track():
sim_conf = lib.load_conf("std_config")
env = TrackSim(map_name_track, sim_conf)
# vehicle = ModVehicleTrain(mod_name, map_name, env.sim_conf)
vehicle = ModVehicleTrain(mod_name_track, map_name_track, sim_conf, load=False, h_size=500)
train_vehicle(env, vehicle, 500000)
def run_pre_map():
fname = "config_test"
conf = lib.load_conf(fname)
# map_name = "example_map"
map_name = "race_track"
pre_map = PreMap(conf, map_name)
pre_map.run_conversion()
def init_agent(self, env_map):
self.env_map = env_map
self.scan_sim.set_check_fcn(self.env_map.check_scan_location)
# self.wpts = self.env_map.get_min_curve_path()
self.wpts = self.env_map.get_reference_path()
self.prev_dist_target = lib.get_distance(self.env_map.start,
self.env_map.end)
return self.wpts
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)
# print(f"Pt added without being adjusted")
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_centerline(self, show=True):
dt = self.dt
d_search = 0.8
n_search = 11
dth = (np.pi * 4/5) / (n_search-1)
# makes a list of search locations
search_list = []
for i in range(n_search):
th = -np.pi/2 + dth * i
x = -np.sin(th) * d_search
y = np.cos(th) * d_search
loc = [x, y]
search_list.append(loc)
pt = start = np.array([self.conf.sx, self.conf.sy])
self.cline = [pt]
th = self.stheta
while (lib.get_distance(pt, start) > d_search/2 or len(self.cline) < 10) and len(self.cline) < 500:
vals = []
self.search_space = []
for i in range(n_search):
d_loc = lib.transform_coords(search_list[i], -th)
search_loc = lib.add_locations(pt, d_loc)
self.search_space.append(search_loc)
x, y = self.xy_to_row_column(search_loc)
val = dt[y, x]
vals.append(val)
ind = np.argmax(vals)
d_loc = lib.transform_coords(search_list[ind], -th)
pt = lib.add_locations(pt, d_loc)
self.cline.append(pt)
if show:
self.plot_raceline_finding()
th = lib.get_bearing(self.cline[-2], pt)
print(f"Adding pt: {pt}")
self.cline = np.array(self.cline)
self.N = len(self.cline)
print(f"Raceline found --> n: {len(self.cline)}")
if show:
self.plot_raceline_finding(True)
self.plot_raceline_finding(False)
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 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 train_mod_f():
sim_conf = lib.load_conf("std_config")
env = ForestSim(map_name_f, sim_conf)
vehicle = ModVehicleTrain(mod_name_f, map_name_f, sim_conf, load=False, h_size=200)
train_vehicle(env, vehicle, 1000)
def test_mod_forest():
sim_conf = lib.load_conf("std_config")
env = ForestSim(map_name_f, sim_conf)
vehicle = ModVehicleTest(mod_name_f, map_name_f, sim_conf)
test_single_vehicle(env, vehicle, True, 100, wait=False)
def train_nav_forest():
sim_conf = lib.load_conf("std_config")
env = ForestSim(map_name_forest, sim_conf)
vehicle = NavTrainVehicle(nav_name_f, sim_conf, h_size=200)
train_vehicle(env, vehicle, 1000)
def test_nav_forest():
sim_conf = lib.load_conf("std_config")
env = ForestSim(map_name_forest, sim_conf)
vehicle = NavTestVehicle(nav_name_f, sim_conf)
test_single_vehicle(env, vehicle, True, 100, wait=False)
def test_nav_track():
sim_conf = lib.load_conf("race_config")
env = TrackSim(map_name_track, sim_conf)
vehicle = NavTestVehicle(nav_name_track, sim_conf)
test_single_vehicle(env, vehicle, False, 100, wait=True, vis=False, add_obs=False)
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
