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 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 __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 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 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