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 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 find_nvecs(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 print_update(self, plot_reward=True): if self.ptr < 10: return mean10 = np.mean(self.rewards[self.ptr - 10:self.ptr]) mean100 = np.mean(self.rewards[max(0, self.ptr - 100):self.ptr]) # score = moving_average(self.rewards[self.ptr-100:self.ptr], 10) print( f"Run: {self.t_counter} --> Moving10: {mean10:.2f} --> Moving100: {mean100:.2f} " ) if plot_reward: lib.plot(self.rewards[0:self.ptr], figure_n=2)
def expand_centerline(self): n = 2 # number of pts per orig pt dz = 1 / n o_line = self.cline new_line = [] 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) self.cline = np.array(new_line) self.N = len(new_line)
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 test_kernel_pp(): conf = lib.load_config_namespace(config_file) planner = PurePursuit(conf) kernel = TrackKernel(conf) safety_planner = Supervisor(planner, kernel, conf) run_kernel_test(conf, safety_planner, 5, obstacles=0)
def __call__(self, s, a, s_p, dev): r = collision_complete_reward(s_p) pos_tt = np.array([s_p['poses_x'][0], s_p['poses_y'][0]]) pt_i, pt_ii, d_i, d_ii = find_closest_pt(pos_tt, 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['poses_theta'][0] d_th = abs(lib.sub_angles_complex(th_ref, th)) v_scale = s_p['linear_vels_x'][0] / self.max_v r_race = self.mh * np.cos(d_th) * v_scale - self.md * d_c return r + r_race
def test_rando_kernel(): conf = lib.load_config_namespace(config_file) planner = RandomPlanner(conf) kernel = TrackKernel(conf) safety_planner = Supervisor(planner, kernel, conf) run_kernel_test(conf, safety_planner, 5, obstacles=0)
def test_e2e(): agent_name = "EndToEnd_1" conf = lib.load_config_namespace(config_file) vehicle = TestEndToEnd(conf, agent_name) run_multi_test(conf, vehicle)
def __call__(self, s, a, s_p, r) -> float: if r == -1: return r else: pt_i, pt_ii, d_i, d_ii = find_closest_pt(s_p[0:2], self.pts) 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 shaped_r = distance_potential(s, s_p, self.end) new_r = self.mh * np.cos(d_th) * v_scale - self.md * d_c return new_r + r + shaped_r
def _expand_wpts(self): n = 5 # number of pts per orig pt #TODO: make this a parameter dz = 1 / n o_line = self.waypoints o_vs = self.vs new_line = [] new_vs = [] for i in range(len(o_line) - 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) dv = o_vs[i + 1] - o_vs[i] new_vs.append(o_vs[i] + dv * j * dz) self.waypoints = np.array(new_line) self.vs = np.array(new_vs)
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.conf.stheta - np.pi / 2 while (lib.get_distance(pt, start) > d_search 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") 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 test_fgm(): conf = lib.load_config_namespace(config_file) vehicle = FollowTheGap(conf) test_vehicle(conf, vehicle)
def train_e2e(): agent_name = "EndToEnd_1" conf = lib.load_config_namespace(config_file) vehicle = TrainEndToEnd(conf, agent_name, False) TrainVehicle(conf, vehicle, 40000, 0)
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
# plt.plot(t[:-1], f_lat) # plt.plot(t[:-1], f_t, linewidth=3) # plt.plot(t, np.ones_like(t) * f_max, '--') # plt.plot(t, np.ones_like(t) * -f_max, '--') # plt.plot(t, np.ones_like(t) * f_long_max, '--') # plt.plot(t, np.ones_like(t) * -f_long_max, '--') # plt.legend(['Flong', "f_lat", "f_t"]) return vs 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 if __name__ == "__main__": # fname = "config_example_map" fname = "config_test" # fname = "vegas" conf = lib.load_config_namespace(fname) pre_map = PreMap(conf) pre_map.run_conversion()
from GeneralTrainTest import TrainVehicle, run_multi_test from auto_race_f110_gym.NavAgents.AgentSerial import SerialVehicleTrain, SerialVehicleTest from auto_race_f110_gym.Utils import LibFunctions as lib config_file = "config_test" conf = lib.load_config_namespace(config_file) n = 4 serial_name = f"SerialPlanner_Berlin_{n}" def train_serial(): vehicle = SerialVehicleTrain(serial_name, conf, False) # vehicle = SerialVehicleTrain(serial_name, conf, True) TrainVehicle(conf, vehicle, 200000, 6) def test_serial(): vehicle = SerialVehicleTest(serial_name, conf) run_multi_test(conf, vehicle) if __name__ == "__main__": # train_serial() test_serial()
def test_pure_pursuit(): conf = lib.load_config_namespace(config_file) vehicle = PurePursuit(conf) run_multi_test(conf, vehicle, 5, obstacles=0)
def MinCurvatureTrajectory(pts, nvecs, ws): """ This function uses optimisation to minimise the curvature of the path """ width_factor = 0.6 w_min = -ws[:, 0] * width_factor w_max = ws[:, 1] * width_factor 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