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