diff --git a/README.md b/README.md index 32877be..6e97399 100644 --- a/README.md +++ b/README.md @@ -8,6 +8,7 @@ Now you can see the examples of control theories as following - **Nonlinear Model Predictive Control with CGMRES** - **Linear Model Predictive Control**(as generic function such as matlab tool box) +- **Extended Linear Model Predictive Control for vehicle model** # Usage and Details you can see the usage in each folder diff --git a/mpc/extend/README.md b/mpc/extend/README.md new file mode 100644 index 0000000..e926aa5 --- /dev/null +++ b/mpc/extend/README.md @@ -0,0 +1,41 @@ +# Model Predictive Control for Vehicle model +This program is for controlling the vehicle model. +I implemented the steering control for vehicle by using Model Predictive Control. + +# Model +Usually, the vehicle model is expressed by extremely complicated nonlinear equation. +Acoording to reference 1, I used the simple model as shown in following equation. + + + +However, it is still a nonlinear equation. +Therefore, I assume that the car is tracking the reference trajectory. +If we get the assumption, the model can turn to linear model by using the path's curvatures. + + + +and \delta_r denoted + + + +R is the curvatures of the reference trajectory. + +Now we can get the linear state equation and can apply the MPC theory. + +However, you should care that this state euation could be changed during the predict horizon. +I implemented this, so if you know about the detail please go to "IteraticeMPC_func.py" + +# Expected Results + + + + + +# Usage + +``` +$ python main_track.py +``` + +# Reference +- 1. https://qiita.com/taka_horibe/items/47f86e02e2db83b0c570#%E8%BB%8A%E4%B8%A1%E3%81%AE%E8%BB%8C%E9%81%93%E8%BF%BD%E5%BE%93%E5%95%8F%E9%A1%8C%E9%9D%9E%E7%B7%9A%E5%BD%A2%E3%81%AB%E9%81%A9%E7%94%A8%E3%81%99%E3%82%8B (Japanese) diff --git a/mpc/with_disturbance/animation.py b/mpc/extend/animation.py similarity index 60% rename from mpc/with_disturbance/animation.py rename to mpc/extend/animation.py index b5e4942..a085b5a 100755 --- a/mpc/with_disturbance/animation.py +++ b/mpc/extend/animation.py @@ -84,6 +84,42 @@ def square_make_with_angles(center_x, center_y, size, angle): return square_xs, square_ys, np.array(angle_line_xs), np.array(angle_line_ys) +def circle_make_with_angles(center_x, center_y, radius, angle): + ''' + Create circle matrix with angle line matrix + + Parameters + ------- + center_x : float + the center x position of the circle + center_y : float + the center y position of the circle + radius : float + angle : float [rad] + + Returns + ------- + circle xs : numpy.ndarray + circle ys : numpy.ndarray + angle line xs : numpy.ndarray + angle line ys : numpy.ndarray + ''' + + point_num = 100 # 分解能 + + circle_xs = [] + circle_ys = [] + + for i in range(point_num + 1): + circle_xs.append(center_x + radius * math.cos(i*2*math.pi/point_num)) + circle_ys.append(center_y + radius * math.sin(i*2*math.pi/point_num)) + + angle_line_xs = [center_x, center_x + math.cos(angle) * radius] + angle_line_ys = [center_y, center_y + math.sin(angle) * radius] + + return np.array(circle_xs), np.array(circle_ys), np.array(angle_line_xs), np.array(angle_line_ys) + + class AnimDrawer(): """create animation of path and robot @@ -99,22 +135,29 @@ class AnimDrawer(): Parameters ------------ objects : list of objects + + Notes + --------- + lead_history_states, lead_history_predict_states, traj_ref, history_traj_ref, history_angle_ref """ self.lead_car_history_state = objects[0] - self.follow_car_history_state = objects[1] + self.lead_car_history_predict_state = objects[1] self.traj = objects[2] + self.history_traj_ref = objects[3] + self.history_angle_ref = objects[4] - self.history_xs = [self.lead_car_history_state[:, 0], self.follow_car_history_state[:, 0]] - self.history_ys = [self.lead_car_history_state[:, 1], self.follow_car_history_state[:, 1]] - self.history_ths = [self.lead_car_history_state[:, 2], self.follow_car_history_state[:, 2]] + self.history_xs = [self.lead_car_history_state[:, 0]] + self.history_ys = [self.lead_car_history_state[:, 1]] + self.history_ths = [self.lead_car_history_state[:, 2]] # setting up figure self.anim_fig = plt.figure(dpi=150) self.axis = self.anim_fig.add_subplot(111) # imgs - self.object_imgs = [] + self.car_imgs = [] self.traj_imgs = [] + self.predict_imgs = [] def draw_anim(self, interval=50): """draw the animation and save @@ -153,10 +196,12 @@ class AnimDrawer(): self.axis.set_ylabel(r'$\it{y}$ [m]') self.axis.set_aspect('equal', adjustable='box') - # (2) set the xlim and ylim - self.axis.set_xlim(-2, 20) - self.axis.set_ylim(-10, 10) + LOW_MARGIN = 5 + HIGH_MARGIN = 5 + self.axis.set_xlim(np.min(self.history_xs) - LOW_MARGIN, np.max(self.history_xs) + HIGH_MARGIN) + self.axis.set_ylim(np.min(self.history_ys) - LOW_MARGIN, np.max(self.history_ys) + HIGH_MARGIN) + def _set_img(self): """ initialize the imgs of animation this private function execute the make initial imgs for animation @@ -167,14 +212,22 @@ class AnimDrawer(): for i in range(len(obj_color_list)): temp_img, = self.axis.plot([], [], color=obj_color_list[i], linestyle=obj_styles[i]) - self.object_imgs.append(temp_img) + self.car_imgs.append(temp_img) - traj_color_list = ["k", "m", "b"] + traj_color_list = ["k", "b"] for i in range(len(traj_color_list)): temp_img, = self.axis.plot([],[], color=traj_color_list[i], linestyle="dashed") self.traj_imgs.append(temp_img) + temp_img, = self.axis.plot([],[], ".", color="m") + self.traj_imgs.append(temp_img) + + # predict + for _ in range(2 * len(self.history_angle_ref[0])): + temp_img, = self.axis.plot([],[], color="g", linewidth=0.5) # point + # temp_img, = self.axis.plot([],[], ".", color="g", linewidth=0.5) # point + self.predict_imgs.append(temp_img) def _update_anim(self, i): """the update animation @@ -193,12 +246,24 @@ class AnimDrawer(): """ i = int(i * self.skip_num) - self._draw_objects(i) + # self._draw_set_axis(i) + self._draw_car(i) self._draw_traj(i) + # self._draw_prediction(i) - return self.object_imgs, self.traj_imgs, - - def _draw_objects(self, i): + return self.car_imgs, self.traj_imgs, self.predict_imgs, + + def _draw_set_axis(self, i): + """ + """ + # (2) set the xlim and ylim + LOW_MARGIN = 20 + HIGH_MARGIN = 20 + OVER_LOOK = 50 + self.axis.set_xlim(np.min(self.history_xs[0][i : i + OVER_LOOK]) - LOW_MARGIN, np.max(self.history_xs[0][i : i + OVER_LOOK]) + HIGH_MARGIN) + self.axis.set_ylim(np.min(self.history_ys[0][i : i + OVER_LOOK]) - LOW_MARGIN, np.max(self.history_ys[0][i : i + OVER_LOOK]) + HIGH_MARGIN) + + def _draw_car(self, i): """ This private function is just divided thing of the _update_anim to see the code more clear @@ -210,16 +275,14 @@ class AnimDrawer(): the sampling time should be related to the sampling time of system """ # cars - for j in range(2): - fix_j = j * 2 - object_x, object_y, angle_x, angle_y = square_make_with_angles(self.history_xs[j][i], - self.history_ys[j][i], - 1.0, - self.history_ths[j][i]) + object_x, object_y, angle_x, angle_y = square_make_with_angles(self.history_xs[0][i], + self.history_ys[0][i], + 5.0, + self.history_ths[0][i]) + + self.car_imgs[0].set_data([object_x, object_y]) + self.car_imgs[1].set_data([angle_x, angle_y]) - self.object_imgs[fix_j].set_data([object_x, object_y]) - self.object_imgs[fix_j + 1].set_data([angle_x, angle_y]) - def _draw_traj(self, i): """ This private function is just divided thing of @@ -231,7 +294,31 @@ class AnimDrawer(): time step of the animation the sampling time should be related to the sampling time of system """ - for j in range(2): - self.traj_imgs[j].set_data(self.history_xs[j][:i], self.history_ys[j][:i]) + # car + self.traj_imgs[0].set_data(self.history_xs[0][:i], self.history_ys[0][:i]) - self.traj_imgs[2].set_data(self.traj[0, :], self.traj[1, :]) \ No newline at end of file + # all traj_ref + self.traj_imgs[1].set_data(self.traj[0, :], self.traj[1, :]) + + # traj_ref + # self.traj_imgs[2].set_data(self.history_traj_ref[i][0, :], self.history_traj_ref[i][1, :]) + + def _draw_prediction(self, i): + """draw prediction + + Parameters + ------------ + i : int + time step of the animation + the sampling time should be related to the sampling time of system + """ + + for j in range(0, len(self.history_angle_ref[0]), 4): + fix_j = j * 2 + object_x, object_y, angle_x, angle_y =\ + circle_make_with_angles(self.lead_car_history_predict_state[i][j, 0], + self.lead_car_history_predict_state[i][j, 1], 1., + self.lead_car_history_predict_state[i][j, 2]) + + self.predict_imgs[fix_j].set_data(object_x, object_y) + self.predict_imgs[fix_j + 1].set_data(angle_x, angle_y) \ No newline at end of file diff --git a/mpc/with_disturbance/coordinate_trans.py b/mpc/extend/coordinate_trans.py similarity index 100% rename from mpc/with_disturbance/coordinate_trans.py rename to mpc/extend/coordinate_trans.py diff --git a/mpc/with_disturbance/iterative_MPC.py b/mpc/extend/extended_MPC.py similarity index 86% rename from mpc/with_disturbance/iterative_MPC.py rename to mpc/extend/extended_MPC.py index bffdbbd..c365aaf 100644 --- a/mpc/with_disturbance/iterative_MPC.py +++ b/mpc/extend/extended_MPC.py @@ -11,11 +11,11 @@ class IterativeMpcController(): """ Attributes ------------ - A : numpy.ndarray + Ad_s : list of numpy.ndarray system matrix - B : numpy.ndarray + Bd_s : list of numpy.ndarray input matrix - W_D : numpy.ndarray + W_D_s : list of numpy.ndarray distubance matrix in state equation Q : numpy.ndarray evaluation function weight for states @@ -107,7 +107,7 @@ class IterativeMpcController(): A_factorials.append(temp_mat) # after we use this factorials - print("phi_mat = \n{0}".format(self.phi_mat)) + # print("phi_mat = \n{0}".format(self.phi_mat)) self.gamma_mat = copy.deepcopy(self.Bd_s[0]) gammma_mat_temp = copy.deepcopy(self.Bd_s[0]) @@ -117,7 +117,7 @@ class IterativeMpcController(): gammma_mat_temp = temp_1_mat + gammma_mat_temp self.gamma_mat = np.vstack((self.gamma_mat, gammma_mat_temp)) - print("gamma_mat = \n{0}".format(self.gamma_mat)) + # print("gamma_mat = \n{0}".format(self.gamma_mat)) self.theta_mat = copy.deepcopy(self.gamma_mat) @@ -127,25 +127,25 @@ class IterativeMpcController(): self.theta_mat = np.hstack((self.theta_mat, temp_mat)) - print("theta_mat = \n{0}".format(self.theta_mat)) + # print("theta_mat = \n{0}".format(self.theta_mat)) # disturbance - print("A_factorials_mat = \n{0}".format(A_factorials)) + # print("A_factorials_mat = \n{0}".format(A_factorials)) A_factorials_mat = np.array(A_factorials).reshape(-1, self.state_size) - print("A_factorials_mat = \n{0}".format(A_factorials_mat)) + # print("A_factorials_mat = \n{0}".format(A_factorials_mat)) eye = np.eye(self.state_size) self.dist_mat = np.vstack((eye, A_factorials_mat[:-self.state_size, :])) base_mat = copy.deepcopy(self.dist_mat) - print("base_mat = \n{0}".format(base_mat)) + # print("base_mat = \n{0}".format(base_mat)) for i in range(self.pre_step - 1): temp_mat = np.zeros_like(A_factorials_mat) temp_mat[int((i + 1)*self.state_size): , :] = base_mat[:-int((i + 1)*self.state_size) , :] self.dist_mat = np.hstack((self.dist_mat, temp_mat)) - print("dist_mat = \n{0}".format(self.dist_mat)) + # print("dist_mat = \n{0}".format(self.dist_mat)) W_Ds = copy.deepcopy(self.W_D_s[0]) @@ -154,7 +154,7 @@ class IterativeMpcController(): self.dist_mat = np.dot(self.dist_mat, W_Ds) - print("dist_mat = \n{0}".format(self.dist_mat)) + # print("dist_mat = \n{0}".format(self.dist_mat)) # evaluation function weight diag_Qs = np.array([np.diag(self.Q) for _ in range(self.pre_step)]) @@ -163,8 +163,8 @@ class IterativeMpcController(): self.Qs = np.diag(diag_Qs.flatten()) self.Rs = np.diag(diag_Rs.flatten()) - print("Qs = \n{0}".format(self.Qs)) - print("Rs = \n{0}".format(self.Rs)) + # print("Qs = \n{0}".format(self.Qs)) + # print("Rs = \n{0}".format(self.Rs)) # constraints # about dt U @@ -175,7 +175,7 @@ class IterativeMpcController(): self.F[i * 2: (i + 1) * 2, i] = np.array([1., -1.]) temp_F = copy.deepcopy(self.F) - print("F = \n{0}".format(self.F)) + # print("F = \n{0}".format(self.F)) for i in range(self.pre_step - 1): temp_F = copy.deepcopy(temp_F) @@ -195,9 +195,9 @@ class IterativeMpcController(): self.f = np.array([temp_f for _ in range(self.pre_step)]).flatten() - print("F = \n{0}".format(self.F)) - print("F1 = \n{0}".format(self.F1)) - print("f = \n{0}".format(self.f)) + # print("F = \n{0}".format(self.F)) + # print("F1 = \n{0}".format(self.F1)) + # print("f = \n{0}".format(self.f)) # about dt_u if self.dt_input_lower is not None: @@ -217,8 +217,8 @@ class IterativeMpcController(): self.omega = np.array([temp_omega for _ in range(self.pre_step)]).flatten() - print("W = \n{0}".format(self.W)) - print("omega = \n{0}".format(self.omega)) + # print("W = \n{0}".format(self.W)) + # print("omega = \n{0}".format(self.omega)) # about state print("check the matrix!! if you think rite, plese push enter") @@ -235,8 +235,9 @@ class IterativeMpcController(): References ------------ - opt_input : numpy.ndarray, shape(input_length, ) + opt_u : numpy.ndarray, shape(input_length, ) optimal input + all_opt_u : numpy.ndarray, shape(PREDICT_STEP, input_length) """ temp_1 = np.dot(self.phi_mat, states.reshape(-1, 1)) temp_2 = np.dot(self.gamma_mat, self.history_us[-1].reshape(-1, 1)) @@ -277,11 +278,19 @@ class IterativeMpcController(): opt_dt_us = list(opt_result['x']) opt_u = opt_dt_us[:self.input_size] + self.history_us[-1] + + # calc all predit u + all_opt_u = [copy.deepcopy(opt_u)] + temp_u = copy.deepcopy(opt_u) + + for i in range(1, self.pre_step): + temp_u += opt_dt_us[i * self.input_size: (i + 1) * self.input_size] + all_opt_u.append(copy.deepcopy(temp_u)) # save self.history_us.append(opt_u) - return opt_u + return opt_u, np.array(all_opt_u) def update_system_model(self, system_model): """update system model diff --git a/mpc/with_disturbance/func_curvature.py b/mpc/extend/func_curvature.py similarity index 91% rename from mpc/with_disturbance/func_curvature.py rename to mpc/extend/func_curvature.py index b525409..cdd99e6 100644 --- a/mpc/with_disturbance/func_curvature.py +++ b/mpc/extend/func_curvature.py @@ -138,6 +138,22 @@ def calc_curvatures(traj_ref, predict_step, num_skip): return angles, curvatures +def calc_ideal_vel(traj_ref, dt): + """ + Parameters + ------------ + traj_ref : numpy.ndarray, shape (2, N) + these points should follow subseqently + dt : float + sampling time of system + """ + # end point and start point + diff = traj_ref[:, -1] - traj_ref[:, 0] + distance = np.sqrt(np.sum(diff**2)) + + V = distance / (dt * traj_ref.shape[1]) + + return V def main(): """ diff --git a/mpc/with_disturbance/main_track.py b/mpc/extend/main_track.py similarity index 76% rename from mpc/with_disturbance/main_track.py rename to mpc/extend/main_track.py index 30eb80a..1ee3c57 100644 --- a/mpc/with_disturbance/main_track.py +++ b/mpc/extend/main_track.py @@ -4,12 +4,12 @@ import math import copy # from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt -from iterative_MPC import IterativeMpcController +from extended_MPC import IterativeMpcController from animation import AnimDrawer # from control import matlab from coordinate_trans import coordinate_transformation_in_angle, coordinate_transformation_in_position from traj_func import make_sample_traj -from func_curvature import calc_curvatures +from func_curvature import calc_curvatures, calc_ideal_vel class WheeledSystem(): """SampleSystem, this is the simulator @@ -21,6 +21,11 @@ class WheeledSystem(): system states, [x, y, phi, beta] history_xs : list time history of state + tau : float + time constant of tire + FRONT_WHEEL_BASE : float + REAR_WHEEL_BASE : float + predict_xs : """ def __init__(self, init_states=None): """ @@ -41,6 +46,7 @@ class WheeledSystem(): self.xs = copy.deepcopy(init_states) self.history_xs = [init_states] + self.history_predict_xs = [] def update_state(self, us, dt=0.01): """ @@ -51,7 +57,6 @@ class WheeledSystem(): dt : float in seconds, optional sampling time of simulation, default is 0.01 [s] """ - # for theta 1, theta 1 dot, theta 2, theta 2 dot k0 = [0.0 for _ in range(self.NUM_STATE)] k1 = [0.0 for _ in range(self.NUM_STATE)] k2 = [0.0 for _ in range(self.NUM_STATE)] @@ -80,7 +85,50 @@ class WheeledSystem(): # save save_states = copy.deepcopy(self.xs) self.history_xs.append(save_states) - print(self.xs) + # print(self.xs) + + def predict_state(self, us, dt=0.01): + """make predict state by using optimal input made by MPC + Paramaters + ----------- + us : array-like, shape(2, N) + optimal input made by MPC + dt : float in seconds, optional + sampling time of simulation, default is 0.01 [s] + """ + + xs = copy.deepcopy(self.xs) + predict_xs = [copy.deepcopy(xs)] + + for i in range(us.shape[1]): + k0 = [0.0 for _ in range(self.NUM_STATE)] + k1 = [0.0 for _ in range(self.NUM_STATE)] + k2 = [0.0 for _ in range(self.NUM_STATE)] + k3 = [0.0 for _ in range(self.NUM_STATE)] + + functions = [self._func_x_1, self._func_x_2, self._func_x_3, self._func_x_4] + + # solve Runge-Kutta + for i, func in enumerate(functions): + k0[i] = dt * func(xs[0], xs[1], xs[2], xs[3], us[0, i], us[1, i]) + + for i, func in enumerate(functions): + k1[i] = dt * func(xs[0] + k0[0]/2., xs[1] + k0[1]/2., xs[2] + k0[2]/2., xs[3] + k0[3]/2., us[0, i], us[1, i]) + + for i, func in enumerate(functions): + k2[i] = dt * func(xs[0] + k1[0]/2., xs[1] + k1[1]/2., xs[2] + k1[2]/2., xs[3] + k1[3]/2., us[0, i], us[1, i]) + + for i, func in enumerate(functions): + k3[i] = dt * func(xs[0] + k2[0], xs[1] + k2[1], xs[2] + k2[2], xs[3] + k2[3], us[0, i], us[1, i]) + + xs[0] += (k0[0] + 2. * k1[0] + 2. * k2[0] + k3[0]) / 6. + xs[1] += (k0[1] + 2. * k1[1] + 2. * k2[1] + k3[1]) / 6. + xs[2] += (k0[2] + 2. * k1[2] + 2. * k2[2] + k3[2]) / 6. + xs[3] += (k0[3] + 2. * k1[3] + 2. * k2[3] + k3[3]) / 6. + + predict_xs.append(copy.deepcopy(xs)) + + self.history_predict_xs.append(np.array(predict_xs)) def _func_x_1(self, y_1, y_2, y_3, y_4, u_1, u_2): """ @@ -184,6 +232,7 @@ class SystemModel(): """ calc next predict systemo models V : float + current speed of car curvatures : list this is the next curvature's list predict_step : int @@ -236,52 +285,53 @@ def main(): # parameters dt = 0.01 simulation_time = 20 # in seconds - PREDICT_STEP = 15 + PREDICT_STEP = 30 iteration_num = int(simulation_time / dt) # make simulator with coninuous matrix - init_xs_lead = np.array([0., 0., math.pi/4, 0.]) - init_xs_follow = np.array([0., 0., math.pi/4, 0.]) + init_xs_lead = np.array([0., 0., math.pi/6, 0.]) lead_car = WheeledSystem(init_states=init_xs_lead) - follow_car = WheeledSystem(init_states=init_xs_follow) # make system model lead_car_system_model = SystemModel() - follow_car_system_model = SystemModel() # reference + history_traj_ref = [] + history_angle_ref = [] traj_ref_xs, traj_ref_ys = make_sample_traj(int(simulation_time/dt)) traj_ref = np.array([traj_ref_xs, traj_ref_ys]) - + # nearest point index_min, nearest_point = search_nearest_point(traj_ref, lead_car.xs[:2].reshape(2, 1)) # get traj's curvature - NUM_SKIP = 5 - MARGIN = 10 + NUM_SKIP = 3 + MARGIN = 20 angles, curvatures = calc_curvatures(traj_ref[:, index_min + MARGIN:index_min + PREDICT_STEP + 2 * NUM_SKIP + MARGIN], PREDICT_STEP, NUM_SKIP) + # save traj ref + history_traj_ref.append(traj_ref[:, index_min + MARGIN:index_min + PREDICT_STEP + 2 * NUM_SKIP + MARGIN]) + history_angle_ref.append(angles) + + # print(history_traj_ref) + # input() + # evaluation function weight - Q = np.diag([100., 1., 1.]) - R = np.diag([0.01]) + Q = np.diag([1000000., 10., 1.]) + R = np.diag([0.1]) # System model update - V = 3.0 # in pratical we should calc from the state + V = calc_ideal_vel(traj_ref, dt) # in pratical we should calc from the state lead_car_system_model.calc_predict_sytem_model(V, curvatures, PREDICT_STEP) - follow_car_system_model.calc_predict_sytem_model(V, curvatures, PREDICT_STEP) # make controller with discreted matrix lead_controller = IterativeMpcController(lead_car_system_model, Q, R, PREDICT_STEP, - dt_input_upper=np.array([30 * dt]), dt_input_lower=np.array([-30 * dt]), - input_upper=np.array([30.]), input_lower=np.array([-30.])) + dt_input_upper=np.array([1 * dt]), dt_input_lower=np.array([-1 * dt]), + input_upper=np.array([1.]), input_lower=np.array([-1.])) - follow_controller = IterativeMpcController(follow_car_system_model, Q, R, PREDICT_STEP, - dt_input_upper=np.array([30 * dt]), dt_input_lower=np.array([-30 * dt]), - input_upper=np.array([30.]), input_lower=np.array([-30.])) # initialize lead_controller.initialize_controller() - follow_controller.initialize_controller() for i in range(iteration_num): print("simulation time = {0}".format(i)) @@ -292,6 +342,7 @@ def main(): # nearest point index_min, nearest_point = search_nearest_point(traj_ref, lead_car.xs[:2].reshape(2, 1)) + # end check if len(traj_ref_ys) <= index_min + PREDICT_STEP + 2 * NUM_SKIP + MARGIN: print("break") @@ -300,8 +351,12 @@ def main(): # get traj's curvature angles, curvatures = calc_curvatures(traj_ref[:, index_min+MARGIN:index_min + PREDICT_STEP + 2 * NUM_SKIP + MARGIN], PREDICT_STEP, NUM_SKIP) + # save + history_traj_ref.append(traj_ref[:, index_min + MARGIN:index_min + PREDICT_STEP + 2 * NUM_SKIP + MARGIN]) + history_angle_ref.append(angles) + # System model update - V = 4.0 # in pratical we should calc from the state + V = calc_ideal_vel(traj_ref, dt) # in pratical we should calc from the state lead_car_system_model.calc_predict_sytem_model(V, curvatures, PREDICT_STEP) # transformation @@ -318,7 +373,7 @@ def main(): relative_ref_angle = np.array(angles) - angles[0] # make ref - lead_reference = np.array([[relative_traj[1, -1], relative_ref_angle[i], 0.] for i in range(PREDICT_STEP)]).flatten() + lead_reference = np.array([[relative_traj[1, -1], relative_ref_angle[-1], 0.] for i in range(PREDICT_STEP)]).flatten() print("relative car state = {}".format(relative_car_state)) print("nearest point = {}".format(nearest_point)) @@ -327,19 +382,27 @@ def main(): # update system matrix lead_controller.update_system_model(lead_car_system_model) - lead_opt_u = lead_controller.calc_input(relative_car_state, lead_reference) + lead_opt_u, all_opt_u = lead_controller.calc_input(relative_car_state, lead_reference) lead_opt_u = np.hstack((np.array([V]), lead_opt_u)) + all_opt_u = np.stack((np.ones(PREDICT_STEP)*V, all_opt_u.flatten())) + print("opt_u = {}".format(lead_opt_u)) - # input() + print("all_opt_u = {}".format(all_opt_u)) + # predict + lead_car.predict_state(all_opt_u, dt=dt) + + # update lead_car.update_state(lead_opt_u, dt=dt) - follow_car.update_state(lead_opt_u, dt=dt) + + # print(lead_car.history_predict_xs) + # input() # figures and animation lead_history_states = np.array(lead_car.history_xs) - follow_history_states = np.array(follow_car.history_xs) + lead_history_predict_states = lead_car.history_predict_xs """ time_history_fig = plt.figure() @@ -394,7 +457,7 @@ def main(): plt.show() """ - animdrawer = AnimDrawer([lead_history_states, follow_history_states, traj_ref]) + animdrawer = AnimDrawer([lead_history_states, lead_history_predict_states, traj_ref, history_traj_ref, history_angle_ref]) animdrawer.draw_anim() if __name__ == "__main__": diff --git a/mpc/with_disturbance/mpc_func_with_cvxopt.py b/mpc/extend/mpc_func_with_cvxopt.py similarity index 100% rename from mpc/with_disturbance/mpc_func_with_cvxopt.py rename to mpc/extend/mpc_func_with_cvxopt.py diff --git a/mpc/with_disturbance/traj_func.py b/mpc/extend/traj_func.py similarity index 85% rename from mpc/with_disturbance/traj_func.py rename to mpc/extend/traj_func.py index 385a08c..8f5ce18 100644 --- a/mpc/with_disturbance/traj_func.py +++ b/mpc/extend/traj_func.py @@ -2,7 +2,7 @@ import numpy as np import matplotlib.pyplot as plt import math -def make_sample_traj(NUM, dt=0.01, a=5.): +def make_sample_traj(NUM, dt=0.01, a=30.): """ make sample trajectory Parameters @@ -21,10 +21,9 @@ def make_sample_traj(NUM, dt=0.01, a=5.): traj_ys = [] for i in range(NUM): - traj_xs.append(dt * i) + traj_xs.append(i * 0.1) traj_ys.append(a * math.sin(dt * i / DELAY)) - plt.plot(traj_xs, traj_ys) plt.show() diff --git a/mpc/sample/pathplanner.py b/mpc/sample/pathplanner.py deleted file mode 100644 index 455ddb4..0000000 --- a/mpc/sample/pathplanner.py +++ /dev/null @@ -1,233 +0,0 @@ -""" -Cubic spline planner -Author: Atsushi Sakai(@Atsushi_twi) -""" -import math -import numpy as np -import bisect - - -class Spline: - """ - Cubic Spline class - """ - - def __init__(self, x, y): - self.b, self.c, self.d, self.w = [], [], [], [] - - self.x = x - self.y = y - - self.nx = len(x) # dimension of x - h = np.diff(x) - - # calc coefficient c - self.a = [iy for iy in y] - - # calc coefficient c - A = self.__calc_A(h) - B = self.__calc_B(h) - self.c = np.linalg.solve(A, B) - # print(self.c1) - - # calc spline coefficient b and d - for i in range(self.nx - 1): - self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i])) - tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \ - (self.c[i + 1] + 2.0 * self.c[i]) / 3.0 - self.b.append(tb) - - def calc(self, t): - """ - Calc position - if t is outside of the input x, return None - """ - - if t < self.x[0]: - return None - elif t > self.x[-1]: - return None - - i = self.__search_index(t) - dx = t - self.x[i] - result = self.a[i] + self.b[i] * dx + \ - self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0 - - return result - - def calcd(self, t): - """ - Calc first derivative - if t is outside of the input x, return None - """ - - if t < self.x[0]: - return None - elif t > self.x[-1]: - return None - - i = self.__search_index(t) - dx = t - self.x[i] - result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0 - return result - - def calcdd(self, t): - """ - Calc second derivative - """ - - if t < self.x[0]: - return None - elif t > self.x[-1]: - return None - - i = self.__search_index(t) - dx = t - self.x[i] - result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx - return result - - def __search_index(self, x): - """ - search data segment index - """ - return bisect.bisect(self.x, x) - 1 - - def __calc_A(self, h): - """ - calc matrix A for spline coefficient c - """ - A = np.zeros((self.nx, self.nx)) - A[0, 0] = 1.0 - for i in range(self.nx - 1): - if i != (self.nx - 2): - A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1]) - A[i + 1, i] = h[i] - A[i, i + 1] = h[i] - - A[0, 1] = 0.0 - A[self.nx - 1, self.nx - 2] = 0.0 - A[self.nx - 1, self.nx - 1] = 1.0 - # print(A) - return A - - def __calc_B(self, h): - """ - calc matrix B for spline coefficient c - """ - B = np.zeros(self.nx) - for i in range(self.nx - 2): - B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \ - h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i] - return B - - -class Spline2D: - """ - 2D Cubic Spline class - """ - - def __init__(self, x, y): - self.s = self.__calc_s(x, y) - self.sx = Spline(self.s, x) - self.sy = Spline(self.s, y) - - def __calc_s(self, x, y): - dx = np.diff(x) - dy = np.diff(y) - self.ds = [math.sqrt(idx ** 2 + idy ** 2) - for (idx, idy) in zip(dx, dy)] - s = [0] - s.extend(np.cumsum(self.ds)) - return s - - def calc_position(self, s): - """ - calc position - """ - x = self.sx.calc(s) - y = self.sy.calc(s) - - return x, y - - def calc_curvature(self, s): - """ - calc curvature - """ - dx = self.sx.calcd(s) - ddx = self.sx.calcdd(s) - dy = self.sy.calcd(s) - ddy = self.sy.calcdd(s) - k = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) - return k - - def calc_yaw(self, s): - """ - calc yaw - """ - dx = self.sx.calcd(s) - dy = self.sy.calcd(s) - yaw = math.atan2(dy, dx) - return yaw - - -def calc_spline_course(x, y, ds=0.1): - sp = Spline2D(x, y) - s = list(np.arange(0, sp.s[-1], ds)) - - rx, ry, ryaw, rk = [], [], [], [] - for i_s in s: - ix, iy = sp.calc_position(i_s) - rx.append(ix) - ry.append(iy) - ryaw.append(sp.calc_yaw(i_s)) - rk.append(sp.calc_curvature(i_s)) - - return rx, ry, ryaw, rk, s - - -def main(): - print("Spline 2D test") - import matplotlib.pyplot as plt - x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] - y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] - ds = 0.1 # [m] distance of each intepolated points - - sp = Spline2D(x, y) - s = np.arange(0, sp.s[-1], ds) - - rx, ry, ryaw, rk = [], [], [], [] - for i_s in s: - ix, iy = sp.calc_position(i_s) - rx.append(ix) - ry.append(iy) - ryaw.append(sp.calc_yaw(i_s)) - rk.append(sp.calc_curvature(i_s)) - - plt.subplots(1) - plt.plot(x, y, "xb", label="input") - plt.plot(rx, ry, "-r", label="spline") - plt.grid(True) - plt.axis("equal") - plt.xlabel("x[m]") - plt.ylabel("y[m]") - plt.legend() - - plt.subplots(1) - plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw") - plt.grid(True) - plt.legend() - plt.xlabel("line length[m]") - plt.ylabel("yaw angle[deg]") - - plt.subplots(1) - plt.plot(s, rk, "-r", label="curvature") - plt.grid(True) - plt.legend() - plt.xlabel("line length[m]") - plt.ylabel("curvature [1/m]") - - plt.show() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/mpc/sample/test.py b/mpc/sample/test.py deleted file mode 100644 index 5328002..0000000 --- a/mpc/sample/test.py +++ /dev/null @@ -1,614 +0,0 @@ -""" -Path tracking simulation with iterative linear model predictive control for speed and steer control -author: Atsushi Sakai (@Atsushi_twi) -""" -import matplotlib.pyplot as plt -import cvxpy -import math -import numpy as np -import sys - -try: - import pathplanner -except: - raise - - -NX = 4 # x = x, y, v, yaw -NU = 2 # a = [accel, steer] -T = 5 # horizon length - -# mpc parameters -R = np.diag([0.01, 0.01]) # input cost matrix -Rd = np.diag([0.01, 1.0]) # input difference cost matrix -Q = np.diag([1.0, 1.0, 0.5, 0.5]) # state cost matrix -Qf = Q # state final matrix -GOAL_DIS = 1.5 # goal distance -STOP_SPEED = 0.5 / 3.6 # stop speed -MAX_TIME = 500.0 # max simulation time - -# iterative paramter -MAX_ITER = 3 # Max iteration -DU_TH = 0.1 # iteration finish param - -TARGET_SPEED = 10.0 / 3.6 # [m/s] target speed -N_IND_SEARCH = 10 # Search index number - -DT = 0.2 # [s] time tick - -# Vehicle parameters -LENGTH = 4.5 # [m] -WIDTH = 2.0 # [m] -BACKTOWHEEL = 1.0 # [m] -WHEEL_LEN = 0.3 # [m] -WHEEL_WIDTH = 0.2 # [m] -TREAD = 0.7 # [m] -WB = 2.5 # [m] - -MAX_STEER = np.deg2rad(45.0) # maximum steering angle [rad] -MAX_DSTEER = np.deg2rad(30.0) # maximum steering speed [rad/s] -MAX_SPEED = 55.0 / 3.6 # maximum speed [m/s] -MIN_SPEED = -20.0 / 3.6 # minimum speed [m/s] -MAX_ACCEL = 1.0 # maximum accel [m/ss] - -show_animation = True - - -class State: - """ - vehicle state class - """ - - def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0): - self.x = x - self.y = y - self.yaw = yaw - self.v = v - self.predelta = None - - -def pi_2_pi(angle): - while(angle > math.pi): - angle = angle - 2.0 * math.pi - - while(angle < -math.pi): - angle = angle + 2.0 * math.pi - - return angle - - -def get_linear_model_matrix(v, phi, delta): - - A = np.zeros((NX, NX)) - A[0, 0] = 1.0 - A[1, 1] = 1.0 - A[2, 2] = 1.0 - A[3, 3] = 1.0 - A[0, 2] = DT * math.cos(phi) - A[0, 3] = - DT * v * math.sin(phi) - A[1, 2] = DT * math.sin(phi) - A[1, 3] = DT * v * math.cos(phi) - A[3, 2] = DT * math.tan(delta) / WB - - B = np.zeros((NX, NU)) - B[2, 0] = DT - B[3, 1] = DT * v / (WB * math.cos(delta) ** 2) - - C = np.zeros(NX) - C[0] = DT * v * math.sin(phi) * phi - C[1] = - DT * v * math.cos(phi) * phi - C[3] = - v * delta / (WB * math.cos(delta) ** 2) - - return A, B, C - - -def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"): # pragma: no cover - - outline = np.array([[-BACKTOWHEEL, (LENGTH - BACKTOWHEEL), (LENGTH - BACKTOWHEEL), -BACKTOWHEEL, -BACKTOWHEEL], - [WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]]) - - fr_wheel = np.array([[WHEEL_LEN, -WHEEL_LEN, -WHEEL_LEN, WHEEL_LEN, WHEEL_LEN], - [-WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD, WHEEL_WIDTH - TREAD, WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD]]) - - rr_wheel = np.copy(fr_wheel) - - fl_wheel = np.copy(fr_wheel) - fl_wheel[1, :] *= -1 - rl_wheel = np.copy(rr_wheel) - rl_wheel[1, :] *= -1 - - Rot1 = np.array([[math.cos(yaw), math.sin(yaw)], - [-math.sin(yaw), math.cos(yaw)]]) - Rot2 = np.array([[math.cos(steer), math.sin(steer)], - [-math.sin(steer), math.cos(steer)]]) - - fr_wheel = (fr_wheel.T.dot(Rot2)).T - fl_wheel = (fl_wheel.T.dot(Rot2)).T - fr_wheel[0, :] += WB - fl_wheel[0, :] += WB - - fr_wheel = (fr_wheel.T.dot(Rot1)).T - fl_wheel = (fl_wheel.T.dot(Rot1)).T - - outline = (outline.T.dot(Rot1)).T - rr_wheel = (rr_wheel.T.dot(Rot1)).T - rl_wheel = (rl_wheel.T.dot(Rot1)).T - - outline[0, :] += x - outline[1, :] += y - fr_wheel[0, :] += x - fr_wheel[1, :] += y - rr_wheel[0, :] += x - rr_wheel[1, :] += y - fl_wheel[0, :] += x - fl_wheel[1, :] += y - rl_wheel[0, :] += x - rl_wheel[1, :] += y - - plt.plot(np.array(outline[0, :]).flatten(), - np.array(outline[1, :]).flatten(), truckcolor) - plt.plot(np.array(fr_wheel[0, :]).flatten(), - np.array(fr_wheel[1, :]).flatten(), truckcolor) - plt.plot(np.array(rr_wheel[0, :]).flatten(), - np.array(rr_wheel[1, :]).flatten(), truckcolor) - plt.plot(np.array(fl_wheel[0, :]).flatten(), - np.array(fl_wheel[1, :]).flatten(), truckcolor) - plt.plot(np.array(rl_wheel[0, :]).flatten(), - np.array(rl_wheel[1, :]).flatten(), truckcolor) - plt.plot(x, y, "*") - - -def update_state(state, a, delta): - - # input check - if delta >= MAX_STEER: - delta = MAX_STEER - elif delta <= -MAX_STEER: - delta = -MAX_STEER - - state.x = state.x + state.v * math.cos(state.yaw) * DT - state.y = state.y + state.v * math.sin(state.yaw) * DT - state.yaw = state.yaw + state.v / WB * math.tan(delta) * DT - state.v = state.v + a * DT - - if state. v > MAX_SPEED: - state.v = MAX_SPEED - elif state. v < MIN_SPEED: - state.v = MIN_SPEED - - return state - - -def get_nparray_from_matrix(x): - return np.array(x).flatten() - - -def calc_nearest_index(state, cx, cy, cyaw, pind): - - dx = [state.x - icx for icx in cx[pind:(pind + N_IND_SEARCH)]] - dy = [state.y - icy for icy in cy[pind:(pind + N_IND_SEARCH)]] - - d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)] - - mind = min(d) - - ind = d.index(mind) + pind - - mind = math.sqrt(mind) - - dxl = cx[ind] - state.x - dyl = cy[ind] - state.y - - angle = pi_2_pi(cyaw[ind] - math.atan2(dyl, dxl)) - if angle < 0: - mind *= -1 - - return ind, mind - - -def predict_motion(x0, oa, od, xref): - xbar = xref * 0.0 - for i, _ in enumerate(x0): - xbar[i, 0] = x0[i] - - state = State(x=x0[0], y=x0[1], yaw=x0[3], v=x0[2]) - for (ai, di, i) in zip(oa, od, range(1, T + 1)): - state = update_state(state, ai, di) - xbar[0, i] = state.x - xbar[1, i] = state.y - xbar[2, i] = state.v - xbar[3, i] = state.yaw - - return xbar - - -def iterative_linear_mpc_control(xref, x0, dref, oa, od): - """ - MPC contorl with updating operational point iteraitvely - """ - - if oa is None or od is None: - oa = [0.0] * T - od = [0.0] * T - - for i in range(MAX_ITER): - xbar = predict_motion(x0, oa, od, xref) - poa, pod = oa[:], od[:] - oa, od, ox, oy, oyaw, ov = linear_mpc_control(xref, xbar, x0, dref) - du = sum(abs(oa - poa)) + sum(abs(od - pod)) # calc u change value - if du <= DU_TH: - break - else: - print("Iterative is max iter") - - return oa, od, ox, oy, oyaw, ov - - -def linear_mpc_control(xref, xbar, x0, dref): - """ - linear mpc control - xref: reference point - xbar: operational point - x0: initial state - dref: reference steer angle - """ - - x = cvxpy.Variable((NX, T + 1)) - u = cvxpy.Variable((NU, T)) - - cost = 0.0 - constraints = [] - - for t in range(T): - cost += cvxpy.quad_form(u[:, t], R) - - if t != 0: - cost += cvxpy.quad_form(xref[:, t] - x[:, t], Q) - - A, B, C = get_linear_model_matrix( - xbar[2, t], xbar[3, t], dref[0, t]) - constraints += [x[:, t + 1] == A * x[:, t] + B * u[:, t] + C] - - if t < (T - 1): - cost += cvxpy.quad_form(u[:, t + 1] - u[:, t], Rd) - constraints += [cvxpy.abs(u[1, t + 1] - u[1, t]) <= - MAX_DSTEER * DT] - - cost += cvxpy.quad_form(xref[:, T] - x[:, T], Qf) - - constraints += [x[:, 0] == x0] - constraints += [x[2, :] <= MAX_SPEED] - constraints += [x[2, :] >= MIN_SPEED] - constraints += [cvxpy.abs(u[0, :]) <= MAX_ACCEL] - constraints += [cvxpy.abs(u[1, :]) <= MAX_STEER] - - prob = cvxpy.Problem(cvxpy.Minimize(cost), constraints) - prob.solve(solver=cvxpy.ECOS, verbose=False) - - if prob.status == cvxpy.OPTIMAL or prob.status == cvxpy.OPTIMAL_INACCURATE: - ox = get_nparray_from_matrix(x.value[0, :]) - oy = get_nparray_from_matrix(x.value[1, :]) - ov = get_nparray_from_matrix(x.value[2, :]) - oyaw = get_nparray_from_matrix(x.value[3, :]) - oa = get_nparray_from_matrix(u.value[0, :]) - odelta = get_nparray_from_matrix(u.value[1, :]) - - else: - print("Error: Cannot solve mpc..") - oa, odelta, ox, oy, oyaw, ov = None, None, None, None, None, None - - return oa, odelta, ox, oy, oyaw, ov - - -def calc_ref_trajectory(state, cx, cy, cyaw, ck, sp, dl, pind): - xref = np.zeros((NX, T + 1)) - dref = np.zeros((1, T + 1)) - ncourse = len(cx) - - ind, _ = calc_nearest_index(state, cx, cy, cyaw, pind) - - if pind >= ind: - ind = pind - - xref[0, 0] = cx[ind] - xref[1, 0] = cy[ind] - xref[2, 0] = sp[ind] - xref[3, 0] = cyaw[ind] - dref[0, 0] = 0.0 # steer operational point should be 0 - - travel = 0.0 - - for i in range(T + 1): - travel += abs(state.v) * DT - dind = int(round(travel / dl)) - - if (ind + dind) < ncourse: - xref[0, i] = cx[ind + dind] - xref[1, i] = cy[ind + dind] - xref[2, i] = sp[ind + dind] - xref[3, i] = cyaw[ind + dind] - dref[0, i] = 0.0 - else: - xref[0, i] = cx[ncourse - 1] - xref[1, i] = cy[ncourse - 1] - xref[2, i] = sp[ncourse - 1] - xref[3, i] = cyaw[ncourse - 1] - dref[0, i] = 0.0 - - return xref, ind, dref - - -def check_goal(state, goal, tind, nind): - - # check goal - dx = state.x - goal[0] - dy = state.y - goal[1] - d = math.sqrt(dx ** 2 + dy ** 2) - - isgoal = (d <= GOAL_DIS) - - if abs(tind - nind) >= 5: - isgoal = False - - isstop = (abs(state.v) <= STOP_SPEED) - - if isgoal and isstop: - return True - - return False - - -def do_simulation(cx, cy, cyaw, ck, sp, dl, initial_state): - """ - Simulation - cx: course x position list - cy: course y position list - cy: course yaw position list - ck: course curvature list - sp: speed profile - dl: course tick [m] - """ - - goal = [cx[-1], cy[-1]] - - state = initial_state - - # initial yaw compensation - if state.yaw - cyaw[0] >= math.pi: - state.yaw -= math.pi * 2.0 - elif state.yaw - cyaw[0] <= -math.pi: - state.yaw += math.pi * 2.0 - - time = 0.0 - x = [state.x] - y = [state.y] - yaw = [state.yaw] - v = [state.v] - t = [0.0] - d = [0.0] - a = [0.0] - target_ind, _ = calc_nearest_index(state, cx, cy, cyaw, 0) - - odelta, oa = None, None - - cyaw = smooth_yaw(cyaw) - - while MAX_TIME >= time: - xref, target_ind, dref = calc_ref_trajectory( - state, cx, cy, cyaw, ck, sp, dl, target_ind) - - x0 = [state.x, state.y, state.v, state.yaw] # current state - - oa, odelta, ox, oy, oyaw, ov = iterative_linear_mpc_control( - xref, x0, dref, oa, odelta) - - if odelta is not None: - di, ai = odelta[0], oa[0] - - state = update_state(state, ai, di) - time = time + DT - - x.append(state.x) - y.append(state.y) - yaw.append(state.yaw) - v.append(state.v) - t.append(time) - d.append(di) - a.append(ai) - - if check_goal(state, goal, target_ind, len(cx)): - print("Goal") - break - - if show_animation: # pragma: no cover - plt.cla() - if ox is not None: - plt.plot(ox, oy, "xr", label="MPC") - plt.plot(cx, cy, "-r", label="course") - plt.plot(x, y, "ob", label="trajectory") - plt.plot(xref[0, :], xref[1, :], "xk", label="xref") - plt.plot(cx[target_ind], cy[target_ind], "xg", label="target") - plot_car(state.x, state.y, state.yaw, steer=di) - plt.axis("equal") - plt.grid(True) - plt.title("Time[s]:" + str(round(time, 2)) - + ", speed[km/h]:" + str(round(state.v * 3.6, 2))) - plt.pause(0.0001) - - return t, x, y, yaw, v, d, a - - -def calc_speed_profile(cx, cy, cyaw, target_speed): - - speed_profile = [target_speed] * len(cx) - direction = 1.0 # forward - - # Set stop point - for i in range(len(cx) - 1): - dx = cx[i + 1] - cx[i] - dy = cy[i + 1] - cy[i] - - move_direction = math.atan2(dy, dx) - - if dx != 0.0 and dy != 0.0: - dangle = abs(pi_2_pi(move_direction - cyaw[i])) - if dangle >= math.pi / 4.0: - direction = -1.0 - else: - direction = 1.0 - - if direction != 1.0: - speed_profile[i] = - target_speed - else: - speed_profile[i] = target_speed - - speed_profile[-1] = 0.0 - - return speed_profile - - -def smooth_yaw(yaw): - - for i in range(len(yaw) - 1): - dyaw = yaw[i + 1] - yaw[i] - - while dyaw >= math.pi / 2.0: - yaw[i + 1] -= math.pi * 2.0 - dyaw = yaw[i + 1] - yaw[i] - - while dyaw <= -math.pi / 2.0: - yaw[i + 1] += math.pi * 2.0 - dyaw = yaw[i + 1] - yaw[i] - - return yaw - - -def get_straight_course(dl): - ax = [0.0, 5.0, 10.0, 20.0, 30.0, 40.0, 50.0] - ay = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - cx, cy, cyaw, ck, s = pathplanner.calc_spline_course( - ax, ay, ds=dl) - - return cx, cy, cyaw, ck - - -def get_straight_course2(dl): - ax = [0.0, -10.0, -20.0, -40.0, -50.0, -60.0, -70.0] - ay = [0.0, -1.0, 1.0, 0.0, -1.0, 1.0, 0.0] - cx, cy, cyaw, ck, s = pathplanner.calc_spline_course( - ax, ay, ds=dl) - - return cx, cy, cyaw, ck - - -def get_straight_course3(dl): - ax = [0.0, -10.0, -20.0, -40.0, -50.0, -60.0, -70.0] - ay = [0.0, -1.0, 1.0, 0.0, -1.0, 1.0, 0.0] - cx, cy, cyaw, ck, s = pathplanner.calc_spline_course( - ax, ay, ds=dl) - - cyaw = [i - math.pi for i in cyaw] - - return cx, cy, cyaw, ck - - -def get_forward_course(dl): - ax = [0.0, 60.0, 125.0, 50.0, 75.0, 30.0, -10.0] - ay = [0.0, 0.0, 50.0, 65.0, 30.0, 50.0, -20.0] - cx, cy, cyaw, ck, s = pathplanner.calc_spline_course( - ax, ay, ds=dl) - - return cx, cy, cyaw, ck - - -def get_switch_back_course(dl): - ax = [0.0, 30.0, 6.0, 20.0, 35.0] - ay = [0.0, 0.0, 20.0, 35.0, 20.0] - cx, cy, cyaw, ck, s = pathplanner.calc_spline_course( - ax, ay, ds=dl) - ax = [35.0, 10.0, 0.0, 0.0] - ay = [20.0, 30.0, 5.0, 0.0] - cx2, cy2, cyaw2, ck2, s2 = pathplanner.calc_spline_course( - ax, ay, ds=dl) - cyaw2 = [i - math.pi for i in cyaw2] - cx.extend(cx2) - cy.extend(cy2) - cyaw.extend(cyaw2) - ck.extend(ck2) - - return cx, cy, cyaw, ck - - -def main(): - print(__file__ + " start!!") - - dl = 1.0 # course tick - # cx, cy, cyaw, ck = get_straight_course(dl) - # cx, cy, cyaw, ck = get_straight_course2(dl) - cx, cy, cyaw, ck = get_straight_course3(dl) - # cx, cy, cyaw, ck = get_forward_course(dl) - # CX, cy, cyaw, ck = get_switch_back_course(dl) - - sp = calc_speed_profile(cx, cy, cyaw, TARGET_SPEED) - - initial_state = State(x=cx[0], y=cy[0], yaw=cyaw[0], v=0.0) - - t, x, y, yaw, v, d, a = do_simulation( - cx, cy, cyaw, ck, sp, dl, initial_state) - - if show_animation: # pragma: no cover - plt.close("all") - plt.subplots() - plt.plot(cx, cy, "-r", label="spline") - plt.plot(x, y, "-g", label="tracking") - plt.grid(True) - plt.axis("equal") - plt.xlabel("x[m]") - plt.ylabel("y[m]") - plt.legend() - - plt.subplots() - plt.plot(t, v, "-r", label="speed") - plt.grid(True) - plt.xlabel("Time [s]") - plt.ylabel("Speed [kmh]") - - plt.show() - - -def main2(): - print(__file__ + " start!!") - - dl = 1.0 # course tick - cx, cy, cyaw, ck = get_straight_course3(dl) - - sp = calc_speed_profile(cx, cy, cyaw, TARGET_SPEED) - - initial_state = State(x=cx[0], y=cy[0], yaw=0.0, v=0.0) - - t, x, y, yaw, v, d, a = do_simulation( - cx, cy, cyaw, ck, sp, dl, initial_state) - - if show_animation: # pragma: no cover - plt.close("all") - plt.subplots() - plt.plot(cx, cy, "-r", label="spline") - plt.plot(x, y, "-g", label="tracking") - plt.grid(True) - plt.axis("equal") - plt.xlabel("x[m]") - plt.ylabel("y[m]") - plt.legend() - - plt.subplots() - plt.plot(t, v, "-r", label="speed") - plt.grid(True) - plt.xlabel("Time [s]") - plt.ylabel("Speed [kmh]") - - plt.show() - - -if __name__ == '__main__': - # main() - main2() \ No newline at end of file