From dcb9aa0689ec888b52202d585fa603a540847a58 Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Sun, 10 Feb 2019 02:47:13 +0900 Subject: [PATCH 01/13] add animation --- mpc/with_disturbance/animation.py | 139 ++++++++++++++++++++----- mpc/with_disturbance/func_curvature.py | 16 +++ mpc/with_disturbance/iterative_MPC.py | 51 +++++---- mpc/with_disturbance/main_track.py | 119 ++++++++++++++++----- mpc/with_disturbance/traj_func.py | 5 +- 5 files changed, 252 insertions(+), 78 deletions(-) diff --git a/mpc/with_disturbance/animation.py b/mpc/with_disturbance/animation.py index b5e4942..a085b5a 100755 --- a/mpc/with_disturbance/animation.py +++ b/mpc/with_disturbance/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/func_curvature.py b/mpc/with_disturbance/func_curvature.py index b525409..cdd99e6 100644 --- a/mpc/with_disturbance/func_curvature.py +++ b/mpc/with_disturbance/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/iterative_MPC.py b/mpc/with_disturbance/iterative_MPC.py index bffdbbd..c365aaf 100644 --- a/mpc/with_disturbance/iterative_MPC.py +++ b/mpc/with_disturbance/iterative_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/main_track.py b/mpc/with_disturbance/main_track.py index 30eb80a..9a1f062 100644 --- a/mpc/with_disturbance/main_track.py +++ b/mpc/with_disturbance/main_track.py @@ -9,7 +9,7 @@ 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([100., 1000., 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/traj_func.py b/mpc/with_disturbance/traj_func.py index 385a08c..8f5ce18 100644 --- a/mpc/with_disturbance/traj_func.py +++ b/mpc/with_disturbance/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() From fe39636d391db184075481924066809956f3b8bf Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Sun, 10 Feb 2019 22:52:24 +0900 Subject: [PATCH 02/13] add readme --- mpc/extend/README.md | 37 ++ mpc/{with_disturbance => extend}/animation.py | 0 .../coordinate_trans.py | 0 .../func_curvature.py | 0 .../iterative_MPC.py | 0 .../main_track.py | 0 .../mpc_func_with_cvxopt.py | 0 mpc/{with_disturbance => extend}/traj_func.py | 0 mpc/sample/pathplanner.py | 233 ------- mpc/sample/test.py | 614 ------------------ 10 files changed, 37 insertions(+), 847 deletions(-) create mode 100644 mpc/extend/README.md rename mpc/{with_disturbance => extend}/animation.py (100%) rename mpc/{with_disturbance => extend}/coordinate_trans.py (100%) rename mpc/{with_disturbance => extend}/func_curvature.py (100%) rename mpc/{with_disturbance => extend}/iterative_MPC.py (100%) rename mpc/{with_disturbance => extend}/main_track.py (100%) rename mpc/{with_disturbance => extend}/mpc_func_with_cvxopt.py (100%) rename mpc/{with_disturbance => extend}/traj_func.py (100%) delete mode 100644 mpc/sample/pathplanner.py delete mode 100644 mpc/sample/test.py diff --git a/mpc/extend/README.md b/mpc/extend/README.md new file mode 100644 index 0000000..2dec8ef --- /dev/null +++ b/mpc/extend/README.md @@ -0,0 +1,37 @@ +# 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 100% rename from mpc/with_disturbance/animation.py rename to mpc/extend/animation.py 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/func_curvature.py b/mpc/extend/func_curvature.py similarity index 100% rename from mpc/with_disturbance/func_curvature.py rename to mpc/extend/func_curvature.py diff --git a/mpc/with_disturbance/iterative_MPC.py b/mpc/extend/iterative_MPC.py similarity index 100% rename from mpc/with_disturbance/iterative_MPC.py rename to mpc/extend/iterative_MPC.py diff --git a/mpc/with_disturbance/main_track.py b/mpc/extend/main_track.py similarity index 100% rename from mpc/with_disturbance/main_track.py rename to mpc/extend/main_track.py 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 100% rename from mpc/with_disturbance/traj_func.py rename to mpc/extend/traj_func.py 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 From c49120ea9661e570ae3c343a00d4e8956099a885 Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Sun, 10 Feb 2019 22:55:16 +0900 Subject: [PATCH 03/13] add readme --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 32877be..5f9b841 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 Lineat Model Predictive Control for vehicle model** # Usage and Details you can see the usage in each folder From 8c21c44cbce7d4e6209c8a59bdef9614c31531ff Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Sun, 10 Feb 2019 23:05:01 +0900 Subject: [PATCH 04/13] fix readme --- mpc/extend/README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/mpc/extend/README.md b/mpc/extend/README.md index 2dec8ef..5f2369d 100644 --- a/mpc/extend/README.md +++ b/mpc/extend/README.md @@ -27,6 +27,10 @@ I implemented this, so if you know about the detail please go to "IteraticeMPC_f # Expected Results + + + + # Usage ``` From d4a1d37e02630d499f9f87273391c074d66863f8 Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Sun, 10 Feb 2019 23:06:10 +0900 Subject: [PATCH 05/13] fix readme --- mpc/extend/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mpc/extend/README.md b/mpc/extend/README.md index 5f2369d..e926aa5 100644 --- a/mpc/extend/README.md +++ b/mpc/extend/README.md @@ -29,7 +29,7 @@ I implemented this, so if you know about the detail please go to "IteraticeMPC_f - + # Usage From ceb6d4fc596561e55127f4fb4b8344653e2a85f3 Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Sun, 10 Feb 2019 23:08:19 +0900 Subject: [PATCH 06/13] fix readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 5f9b841..6e97399 100644 --- a/README.md +++ b/README.md @@ -8,7 +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 Lineat Model Predictive Control for vehicle model** +- **Extended Linear Model Predictive Control for vehicle model** # Usage and Details you can see the usage in each folder From 24a2568de014e8d77a02e7e09372c27a2706408f Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Wed, 13 Feb 2019 18:06:50 +0900 Subject: [PATCH 07/13] add memo and IOC --- mpc/basic_IOC/README.md | 146 ++++++++++++++ mpc/basic_IOC/animation.py | 233 +++++++++++++++++++++++ mpc/basic_IOC/main_ACC.py | 246 ++++++++++++++++++++++++ mpc/basic_IOC/main_ACC_TEMP.py | 243 ++++++++++++++++++++++++ mpc/basic_IOC/main_example.py | 184 ++++++++++++++++++ mpc/basic_IOC/memo.md | 5 + mpc/basic_IOC/mpc_func_with_cvxopt.py | 256 +++++++++++++++++++++++++ mpc/basic_IOC/mpc_func_with_scipy.py | 262 ++++++++++++++++++++++++++ mpc/basic_IOC/test_compare_methods.py | 211 +++++++++++++++++++++ mpc/extend/main_track.py | 2 +- 10 files changed, 1787 insertions(+), 1 deletion(-) create mode 100644 mpc/basic_IOC/README.md create mode 100755 mpc/basic_IOC/animation.py create mode 100644 mpc/basic_IOC/main_ACC.py create mode 100644 mpc/basic_IOC/main_ACC_TEMP.py create mode 100644 mpc/basic_IOC/main_example.py create mode 100644 mpc/basic_IOC/memo.md create mode 100644 mpc/basic_IOC/mpc_func_with_cvxopt.py create mode 100644 mpc/basic_IOC/mpc_func_with_scipy.py create mode 100644 mpc/basic_IOC/test_compare_methods.py diff --git a/mpc/basic_IOC/README.md b/mpc/basic_IOC/README.md new file mode 100644 index 0000000..0d64664 --- /dev/null +++ b/mpc/basic_IOC/README.md @@ -0,0 +1,146 @@ +# Model Predictive Control Basic Tool +This program is about template, generic function of linear model predictive control + +# Documentation of the MPC function +Linear model predicitive control should have state equation. +So if you want to use this function, you should model the plant as state equation. +Therefore, the parameters of this class are as following + +**class MpcController()** + +Attributes : + +- A : numpy.ndarray + - system matrix +- B : numpy.ndarray + - input matrix +- Q : numpy.ndarray + - evaluation function weight for states +- Qs : numpy.ndarray + - concatenated evaluation function weight for states +- R : numpy.ndarray + - evaluation function weight for inputs +- Rs : numpy.ndarray + - concatenated evaluation function weight for inputs +- pre_step : int + - prediction step +- state_size : int + - state size of the plant +- input_size : int + - input size of the plant +- dt_input_upper : numpy.ndarray, shape(input_size, ), optional + - constraints of input dt, default is None +- dt_input_lower : numpy.ndarray, shape(input_size, ), optional + - constraints of input dt, default is None +- input_upper : numpy.ndarray, shape(input_size, ), optional + - constraints of input, default is None +- input_lower : numpy.ndarray, shape(input_size, ), optional + - constraints of input, default is None + +Methods: + +- initialize_controller() initialize the controller +- calc_input(states, references) calculating optimal input + +More details, please look the **mpc_func_with_scipy.py** and **mpc_func_with_cvxopt.py** + +We have two function, mpc_func_with_cvxopt.py and mpc_func_with_scipy.py +Both functions have same variable and member function. However the solver is different. +Plese choose the right method for your environment. + +- example of import + +```py +from mpc_func_with_scipy import MpcController as MpcController_scipy +from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt +``` + +# Examples +## Problem Formulation + +- **first order system** + + + +- **ACC (Adaptive cruise control)** + +The two wheeled model are expressed the following equation. + + + +However, if we assume the velocity are constant, we can approximate the equation as following, + + + +then we can apply this model to linear mpc, we should give the model reference V although. + +- **evaluation function** + +the both examples have same evaluation function form as following equation. + + + +- is predicit state by using predict input + +- is reference state + +- is predict amount of change of input + +- are evaluation function weights + +## Expected Results + +- first order system + +- time history + + + +- input + + + +- ACC (Adaptive cruise control) + +- time history of states + + + +- animation + + + + +# Usage + +- for example(first order system) + +``` +$ python main_example.py +``` + +- for example(ACC (Adaptive cruise control)) + +``` +$ python main_ACC.py +``` + +- for comparing two methods of optimization solvers + +``` +$ python test_compare_methods.py +``` + +# Requirement + +- python3.5 or more +- numpy +- matplotlib +- cvxopt +- scipy1.2.0 or more +- python-control + +# Reference +I`m sorry that main references are written in Japanese + +- モデル予測制御―制約のもとでの最適制御 著：Jan M. Maciejowski　訳：足立修一　東京電機大学出版局 \ No newline at end of file diff --git a/mpc/basic_IOC/animation.py b/mpc/basic_IOC/animation.py new file mode 100755 index 0000000..6ece541 --- /dev/null +++ b/mpc/basic_IOC/animation.py @@ -0,0 +1,233 @@ +import numpy as np +import matplotlib.pyplot as plt +import matplotlib.animation as ani +import matplotlib.font_manager as fon +import sys +import math + +# default setting of figures +plt.rcParams["mathtext.fontset"] = 'stix' # math fonts +plt.rcParams['xtick.direction'] = 'in' # x axis in +plt.rcParams['ytick.direction'] = 'in' # y axis in +plt.rcParams["font.size"] = 10 +plt.rcParams['axes.linewidth'] = 1.0 # axis line width +plt.rcParams['axes.grid'] = True # make grid + +def coordinate_transformation_in_angle(positions, base_angle): + ''' + Transformation the coordinate in the angle + + Parameters + ------- + positions : numpy.ndarray + this parameter is composed of xs, ys + should have (2, N) shape + base_angle : float [rad] + + Returns + ------- + traslated_positions : numpy.ndarray + the shape is (2, N) + + ''' + if positions.shape[0] != 2: + raise ValueError('the input data should have (2, N)') + + positions = np.array(positions) + positions = positions.reshape(2, -1) + + rot_matrix = [[np.cos(base_angle), np.sin(base_angle)], + [-1*np.sin(base_angle), np.cos(base_angle)]] + + rot_matrix = np.array(rot_matrix) + + translated_positions = np.dot(rot_matrix, positions) + + return translated_positions + +def square_make_with_angles(center_x, center_y, size, angle): + ''' + Create square matrix with angle line matrix(2D) + + Parameters + ------- + center_x : float in meters + the center x position of the square + center_y : float in meters + the center y position of the square + size : float in meters + the square's half-size + angle : float in radians + + Returns + ------- + square xs : numpy.ndarray + lenght is 5 (counterclockwise from right-up) + square ys : numpy.ndarray + length is 5 (counterclockwise from right-up) + angle line xs : numpy.ndarray + angle line ys : numpy.ndarray + ''' + + # start with the up right points + # create point in counterclockwise + square_xys = np.array([[size, 0.5 * size], [-size, 0.5 * size], [-size, -0.5 * size], [size, -0.5 * size], [size, 0.5 * size]]) + trans_points = coordinate_transformation_in_angle(square_xys.T, -angle) # this is inverse type + trans_points += np.array([[center_x], [center_y]]) + + square_xs = trans_points[0, :] + square_ys = trans_points[1, :] + + angle_line_xs = [center_x, center_x + math.cos(angle) * size] + angle_line_ys = [center_y, center_y + math.sin(angle) * size] + + return square_xs, square_ys, np.array(angle_line_xs), np.array(angle_line_ys) + + +class AnimDrawer(): + """create animation of path and robot + + Attributes + ------------ + cars : + anim_fig : figure of matplotlib + axis : axis of matplotlib + + """ + def __init__(self, objects): + """ + Parameters + ------------ + objects : list of objects + """ + self.lead_car_history_state = objects[0] + self.follow_car_history_state = objects[1] + + 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]] + + # setting up figure + self.anim_fig = plt.figure(dpi=150) + self.axis = self.anim_fig.add_subplot(111) + + # imgs + self.object_imgs = [] + self.traj_imgs = [] + + def draw_anim(self, interval=50): + """draw the animation and save + + Parameteres + ------------- + interval : int, optional + animation's interval time, you should link the sampling time of systems + default is 50 [ms] + """ + self._set_axis() + self._set_img() + + self.skip_num = 1 + frame_num = int((len(self.history_xs[0])-1) / self.skip_num) + + animation = ani.FuncAnimation(self.anim_fig, self._update_anim, interval=interval, frames=frame_num) + + # self.axis.legend() + print('save_animation?') + shuold_save_animation = int(input()) + + if shuold_save_animation: + print('animation_number?') + num = int(input()) + animation.save('animation_{0}.mp4'.format(num), writer='ffmpeg') + # animation.save("Sample.gif", writer = 'imagemagick') # gif保存 + + plt.show() + + def _set_axis(self): + """ initialize the animation axies + """ + # (1) set the axis name + self.axis.set_xlabel(r'$\it{x}$ [m]') + 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(-5, 50) + self.axis.set_ylim(-2, 5) + + def _set_img(self): + """ initialize the imgs of animation + this private function execute the make initial imgs for animation + """ + # object imgs + obj_color_list = ["k", "k", "m", "m"] + obj_styles = ["solid", "solid", "solid", "solid"] + + 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) + + traj_color_list = ["k", "m"] + + 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) + + def _update_anim(self, i): + """the update animation + this function should be used in the animation functions + + Parameters + ------------ + i : int + time step of the animation + the sampling time should be related to the sampling time of system + + Returns + ----------- + object_imgs : list of img + traj_imgs : list of img + """ + i = int(i * self.skip_num) + + self._draw_objects(i) + self._draw_traj(i) + + return self.object_imgs, self.traj_imgs, + + def _draw_objects(self, i): + """ + This private function is just divided thing of + the _update_anim to see the code more clear + + Parameters + ------------ + i : int + time step of the animation + 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]) + + 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 + the _update_anim to see the code more clear + + Parameters + ------------ + i : int + 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]) \ No newline at end of file diff --git a/mpc/basic_IOC/main_ACC.py b/mpc/basic_IOC/main_ACC.py new file mode 100644 index 0000000..a868559 --- /dev/null +++ b/mpc/basic_IOC/main_ACC.py @@ -0,0 +1,246 @@ +import numpy as np +import matplotlib.pyplot as plt +import math +import copy + +from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt +from animation import AnimDrawer +from control import matlab + +class TwoWheeledSystem(): + """SampleSystem, this is the simulator + Attributes + ----------- + xs : numpy.ndarray + system states, [x, y, theta] + history_xs : list + time history of state + """ + def __init__(self, init_states=None): + """ + Palameters + ----------- + init_state : float, optional, shape(3, ) + initial state of system default is None + """ + self.xs = np.zeros(3) + + if init_states is not None: + self.xs = copy.deepcopy(init_states) + + self.history_xs = [init_states] + + def update_state(self, us, dt=0.01): + """ + Palameters + ------------ + u : numpy.ndarray + inputs of system in some cases this means the reference + 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(3)] + k1 = [0.0 for _ in range(3)] + k2 = [0.0 for _ in range(3)] + k3 = [0.0 for _ in range(3)] + + functions = [self._func_x_1, self._func_x_2, self._func_x_3] + + # solve Runge-Kutta + for i, func in enumerate(functions): + k0[i] = dt * func(self.xs[0], self.xs[1], self.xs[2], us[0], us[1]) + + for i, func in enumerate(functions): + k1[i] = dt * func(self.xs[0] + k0[0]/2., self.xs[1] + k0[1]/2., self.xs[2] + k0[2]/2., us[0], us[1]) + + for i, func in enumerate(functions): + k2[i] = dt * func(self.xs[0] + k0[0]/2., self.xs[1] + k0[1]/2., self.xs[2] + k0[2]/2., us[0], us[1]) + + for i, func in enumerate(functions): + k3[i] = dt * func(self.xs[0] + k2[0], self.xs[1] + k2[1], self.xs[2] + k2[2], us[0], us[1]) + + self.xs[0] += (k0[0] + 2. * k1[0] + 2. * k2[0] + k3[0]) / 6. + self.xs[1] += (k0[1] + 2. * k1[1] + 2. * k2[1] + k3[1]) / 6. + self.xs[2] += (k0[2] + 2. * k1[2] + 2. * k2[2] + k3[2]) / 6. + + # save + save_states = copy.deepcopy(self.xs) + self.history_xs.append(save_states) + print(self.xs) + + def _func_x_1(self, y_1, y_2, y_3, u_1, u_2): + """ + Parameters + ------------ + y_1 : float + y_2 : float + y_3 : float + u_1 : float + system input + u_2 : float + system input + """ + y_dot = math.cos(y_3) * u_1 + return y_dot + + def _func_x_2(self, y_1, y_2, y_3, u_1, u_2): + """ + Parameters + ------------ + y_1 : float + y_2 : float + y_3 : float + u_1 : float + system input + u_2 : float + system input + """ + y_dot = math.sin(y_3) * u_1 + return y_dot + + def _func_x_3(self, y_1, y_2, y_3, u_1, u_2): + """ + Parameters + ------------ + y_1 : float + y_2 : float + y_3 : float + u_1 : float + system input + u_2 : float + system input + """ + y_dot = u_2 + return y_dot + +def main(): + dt = 0.05 + simulation_time = 10 # in seconds + iteration_num = int(simulation_time / dt) + + # you must be care about this matrix + # these A and B are for continuos system if you want to use discret system matrix please skip this step + # lineared car system + V = 5.0 + A = np.array([[0., V], [0., 0.]]) + B = np.array([[0.], [1.]]) + + C = np.eye(2) + D = np.zeros((2, 1)) + + # make simulator with coninuous matrix + init_xs_lead = np.array([5., 0., 0.]) + init_xs_follow = np.array([0., 0., 0.]) + lead_car = TwoWheeledSystem(init_states=init_xs_lead) + follow_car = TwoWheeledSystem(init_states=init_xs_follow) + + # create system + sysc = matlab.ss(A, B, C, D) + # discrete system + sysd = matlab.c2d(sysc, dt) + + Ad = sysd.A + Bd = sysd.B + + # evaluation function weight + Q = np.diag([1., 1.]) + R = np.diag([5.]) + pre_step = 15 + + # make controller with discreted matrix + # please check the solver, if you want to use the scipy, set the MpcController_scipy + lead_controller = MpcController_cvxopt(Ad, Bd, Q, R, pre_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.])) + + follow_controller = MpcController_cvxopt(Ad, Bd, Q, R, pre_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.])) + + lead_controller.initialize_controller() + follow_controller.initialize_controller() + + # reference + lead_reference = np.array([[0., 0.] for _ in range(pre_step)]).flatten() + + for i in range(iteration_num): + print("simulation time = {0}".format(i)) + # make lead car's move + if i > int(iteration_num / 3): + lead_reference = np.array([[4., 0.] for _ in range(pre_step)]).flatten() + + lead_states = lead_car.xs + lead_opt_u = lead_controller.calc_input(lead_states[1:], lead_reference) + lead_opt_u = np.hstack((np.array([V]), lead_opt_u)) + + # make follow car + follow_reference = np.array([lead_states[1:] for _ in range(pre_step)]).flatten() + follow_states = follow_car.xs + + follow_opt_u = follow_controller.calc_input(follow_states[1:], follow_reference) + follow_opt_u = np.hstack((np.array([V]), follow_opt_u)) + + lead_car.update_state(lead_opt_u, dt=dt) + follow_car.update_state(follow_opt_u, dt=dt) + + # figures and animation + lead_history_states = np.array(lead_car.history_xs) + follow_history_states = np.array(follow_car.history_xs) + + time_history_fig = plt.figure() + x_fig = time_history_fig.add_subplot(311) + y_fig = time_history_fig.add_subplot(312) + theta_fig = time_history_fig.add_subplot(313) + + car_traj_fig = plt.figure() + traj_fig = car_traj_fig.add_subplot(111) + traj_fig.set_aspect('equal') + + x_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_states[:, 0], label="lead") + x_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_states[:, 0], label="follow") + x_fig.set_xlabel("time [s]") + x_fig.set_ylabel("x") + x_fig.legend() + + y_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_states[:, 1], label="lead") + y_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_states[:, 1], label="follow") + y_fig.plot(np.arange(0, simulation_time+0.01, dt), [4. for _ in range(iteration_num+1)], linestyle="dashed") + y_fig.set_xlabel("time [s]") + y_fig.set_ylabel("y") + y_fig.legend() + + theta_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_states[:, 2], label="lead") + theta_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_states[:, 2], label="follow") + theta_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed") + theta_fig.set_xlabel("time [s]") + theta_fig.set_ylabel("theta") + theta_fig.legend() + + time_history_fig.tight_layout() + + traj_fig.plot(lead_history_states[:, 0], lead_history_states[:, 1], label="lead") + traj_fig.plot(follow_history_states[:, 0], follow_history_states[:, 1], label="follow") + traj_fig.set_xlabel("x") + traj_fig.set_ylabel("y") + traj_fig.legend() + plt.show() + + lead_history_us = np.array(lead_controller.history_us) + follow_history_us = np.array(follow_controller.history_us) + input_history_fig = plt.figure() + u_1_fig = input_history_fig.add_subplot(111) + + u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_us[:, 0], label="lead") + u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_us[:, 0], label="follow") + u_1_fig.set_xlabel("time [s]") + u_1_fig.set_ylabel("u_omega") + + input_history_fig.tight_layout() + plt.show() + + animdrawer = AnimDrawer([lead_history_states, follow_history_states]) + animdrawer.draw_anim() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/mpc/basic_IOC/main_ACC_TEMP.py b/mpc/basic_IOC/main_ACC_TEMP.py new file mode 100644 index 0000000..9ad7c97 --- /dev/null +++ b/mpc/basic_IOC/main_ACC_TEMP.py @@ -0,0 +1,243 @@ +import numpy as np +import matplotlib.pyplot as plt +import math +import copy + +from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt +from animation import AnimDrawer +# from control import matlab + +class TwoWheeledSystem(): + """SampleSystem, this is the simulator + Attributes + ----------- + xs : numpy.ndarray + system states, [x, y, theta] + history_xs : list + time history of state + """ + def __init__(self, init_states=None): + """ + Palameters + ----------- + init_state : float, optional, shape(3, ) + initial state of system default is None + """ + self.xs = np.zeros(3) + + if init_states is not None: + self.xs = copy.deepcopy(init_states) + + self.history_xs = [init_states] + + def update_state(self, us, dt=0.01): + """ + Palameters + ------------ + u : numpy.ndarray + inputs of system in some cases this means the reference + 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(3)] + k1 = [0.0 for _ in range(3)] + k2 = [0.0 for _ in range(3)] + k3 = [0.0 for _ in range(3)] + + functions = [self._func_x_1, self._func_x_2, self._func_x_3] + + # solve Runge-Kutta + for i, func in enumerate(functions): + k0[i] = dt * func(self.xs[0], self.xs[1], self.xs[2], us[0], us[1]) + + for i, func in enumerate(functions): + k1[i] = dt * func(self.xs[0] + k0[0]/2., self.xs[1] + k0[1]/2., self.xs[2] + k0[2]/2., us[0], us[1]) + + for i, func in enumerate(functions): + k2[i] = dt * func(self.xs[0] + k0[0]/2., self.xs[1] + k0[1]/2., self.xs[2] + k0[2]/2., us[0], us[1]) + + for i, func in enumerate(functions): + k3[i] = dt * func(self.xs[0] + k2[0], self.xs[1] + k2[1], self.xs[2] + k2[2], us[0], us[1]) + + self.xs[0] += (k0[0] + 2. * k1[0] + 2. * k2[0] + k3[0]) / 6. + self.xs[1] += (k0[1] + 2. * k1[1] + 2. * k2[1] + k3[1]) / 6. + self.xs[2] += (k0[2] + 2. * k1[2] + 2. * k2[2] + k3[2]) / 6. + + # save + save_states = copy.deepcopy(self.xs) + self.history_xs.append(save_states) + print(self.xs) + + def _func_x_1(self, y_1, y_2, y_3, u_1, u_2): + """ + Parameters + ------------ + y_1 : float + y_2 : float + y_3 : float + u_1 : float + system input + u_2 : float + system input + """ + y_dot = math.cos(y_3) * u_1 + return y_dot + + def _func_x_2(self, y_1, y_2, y_3, u_1, u_2): + """ + Parameters + ------------ + y_1 : float + y_2 : float + y_3 : float + u_1 : float + system input + u_2 : float + system input + """ + y_dot = math.sin(y_3) * u_1 + return y_dot + + def _func_x_3(self, y_1, y_2, y_3, u_1, u_2): + """ + Parameters + ------------ + y_1 : float + y_2 : float + y_3 : float + u_1 : float + system input + u_2 : float + system input + """ + y_dot = u_2 + return y_dot + +def main(): + dt = 0.05 + simulation_time = 10 # in seconds + iteration_num = int(simulation_time / dt) + + # you must be care about this matrix + # these A and B are for continuos system if you want to use discret system matrix please skip this step + # lineared car system + V = 5.0 + Ad = np.array([[1., V * dt], [0., 1.]]) + Bd = np.array([[0.], [1. * dt]]) + + C = np.eye(2) + D = np.zeros((2, 1)) + + # make simulator with coninuous matrix + init_xs_lead = np.array([5., 0., 0.]) + init_xs_follow = np.array([0., 0., 0.]) + lead_car = TwoWheeledSystem(init_states=init_xs_lead) + follow_car = TwoWheeledSystem(init_states=init_xs_follow) + + # create system + # sysc = matlab.ss(A, B, C, D) + # discrete system + # sysd = matlab.c2d(sysc, dt) + + # evaluation function weight + Q = np.diag([1., 1.]) + R = np.diag([5.]) + pre_step = 15 + + # make controller with discreted matrix + # please check the solver, if you want to use the scipy, set the MpcController_scipy + lead_controller = MpcController_cvxopt(Ad, Bd, Q, R, pre_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.])) + + follow_controller = MpcController_cvxopt(Ad, Bd, Q, R, pre_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.])) + + lead_controller.initialize_controller() + follow_controller.initialize_controller() + + # reference + lead_reference = np.array([[0., 0.] for _ in range(pre_step)]).flatten() + + for i in range(iteration_num): + print("simulation time = {0}".format(i)) + # make lead car's move + if i > int(iteration_num / 3): + lead_reference = np.array([[4., 0.] for _ in range(pre_step)]).flatten() + + lead_states = lead_car.xs + lead_opt_u = lead_controller.calc_input(lead_states[1:], lead_reference) + lead_opt_u = np.hstack((np.array([V]), lead_opt_u)) + + # make follow car + follow_reference = np.array([lead_states[1:] for _ in range(pre_step)]).flatten() + follow_states = follow_car.xs + + follow_opt_u = follow_controller.calc_input(follow_states[1:], follow_reference) + follow_opt_u = np.hstack((np.array([V]), follow_opt_u)) + + lead_car.update_state(lead_opt_u, dt=dt) + follow_car.update_state(follow_opt_u, dt=dt) + + # figures and animation + lead_history_states = np.array(lead_car.history_xs) + follow_history_states = np.array(follow_car.history_xs) + + time_history_fig = plt.figure() + x_fig = time_history_fig.add_subplot(311) + y_fig = time_history_fig.add_subplot(312) + theta_fig = time_history_fig.add_subplot(313) + + car_traj_fig = plt.figure() + traj_fig = car_traj_fig.add_subplot(111) + traj_fig.set_aspect('equal') + + x_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_states[:, 0], label="lead") + x_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_states[:, 0], label="follow") + x_fig.set_xlabel("time [s]") + x_fig.set_ylabel("x") + x_fig.legend() + + y_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_states[:, 1], label="lead") + y_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_states[:, 1], label="follow") + y_fig.plot(np.arange(0, simulation_time+0.01, dt), [4. for _ in range(iteration_num+1)], linestyle="dashed") + y_fig.set_xlabel("time [s]") + y_fig.set_ylabel("y") + y_fig.legend() + + theta_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_states[:, 2], label="lead") + theta_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_states[:, 2], label="follow") + theta_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed") + theta_fig.set_xlabel("time [s]") + theta_fig.set_ylabel("theta") + theta_fig.legend() + + time_history_fig.tight_layout() + + traj_fig.plot(lead_history_states[:, 0], lead_history_states[:, 1], label="lead") + traj_fig.plot(follow_history_states[:, 0], follow_history_states[:, 1], label="follow") + traj_fig.set_xlabel("x") + traj_fig.set_ylabel("y") + traj_fig.legend() + plt.show() + + lead_history_us = np.array(lead_controller.history_us) + follow_history_us = np.array(follow_controller.history_us) + input_history_fig = plt.figure() + u_1_fig = input_history_fig.add_subplot(111) + + u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_us[:, 0], label="lead") + u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_us[:, 0], label="follow") + u_1_fig.set_xlabel("time [s]") + u_1_fig.set_ylabel("u_omega") + + input_history_fig.tight_layout() + plt.show() + + animdrawer = AnimDrawer([lead_history_states, follow_history_states]) + animdrawer.draw_anim() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/mpc/basic_IOC/main_example.py b/mpc/basic_IOC/main_example.py new file mode 100644 index 0000000..082693a --- /dev/null +++ b/mpc/basic_IOC/main_example.py @@ -0,0 +1,184 @@ +import numpy as np +import matplotlib.pyplot as plt +import math +import copy + +from mpc_func_with_scipy import MpcController as MpcController_scipy +from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt +from control import matlab + +class FirstOrderSystem(): + """FirstOrderSystemWithStates + + Attributes + ----------- + xs : numpy.ndarray + system states + A : numpy.ndarray + system matrix + B : numpy.ndarray + control matrix + C : numpy.ndarray + observation matrix + history_xs : list + time history of state + """ + def __init__(self, A, B, C, D=None, init_states=None): + """ + Parameters + ----------- + A : numpy.ndarray + system matrix + B : numpy.ndarray + control matrix + C : numpy.ndarray + observation matrix + D : numpy.ndarray + directly matrix + init_state : float, optional + initial state of system default is None + history_xs : list + time history of system states + """ + + self.A = A + self.B = B + self.C = C + + if D is not None: + self.D = D + + self.xs = np.zeros(self.A.shape[0]) + + if init_states is not None: + self.xs = copy.deepcopy(init_states) + + self.history_xs = [init_states] + + def update_state(self, u, dt=0.01): + """calculating input + Parameters + ------------ + u : numpy.ndarray + inputs of system in some cases this means the reference + dt : float in seconds, optional + sampling time of simulation, default is 0.01 [s] + """ + temp_x = self.xs.reshape(-1, 1) + temp_u = u.reshape(-1, 1) + + # solve Runge-Kutta + k0 = dt * (np.dot(self.A, temp_x) + np.dot(self.B, temp_u)) + k1 = dt * (np.dot(self.A, temp_x + k0/2.) + np.dot(self.B, temp_u)) + k2 = dt * (np.dot(self.A, temp_x + k1/2.) + np.dot(self.B, temp_u)) + k3 = dt * (np.dot(self.A, temp_x + k2) + np.dot(self.B, temp_u)) + + self.xs += ((k0 + 2 * k1 + 2 * k2 + k3) / 6.).flatten() + + # for oylar + # self.xs += k0.flatten() + # print("xs = {0}".format(self.xs)) + + # save + save_states = copy.deepcopy(self.xs) + self.history_xs.append(save_states) + +def main(): + dt = 0.05 + simulation_time = 30 # in seconds + iteration_num = int(simulation_time / dt) + + # you must be care about this matrix + # these A and B are for continuos system if you want to use discret system matrix please skip this step + tau = 0.63 + A = np.array([[-1./tau, 0., 0., 0.], + [0., -1./tau, 0., 0.], + [1., 0., 0., 0.], + [0., 1., 0., 0.]]) + B = np.array([[1./tau, 0.], + [0., 1./tau], + [0., 0.], + [0., 0.]]) + + C = np.eye(4) + D = np.zeros((4, 2)) + + # make simulator with coninuous matrix + init_xs = np.array([0., 0., 0., 0.]) + plant = FirstOrderSystem(A, B, C, init_states=init_xs) + + # create system + sysc = matlab.ss(A, B, C, D) + # discrete system + sysd = matlab.c2d(sysc, dt) + + Ad = sysd.A + Bd = sysd.B + + # evaluation function weight + Q = np.diag([1., 1., 1., 1.]) + R = np.diag([1., 1.]) + pre_step = 10 + + # make controller with discreted matrix + # please check the solver, if you want to use the scipy, set the MpcController_scipy + controller = MpcController_cvxopt(Ad, Bd, Q, R, pre_step, + dt_input_upper=np.array([0.25 * dt, 0.25 * dt]), dt_input_lower=np.array([-0.5 * dt, -0.5 * dt]), + input_upper=np.array([1. ,3.]), input_lower=np.array([-1., -3.])) + + controller.initialize_controller() + + for i in range(iteration_num): + print("simulation time = {0}".format(i)) + reference = np.array([[0., 0., -5., 7.5] for _ in range(pre_step)]).flatten() + states = plant.xs + opt_u = controller.calc_input(states, reference) + plant.update_state(opt_u, dt=dt) + + history_states = np.array(plant.history_xs) + + time_history_fig = plt.figure() + x_fig = time_history_fig.add_subplot(411) + y_fig = time_history_fig.add_subplot(412) + v_x_fig = time_history_fig.add_subplot(413) + v_y_fig = time_history_fig.add_subplot(414) + + v_x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 0]) + v_x_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed") + v_x_fig.set_xlabel("time [s]") + v_x_fig.set_ylabel("v_x") + + v_y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 1]) + v_y_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed") + v_y_fig.set_xlabel("time [s]") + v_y_fig.set_ylabel("v_y") + + x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 2]) + x_fig.plot(np.arange(0, simulation_time+0.01, dt), [-5. for _ in range(iteration_num+1)], linestyle="dashed") + x_fig.set_xlabel("time [s]") + x_fig.set_ylabel("x") + + y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 3]) + y_fig.plot(np.arange(0, simulation_time+0.01, dt), [7.5 for _ in range(iteration_num+1)], linestyle="dashed") + y_fig.set_xlabel("time [s]") + y_fig.set_ylabel("y") + time_history_fig.tight_layout() + plt.show() + + history_us = np.array(controller.history_us) + input_history_fig = plt.figure() + u_1_fig = input_history_fig.add_subplot(211) + u_2_fig = input_history_fig.add_subplot(212) + + u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us[:, 0]) + u_1_fig.set_xlabel("time [s]") + u_1_fig.set_ylabel("u_x") + + u_2_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us[:, 1]) + u_2_fig.set_xlabel("time [s]") + u_2_fig.set_ylabel("u_y") + input_history_fig.tight_layout() + plt.show() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/mpc/basic_IOC/memo.md b/mpc/basic_IOC/memo.md new file mode 100644 index 0000000..65d983c --- /dev/null +++ b/mpc/basic_IOC/memo.md @@ -0,0 +1,5 @@ + # ラプラス近似 + 参考 + - http://triadsou.hatenablog.com/entry/20090217/1234865055 + + # \ No newline at end of file diff --git a/mpc/basic_IOC/mpc_func_with_cvxopt.py b/mpc/basic_IOC/mpc_func_with_cvxopt.py new file mode 100644 index 0000000..4e04736 --- /dev/null +++ b/mpc/basic_IOC/mpc_func_with_cvxopt.py @@ -0,0 +1,256 @@ +import numpy as np +import matplotlib.pyplot as plt +import math +import copy + +from cvxopt import matrix, solvers + +class MpcController(): + """ + Attributes + ------------ + A : numpy.ndarray + system matrix + B : numpy.ndarray + input matrix + Q : numpy.ndarray + evaluation function weight for states + Qs : numpy.ndarray + concatenated evaluation function weight for states + R : numpy.ndarray + evaluation function weight for inputs + Rs : numpy.ndarray + concatenated evaluation function weight for inputs + pre_step : int + prediction step + state_size : int + state size of the plant + input_size : int + input size of the plant + dt_input_upper : numpy.ndarray, shape(input_size, ), optional + constraints of input dt, default is None + dt_input_lower : numpy.ndarray, shape(input_size, ), optional + constraints of input dt, default is None + input_upper : numpy.ndarray, shape(input_size, ), optional + constraints of input, default is None + input_lower : numpy.ndarray, shape(input_size, ), optional + constraints of input, default is None + """ + def __init__(self, A, B, Q, R, pre_step, initial_input=None, dt_input_upper=None, dt_input_lower=None, input_upper=None, input_lower=None): + """ + Parameters + ------------ + A : numpy.ndarray + system matrix + B : numpy.ndarray + input matrix + Q : numpy.ndarray + evaluation function weight for states + R : numpy.ndarray + evaluation function weight for inputs + pre_step : int + prediction step + dt_input_upper : numpy.ndarray, shape(input_size, ), optional + constraints of input dt, default is None + dt_input_lower : numpy.ndarray, shape(input_size, ), optional + constraints of input dt, default is None + input_upper : numpy.ndarray, shape(input_size, ), optional + constraints of input, default is None + input_lower : numpy.ndarray, shape(input_size, ), optional + constraints of input, default is None + history_us : list + time history of optimal input us(numpy.ndarray) + """ + self.A = np.array(A) + self.B = np.array(B) + self.Q = np.array(Q) + self.R = np.array(R) + self.pre_step = pre_step + + self.Qs = None + self.Rs = None + + self.state_size = self.A.shape[0] + self.input_size = self.B.shape[1] + + self.history_us = [np.zeros(self.input_size)] + + # initial state + if initial_input is not None: + self.history_us = [initial_input] + + # constraints + self.dt_input_lower = dt_input_lower + self.dt_input_upper = dt_input_upper + self.input_upper = input_upper + self.input_lower = input_lower + + # about mpc matrix + self.W = None + self.omega = None + self.F = None + self.f = None + + def initialize_controller(self): + """ + make matrix to calculate optimal control input + + """ + A_factorials = [self.A] + + self.phi_mat = copy.deepcopy(self.A) + + for _ in range(self.pre_step - 1): + temp_mat = np.dot(A_factorials[-1], self.A) + self.phi_mat = np.vstack((self.phi_mat, temp_mat)) + + A_factorials.append(temp_mat) # after we use this factorials + + print("phi_mat = \n{0}".format(self.phi_mat)) + + self.gamma_mat = copy.deepcopy(self.B) + gammma_mat_temp = copy.deepcopy(self.B) + + for i in range(self.pre_step - 1): + temp_1_mat = np.dot(A_factorials[i], self.B) + 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)) + + self.theta_mat = copy.deepcopy(self.gamma_mat) + + for i in range(self.pre_step - 1): + temp_mat = np.zeros_like(self.gamma_mat) + temp_mat[int((i + 1)*self.state_size): , :] = self.gamma_mat[:-int((i + 1)*self.state_size) , :] + + self.theta_mat = np.hstack((self.theta_mat, temp_mat)) + + print("theta_mat = \n{0}".format(self.theta_mat)) + + # evaluation function weight + diag_Qs = np.array([np.diag(self.Q) for _ in range(self.pre_step)]) + diag_Rs = np.array([np.diag(self.R) for _ in range(self.pre_step)]) + + 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)) + + # constraints + # about dt U + if self.input_lower is not None: + # initialize + self.F = np.zeros((self.input_size * 2, self.pre_step * self.input_size)) + for i in range(self.input_size): + 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)) + + for i in range(self.pre_step - 1): + temp_F = copy.deepcopy(temp_F) + + for j in range(self.input_size): + temp_F[j * 2: (j + 1) * 2, ((i+1) * self.input_size) + j] = np.array([1., -1.]) + + self.F = np.vstack((self.F, temp_F)) + + self.F1 = self.F[:, :self.input_size] + + temp_f = [] + + for i in range(self.input_size): + temp_f.append(-1 * self.input_upper[i]) + temp_f.append(self.input_lower[i]) + + 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)) + + # about dt_u + if self.dt_input_lower is not None: + self.W = np.zeros((2, self.pre_step * self.input_size)) + self.W[:, 0] = np.array([1., -1.]) + + for i in range(self.pre_step * self.input_size - 1): + temp_W = np.zeros((2, self.pre_step * self.input_size)) + temp_W[:, i+1] = np.array([1., -1.]) + self.W = np.vstack((self.W, temp_W)) + + temp_omega = [] + + for i in range(self.input_size): + temp_omega.append(self.dt_input_upper[i]) + temp_omega.append(-1. * self.dt_input_lower[i]) + + 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)) + + # about state + print("check the matrix!! if you think rite, plese push enter") + input() + + def calc_input(self, states, references): + """calculate optimal input + Parameters + ----------- + states : numpy.ndarray, shape(state length, ) + current state of system + references : numpy.ndarray, shape(state length * pre_step, ) + reference of the system, you should set this value as reachable goal + + References + ------------ + opt_input : numpy.ndarray, shape(input_length, ) + optimal input + """ + 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)) + + error = references.reshape(-1, 1) - temp_1 - temp_2 + + G = 2. * np.dot(self.theta_mat.T, np.dot(self.Qs, error)) + + H = np.dot(self.theta_mat.T, np.dot(self.Qs, self.theta_mat)) + self.Rs + + # constraints + A = [] + b = [] + + if self.W is not None: + A.append(self.W) + b.append(self.omega.reshape(-1, 1)) + + if self.F is not None: + b_F = - np.dot(self.F1, self.history_us[-1].reshape(-1, 1)) - self.f.reshape(-1, 1) + A.append(self.F) + b.append(b_F) + + A = np.array(A).reshape(-1, self.input_size * self.pre_step) + + ub = np.array(b).flatten() + + # make cvxpy problem formulation + P = 2*matrix(H) + q = matrix(-1 * G) + A = matrix(A) + b = matrix(ub) + + # constraint + if self.W is not None or self.F is not None : + opt_result = solvers.qp(P, q, G=A, h=b) + + opt_dt_us = list(opt_result['x']) + + opt_u = opt_dt_us[:self.input_size] + self.history_us[-1] + + # save + self.history_us.append(opt_u) + + return opt_u \ No newline at end of file diff --git a/mpc/basic_IOC/mpc_func_with_scipy.py b/mpc/basic_IOC/mpc_func_with_scipy.py new file mode 100644 index 0000000..54ef3c9 --- /dev/null +++ b/mpc/basic_IOC/mpc_func_with_scipy.py @@ -0,0 +1,262 @@ +import numpy as np +import matplotlib.pyplot as plt +import math +import copy + +from scipy.optimize import minimize +from scipy.optimize import LinearConstraint + +class MpcController(): + """ + Attributes + ------------ + A : numpy.ndarray + system matrix + B : numpy.ndarray + input matrix + Q : numpy.ndarray + evaluation function weight for states + Qs : numpy.ndarray + concatenated evaluation function weight for states + R : numpy.ndarray + evaluation function weight for inputs + Rs : numpy.ndarray + concatenated evaluation function weight for inputs + pre_step : int + prediction step + state_size : int + state size of the plant + input_size : int + input size of the plant + dt_input_upper : numpy.ndarray, shape(input_size, ), optional + constraints of input dt, default is None + dt_input_lower : numpy.ndarray, shape(input_size, ), optional + constraints of input dt, default is None + input_upper : numpy.ndarray, shape(input_size, ), optional + constraints of input, default is None + input_lower : numpy.ndarray, shape(input_size, ), optional + constraints of input, default is None + """ + def __init__(self, A, B, Q, R, pre_step, initial_input=None, dt_input_upper=None, dt_input_lower=None, input_upper=None, input_lower=None): + """ + Parameters + ------------ + A : numpy.ndarray + system matrix + B : numpy.ndarray + input matrix + Q : numpy.ndarray + evaluation function weight for states + R : numpy.ndarray + evaluation function weight for inputs + pre_step : int + prediction step + dt_input_upper : numpy.ndarray, shape(input_size, ), optional + constraints of input dt, default is None + dt_input_lower : numpy.ndarray, shape(input_size, ), optional + constraints of input dt, default is None + input_upper : numpy.ndarray, shape(input_size, ), optional + constraints of input, default is None + input_lower : numpy.ndarray, shape(input_size, ), optional + constraints of input, default is None + history_us : list + time history of optimal input us(numpy.ndarray) + """ + self.A = np.array(A) + self.B = np.array(B) + self.Q = np.array(Q) + self.R = np.array(R) + self.pre_step = pre_step + + self.Qs = None + self.Rs = None + + self.state_size = self.A.shape[0] + self.input_size = self.B.shape[1] + + self.history_us = [np.zeros(self.input_size)] + + # initial state + if initial_input is not None: + self.history_us = [initial_input] + + # constraints + self.dt_input_lower = dt_input_lower + self.dt_input_upper = dt_input_upper + self.input_upper = input_upper + self.input_lower = input_lower + + self.W = None + self.omega = None + self.F = None + self.f = None + + def initialize_controller(self): + """ + make matrix to calculate optimal control input + """ + A_factorials = [self.A] + + self.phi_mat = copy.deepcopy(self.A) + + for _ in range(self.pre_step - 1): + temp_mat = np.dot(A_factorials[-1], self.A) + self.phi_mat = np.vstack((self.phi_mat, temp_mat)) + + A_factorials.append(temp_mat) # after we use this factorials + + print("phi_mat = \n{0}".format(self.phi_mat)) + + self.gamma_mat = copy.deepcopy(self.B) + gammma_mat_temp = copy.deepcopy(self.B) + + for i in range(self.pre_step - 1): + temp_1_mat = np.dot(A_factorials[i], self.B) + 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)) + + self.theta_mat = copy.deepcopy(self.gamma_mat) + + for i in range(self.pre_step - 1): + temp_mat = np.zeros_like(self.gamma_mat) + temp_mat[int((i + 1)*self.state_size): , :] = self.gamma_mat[:-int((i + 1)*self.state_size) , :] + + self.theta_mat = np.hstack((self.theta_mat, temp_mat)) + + print("theta_mat = \n{0}".format(self.theta_mat)) + + # evaluation function weight + diag_Qs = np.array([np.diag(self.Q) for _ in range(self.pre_step)]) + diag_Rs = np.array([np.diag(self.R) for _ in range(self.pre_step)]) + + 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)) + + # constraints + # about dt U + if self.input_lower is not None: + # initialize + self.F = np.zeros((self.input_size * 2, self.pre_step * self.input_size)) + for i in range(self.input_size): + 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)) + + for i in range(self.pre_step - 1): + temp_F = copy.deepcopy(temp_F) + + for j in range(self.input_size): + temp_F[j * 2: (j + 1) * 2, ((i+1) * self.input_size) + j] = np.array([1., -1.]) + + self.F = np.vstack((self.F, temp_F)) + + self.F1 = self.F[:, :self.input_size] + + temp_f = [] + + for i in range(self.input_size): + temp_f.append(-1 * self.input_upper[i]) + temp_f.append(self.input_lower[i]) + + 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)) + + # about dt_u + if self.dt_input_lower is not None: + self.W = np.zeros((2, self.pre_step * self.input_size)) + self.W[:, 0] = np.array([1., -1.]) + + for i in range(self.pre_step * self.input_size - 1): + temp_W = np.zeros((2, self.pre_step * self.input_size)) + temp_W[:, i+1] = np.array([1., -1.]) + self.W = np.vstack((self.W, temp_W)) + + temp_omega = [] + + for i in range(self.input_size): + temp_omega.append(self.dt_input_upper[i]) + temp_omega.append(-1. * self.dt_input_lower[i]) + + 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)) + + # about state + print("check the matrix!! if you think rite, plese push enter") + input() + + def calc_input(self, states, references): + """calculate optimal input + Parameters + ----------- + states : numpy.ndarray, shape(state length, ) + current state of system + references : numpy.ndarray, shape(state length * pre_step, ) + reference of the system, you should set this value as reachable goal + + References + ------------ + opt_input : numpy.ndarray, shape(input_length, ) + optimal input + """ + 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)) + + error = references.reshape(-1, 1) - temp_1 - temp_2 + + G = 2. * np.dot(self.theta_mat.T, np.dot(self.Qs, error) ) + + H = np.dot(self.theta_mat.T, np.dot(self.Qs, self.theta_mat)) + self.Rs + + # constraints + A = [] + b = [] + + if self.W is not None: + A.append(self.W) + b.append(self.omega.reshape(-1, 1)) + + if self.F is not None: + b_F = - np.dot(self.F1, self.history_us[-1].reshape(-1, 1)) - self.f.reshape(-1, 1) + A.append(self.F) + b.append(b_F) + + A = np.array(A).reshape(-1, self.input_size * self.pre_step) + + ub = np.array(b).flatten() + + def optimized_func(dt_us): + """ + """ + temp_dt_us = np.array([dt_us[i] for i in range(self.input_size * self.pre_step)]) + + return (np.dot(temp_dt_us, np.dot(H, temp_dt_us.reshape(-1, 1))) - np.dot(G.T, temp_dt_us.reshape(-1, 1)))[0] + + # constraint + lb = np.array([-np.inf for _ in range(len(ub))]) + linear_cons = LinearConstraint(A, lb, ub) + + init_dt_us = np.zeros(self.input_size * self.pre_step) + + # constraint + if self.W is not None or self.F is not None : + opt_result = minimize(optimized_func, init_dt_us, constraints=[linear_cons]) + + opt_dt_us = opt_result.x + + opt_u = opt_dt_us[:self.input_size] + self.history_us[-1] + + # save + self.history_us.append(opt_u) + + return opt_u \ No newline at end of file diff --git a/mpc/basic_IOC/test_compare_methods.py b/mpc/basic_IOC/test_compare_methods.py new file mode 100644 index 0000000..5ddaea4 --- /dev/null +++ b/mpc/basic_IOC/test_compare_methods.py @@ -0,0 +1,211 @@ +import numpy as np +import matplotlib.pyplot as plt +import math +import copy + +from mpc_func_with_scipy import MpcController as MpcController_scipy +from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt +from control import matlab + +class FirstOrderSystem(): + """FirstOrderSystemWithStates + + Attributes + ----------- + states : float + system states + A : numpy.ndarray + system matrix + B : numpy.ndarray + control matrix + C : numpy.ndarray + observation matrix + history_state : list + time history of state + """ + def __init__(self, A, B, C, D=None, init_states=None): + """ + Parameters + ----------- + A : numpy.ndarray + system matrix + B : numpy.ndarray + control matrix + C : numpy.ndarray + observation matrix + C : numpy.ndarray + directly matrix + init_state : float, optional + initial state of system default is None + history_xs : list + time history of system states + """ + + self.A = A + self.B = B + self.C = C + + if D is not None: + self.D = D + + self.xs = np.zeros(self.A.shape[0]) + + if init_states is not None: + self.xs = copy.deepcopy(init_states) + + self.history_xs = [init_states] + + def update_state(self, u, dt=0.01): + """calculating input + Parameters + ------------ + u : float + input of system in some cases this means the reference + dt : float in seconds, optional + sampling time of simulation, default is 0.01 [s] + """ + temp_x = self.xs.reshape(-1, 1) + temp_u = u.reshape(-1, 1) + + # solve Runge-Kutta + k0 = dt * (np.dot(self.A, temp_x) + np.dot(self.B, temp_u)) + k1 = dt * (np.dot(self.A, temp_x + k0/2.) + np.dot(self.B, temp_u)) + k2 = dt * (np.dot(self.A, temp_x + k1/2.) + np.dot(self.B, temp_u)) + k3 = dt * (np.dot(self.A, temp_x + k2) + np.dot(self.B, temp_u)) + + self.xs += ((k0 + 2 * k1 + 2 * k2 + k3) / 6.).flatten() + + # for oylar + # self.xs += k0.flatten() + + # print("xs = {0}".format(self.xs)) + # a = input() + # save + save_states = copy.deepcopy(self.xs) + self.history_xs.append(save_states) + # print(self.history_xs) + +def main(): + dt = 0.05 + simulation_time = 50 # in seconds + iteration_num = int(simulation_time / dt) + + # you must be care about this matrix + # these A and B are for continuos system if you want to use discret system matrix please skip this step + tau = 0.63 + A = np.array([[-1./tau, 0., 0., 0.], + [0., -1./tau, 0., 0.], + [1., 0., 0., 0.], + [0., 1., 0., 0.]]) + B = np.array([[1./tau, 0.], + [0., 1./tau], + [0., 0.], + [0., 0.]]) + + C = np.eye(4) + D = np.zeros((4, 2)) + + # make simulator with coninuous matrix + init_xs = np.array([0., 0., 0., 0.]) + plant_cvxopt = FirstOrderSystem(A, B, C, init_states=init_xs) + plant_scipy = FirstOrderSystem(A, B, C, init_states=init_xs) + + # create system + sysc = matlab.ss(A, B, C, D) + # discrete system + sysd = matlab.c2d(sysc, dt) + + Ad = sysd.A + Bd = sysd.B + + # evaluation function weight + Q = np.diag([1., 1., 10., 10.]) + R = np.diag([0.01, 0.01]) + pre_step = 5 + + # make controller with discreted matrix + # please check the solver, if you want to use the scipy, set the MpcController_scipy + controller_cvxopt = MpcController_cvxopt(Ad, Bd, Q, R, pre_step, + dt_input_upper=np.array([0.25 * dt, 0.25 * dt]), dt_input_lower=np.array([-0.5 * dt, -0.5 * dt]), + input_upper=np.array([1. ,3.]), input_lower=np.array([-1., -3.])) + + controller_scipy = MpcController_scipy(Ad, Bd, Q, R, pre_step, + dt_input_upper=np.array([0.25 * dt, 0.25 * dt]), dt_input_lower=np.array([-0.5 * dt, -0.5 * dt]), + input_upper=np.array([1. ,3.]), input_lower=np.array([-1., -3.])) + + controller_cvxopt.initialize_controller() + controller_scipy.initialize_controller() + + for i in range(iteration_num): + print("simulation time = {0}".format(i)) + reference = np.array([[0., 0., -5., 7.5] for _ in range(pre_step)]).flatten() + + states_cvxopt = plant_cvxopt.xs + states_scipy = plant_scipy.xs + + opt_u_cvxopt = controller_cvxopt.calc_input(states_cvxopt, reference) + opt_u_scipy = controller_scipy.calc_input(states_scipy, reference) + + plant_cvxopt.update_state(opt_u_cvxopt) + plant_scipy.update_state(opt_u_scipy) + + history_states_cvxopt = np.array(plant_cvxopt.history_xs) + history_states_scipy = np.array(plant_scipy.history_xs) + + time_history_fig = plt.figure(dpi=75) + x_fig = time_history_fig.add_subplot(411) + y_fig = time_history_fig.add_subplot(412) + v_x_fig = time_history_fig.add_subplot(413) + v_y_fig = time_history_fig.add_subplot(414) + + v_x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_cvxopt[:, 0], label="cvxopt") + v_x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_scipy[:, 0], label="scipy", linestyle="dashdot") + v_x_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed") + v_x_fig.set_xlabel("time [s]") + v_x_fig.set_ylabel("v_x") + v_x_fig.legend() + + v_y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_cvxopt[:, 1], label="cvxopt") + v_y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_scipy[:, 1], label="scipy", linestyle="dashdot") + v_y_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed") + v_y_fig.set_xlabel("time [s]") + v_y_fig.set_ylabel("v_y") + v_y_fig.legend() + + x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_cvxopt[:, 2], label="cvxopt") + x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_scipy[:, 2], label="scipy", linestyle="dashdot") + x_fig.plot(np.arange(0, simulation_time+0.01, dt), [-5. for _ in range(iteration_num+1)], linestyle="dashed") + x_fig.set_xlabel("time [s]") + x_fig.set_ylabel("x") + + y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_cvxopt[:, 3], label ="cvxopt") + y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_scipy[:, 3], label="scipy", linestyle="dashdot") + y_fig.plot(np.arange(0, simulation_time+0.01, dt), [7.5 for _ in range(iteration_num+1)], linestyle="dashed") + y_fig.set_xlabel("time [s]") + y_fig.set_ylabel("y") + time_history_fig.tight_layout() + plt.show() + + history_us_cvxopt = np.array(controller_cvxopt.history_us) + history_us_scipy = np.array(controller_scipy.history_us) + + input_history_fig = plt.figure(dpi=75) + u_1_fig = input_history_fig.add_subplot(211) + u_2_fig = input_history_fig.add_subplot(212) + + u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us_cvxopt[:, 0], label="cvxopt") + u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us_scipy[:, 0], label="scipy", linestyle="dashdot") + u_1_fig.set_xlabel("time [s]") + u_1_fig.set_ylabel("u_x") + u_1_fig.legend() + + u_2_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us_cvxopt[:, 1], label="cvxopt") + u_2_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us_scipy[:, 1], label="scipy", linestyle="dashdot") + u_2_fig.set_xlabel("time [s]") + u_2_fig.set_ylabel("u_y") + u_2_fig.legend() + input_history_fig.tight_layout() + plt.show() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/mpc/extend/main_track.py b/mpc/extend/main_track.py index 9a1f062..3b8d6a2 100644 --- a/mpc/extend/main_track.py +++ b/mpc/extend/main_track.py @@ -317,7 +317,7 @@ def main(): # input() # evaluation function weight - Q = np.diag([100., 1000., 1.]) + Q = np.diag([1000000., 10., 1.]) R = np.diag([0.1]) # System model update From b013192976c5afb51e876561547a9d578a32e659 Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Fri, 15 Feb 2019 18:16:47 +0900 Subject: [PATCH 08/13] add --- {mpc/basic_IOC => IOC}/README.md | 0 {mpc/basic_IOC => IOC}/animation.py | 0 {mpc/basic_IOC => IOC}/main_ACC.py | 0 IOC/main_ACC_TEMP.py | 3 + {mpc/basic_IOC => IOC}/main_example.py | 0 IOC/memo.md | 23 ++ .../basic_IOC => IOC}/mpc_func_with_cvxopt.py | 0 {mpc/basic_IOC => IOC}/mpc_func_with_scipy.py | 0 .../basic_IOC => IOC}/test_compare_methods.py | 0 mpc/basic_IOC/main_ACC_TEMP.py | 243 ------------------ mpc/basic_IOC/memo.md | 5 - 11 files changed, 26 insertions(+), 248 deletions(-) rename {mpc/basic_IOC => IOC}/README.md (100%) rename {mpc/basic_IOC => IOC}/animation.py (100%) rename {mpc/basic_IOC => IOC}/main_ACC.py (100%) create mode 100644 IOC/main_ACC_TEMP.py rename {mpc/basic_IOC => IOC}/main_example.py (100%) create mode 100644 IOC/memo.md rename {mpc/basic_IOC => IOC}/mpc_func_with_cvxopt.py (100%) rename {mpc/basic_IOC => IOC}/mpc_func_with_scipy.py (100%) rename {mpc/basic_IOC => IOC}/test_compare_methods.py (100%) delete mode 100644 mpc/basic_IOC/main_ACC_TEMP.py delete mode 100644 mpc/basic_IOC/memo.md diff --git a/mpc/basic_IOC/README.md b/IOC/README.md similarity index 100% rename from mpc/basic_IOC/README.md rename to IOC/README.md diff --git a/mpc/basic_IOC/animation.py b/IOC/animation.py similarity index 100% rename from mpc/basic_IOC/animation.py rename to IOC/animation.py diff --git a/mpc/basic_IOC/main_ACC.py b/IOC/main_ACC.py similarity index 100% rename from mpc/basic_IOC/main_ACC.py rename to IOC/main_ACC.py diff --git a/IOC/main_ACC_TEMP.py b/IOC/main_ACC_TEMP.py new file mode 100644 index 0000000..109f2e9 --- /dev/null +++ b/IOC/main_ACC_TEMP.py @@ -0,0 +1,3 @@ +import numpy as np +import matplotlib.pyplot as plt + diff --git a/mpc/basic_IOC/main_example.py b/IOC/main_example.py similarity index 100% rename from mpc/basic_IOC/main_example.py rename to IOC/main_example.py diff --git a/IOC/memo.md b/IOC/memo.md new file mode 100644 index 0000000..7e63ac2 --- /dev/null +++ b/IOC/memo.md @@ -0,0 +1,23 @@ + # ラプラス近似 + 参考 + - http://triadsou.hatenablog.com/entry/20090217/1234865055 + + # iterative LQR +これは概要 + https://people.eecs.berkeley.edu/~pabbeel/cs287-fa12/slides/LQR.pdf + +こっちのほうはがわかりやすいかも +https://katefvision.github.io/katefSlides/RECITATIONtrajectoryoptimization_katef.pdf + +直感的に説明してくれているやつ + +https://medium.com/@jonathan_hui/rl-lqr-ilqr-linear-quadratic-regulator-a5de5104c750 + +これはsergey先生のやつ +http://rll.berkeley.edu/deeprlcoursesp17/docs/week_2_lecture_2_optimal_control.pdf + +Iterative LQRはおそらく、終端状態が固定（目標値になる）と仮定して解く問題っぽい +これはMPCみたいにしないとダメだわ +結局やっていることとしては、ある時間までの有限時間最適化問題なんだけど +そのときに各タイムステップのモデルが変わっても大丈夫的な話をしている気がする +非線形かつ時変の問題を解法できるようなイメージ diff --git a/mpc/basic_IOC/mpc_func_with_cvxopt.py b/IOC/mpc_func_with_cvxopt.py similarity index 100% rename from mpc/basic_IOC/mpc_func_with_cvxopt.py rename to IOC/mpc_func_with_cvxopt.py diff --git a/mpc/basic_IOC/mpc_func_with_scipy.py b/IOC/mpc_func_with_scipy.py similarity index 100% rename from mpc/basic_IOC/mpc_func_with_scipy.py rename to IOC/mpc_func_with_scipy.py diff --git a/mpc/basic_IOC/test_compare_methods.py b/IOC/test_compare_methods.py similarity index 100% rename from mpc/basic_IOC/test_compare_methods.py rename to IOC/test_compare_methods.py diff --git a/mpc/basic_IOC/main_ACC_TEMP.py b/mpc/basic_IOC/main_ACC_TEMP.py deleted file mode 100644 index 9ad7c97..0000000 --- a/mpc/basic_IOC/main_ACC_TEMP.py +++ /dev/null @@ -1,243 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt -import math -import copy - -from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt -from animation import AnimDrawer -# from control import matlab - -class TwoWheeledSystem(): - """SampleSystem, this is the simulator - Attributes - ----------- - xs : numpy.ndarray - system states, [x, y, theta] - history_xs : list - time history of state - """ - def __init__(self, init_states=None): - """ - Palameters - ----------- - init_state : float, optional, shape(3, ) - initial state of system default is None - """ - self.xs = np.zeros(3) - - if init_states is not None: - self.xs = copy.deepcopy(init_states) - - self.history_xs = [init_states] - - def update_state(self, us, dt=0.01): - """ - Palameters - ------------ - u : numpy.ndarray - inputs of system in some cases this means the reference - 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(3)] - k1 = [0.0 for _ in range(3)] - k2 = [0.0 for _ in range(3)] - k3 = [0.0 for _ in range(3)] - - functions = [self._func_x_1, self._func_x_2, self._func_x_3] - - # solve Runge-Kutta - for i, func in enumerate(functions): - k0[i] = dt * func(self.xs[0], self.xs[1], self.xs[2], us[0], us[1]) - - for i, func in enumerate(functions): - k1[i] = dt * func(self.xs[0] + k0[0]/2., self.xs[1] + k0[1]/2., self.xs[2] + k0[2]/2., us[0], us[1]) - - for i, func in enumerate(functions): - k2[i] = dt * func(self.xs[0] + k0[0]/2., self.xs[1] + k0[1]/2., self.xs[2] + k0[2]/2., us[0], us[1]) - - for i, func in enumerate(functions): - k3[i] = dt * func(self.xs[0] + k2[0], self.xs[1] + k2[1], self.xs[2] + k2[2], us[0], us[1]) - - self.xs[0] += (k0[0] + 2. * k1[0] + 2. * k2[0] + k3[0]) / 6. - self.xs[1] += (k0[1] + 2. * k1[1] + 2. * k2[1] + k3[1]) / 6. - self.xs[2] += (k0[2] + 2. * k1[2] + 2. * k2[2] + k3[2]) / 6. - - # save - save_states = copy.deepcopy(self.xs) - self.history_xs.append(save_states) - print(self.xs) - - def _func_x_1(self, y_1, y_2, y_3, u_1, u_2): - """ - Parameters - ------------ - y_1 : float - y_2 : float - y_3 : float - u_1 : float - system input - u_2 : float - system input - """ - y_dot = math.cos(y_3) * u_1 - return y_dot - - def _func_x_2(self, y_1, y_2, y_3, u_1, u_2): - """ - Parameters - ------------ - y_1 : float - y_2 : float - y_3 : float - u_1 : float - system input - u_2 : float - system input - """ - y_dot = math.sin(y_3) * u_1 - return y_dot - - def _func_x_3(self, y_1, y_2, y_3, u_1, u_2): - """ - Parameters - ------------ - y_1 : float - y_2 : float - y_3 : float - u_1 : float - system input - u_2 : float - system input - """ - y_dot = u_2 - return y_dot - -def main(): - dt = 0.05 - simulation_time = 10 # in seconds - iteration_num = int(simulation_time / dt) - - # you must be care about this matrix - # these A and B are for continuos system if you want to use discret system matrix please skip this step - # lineared car system - V = 5.0 - Ad = np.array([[1., V * dt], [0., 1.]]) - Bd = np.array([[0.], [1. * dt]]) - - C = np.eye(2) - D = np.zeros((2, 1)) - - # make simulator with coninuous matrix - init_xs_lead = np.array([5., 0., 0.]) - init_xs_follow = np.array([0., 0., 0.]) - lead_car = TwoWheeledSystem(init_states=init_xs_lead) - follow_car = TwoWheeledSystem(init_states=init_xs_follow) - - # create system - # sysc = matlab.ss(A, B, C, D) - # discrete system - # sysd = matlab.c2d(sysc, dt) - - # evaluation function weight - Q = np.diag([1., 1.]) - R = np.diag([5.]) - pre_step = 15 - - # make controller with discreted matrix - # please check the solver, if you want to use the scipy, set the MpcController_scipy - lead_controller = MpcController_cvxopt(Ad, Bd, Q, R, pre_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.])) - - follow_controller = MpcController_cvxopt(Ad, Bd, Q, R, pre_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.])) - - lead_controller.initialize_controller() - follow_controller.initialize_controller() - - # reference - lead_reference = np.array([[0., 0.] for _ in range(pre_step)]).flatten() - - for i in range(iteration_num): - print("simulation time = {0}".format(i)) - # make lead car's move - if i > int(iteration_num / 3): - lead_reference = np.array([[4., 0.] for _ in range(pre_step)]).flatten() - - lead_states = lead_car.xs - lead_opt_u = lead_controller.calc_input(lead_states[1:], lead_reference) - lead_opt_u = np.hstack((np.array([V]), lead_opt_u)) - - # make follow car - follow_reference = np.array([lead_states[1:] for _ in range(pre_step)]).flatten() - follow_states = follow_car.xs - - follow_opt_u = follow_controller.calc_input(follow_states[1:], follow_reference) - follow_opt_u = np.hstack((np.array([V]), follow_opt_u)) - - lead_car.update_state(lead_opt_u, dt=dt) - follow_car.update_state(follow_opt_u, dt=dt) - - # figures and animation - lead_history_states = np.array(lead_car.history_xs) - follow_history_states = np.array(follow_car.history_xs) - - time_history_fig = plt.figure() - x_fig = time_history_fig.add_subplot(311) - y_fig = time_history_fig.add_subplot(312) - theta_fig = time_history_fig.add_subplot(313) - - car_traj_fig = plt.figure() - traj_fig = car_traj_fig.add_subplot(111) - traj_fig.set_aspect('equal') - - x_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_states[:, 0], label="lead") - x_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_states[:, 0], label="follow") - x_fig.set_xlabel("time [s]") - x_fig.set_ylabel("x") - x_fig.legend() - - y_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_states[:, 1], label="lead") - y_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_states[:, 1], label="follow") - y_fig.plot(np.arange(0, simulation_time+0.01, dt), [4. for _ in range(iteration_num+1)], linestyle="dashed") - y_fig.set_xlabel("time [s]") - y_fig.set_ylabel("y") - y_fig.legend() - - theta_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_states[:, 2], label="lead") - theta_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_states[:, 2], label="follow") - theta_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed") - theta_fig.set_xlabel("time [s]") - theta_fig.set_ylabel("theta") - theta_fig.legend() - - time_history_fig.tight_layout() - - traj_fig.plot(lead_history_states[:, 0], lead_history_states[:, 1], label="lead") - traj_fig.plot(follow_history_states[:, 0], follow_history_states[:, 1], label="follow") - traj_fig.set_xlabel("x") - traj_fig.set_ylabel("y") - traj_fig.legend() - plt.show() - - lead_history_us = np.array(lead_controller.history_us) - follow_history_us = np.array(follow_controller.history_us) - input_history_fig = plt.figure() - u_1_fig = input_history_fig.add_subplot(111) - - u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_us[:, 0], label="lead") - u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_us[:, 0], label="follow") - u_1_fig.set_xlabel("time [s]") - u_1_fig.set_ylabel("u_omega") - - input_history_fig.tight_layout() - plt.show() - - animdrawer = AnimDrawer([lead_history_states, follow_history_states]) - animdrawer.draw_anim() - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/mpc/basic_IOC/memo.md b/mpc/basic_IOC/memo.md deleted file mode 100644 index 65d983c..0000000 --- a/mpc/basic_IOC/memo.md +++ /dev/null @@ -1,5 +0,0 @@ - # ラプラス近似 - 参考 - - http://triadsou.hatenablog.com/entry/20090217/1234865055 - - # \ No newline at end of file From 80e8a112cdde20e2ba1d0d1b4cc410250cf1d806 Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Fri, 22 Feb 2019 18:39:04 +0900 Subject: [PATCH 09/13] add iLQR --- IOC/animation.py | 233 ---------------- IOC/main_ACC.py | 246 ---------------- IOC/main_ACC_TEMP.py | 3 - IOC/main_example.py | 184 ------------ IOC/memo.md | 23 -- IOC/mpc_func_with_cvxopt.py | 256 ----------------- IOC/mpc_func_with_scipy.py | 262 ------------------ IOC/test_compare_methods.py | 211 -------------- {IOC => iLQR}/README.md | 0 .../{iterative_MPC.py => extended_MPC.py} | 0 mpc/extend/main_track.py | 2 +- 11 files changed, 1 insertion(+), 1419 deletions(-) delete mode 100755 IOC/animation.py delete mode 100644 IOC/main_ACC.py delete mode 100644 IOC/main_ACC_TEMP.py delete mode 100644 IOC/main_example.py delete mode 100644 IOC/memo.md delete mode 100644 IOC/mpc_func_with_cvxopt.py delete mode 100644 IOC/mpc_func_with_scipy.py delete mode 100644 IOC/test_compare_methods.py rename {IOC => iLQR}/README.md (100%) rename mpc/extend/{iterative_MPC.py => extended_MPC.py} (100%) diff --git a/IOC/animation.py b/IOC/animation.py deleted file mode 100755 index 6ece541..0000000 --- a/IOC/animation.py +++ /dev/null @@ -1,233 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt -import matplotlib.animation as ani -import matplotlib.font_manager as fon -import sys -import math - -# default setting of figures -plt.rcParams["mathtext.fontset"] = 'stix' # math fonts -plt.rcParams['xtick.direction'] = 'in' # x axis in -plt.rcParams['ytick.direction'] = 'in' # y axis in -plt.rcParams["font.size"] = 10 -plt.rcParams['axes.linewidth'] = 1.0 # axis line width -plt.rcParams['axes.grid'] = True # make grid - -def coordinate_transformation_in_angle(positions, base_angle): - ''' - Transformation the coordinate in the angle - - Parameters - ------- - positions : numpy.ndarray - this parameter is composed of xs, ys - should have (2, N) shape - base_angle : float [rad] - - Returns - ------- - traslated_positions : numpy.ndarray - the shape is (2, N) - - ''' - if positions.shape[0] != 2: - raise ValueError('the input data should have (2, N)') - - positions = np.array(positions) - positions = positions.reshape(2, -1) - - rot_matrix = [[np.cos(base_angle), np.sin(base_angle)], - [-1*np.sin(base_angle), np.cos(base_angle)]] - - rot_matrix = np.array(rot_matrix) - - translated_positions = np.dot(rot_matrix, positions) - - return translated_positions - -def square_make_with_angles(center_x, center_y, size, angle): - ''' - Create square matrix with angle line matrix(2D) - - Parameters - ------- - center_x : float in meters - the center x position of the square - center_y : float in meters - the center y position of the square - size : float in meters - the square's half-size - angle : float in radians - - Returns - ------- - square xs : numpy.ndarray - lenght is 5 (counterclockwise from right-up) - square ys : numpy.ndarray - length is 5 (counterclockwise from right-up) - angle line xs : numpy.ndarray - angle line ys : numpy.ndarray - ''' - - # start with the up right points - # create point in counterclockwise - square_xys = np.array([[size, 0.5 * size], [-size, 0.5 * size], [-size, -0.5 * size], [size, -0.5 * size], [size, 0.5 * size]]) - trans_points = coordinate_transformation_in_angle(square_xys.T, -angle) # this is inverse type - trans_points += np.array([[center_x], [center_y]]) - - square_xs = trans_points[0, :] - square_ys = trans_points[1, :] - - angle_line_xs = [center_x, center_x + math.cos(angle) * size] - angle_line_ys = [center_y, center_y + math.sin(angle) * size] - - return square_xs, square_ys, np.array(angle_line_xs), np.array(angle_line_ys) - - -class AnimDrawer(): - """create animation of path and robot - - Attributes - ------------ - cars : - anim_fig : figure of matplotlib - axis : axis of matplotlib - - """ - def __init__(self, objects): - """ - Parameters - ------------ - objects : list of objects - """ - self.lead_car_history_state = objects[0] - self.follow_car_history_state = objects[1] - - 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]] - - # setting up figure - self.anim_fig = plt.figure(dpi=150) - self.axis = self.anim_fig.add_subplot(111) - - # imgs - self.object_imgs = [] - self.traj_imgs = [] - - def draw_anim(self, interval=50): - """draw the animation and save - - Parameteres - ------------- - interval : int, optional - animation's interval time, you should link the sampling time of systems - default is 50 [ms] - """ - self._set_axis() - self._set_img() - - self.skip_num = 1 - frame_num = int((len(self.history_xs[0])-1) / self.skip_num) - - animation = ani.FuncAnimation(self.anim_fig, self._update_anim, interval=interval, frames=frame_num) - - # self.axis.legend() - print('save_animation?') - shuold_save_animation = int(input()) - - if shuold_save_animation: - print('animation_number?') - num = int(input()) - animation.save('animation_{0}.mp4'.format(num), writer='ffmpeg') - # animation.save("Sample.gif", writer = 'imagemagick') # gif保存 - - plt.show() - - def _set_axis(self): - """ initialize the animation axies - """ - # (1) set the axis name - self.axis.set_xlabel(r'$\it{x}$ [m]') - 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(-5, 50) - self.axis.set_ylim(-2, 5) - - def _set_img(self): - """ initialize the imgs of animation - this private function execute the make initial imgs for animation - """ - # object imgs - obj_color_list = ["k", "k", "m", "m"] - obj_styles = ["solid", "solid", "solid", "solid"] - - 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) - - traj_color_list = ["k", "m"] - - 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) - - def _update_anim(self, i): - """the update animation - this function should be used in the animation functions - - Parameters - ------------ - i : int - time step of the animation - the sampling time should be related to the sampling time of system - - Returns - ----------- - object_imgs : list of img - traj_imgs : list of img - """ - i = int(i * self.skip_num) - - self._draw_objects(i) - self._draw_traj(i) - - return self.object_imgs, self.traj_imgs, - - def _draw_objects(self, i): - """ - This private function is just divided thing of - the _update_anim to see the code more clear - - Parameters - ------------ - i : int - time step of the animation - 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]) - - 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 - the _update_anim to see the code more clear - - Parameters - ------------ - i : int - 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]) \ No newline at end of file diff --git a/IOC/main_ACC.py b/IOC/main_ACC.py deleted file mode 100644 index a868559..0000000 --- a/IOC/main_ACC.py +++ /dev/null @@ -1,246 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt -import math -import copy - -from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt -from animation import AnimDrawer -from control import matlab - -class TwoWheeledSystem(): - """SampleSystem, this is the simulator - Attributes - ----------- - xs : numpy.ndarray - system states, [x, y, theta] - history_xs : list - time history of state - """ - def __init__(self, init_states=None): - """ - Palameters - ----------- - init_state : float, optional, shape(3, ) - initial state of system default is None - """ - self.xs = np.zeros(3) - - if init_states is not None: - self.xs = copy.deepcopy(init_states) - - self.history_xs = [init_states] - - def update_state(self, us, dt=0.01): - """ - Palameters - ------------ - u : numpy.ndarray - inputs of system in some cases this means the reference - 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(3)] - k1 = [0.0 for _ in range(3)] - k2 = [0.0 for _ in range(3)] - k3 = [0.0 for _ in range(3)] - - functions = [self._func_x_1, self._func_x_2, self._func_x_3] - - # solve Runge-Kutta - for i, func in enumerate(functions): - k0[i] = dt * func(self.xs[0], self.xs[1], self.xs[2], us[0], us[1]) - - for i, func in enumerate(functions): - k1[i] = dt * func(self.xs[0] + k0[0]/2., self.xs[1] + k0[1]/2., self.xs[2] + k0[2]/2., us[0], us[1]) - - for i, func in enumerate(functions): - k2[i] = dt * func(self.xs[0] + k0[0]/2., self.xs[1] + k0[1]/2., self.xs[2] + k0[2]/2., us[0], us[1]) - - for i, func in enumerate(functions): - k3[i] = dt * func(self.xs[0] + k2[0], self.xs[1] + k2[1], self.xs[2] + k2[2], us[0], us[1]) - - self.xs[0] += (k0[0] + 2. * k1[0] + 2. * k2[0] + k3[0]) / 6. - self.xs[1] += (k0[1] + 2. * k1[1] + 2. * k2[1] + k3[1]) / 6. - self.xs[2] += (k0[2] + 2. * k1[2] + 2. * k2[2] + k3[2]) / 6. - - # save - save_states = copy.deepcopy(self.xs) - self.history_xs.append(save_states) - print(self.xs) - - def _func_x_1(self, y_1, y_2, y_3, u_1, u_2): - """ - Parameters - ------------ - y_1 : float - y_2 : float - y_3 : float - u_1 : float - system input - u_2 : float - system input - """ - y_dot = math.cos(y_3) * u_1 - return y_dot - - def _func_x_2(self, y_1, y_2, y_3, u_1, u_2): - """ - Parameters - ------------ - y_1 : float - y_2 : float - y_3 : float - u_1 : float - system input - u_2 : float - system input - """ - y_dot = math.sin(y_3) * u_1 - return y_dot - - def _func_x_3(self, y_1, y_2, y_3, u_1, u_2): - """ - Parameters - ------------ - y_1 : float - y_2 : float - y_3 : float - u_1 : float - system input - u_2 : float - system input - """ - y_dot = u_2 - return y_dot - -def main(): - dt = 0.05 - simulation_time = 10 # in seconds - iteration_num = int(simulation_time / dt) - - # you must be care about this matrix - # these A and B are for continuos system if you want to use discret system matrix please skip this step - # lineared car system - V = 5.0 - A = np.array([[0., V], [0., 0.]]) - B = np.array([[0.], [1.]]) - - C = np.eye(2) - D = np.zeros((2, 1)) - - # make simulator with coninuous matrix - init_xs_lead = np.array([5., 0., 0.]) - init_xs_follow = np.array([0., 0., 0.]) - lead_car = TwoWheeledSystem(init_states=init_xs_lead) - follow_car = TwoWheeledSystem(init_states=init_xs_follow) - - # create system - sysc = matlab.ss(A, B, C, D) - # discrete system - sysd = matlab.c2d(sysc, dt) - - Ad = sysd.A - Bd = sysd.B - - # evaluation function weight - Q = np.diag([1., 1.]) - R = np.diag([5.]) - pre_step = 15 - - # make controller with discreted matrix - # please check the solver, if you want to use the scipy, set the MpcController_scipy - lead_controller = MpcController_cvxopt(Ad, Bd, Q, R, pre_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.])) - - follow_controller = MpcController_cvxopt(Ad, Bd, Q, R, pre_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.])) - - lead_controller.initialize_controller() - follow_controller.initialize_controller() - - # reference - lead_reference = np.array([[0., 0.] for _ in range(pre_step)]).flatten() - - for i in range(iteration_num): - print("simulation time = {0}".format(i)) - # make lead car's move - if i > int(iteration_num / 3): - lead_reference = np.array([[4., 0.] for _ in range(pre_step)]).flatten() - - lead_states = lead_car.xs - lead_opt_u = lead_controller.calc_input(lead_states[1:], lead_reference) - lead_opt_u = np.hstack((np.array([V]), lead_opt_u)) - - # make follow car - follow_reference = np.array([lead_states[1:] for _ in range(pre_step)]).flatten() - follow_states = follow_car.xs - - follow_opt_u = follow_controller.calc_input(follow_states[1:], follow_reference) - follow_opt_u = np.hstack((np.array([V]), follow_opt_u)) - - lead_car.update_state(lead_opt_u, dt=dt) - follow_car.update_state(follow_opt_u, dt=dt) - - # figures and animation - lead_history_states = np.array(lead_car.history_xs) - follow_history_states = np.array(follow_car.history_xs) - - time_history_fig = plt.figure() - x_fig = time_history_fig.add_subplot(311) - y_fig = time_history_fig.add_subplot(312) - theta_fig = time_history_fig.add_subplot(313) - - car_traj_fig = plt.figure() - traj_fig = car_traj_fig.add_subplot(111) - traj_fig.set_aspect('equal') - - x_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_states[:, 0], label="lead") - x_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_states[:, 0], label="follow") - x_fig.set_xlabel("time [s]") - x_fig.set_ylabel("x") - x_fig.legend() - - y_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_states[:, 1], label="lead") - y_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_states[:, 1], label="follow") - y_fig.plot(np.arange(0, simulation_time+0.01, dt), [4. for _ in range(iteration_num+1)], linestyle="dashed") - y_fig.set_xlabel("time [s]") - y_fig.set_ylabel("y") - y_fig.legend() - - theta_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_states[:, 2], label="lead") - theta_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_states[:, 2], label="follow") - theta_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed") - theta_fig.set_xlabel("time [s]") - theta_fig.set_ylabel("theta") - theta_fig.legend() - - time_history_fig.tight_layout() - - traj_fig.plot(lead_history_states[:, 0], lead_history_states[:, 1], label="lead") - traj_fig.plot(follow_history_states[:, 0], follow_history_states[:, 1], label="follow") - traj_fig.set_xlabel("x") - traj_fig.set_ylabel("y") - traj_fig.legend() - plt.show() - - lead_history_us = np.array(lead_controller.history_us) - follow_history_us = np.array(follow_controller.history_us) - input_history_fig = plt.figure() - u_1_fig = input_history_fig.add_subplot(111) - - u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_us[:, 0], label="lead") - u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_us[:, 0], label="follow") - u_1_fig.set_xlabel("time [s]") - u_1_fig.set_ylabel("u_omega") - - input_history_fig.tight_layout() - plt.show() - - animdrawer = AnimDrawer([lead_history_states, follow_history_states]) - animdrawer.draw_anim() - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/IOC/main_ACC_TEMP.py b/IOC/main_ACC_TEMP.py deleted file mode 100644 index 109f2e9..0000000 --- a/IOC/main_ACC_TEMP.py +++ /dev/null @@ -1,3 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt - diff --git a/IOC/main_example.py b/IOC/main_example.py deleted file mode 100644 index 082693a..0000000 --- a/IOC/main_example.py +++ /dev/null @@ -1,184 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt -import math -import copy - -from mpc_func_with_scipy import MpcController as MpcController_scipy -from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt -from control import matlab - -class FirstOrderSystem(): - """FirstOrderSystemWithStates - - Attributes - ----------- - xs : numpy.ndarray - system states - A : numpy.ndarray - system matrix - B : numpy.ndarray - control matrix - C : numpy.ndarray - observation matrix - history_xs : list - time history of state - """ - def __init__(self, A, B, C, D=None, init_states=None): - """ - Parameters - ----------- - A : numpy.ndarray - system matrix - B : numpy.ndarray - control matrix - C : numpy.ndarray - observation matrix - D : numpy.ndarray - directly matrix - init_state : float, optional - initial state of system default is None - history_xs : list - time history of system states - """ - - self.A = A - self.B = B - self.C = C - - if D is not None: - self.D = D - - self.xs = np.zeros(self.A.shape[0]) - - if init_states is not None: - self.xs = copy.deepcopy(init_states) - - self.history_xs = [init_states] - - def update_state(self, u, dt=0.01): - """calculating input - Parameters - ------------ - u : numpy.ndarray - inputs of system in some cases this means the reference - dt : float in seconds, optional - sampling time of simulation, default is 0.01 [s] - """ - temp_x = self.xs.reshape(-1, 1) - temp_u = u.reshape(-1, 1) - - # solve Runge-Kutta - k0 = dt * (np.dot(self.A, temp_x) + np.dot(self.B, temp_u)) - k1 = dt * (np.dot(self.A, temp_x + k0/2.) + np.dot(self.B, temp_u)) - k2 = dt * (np.dot(self.A, temp_x + k1/2.) + np.dot(self.B, temp_u)) - k3 = dt * (np.dot(self.A, temp_x + k2) + np.dot(self.B, temp_u)) - - self.xs += ((k0 + 2 * k1 + 2 * k2 + k3) / 6.).flatten() - - # for oylar - # self.xs += k0.flatten() - # print("xs = {0}".format(self.xs)) - - # save - save_states = copy.deepcopy(self.xs) - self.history_xs.append(save_states) - -def main(): - dt = 0.05 - simulation_time = 30 # in seconds - iteration_num = int(simulation_time / dt) - - # you must be care about this matrix - # these A and B are for continuos system if you want to use discret system matrix please skip this step - tau = 0.63 - A = np.array([[-1./tau, 0., 0., 0.], - [0., -1./tau, 0., 0.], - [1., 0., 0., 0.], - [0., 1., 0., 0.]]) - B = np.array([[1./tau, 0.], - [0., 1./tau], - [0., 0.], - [0., 0.]]) - - C = np.eye(4) - D = np.zeros((4, 2)) - - # make simulator with coninuous matrix - init_xs = np.array([0., 0., 0., 0.]) - plant = FirstOrderSystem(A, B, C, init_states=init_xs) - - # create system - sysc = matlab.ss(A, B, C, D) - # discrete system - sysd = matlab.c2d(sysc, dt) - - Ad = sysd.A - Bd = sysd.B - - # evaluation function weight - Q = np.diag([1., 1., 1., 1.]) - R = np.diag([1., 1.]) - pre_step = 10 - - # make controller with discreted matrix - # please check the solver, if you want to use the scipy, set the MpcController_scipy - controller = MpcController_cvxopt(Ad, Bd, Q, R, pre_step, - dt_input_upper=np.array([0.25 * dt, 0.25 * dt]), dt_input_lower=np.array([-0.5 * dt, -0.5 * dt]), - input_upper=np.array([1. ,3.]), input_lower=np.array([-1., -3.])) - - controller.initialize_controller() - - for i in range(iteration_num): - print("simulation time = {0}".format(i)) - reference = np.array([[0., 0., -5., 7.5] for _ in range(pre_step)]).flatten() - states = plant.xs - opt_u = controller.calc_input(states, reference) - plant.update_state(opt_u, dt=dt) - - history_states = np.array(plant.history_xs) - - time_history_fig = plt.figure() - x_fig = time_history_fig.add_subplot(411) - y_fig = time_history_fig.add_subplot(412) - v_x_fig = time_history_fig.add_subplot(413) - v_y_fig = time_history_fig.add_subplot(414) - - v_x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 0]) - v_x_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed") - v_x_fig.set_xlabel("time [s]") - v_x_fig.set_ylabel("v_x") - - v_y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 1]) - v_y_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed") - v_y_fig.set_xlabel("time [s]") - v_y_fig.set_ylabel("v_y") - - x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 2]) - x_fig.plot(np.arange(0, simulation_time+0.01, dt), [-5. for _ in range(iteration_num+1)], linestyle="dashed") - x_fig.set_xlabel("time [s]") - x_fig.set_ylabel("x") - - y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 3]) - y_fig.plot(np.arange(0, simulation_time+0.01, dt), [7.5 for _ in range(iteration_num+1)], linestyle="dashed") - y_fig.set_xlabel("time [s]") - y_fig.set_ylabel("y") - time_history_fig.tight_layout() - plt.show() - - history_us = np.array(controller.history_us) - input_history_fig = plt.figure() - u_1_fig = input_history_fig.add_subplot(211) - u_2_fig = input_history_fig.add_subplot(212) - - u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us[:, 0]) - u_1_fig.set_xlabel("time [s]") - u_1_fig.set_ylabel("u_x") - - u_2_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us[:, 1]) - u_2_fig.set_xlabel("time [s]") - u_2_fig.set_ylabel("u_y") - input_history_fig.tight_layout() - plt.show() - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/IOC/memo.md b/IOC/memo.md deleted file mode 100644 index 7e63ac2..0000000 --- a/IOC/memo.md +++ /dev/null @@ -1,23 +0,0 @@ - # ラプラス近似 - 参考 - - http://triadsou.hatenablog.com/entry/20090217/1234865055 - - # iterative LQR -これは概要 - https://people.eecs.berkeley.edu/~pabbeel/cs287-fa12/slides/LQR.pdf - -こっちのほうはがわかりやすいかも -https://katefvision.github.io/katefSlides/RECITATIONtrajectoryoptimization_katef.pdf - -直感的に説明してくれているやつ - -https://medium.com/@jonathan_hui/rl-lqr-ilqr-linear-quadratic-regulator-a5de5104c750 - -これはsergey先生のやつ -http://rll.berkeley.edu/deeprlcoursesp17/docs/week_2_lecture_2_optimal_control.pdf - -Iterative LQRはおそらく、終端状態が固定（目標値になる）と仮定して解く問題っぽい -これはMPCみたいにしないとダメだわ -結局やっていることとしては、ある時間までの有限時間最適化問題なんだけど -そのときに各タイムステップのモデルが変わっても大丈夫的な話をしている気がする -非線形かつ時変の問題を解法できるようなイメージ diff --git a/IOC/mpc_func_with_cvxopt.py b/IOC/mpc_func_with_cvxopt.py deleted file mode 100644 index 4e04736..0000000 --- a/IOC/mpc_func_with_cvxopt.py +++ /dev/null @@ -1,256 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt -import math -import copy - -from cvxopt import matrix, solvers - -class MpcController(): - """ - Attributes - ------------ - A : numpy.ndarray - system matrix - B : numpy.ndarray - input matrix - Q : numpy.ndarray - evaluation function weight for states - Qs : numpy.ndarray - concatenated evaluation function weight for states - R : numpy.ndarray - evaluation function weight for inputs - Rs : numpy.ndarray - concatenated evaluation function weight for inputs - pre_step : int - prediction step - state_size : int - state size of the plant - input_size : int - input size of the plant - dt_input_upper : numpy.ndarray, shape(input_size, ), optional - constraints of input dt, default is None - dt_input_lower : numpy.ndarray, shape(input_size, ), optional - constraints of input dt, default is None - input_upper : numpy.ndarray, shape(input_size, ), optional - constraints of input, default is None - input_lower : numpy.ndarray, shape(input_size, ), optional - constraints of input, default is None - """ - def __init__(self, A, B, Q, R, pre_step, initial_input=None, dt_input_upper=None, dt_input_lower=None, input_upper=None, input_lower=None): - """ - Parameters - ------------ - A : numpy.ndarray - system matrix - B : numpy.ndarray - input matrix - Q : numpy.ndarray - evaluation function weight for states - R : numpy.ndarray - evaluation function weight for inputs - pre_step : int - prediction step - dt_input_upper : numpy.ndarray, shape(input_size, ), optional - constraints of input dt, default is None - dt_input_lower : numpy.ndarray, shape(input_size, ), optional - constraints of input dt, default is None - input_upper : numpy.ndarray, shape(input_size, ), optional - constraints of input, default is None - input_lower : numpy.ndarray, shape(input_size, ), optional - constraints of input, default is None - history_us : list - time history of optimal input us(numpy.ndarray) - """ - self.A = np.array(A) - self.B = np.array(B) - self.Q = np.array(Q) - self.R = np.array(R) - self.pre_step = pre_step - - self.Qs = None - self.Rs = None - - self.state_size = self.A.shape[0] - self.input_size = self.B.shape[1] - - self.history_us = [np.zeros(self.input_size)] - - # initial state - if initial_input is not None: - self.history_us = [initial_input] - - # constraints - self.dt_input_lower = dt_input_lower - self.dt_input_upper = dt_input_upper - self.input_upper = input_upper - self.input_lower = input_lower - - # about mpc matrix - self.W = None - self.omega = None - self.F = None - self.f = None - - def initialize_controller(self): - """ - make matrix to calculate optimal control input - - """ - A_factorials = [self.A] - - self.phi_mat = copy.deepcopy(self.A) - - for _ in range(self.pre_step - 1): - temp_mat = np.dot(A_factorials[-1], self.A) - self.phi_mat = np.vstack((self.phi_mat, temp_mat)) - - A_factorials.append(temp_mat) # after we use this factorials - - print("phi_mat = \n{0}".format(self.phi_mat)) - - self.gamma_mat = copy.deepcopy(self.B) - gammma_mat_temp = copy.deepcopy(self.B) - - for i in range(self.pre_step - 1): - temp_1_mat = np.dot(A_factorials[i], self.B) - 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)) - - self.theta_mat = copy.deepcopy(self.gamma_mat) - - for i in range(self.pre_step - 1): - temp_mat = np.zeros_like(self.gamma_mat) - temp_mat[int((i + 1)*self.state_size): , :] = self.gamma_mat[:-int((i + 1)*self.state_size) , :] - - self.theta_mat = np.hstack((self.theta_mat, temp_mat)) - - print("theta_mat = \n{0}".format(self.theta_mat)) - - # evaluation function weight - diag_Qs = np.array([np.diag(self.Q) for _ in range(self.pre_step)]) - diag_Rs = np.array([np.diag(self.R) for _ in range(self.pre_step)]) - - 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)) - - # constraints - # about dt U - if self.input_lower is not None: - # initialize - self.F = np.zeros((self.input_size * 2, self.pre_step * self.input_size)) - for i in range(self.input_size): - 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)) - - for i in range(self.pre_step - 1): - temp_F = copy.deepcopy(temp_F) - - for j in range(self.input_size): - temp_F[j * 2: (j + 1) * 2, ((i+1) * self.input_size) + j] = np.array([1., -1.]) - - self.F = np.vstack((self.F, temp_F)) - - self.F1 = self.F[:, :self.input_size] - - temp_f = [] - - for i in range(self.input_size): - temp_f.append(-1 * self.input_upper[i]) - temp_f.append(self.input_lower[i]) - - 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)) - - # about dt_u - if self.dt_input_lower is not None: - self.W = np.zeros((2, self.pre_step * self.input_size)) - self.W[:, 0] = np.array([1., -1.]) - - for i in range(self.pre_step * self.input_size - 1): - temp_W = np.zeros((2, self.pre_step * self.input_size)) - temp_W[:, i+1] = np.array([1., -1.]) - self.W = np.vstack((self.W, temp_W)) - - temp_omega = [] - - for i in range(self.input_size): - temp_omega.append(self.dt_input_upper[i]) - temp_omega.append(-1. * self.dt_input_lower[i]) - - 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)) - - # about state - print("check the matrix!! if you think rite, plese push enter") - input() - - def calc_input(self, states, references): - """calculate optimal input - Parameters - ----------- - states : numpy.ndarray, shape(state length, ) - current state of system - references : numpy.ndarray, shape(state length * pre_step, ) - reference of the system, you should set this value as reachable goal - - References - ------------ - opt_input : numpy.ndarray, shape(input_length, ) - optimal input - """ - 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)) - - error = references.reshape(-1, 1) - temp_1 - temp_2 - - G = 2. * np.dot(self.theta_mat.T, np.dot(self.Qs, error)) - - H = np.dot(self.theta_mat.T, np.dot(self.Qs, self.theta_mat)) + self.Rs - - # constraints - A = [] - b = [] - - if self.W is not None: - A.append(self.W) - b.append(self.omega.reshape(-1, 1)) - - if self.F is not None: - b_F = - np.dot(self.F1, self.history_us[-1].reshape(-1, 1)) - self.f.reshape(-1, 1) - A.append(self.F) - b.append(b_F) - - A = np.array(A).reshape(-1, self.input_size * self.pre_step) - - ub = np.array(b).flatten() - - # make cvxpy problem formulation - P = 2*matrix(H) - q = matrix(-1 * G) - A = matrix(A) - b = matrix(ub) - - # constraint - if self.W is not None or self.F is not None : - opt_result = solvers.qp(P, q, G=A, h=b) - - opt_dt_us = list(opt_result['x']) - - opt_u = opt_dt_us[:self.input_size] + self.history_us[-1] - - # save - self.history_us.append(opt_u) - - return opt_u \ No newline at end of file diff --git a/IOC/mpc_func_with_scipy.py b/IOC/mpc_func_with_scipy.py deleted file mode 100644 index 54ef3c9..0000000 --- a/IOC/mpc_func_with_scipy.py +++ /dev/null @@ -1,262 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt -import math -import copy - -from scipy.optimize import minimize -from scipy.optimize import LinearConstraint - -class MpcController(): - """ - Attributes - ------------ - A : numpy.ndarray - system matrix - B : numpy.ndarray - input matrix - Q : numpy.ndarray - evaluation function weight for states - Qs : numpy.ndarray - concatenated evaluation function weight for states - R : numpy.ndarray - evaluation function weight for inputs - Rs : numpy.ndarray - concatenated evaluation function weight for inputs - pre_step : int - prediction step - state_size : int - state size of the plant - input_size : int - input size of the plant - dt_input_upper : numpy.ndarray, shape(input_size, ), optional - constraints of input dt, default is None - dt_input_lower : numpy.ndarray, shape(input_size, ), optional - constraints of input dt, default is None - input_upper : numpy.ndarray, shape(input_size, ), optional - constraints of input, default is None - input_lower : numpy.ndarray, shape(input_size, ), optional - constraints of input, default is None - """ - def __init__(self, A, B, Q, R, pre_step, initial_input=None, dt_input_upper=None, dt_input_lower=None, input_upper=None, input_lower=None): - """ - Parameters - ------------ - A : numpy.ndarray - system matrix - B : numpy.ndarray - input matrix - Q : numpy.ndarray - evaluation function weight for states - R : numpy.ndarray - evaluation function weight for inputs - pre_step : int - prediction step - dt_input_upper : numpy.ndarray, shape(input_size, ), optional - constraints of input dt, default is None - dt_input_lower : numpy.ndarray, shape(input_size, ), optional - constraints of input dt, default is None - input_upper : numpy.ndarray, shape(input_size, ), optional - constraints of input, default is None - input_lower : numpy.ndarray, shape(input_size, ), optional - constraints of input, default is None - history_us : list - time history of optimal input us(numpy.ndarray) - """ - self.A = np.array(A) - self.B = np.array(B) - self.Q = np.array(Q) - self.R = np.array(R) - self.pre_step = pre_step - - self.Qs = None - self.Rs = None - - self.state_size = self.A.shape[0] - self.input_size = self.B.shape[1] - - self.history_us = [np.zeros(self.input_size)] - - # initial state - if initial_input is not None: - self.history_us = [initial_input] - - # constraints - self.dt_input_lower = dt_input_lower - self.dt_input_upper = dt_input_upper - self.input_upper = input_upper - self.input_lower = input_lower - - self.W = None - self.omega = None - self.F = None - self.f = None - - def initialize_controller(self): - """ - make matrix to calculate optimal control input - """ - A_factorials = [self.A] - - self.phi_mat = copy.deepcopy(self.A) - - for _ in range(self.pre_step - 1): - temp_mat = np.dot(A_factorials[-1], self.A) - self.phi_mat = np.vstack((self.phi_mat, temp_mat)) - - A_factorials.append(temp_mat) # after we use this factorials - - print("phi_mat = \n{0}".format(self.phi_mat)) - - self.gamma_mat = copy.deepcopy(self.B) - gammma_mat_temp = copy.deepcopy(self.B) - - for i in range(self.pre_step - 1): - temp_1_mat = np.dot(A_factorials[i], self.B) - 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)) - - self.theta_mat = copy.deepcopy(self.gamma_mat) - - for i in range(self.pre_step - 1): - temp_mat = np.zeros_like(self.gamma_mat) - temp_mat[int((i + 1)*self.state_size): , :] = self.gamma_mat[:-int((i + 1)*self.state_size) , :] - - self.theta_mat = np.hstack((self.theta_mat, temp_mat)) - - print("theta_mat = \n{0}".format(self.theta_mat)) - - # evaluation function weight - diag_Qs = np.array([np.diag(self.Q) for _ in range(self.pre_step)]) - diag_Rs = np.array([np.diag(self.R) for _ in range(self.pre_step)]) - - 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)) - - # constraints - # about dt U - if self.input_lower is not None: - # initialize - self.F = np.zeros((self.input_size * 2, self.pre_step * self.input_size)) - for i in range(self.input_size): - 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)) - - for i in range(self.pre_step - 1): - temp_F = copy.deepcopy(temp_F) - - for j in range(self.input_size): - temp_F[j * 2: (j + 1) * 2, ((i+1) * self.input_size) + j] = np.array([1., -1.]) - - self.F = np.vstack((self.F, temp_F)) - - self.F1 = self.F[:, :self.input_size] - - temp_f = [] - - for i in range(self.input_size): - temp_f.append(-1 * self.input_upper[i]) - temp_f.append(self.input_lower[i]) - - 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)) - - # about dt_u - if self.dt_input_lower is not None: - self.W = np.zeros((2, self.pre_step * self.input_size)) - self.W[:, 0] = np.array([1., -1.]) - - for i in range(self.pre_step * self.input_size - 1): - temp_W = np.zeros((2, self.pre_step * self.input_size)) - temp_W[:, i+1] = np.array([1., -1.]) - self.W = np.vstack((self.W, temp_W)) - - temp_omega = [] - - for i in range(self.input_size): - temp_omega.append(self.dt_input_upper[i]) - temp_omega.append(-1. * self.dt_input_lower[i]) - - 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)) - - # about state - print("check the matrix!! if you think rite, plese push enter") - input() - - def calc_input(self, states, references): - """calculate optimal input - Parameters - ----------- - states : numpy.ndarray, shape(state length, ) - current state of system - references : numpy.ndarray, shape(state length * pre_step, ) - reference of the system, you should set this value as reachable goal - - References - ------------ - opt_input : numpy.ndarray, shape(input_length, ) - optimal input - """ - 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)) - - error = references.reshape(-1, 1) - temp_1 - temp_2 - - G = 2. * np.dot(self.theta_mat.T, np.dot(self.Qs, error) ) - - H = np.dot(self.theta_mat.T, np.dot(self.Qs, self.theta_mat)) + self.Rs - - # constraints - A = [] - b = [] - - if self.W is not None: - A.append(self.W) - b.append(self.omega.reshape(-1, 1)) - - if self.F is not None: - b_F = - np.dot(self.F1, self.history_us[-1].reshape(-1, 1)) - self.f.reshape(-1, 1) - A.append(self.F) - b.append(b_F) - - A = np.array(A).reshape(-1, self.input_size * self.pre_step) - - ub = np.array(b).flatten() - - def optimized_func(dt_us): - """ - """ - temp_dt_us = np.array([dt_us[i] for i in range(self.input_size * self.pre_step)]) - - return (np.dot(temp_dt_us, np.dot(H, temp_dt_us.reshape(-1, 1))) - np.dot(G.T, temp_dt_us.reshape(-1, 1)))[0] - - # constraint - lb = np.array([-np.inf for _ in range(len(ub))]) - linear_cons = LinearConstraint(A, lb, ub) - - init_dt_us = np.zeros(self.input_size * self.pre_step) - - # constraint - if self.W is not None or self.F is not None : - opt_result = minimize(optimized_func, init_dt_us, constraints=[linear_cons]) - - opt_dt_us = opt_result.x - - opt_u = opt_dt_us[:self.input_size] + self.history_us[-1] - - # save - self.history_us.append(opt_u) - - return opt_u \ No newline at end of file diff --git a/IOC/test_compare_methods.py b/IOC/test_compare_methods.py deleted file mode 100644 index 5ddaea4..0000000 --- a/IOC/test_compare_methods.py +++ /dev/null @@ -1,211 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt -import math -import copy - -from mpc_func_with_scipy import MpcController as MpcController_scipy -from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt -from control import matlab - -class FirstOrderSystem(): - """FirstOrderSystemWithStates - - Attributes - ----------- - states : float - system states - A : numpy.ndarray - system matrix - B : numpy.ndarray - control matrix - C : numpy.ndarray - observation matrix - history_state : list - time history of state - """ - def __init__(self, A, B, C, D=None, init_states=None): - """ - Parameters - ----------- - A : numpy.ndarray - system matrix - B : numpy.ndarray - control matrix - C : numpy.ndarray - observation matrix - C : numpy.ndarray - directly matrix - init_state : float, optional - initial state of system default is None - history_xs : list - time history of system states - """ - - self.A = A - self.B = B - self.C = C - - if D is not None: - self.D = D - - self.xs = np.zeros(self.A.shape[0]) - - if init_states is not None: - self.xs = copy.deepcopy(init_states) - - self.history_xs = [init_states] - - def update_state(self, u, dt=0.01): - """calculating input - Parameters - ------------ - u : float - input of system in some cases this means the reference - dt : float in seconds, optional - sampling time of simulation, default is 0.01 [s] - """ - temp_x = self.xs.reshape(-1, 1) - temp_u = u.reshape(-1, 1) - - # solve Runge-Kutta - k0 = dt * (np.dot(self.A, temp_x) + np.dot(self.B, temp_u)) - k1 = dt * (np.dot(self.A, temp_x + k0/2.) + np.dot(self.B, temp_u)) - k2 = dt * (np.dot(self.A, temp_x + k1/2.) + np.dot(self.B, temp_u)) - k3 = dt * (np.dot(self.A, temp_x + k2) + np.dot(self.B, temp_u)) - - self.xs += ((k0 + 2 * k1 + 2 * k2 + k3) / 6.).flatten() - - # for oylar - # self.xs += k0.flatten() - - # print("xs = {0}".format(self.xs)) - # a = input() - # save - save_states = copy.deepcopy(self.xs) - self.history_xs.append(save_states) - # print(self.history_xs) - -def main(): - dt = 0.05 - simulation_time = 50 # in seconds - iteration_num = int(simulation_time / dt) - - # you must be care about this matrix - # these A and B are for continuos system if you want to use discret system matrix please skip this step - tau = 0.63 - A = np.array([[-1./tau, 0., 0., 0.], - [0., -1./tau, 0., 0.], - [1., 0., 0., 0.], - [0., 1., 0., 0.]]) - B = np.array([[1./tau, 0.], - [0., 1./tau], - [0., 0.], - [0., 0.]]) - - C = np.eye(4) - D = np.zeros((4, 2)) - - # make simulator with coninuous matrix - init_xs = np.array([0., 0., 0., 0.]) - plant_cvxopt = FirstOrderSystem(A, B, C, init_states=init_xs) - plant_scipy = FirstOrderSystem(A, B, C, init_states=init_xs) - - # create system - sysc = matlab.ss(A, B, C, D) - # discrete system - sysd = matlab.c2d(sysc, dt) - - Ad = sysd.A - Bd = sysd.B - - # evaluation function weight - Q = np.diag([1., 1., 10., 10.]) - R = np.diag([0.01, 0.01]) - pre_step = 5 - - # make controller with discreted matrix - # please check the solver, if you want to use the scipy, set the MpcController_scipy - controller_cvxopt = MpcController_cvxopt(Ad, Bd, Q, R, pre_step, - dt_input_upper=np.array([0.25 * dt, 0.25 * dt]), dt_input_lower=np.array([-0.5 * dt, -0.5 * dt]), - input_upper=np.array([1. ,3.]), input_lower=np.array([-1., -3.])) - - controller_scipy = MpcController_scipy(Ad, Bd, Q, R, pre_step, - dt_input_upper=np.array([0.25 * dt, 0.25 * dt]), dt_input_lower=np.array([-0.5 * dt, -0.5 * dt]), - input_upper=np.array([1. ,3.]), input_lower=np.array([-1., -3.])) - - controller_cvxopt.initialize_controller() - controller_scipy.initialize_controller() - - for i in range(iteration_num): - print("simulation time = {0}".format(i)) - reference = np.array([[0., 0., -5., 7.5] for _ in range(pre_step)]).flatten() - - states_cvxopt = plant_cvxopt.xs - states_scipy = plant_scipy.xs - - opt_u_cvxopt = controller_cvxopt.calc_input(states_cvxopt, reference) - opt_u_scipy = controller_scipy.calc_input(states_scipy, reference) - - plant_cvxopt.update_state(opt_u_cvxopt) - plant_scipy.update_state(opt_u_scipy) - - history_states_cvxopt = np.array(plant_cvxopt.history_xs) - history_states_scipy = np.array(plant_scipy.history_xs) - - time_history_fig = plt.figure(dpi=75) - x_fig = time_history_fig.add_subplot(411) - y_fig = time_history_fig.add_subplot(412) - v_x_fig = time_history_fig.add_subplot(413) - v_y_fig = time_history_fig.add_subplot(414) - - v_x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_cvxopt[:, 0], label="cvxopt") - v_x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_scipy[:, 0], label="scipy", linestyle="dashdot") - v_x_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed") - v_x_fig.set_xlabel("time [s]") - v_x_fig.set_ylabel("v_x") - v_x_fig.legend() - - v_y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_cvxopt[:, 1], label="cvxopt") - v_y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_scipy[:, 1], label="scipy", linestyle="dashdot") - v_y_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed") - v_y_fig.set_xlabel("time [s]") - v_y_fig.set_ylabel("v_y") - v_y_fig.legend() - - x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_cvxopt[:, 2], label="cvxopt") - x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_scipy[:, 2], label="scipy", linestyle="dashdot") - x_fig.plot(np.arange(0, simulation_time+0.01, dt), [-5. for _ in range(iteration_num+1)], linestyle="dashed") - x_fig.set_xlabel("time [s]") - x_fig.set_ylabel("x") - - y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_cvxopt[:, 3], label ="cvxopt") - y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_scipy[:, 3], label="scipy", linestyle="dashdot") - y_fig.plot(np.arange(0, simulation_time+0.01, dt), [7.5 for _ in range(iteration_num+1)], linestyle="dashed") - y_fig.set_xlabel("time [s]") - y_fig.set_ylabel("y") - time_history_fig.tight_layout() - plt.show() - - history_us_cvxopt = np.array(controller_cvxopt.history_us) - history_us_scipy = np.array(controller_scipy.history_us) - - input_history_fig = plt.figure(dpi=75) - u_1_fig = input_history_fig.add_subplot(211) - u_2_fig = input_history_fig.add_subplot(212) - - u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us_cvxopt[:, 0], label="cvxopt") - u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us_scipy[:, 0], label="scipy", linestyle="dashdot") - u_1_fig.set_xlabel("time [s]") - u_1_fig.set_ylabel("u_x") - u_1_fig.legend() - - u_2_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us_cvxopt[:, 1], label="cvxopt") - u_2_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us_scipy[:, 1], label="scipy", linestyle="dashdot") - u_2_fig.set_xlabel("time [s]") - u_2_fig.set_ylabel("u_y") - u_2_fig.legend() - input_history_fig.tight_layout() - plt.show() - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/IOC/README.md b/iLQR/README.md similarity index 100% rename from IOC/README.md rename to iLQR/README.md diff --git a/mpc/extend/iterative_MPC.py b/mpc/extend/extended_MPC.py similarity index 100% rename from mpc/extend/iterative_MPC.py rename to mpc/extend/extended_MPC.py diff --git a/mpc/extend/main_track.py b/mpc/extend/main_track.py index 3b8d6a2..1ee3c57 100644 --- a/mpc/extend/main_track.py +++ b/mpc/extend/main_track.py @@ -4,7 +4,7 @@ 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 3beff53d83983848def7cb866189cf1661c01058 Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Sun, 24 Feb 2019 22:28:21 +0900 Subject: [PATCH 10/13] add iLQR --- iLQR/README.md | 138 ++--------------- iLQR/animation.py | 294 +++++++++++++++++++++++++++++++++++ iLQR/ilqr.py | 380 ++++++++++++++++++++++++++++++++++++++++++++++ iLQR/main.py | 58 +++++++ iLQR/model.py | 177 +++++++++++++++++++++ 5 files changed, 922 insertions(+), 125 deletions(-) create mode 100644 iLQR/animation.py create mode 100644 iLQR/ilqr.py create mode 100644 iLQR/main.py create mode 100644 iLQR/model.py diff --git a/iLQR/README.md b/iLQR/README.md index 0d64664..ed73520 100644 --- a/iLQR/README.md +++ b/iLQR/README.md @@ -1,135 +1,16 @@ -# Model Predictive Control Basic Tool -This program is about template, generic function of linear model predictive control +# Iterative Linear Quadratic Regulator +This program is about iLQR (Iteratice Linear Quadratic Regulator) -# Documentation of the MPC function -Linear model predicitive control should have state equation. -So if you want to use this function, you should model the plant as state equation. -Therefore, the parameters of this class are as following - -**class MpcController()** - -Attributes : - -- A : numpy.ndarray - - system matrix -- B : numpy.ndarray - - input matrix -- Q : numpy.ndarray - - evaluation function weight for states -- Qs : numpy.ndarray - - concatenated evaluation function weight for states -- R : numpy.ndarray - - evaluation function weight for inputs -- Rs : numpy.ndarray - - concatenated evaluation function weight for inputs -- pre_step : int - - prediction step -- state_size : int - - state size of the plant -- input_size : int - - input size of the plant -- dt_input_upper : numpy.ndarray, shape(input_size, ), optional - - constraints of input dt, default is None -- dt_input_lower : numpy.ndarray, shape(input_size, ), optional - - constraints of input dt, default is None -- input_upper : numpy.ndarray, shape(input_size, ), optional - - constraints of input, default is None -- input_lower : numpy.ndarray, shape(input_size, ), optional - - constraints of input, default is None - -Methods: - -- initialize_controller() initialize the controller -- calc_input(states, references) calculating optimal input - -More details, please look the **mpc_func_with_scipy.py** and **mpc_func_with_cvxopt.py** - -We have two function, mpc_func_with_cvxopt.py and mpc_func_with_scipy.py -Both functions have same variable and member function. However the solver is different. -Plese choose the right method for your environment. - -- example of import - -```py -from mpc_func_with_scipy import MpcController as MpcController_scipy -from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt -``` - -# Examples -## Problem Formulation - -- **first order system** - - - -- **ACC (Adaptive cruise control)** - -The two wheeled model are expressed the following equation. - - - -However, if we assume the velocity are constant, we can approximate the equation as following, - - - -then we can apply this model to linear mpc, we should give the model reference V although. - -- **evaluation function** - -the both examples have same evaluation function form as following equation. - - - -- is predicit state by using predict input - -- is reference state - -- is predict amount of change of input - -- are evaluation function weights - -## Expected Results - -- first order system - -- time history - - - -- input - - - -- ACC (Adaptive cruise control) - -- time history of states - - - -- animation - - +# Problem Formulation # Usage -- for example(first order system) -``` -$ python main_example.py -``` -- for example(ACC (Adaptive cruise control)) +# Expected Results -``` -$ python main_ACC.py -``` -- for comparing two methods of optimization solvers - -``` -$ python test_compare_methods.py -``` # Requirement @@ -141,6 +22,13 @@ $ python test_compare_methods.py - python-control # Reference -I`m sorry that main references are written in Japanese -- モデル予測制御―制約のもとでの最適制御 著：Jan M. Maciejowski　訳：足立修一　東京電機大学出版局 \ No newline at end of file +- study wolf +https://github.com/studywolf/control + +- Sergey Levine's lecture +http://rail.eecs.berkeley.edu/deeprlcourse/ + +- Tassa, Y., Erez, T., & Todorov, E. (2012). Synthesis and stabilization of complex behaviors through online trajectory optimization. IEEE International Conference on Intelligent Robots and Systems, 4906–4913. https://doi.org/10.1109/IROS.2012.6386025 + +- Li, W., & Todorov, E. (n.d.). Iterative Linear Quadratic Regulator Design for Nonlinear Biological Movement Systems. Retrieved from https://homes.cs.washington.edu/~todorov/papers/LiICINCO04.pdf \ No newline at end of file diff --git a/iLQR/animation.py b/iLQR/animation.py new file mode 100644 index 0000000..d6970d7 --- /dev/null +++ b/iLQR/animation.py @@ -0,0 +1,294 @@ +import numpy as np +import matplotlib.pyplot as plt +import matplotlib.animation as ani +import matplotlib.font_manager as fon +import sys +import math + +# default setting of figures +plt.rcParams["mathtext.fontset"] = 'stix' # math fonts +plt.rcParams['xtick.direction'] = 'in' # x axis in +plt.rcParams['ytick.direction'] = 'in' # y axis in +plt.rcParams["font.size"] = 10 +plt.rcParams['axes.linewidth'] = 1.0 # axis line width +plt.rcParams['axes.grid'] = True # make grid + +def coordinate_transformation_in_angle(positions, base_angle): + ''' + Transformation the coordinate in the angle + + Parameters + ------- + positions : numpy.ndarray + this parameter is composed of xs, ys + should have (2, N) shape + base_angle : float [rad] + + Returns + ------- + traslated_positions : numpy.ndarray + the shape is (2, N) + + ''' + if positions.shape[0] != 2: + raise ValueError('the input data should have (2, N)') + + positions = np.array(positions) + positions = positions.reshape(2, -1) + + rot_matrix = [[np.cos(base_angle), np.sin(base_angle)], + [-1*np.sin(base_angle), np.cos(base_angle)]] + + rot_matrix = np.array(rot_matrix) + + translated_positions = np.dot(rot_matrix, positions) + + return translated_positions + +def square_make_with_angles(center_x, center_y, size, angle): + ''' + Create square matrix with angle line matrix(2D) + + Parameters + ------- + center_x : float in meters + the center x position of the square + center_y : float in meters + the center y position of the square + size : float in meters + the square's half-size + angle : float in radians + + Returns + ------- + square xs : numpy.ndarray + lenght is 5 (counterclockwise from right-up) + square ys : numpy.ndarray + length is 5 (counterclockwise from right-up) + angle line xs : numpy.ndarray + angle line ys : numpy.ndarray + ''' + + # start with the up right points + # create point in counterclockwise + square_xys = np.array([[size, 0.5 * size], [-size, 0.5 * size], [-size, -0.5 * size], [size, -0.5 * size], [size, 0.5 * size]]) + trans_points = coordinate_transformation_in_angle(square_xys.T, -angle) # this is inverse type + trans_points += np.array([[center_x], [center_y]]) + + square_xs = trans_points[0, :] + square_ys = trans_points[1, :] + + angle_line_xs = [center_x, center_x + math.cos(angle) * size] + angle_line_ys = [center_y, center_y + math.sin(angle) * size] + + 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 + + Attributes + ------------ + cars : + anim_fig : figure of matplotlib + axis : axis of matplotlib + + """ + def __init__(self, objects): + """ + Parameters + ------------ + objects : list of objects + + Notes + --------- + lead_history_states, lead_history_predict_states, traj_ref, history_traj_ref, history_angle_ref + """ + self.car_history_state = objects[0] + self.traget = objects[1] + + self.history_xs = [self.car_history_state[:, 0]] + self.history_ys = [self.car_history_state[:, 1]] + self.history_ths = [self.car_history_state[:, 2]] + + # setting up figure + self.anim_fig = plt.figure(dpi=150) + self.axis = self.anim_fig.add_subplot(111) + + # imgs + self.car_imgs = [] + self.traj_imgs = [] + + def draw_anim(self, interval=50): + """draw the animation and save + + Parameteres + ------------- + interval : int, optional + animation's interval time, you should link the sampling time of systems + default is 50 [ms] + """ + self._set_axis() + self._set_img() + + self.skip_num = 1 + frame_num = int((len(self.history_xs[0])-1) / self.skip_num) + + animation = ani.FuncAnimation(self.anim_fig, self._update_anim, interval=interval, frames=frame_num) + + # self.axis.legend() + print('save_animation?') + shuold_save_animation = int(input()) + + if shuold_save_animation: + print('animation_number?') + num = int(input()) + animation.save('animation_{0}.mp4'.format(num), writer='ffmpeg') + # animation.save("Sample.gif", writer = 'imagemagick') # gif保存 + + plt.show() + + def _set_axis(self): + """ initialize the animation axies + """ + # (1) set the axis name + self.axis.set_xlabel(r'$\it{x}$ [m]') + self.axis.set_ylabel(r'$\it{y}$ [m]') + self.axis.set_aspect('equal', adjustable='box') + + 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 + """ + # object imgs + obj_color_list = ["k", "k", "m", "m"] + obj_styles = ["solid", "solid", "solid", "solid"] + + for i in range(len(obj_color_list)): + temp_img, = self.axis.plot([], [], color=obj_color_list[i], linestyle=obj_styles[i]) + self.car_imgs.append(temp_img) + + 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="b") + self.traj_imgs.append(temp_img) + + def _update_anim(self, i): + """the update animation + this function should be used in the animation functions + + Parameters + ------------ + i : int + time step of the animation + the sampling time should be related to the sampling time of system + + Returns + ----------- + object_imgs : list of img + traj_imgs : list of img + """ + i = int(i * self.skip_num) + + # self._draw_set_axis(i) + self._draw_car(i) + self._draw_traj(i) + # self._draw_prediction(i) + + return self.car_imgs, self.traj_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 + + Parameters + ------------ + i : int + time step of the animation + the sampling time should be related to the sampling time of system + """ + # cars + object_x, object_y, angle_x, angle_y = square_make_with_angles(self.history_xs[0][i], + self.history_ys[0][i], + 1.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]) + + def _draw_traj(self, i): + """ + This private function is just divided thing of + the _update_anim to see the code more clear + + Parameters + ------------ + i : int + time step of the animation + the sampling time should be related to the sampling time of system + """ + # car + self.traj_imgs[0].set_data(self.history_xs[0][:i], self.history_ys[0][:i]) + + # goal + self.traj_imgs[-1].set_data(self.traget[0], self.traget[1]) + + # traj_ref + # self.traj_imgs[2].set_data(self.history_traj_ref[i][0, :], self.history_traj_ref[i][1, :]) \ No newline at end of file diff --git a/iLQR/ilqr.py b/iLQR/ilqr.py new file mode 100644 index 0000000..83816da --- /dev/null +++ b/iLQR/ilqr.py @@ -0,0 +1,380 @@ +import numpy as np +from copy import copy, deepcopy + +from model import TwoWheeledCar + +class iLQRController(): + """ + A controller that implements iterative Linear Quadratic Gaussian control. + Controls the (x, y, th) of the two wheeled car + """ + + def __init__(self, N=100, max_iter=100, dt=0.016): + ''' + n int: length of the control sequence + max_iter int: limit on number of optimization iterations + ''' + self.old_target = [None, None] + + self.tN = N # number of timesteps + self.STATE_SIZE = 3 + self.INPUT_SIZE = 2 + self.dt = dt + + self.max_iter = max_iter + self.lamb_factor = 10 + self.lamb_max = 1e4 + self.eps_converge = 0.001 # exit if relative improvement below threshold + + def calc_input(self, car, x_target, changed=False): + """Generates a control signal to move the + arm to the specified target. + + car : the arm model being controlled NOTE:これが実際にコントロールされるやつ + des list : the desired system position + x_des np.array: desired task-space force, + irrelevant here. + """ + + # if the target has changed, reset things and re-optimize + # for this movement、目標が変わっている場合があるので確認 + if changed: + self.reset(x_target) + + # Reset k if at the end of the sequence + if self.t >= self.tN - 1: # 最初のSTEPのみ計算 + self.t = 0 + + # Compute the optimization + """ + NOTE : ここに条件を追加してもいいかもしれない、何サイクルも回す必要ないし、理想軌道とずれたらとか + """ + if self.t % 1 == 0: + x0 = np.zeros(self.STATE_SIZE) # 初期化、速度は0 + + self.simulator, x0 = self.initialize_simulator(car) # 前の時刻のものを確保 + + U = np.copy(self.U[self.t:]) # 初期入力かなこれ + + self.X, self.U[self.t:], cost = self.ilqr(x0, U) # 入力列が入ってくる + + self.u = self.U[self.t] + + # move us a step forward in our control sequence + self.t += 1 + + return self.u + + def initialize_simulator(self, car): + """ make a copy of the car model, to make sure that the + actual car model isn't affected during the iLQR process + """ + # need to make a copy the real car + simulator = TwoWheeledCar(deepcopy(car.xs)) + + return simulator, deepcopy(simulator.xs) + + def cost(self, xs, us): + """ the immediate state cost function + + Parameters + ------------ + xs : shape(STATE_SIZE, tN + 1) + us : shape(STATE_SIZE, tN) + """ + + """ + NOTE : 拡張する説ありますがとりあえず飛ばします + """ + # total cost + l = np.sum(us**2) + + # compute derivatives of cost + l_x = np.zeros(self.STATE_SIZE) + l_xx = np.zeros((self.STATE_SIZE, self.STATE_SIZE)) + + # quadratic のもののみ計算 + R = 0.01 + l_u = 2. * us * R + l_uu = 2. * np.eye(self.INPUT_SIZE) * R + l_ux = np.zeros((self.INPUT_SIZE, self.STATE_SIZE)) + + # returned in an array for easy multiplication by time step + return l, l_x, l_xx, l_u, l_uu, l_ux + + def cost_final(self, x): + """ the final state cost function + + Parameters + ------------- + xs : shape(STATE_SIZE,) + + Notes : + --------- + l_x = np.zeros((self.STATE_SIZE)) + l_xx = np.zeros((self.STATE_SIZE, self.STATE_SIZE)) + """ + Q_11 = 1.e4 # terminal x cost weight + Q_22 = 1.e4 # terminal y cost weight + Q_33 = 1.e-1 # terminal theta cost weight + + l = np.sum((self.simulator.xs - self.target)**2) + + # about L_x + l_x1 = 2. * (x[0] - self.target[0]) * Q_11 + l_x2 = 2. * (x[1] - self.target[1]) * Q_22 + l_x3 = 2. * (x[2] -self.target[2]) * Q_33 + l_x = np.array([l_x1, l_x2, l_x3]) + + # about l_xx + l_xx = 2. * np.diag([Q_11, Q_22, Q_33]) + + # Final cost only requires these three values + return l, l_x, l_xx + + def finite_differences(self, x, u): + """ calculate gradient of plant dynamics using finite differences + + x np.array: the state of the system + u np.array: the control signal + """ + + A = np.zeros((self.STATE_SIZE, self.STATE_SIZE)) + B = np.zeros((self.STATE_SIZE, self.INPUT_SIZE)) + + eps = 1e-4 # finite differences epsilon + + for ii in range(self.STATE_SIZE): + # calculate partial differential w.r.t. x + inc_x = x.copy() + inc_x[ii] += eps + state_inc,_ = self.plant_dynamics(inc_x, u.copy()) + dec_x = x.copy() + dec_x[ii] -= eps + state_dec,_ = self.plant_dynamics(dec_x, u.copy()) + A[:, ii] = (state_inc - state_dec) / (2 * eps) + + for ii in range(self.INPUT_SIZE): + # calculate partial differential w.r.t. u + inc_u = u.copy() + inc_u[ii] += eps + state_inc,_ = self.plant_dynamics(x.copy(), inc_u) + dec_u = u.copy() + dec_u[ii] -= eps + state_dec,_ = self.plant_dynamics(x.copy(), dec_u) + B[:, ii] = (state_inc - state_dec) / (2 * eps) + + return A, B + + def ilqr(self, x0, U=None): + """ use iterative linear quadratic regulation to find a control + sequence that minimizes the cost function + + x0 np.array: the initial state of the system + U np.array: the initial control trajectory dimensions = [dof, time] + """ + + U = self.U if U is None else U + + lamb = 1.0 # regularization parameter これが正規化項の1つ + sim_new_trajectory = True + tN = U.shape[0] # number of time steps + + for ii in range(self.max_iter): + + if sim_new_trajectory == True: + # simulate forward using the current control trajectory + """ + NOTE: 単純な計算でpredictする + """ + X, cost = self.simulate(x0, U) + oldcost = np.copy(cost) # copy for exit condition check + + # now we linearly approximate the dynamics, and quadratically + # approximate the cost function so we can use LQR methods + + # for storing linearized dynamics + # x(t+1) = f(x(t), u(t)) + """ + NOTE: Gradiantの取得 + """ + f_x = np.zeros((tN, self.STATE_SIZE, self.STATE_SIZE)) # df / dx + f_u = np.zeros((tN, self.STATE_SIZE, self.INPUT_SIZE)) # df / du + # for storing quadratized cost function + + l = np.zeros((tN,1)) # immediate state cost + l_x = np.zeros((tN, self.STATE_SIZE)) # dl / dx + l_xx = np.zeros((tN, self.STATE_SIZE, self.STATE_SIZE)) # d^2 l / dx^2 + l_u = np.zeros((tN, self.INPUT_SIZE)) # dl / du + l_uu = np.zeros((tN, self.INPUT_SIZE, self.INPUT_SIZE)) # d^2 l / du^2 + l_ux = np.zeros((tN, self.INPUT_SIZE, self.STATE_SIZE)) # d^2 l / du / dx + # for everything except final state + for t in range(tN-1): + # x(t+1) = f(x(t), u(t)) = x(t) + dx(t) * dt + # linearized dx(t) = np.dot(A(t), x(t)) + np.dot(B(t), u(t)) + # f_x = np.eye + A(t) + # f_u = B(t) + A, B = self.finite_differences(X[t], U[t]) + f_x[t] = np.eye(self.STATE_SIZE) + A * self.dt + f_u[t] = B * self.dt + + (l[t], l_x[t], l_xx[t], l_u[t], l_uu[t], l_ux[t]) = self.cost(X[t], U[t]) + l[t] *= self.dt + l_x[t] *= self.dt + l_xx[t] *= self.dt + l_u[t] *= self.dt + l_uu[t] *= self.dt + l_ux[t] *= self.dt + + # and for final state + l[-1], l_x[-1], l_xx[-1] = self.cost_final(X[-1]) + + sim_new_trajectory = False + + # optimize things! + # initialize Vs with final state cost and set up k, K + V = l[-1].copy() # value function + V_x = l_x[-1].copy() # dV / dx + V_xx = l_xx[-1].copy() # d^2 V / dx^2 + k = np.zeros((tN, self.INPUT_SIZE)) # feedforward modification + K = np.zeros((tN, self.INPUT_SIZE, self.STATE_SIZE)) # feedback gain + + # NOTE: they use V' to denote the value at the next timestep, + # they have this redundant in their notation making it a + # function of f(x + dx, u + du) and using the ', but it makes for + # convenient shorthand when you drop function dependencies + + # work backwards to solve for V, Q, k, and K + for t in range(self.tN-2, -1, -1): + + # NOTE: we're working backwards, so V_x = V_x[t+1] = V'_x + + # 4a) Q_x = l_x + np.dot(f_x^T, V'_x) + Q_x = l_x[t] + np.dot(f_x[t].T, V_x) + # 4b) Q_u = l_u + np.dot(f_u^T, V'_x) + Q_u = l_u[t] + np.dot(f_u[t].T, V_x) + + # NOTE: last term for Q_xx, Q_uu, and Q_ux is vector / tensor product + # but also note f_xx = f_uu = f_ux = 0 so they're all 0 anyways. + + # 4c) Q_xx = l_xx + np.dot(f_x^T, np.dot(V'_xx, f_x)) + np.einsum(V'_x, f_xx) + Q_xx = l_xx[t] + np.dot(f_x[t].T, np.dot(V_xx, f_x[t])) + # 4d) Q_ux = l_ux + np.dot(f_u^T, np.dot(V'_xx, f_x)) + np.einsum(V'_x, f_ux) + Q_ux = l_ux[t] + np.dot(f_u[t].T, np.dot(V_xx, f_x[t])) + # 4e) Q_uu = l_uu + np.dot(f_u^T, np.dot(V'_xx, f_u)) + np.einsum(V'_x, f_uu) + Q_uu = l_uu[t] + np.dot(f_u[t].T, np.dot(V_xx, f_u[t])) + + # Calculate Q_uu^-1 with regularization term set by + # Levenberg-Marquardt heuristic (at end of this loop) + Q_uu_evals, Q_uu_evecs = np.linalg.eig(Q_uu) + Q_uu_evals[Q_uu_evals < 0] = 0.0 + Q_uu_evals += lamb + Q_uu_inv = np.dot(Q_uu_evecs, np.dot(np.diag(1.0/Q_uu_evals), Q_uu_evecs.T)) + + # 5b) k = -np.dot(Q_uu^-1, Q_u) + k[t] = -np.dot(Q_uu_inv, Q_u) + # 5b) K = -np.dot(Q_uu^-1, Q_ux) + K[t] = -np.dot(Q_uu_inv, Q_ux) + + # 6a) DV = -.5 np.dot(k^T, np.dot(Q_uu, k)) + # 6b) V_x = Q_x - np.dot(K^T, np.dot(Q_uu, k)) + V_x = Q_x - np.dot(K[t].T, np.dot(Q_uu, k[t])) + # 6c) V_xx = Q_xx - np.dot(-K^T, np.dot(Q_uu, K)) + V_xx = Q_xx - np.dot(K[t].T, np.dot(Q_uu, K[t])) + + U_new = np.zeros((tN, self.INPUT_SIZE)) + # calculate the optimal change to the control trajectory + x_new = x0.copy() # 7a) + for t in range(tN - 1): + # use feedforward (k) and feedback (K) gain matrices + # calculated from our value function approximation + # to take a stab at the optimal control signal + U_new[t] = U[t] + k[t] + np.dot(K[t], x_new - X[t]) # 7b) + # given this u, find our next state + _,x_new = self.plant_dynamics(x_new, U_new[t]) # 7c) + + # evaluate the new trajectory + X_new, cost_new = self.simulate(x0, U_new) + + # Levenberg-Marquardt heuristic + if cost_new < cost: + # decrease lambda (get closer to Newton's method) + lamb /= self.lamb_factor + + X = np.copy(X_new) # update trajectory + U = np.copy(U_new) # update control signal + oldcost = np.copy(cost) + cost = np.copy(cost_new) + + sim_new_trajectory = True # do another rollout + + # print("iteration = %d; Cost = %.4f;"%(ii, costnew) + + # " logLambda = %.1f"%np.log(lamb)) + # check to see if update is small enough to exit + if ii > 0 and ((abs(oldcost-cost)/cost) < self.eps_converge): + print("Converged at iteration = %d; Cost = %.4f;"%(ii,cost_new) + + " logLambda = %.1f"%np.log(lamb)) + break + + else: + # increase lambda (get closer to gradient descent) + lamb *= self.lamb_factor + # print("cost: %.4f, increasing lambda to %.4f")%(cost, lamb) + if lamb > self.lamb_max: + print("lambda > max_lambda at iteration = %d;"%ii + + " Cost = %.4f; logLambda = %.1f"%(cost, + np.log(lamb))) + break + + return X, U, cost + + def plant_dynamics(self, x, u): + """ simulate a single time step of the plant, from + initial state x and applying control signal u + + x np.array: the state of the system + u np.array: the control signal + """ + + # set the arm position to x + self.simulator.initialize_state(x) + + # apply the control signal + x_next = self.simulator.update_state(u, self.dt) + + # calculate the change in state + xdot = ((x_next - x) / self.dt).squeeze() + + return xdot, x_next + + def reset(self, target): + """ reset the state of the system """ + + # Index along current control sequence + self.t = 0 + self.U = np.zeros((self.tN, self.INPUT_SIZE)) + self.target = target.copy() + + def simulate(self, x0, U): + """ do a rollout of the system, starting at x0 and + applying the control sequence U + + x0 np.array: the initial state of the system + U np.array: the control sequence to apply + """ + tN = U.shape[0] + X = np.zeros((tN, self.STATE_SIZE)) + X[0] = x0 + cost = 0 + + # Run simulation with substeps + for t in range(tN-1): + _,X[t+1] = self.plant_dynamics(X[t], U[t]) + l, _ , _, _ , _ , _ = self.cost(X[t], U[t]) + cost = cost + self.dt * l + + # Adjust for final cost, subsample trajectory + l_f, _, _ = self.cost_final(X[-1]) + cost = cost + l_f + + return X, cost diff --git a/iLQR/main.py b/iLQR/main.py new file mode 100644 index 0000000..65a0f96 --- /dev/null +++ b/iLQR/main.py @@ -0,0 +1,58 @@ +import numpy as np +import matplotlib.pyplot as plt + +from model import TwoWheeledCar +from ilqr import iLQRController +from animation import AnimDrawer + +def main(): + """ + """ + # iteration parameters + NUM_ITERARIONS = 1000 + dt = 0.01 + + # make plant + init_x = np.array([0., 0., 0.]) + car = TwoWheeledCar(init_x) + + # make goal + target = np.array([5., 3., 0.]) + + # controller + controller = iLQRController() + + + for iteration in range(NUM_ITERARIONS): + print("iteration num = {} / {}".format(iteration, NUM_ITERARIONS)) + + if iteration == 0: + changed = True + + u = controller.calc_input(car, target, changed=changed) + + car.update_state(u, dt) + + # figures and animation + history_states = np.array(car.history_xs) + + time_fig = plt.figure() + + x_fig = time_fig.add_subplot(311) + y_fig = time_fig.add_subplot(312) + th_fig = time_fig.add_subplot(313) + + time = len(history_states) + x_fig.plot(np.arange(time), history_states[:, 0]) + y_fig.plot(np.arange(time), history_states[:, 1]) + th_fig.plot(np.arange(time), history_states[:, 2]) + + plt.show() + + history_states = np.array(car.history_xs) + + animdrawer = AnimDrawer([history_states, target]) + animdrawer.draw_anim() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/iLQR/model.py b/iLQR/model.py new file mode 100644 index 0000000..a09fd40 --- /dev/null +++ b/iLQR/model.py @@ -0,0 +1,177 @@ +import numpy as np +import matplotlib.pyplot as plt +import math +import copy + + +""" +このWheeled modelはコントローラー用 +ホントはbase作って、継承すべきですが省略 +""" +class TwoWheeledCar(): + """SampleSystem, this is the simulator + Attributes + ----------- + xs : numpy.ndarray + system states, [x, y, theta] + history_xs : list + time history of state + """ + def __init__(self, init_states=None): + """ + Palameters + ----------- + init_state : float, optional, shape(3, ) + initial state of system default is None + """ + self.STATE_SIZE = 3 + self.INPUT_SIZE = 2 + + self.xs = np.zeros(3) + + if init_states is not None: + self.xs = copy.deepcopy(init_states) + + self.history_xs = [init_states] + self.history_predict_xs = [] + + def update_state(self, us, dt): + """ + Palameters + ------------ + us : numpy.ndarray + inputs of system in some cases this means the reference + 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(3)] + k1 = [0.0 for _ in range(3)] + k2 = [0.0 for _ in range(3)] + k3 = [0.0 for _ in range(3)] + + functions = [self._func_x_1, self._func_x_2, self._func_x_3] + + # solve Runge-Kutta + for i, func in enumerate(functions): + k0[i] = dt * func(self.xs[0], self.xs[1], self.xs[2], us[0], us[1]) + + for i, func in enumerate(functions): + k1[i] = dt * func(self.xs[0] + k0[0]/2., self.xs[1] + k0[1]/2., self.xs[2] + k0[2]/2., us[0], us[1]) + + for i, func in enumerate(functions): + k2[i] = dt * func(self.xs[0] + k0[0]/2., self.xs[1] + k0[1]/2., self.xs[2] + k0[2]/2., us[0], us[1]) + + for i, func in enumerate(functions): + k3[i] = dt * func(self.xs[0] + k2[0], self.xs[1] + k2[1], self.xs[2] + k2[2], us[0], us[1]) + + self.xs[0] += (k0[0] + 2. * k1[0] + 2. * k2[0] + k3[0]) / 6. + self.xs[1] += (k0[1] + 2. * k1[1] + 2. * k2[1] + k3[1]) / 6. + self.xs[2] += (k0[2] + 2. * k1[2] + 2. * k2[2] + k3[2]) / 6. + + # save + save_states = copy.deepcopy(self.xs) + self.history_xs.append(save_states) + + return self.xs.copy() + + def predict_state(self, init_xs, 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] + """ + ## test + # assert us.shape[0] == 2 and us.shape[1] == 15, "wrong shape" + + xs = copy.deepcopy(init_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] + + # solve Runge-Kutta + for i, func in enumerate(functions): + k0[i] = dt * func(xs[0], xs[1], xs[2], 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., 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., 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], 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. + + predict_xs.append(copy.deepcopy(xs)) + + self.history_predict_xs.append(np.array(predict_xs)) + + return np.array(predict_xs) + + def initialize_state(self, init_xs): + """ + initialize the state + + Parameters + ------------ + init_xs : numpy.ndarray + """ + self.xs = init_xs.flatten() + + def _func_x_1(self, y_1, y_2, y_3, u_1, u_2): + """ + Parameters + ------------ + y_1 : float + y_2 : float + y_3 : float + u_1 : float + system input + u_2 : float + system input + """ + y_dot = math.cos(y_3) * u_1 + return y_dot + + def _func_x_2(self, y_1, y_2, y_3, u_1, u_2): + """ + Parameters + ------------ + y_1 : float + y_2 : float + y_3 : float + u_1 : float + system input + u_2 : float + system input + """ + y_dot = math.sin(y_3) * u_1 + return y_dot + + def _func_x_3(self, y_1, y_2, y_3, u_1, u_2): + """ + Parameters + ------------ + y_1 : float + y_2 : float + y_3 : float + u_1 : float + system input + u_2 : float + system input + """ + y_dot = u_2 + return y_dot \ No newline at end of file From addbaacc7a972ad3ec8cd0ec123f5cdbf39ee246 Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Mon, 25 Feb 2019 18:17:52 +0900 Subject: [PATCH 11/13] modify bag --- iLQR/ilqr.py | 50 ++++++++++++++++++++++++++++++++++++++++---------- 1 file changed, 40 insertions(+), 10 deletions(-) diff --git a/iLQR/ilqr.py b/iLQR/ilqr.py index 83816da..e4edf26 100644 --- a/iLQR/ilqr.py +++ b/iLQR/ilqr.py @@ -87,16 +87,23 @@ class iLQRController(): NOTE : 拡張する説ありますがとりあえず飛ばします """ # total cost - l = np.sum(us**2) + # quadratic のもののみ計算 + R_11 = 1. # terminal u thorottle cost weight + R_22 = 0.01 # terminal u steering cost weight + + l = np.dot(us.T, np.dot(np.diag([R_11, R_22]), us)) # compute derivatives of cost l_x = np.zeros(self.STATE_SIZE) l_xx = np.zeros((self.STATE_SIZE, self.STATE_SIZE)) - # quadratic のもののみ計算 - R = 0.01 - l_u = 2. * us * R - l_uu = 2. * np.eye(self.INPUT_SIZE) * R + l_u1 = 2. * us[0] * R_11 + l_u2 = 2. * us[1] * R_22 + + l_u = np.array([l_u1, l_u2]) + + l_uu = 2. * np.diag([R_11, R_22]) + l_ux = np.zeros((self.INPUT_SIZE, self.STATE_SIZE)) # returned in an array for easy multiplication by time step @@ -114,11 +121,13 @@ class iLQRController(): l_x = np.zeros((self.STATE_SIZE)) l_xx = np.zeros((self.STATE_SIZE, self.STATE_SIZE)) """ - Q_11 = 1.e4 # terminal x cost weight - Q_22 = 1.e4 # terminal y cost weight - Q_33 = 1.e-1 # terminal theta cost weight + Q_11 = 10. # terminal x cost weight + Q_22 = 10. # terminal y cost weight + Q_33 = 0.0001 # terminal theta cost weight - l = np.sum((self.simulator.xs - self.target)**2) + error = self.simulator.xs - self.target + + l = np.dot(error.T, np.dot(np.diag([Q_11, Q_22, Q_33]), error)) # about L_x l_x1 = 2. * (x[0] - self.target[0]) * Q_11 @@ -140,8 +149,12 @@ class iLQRController(): """ A = np.zeros((self.STATE_SIZE, self.STATE_SIZE)) + A_ideal = np.zeros((self.STATE_SIZE, self.STATE_SIZE)) + B = np.zeros((self.STATE_SIZE, self.INPUT_SIZE)) + B_ideal = np.zeros((self.STATE_SIZE, self.INPUT_SIZE)) + eps = 1e-4 # finite differences epsilon for ii in range(self.STATE_SIZE): @@ -154,6 +167,14 @@ class iLQRController(): state_dec,_ = self.plant_dynamics(dec_x, u.copy()) A[:, ii] = (state_inc - state_dec) / (2 * eps) + + A_ideal[0, 2] = -np.sin(x[2]) * u[1] + A_ideal[1, 2] = np.cos(x[2]) * u[1] + + # print("A = \n{}".format(A)) + # print("ideal A = \n{}".format(A_ideal)) + + for ii in range(self.INPUT_SIZE): # calculate partial differential w.r.t. u inc_u = u.copy() @@ -164,7 +185,16 @@ class iLQRController(): state_dec,_ = self.plant_dynamics(x.copy(), dec_u) B[:, ii] = (state_inc - state_dec) / (2 * eps) - return A, B + B_ideal[0, 0] = np.cos(x[2]) + B_ideal[1, 0] = np.sin(x[2]) + B_ideal[2, 1] = 1. + + # print("B = \n{}".format(B)) + # print("ideal B = \n{}".format(B_ideal)) + + # input() + + return A_ideal, B_ideal def ilqr(self, x0, U=None): """ use iterative linear quadratic regulation to find a control From fdce29cb9ed8b33ef4b144359a66d141ec00957f Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Mon, 25 Feb 2019 23:00:35 +0900 Subject: [PATCH 12/13] add README --- README.md | 1 + iLQR/README.md | 33 ++++++++++++++++++++++++++++++--- 2 files changed, 31 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 6e97399..e35db85 100644 --- a/README.md +++ b/README.md @@ -9,6 +9,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** +- **Iterative LQR (linear quadratic regulator)** # Usage and Details you can see the usage in each folder diff --git a/iLQR/README.md b/iLQR/README.md index ed73520..d893b05 100644 --- a/iLQR/README.md +++ b/iLQR/README.md @@ -3,13 +3,43 @@ This program is about iLQR (Iteratice Linear Quadratic Regulator) # Problem Formulation +Two wheeled robot is expressed by the following equation. +It is nonlinear and nonholonomic system. Sometimes, it's extremely difficult to control the +steering(or angular velocity) and velocity of the wheeled robot. Therefore, many methods control only steering, like purepersuit, Linear MPC. +However, sometimes we should consider the velocity and steering simultaneously when the car or robots move fast. +To solve the problem, we should apply the control methods which can treat the nonlinear system. + + + +Nonliner Model Predictive Control is one of the famous methods, so I applied the method in the folder of this repository. +(if you are interested, please look it) + +NMPC is very effecitive method to solve nonlinear optimal control problem but it is a handcraft method. +This program is about one more other methods to solve the nonlinear optimal control problem. + +The method is iterative LQR. +Iterative LQR is one of the DDP(differential dynamic programming) method. +Recently, this method is used in IRL(inverse reinforcement learning), such as GPS(guided policy search) + +If you want to know more about the iLQR, please look the references. +The paper and website is great. # Usage +``` +``` # Expected Results +- static goal + + +- track the goal + + +# Applied other model + # Requirement @@ -17,9 +47,6 @@ This program is about iLQR (Iteratice Linear Quadratic Regulator) - python3.5 or more - numpy - matplotlib -- cvxopt -- scipy1.2.0 or more -- python-control # Reference From 5efca3c31613d9164835058bddeadeeace654426 Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Thu, 28 Feb 2019 13:30:01 +0900 Subject: [PATCH 13/13] delete unnecessary files --- README.md | 1 - iLQR/README.md | 61 ------- iLQR/animation.py | 294 --------------------------------- iLQR/ilqr.py | 410 ---------------------------------------------- iLQR/main.py | 58 ------- iLQR/model.py | 177 -------------------- 6 files changed, 1001 deletions(-) delete mode 100644 iLQR/README.md delete mode 100644 iLQR/animation.py delete mode 100644 iLQR/ilqr.py delete mode 100644 iLQR/main.py delete mode 100644 iLQR/model.py diff --git a/README.md b/README.md index e35db85..6e97399 100644 --- a/README.md +++ b/README.md @@ -9,7 +9,6 @@ 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** -- **Iterative LQR (linear quadratic regulator)** # Usage and Details you can see the usage in each folder diff --git a/iLQR/README.md b/iLQR/README.md deleted file mode 100644 index d893b05..0000000 --- a/iLQR/README.md +++ /dev/null @@ -1,61 +0,0 @@ -# Iterative Linear Quadratic Regulator -This program is about iLQR (Iteratice Linear Quadratic Regulator) - -# Problem Formulation - -Two wheeled robot is expressed by the following equation. -It is nonlinear and nonholonomic system. Sometimes, it's extremely difficult to control the -steering(or angular velocity) and velocity of the wheeled robot. Therefore, many methods control only steering, like purepersuit, Linear MPC. -However, sometimes we should consider the velocity and steering simultaneously when the car or robots move fast. -To solve the problem, we should apply the control methods which can treat the nonlinear system. - - - -Nonliner Model Predictive Control is one of the famous methods, so I applied the method in the folder of this repository. -(if you are interested, please look it) - -NMPC is very effecitive method to solve nonlinear optimal control problem but it is a handcraft method. -This program is about one more other methods to solve the nonlinear optimal control problem. - -The method is iterative LQR. -Iterative LQR is one of the DDP(differential dynamic programming) method. -Recently, this method is used in IRL(inverse reinforcement learning), such as GPS(guided policy search) - -If you want to know more about the iLQR, please look the references. -The paper and website is great. - -# Usage - -``` - -``` - -# Expected Results - -- static goal - - -- track the goal - - -# Applied other model - - - -# Requirement - -- python3.5 or more -- numpy -- matplotlib - -# Reference - -- study wolf -https://github.com/studywolf/control - -- Sergey Levine's lecture -http://rail.eecs.berkeley.edu/deeprlcourse/ - -- Tassa, Y., Erez, T., & Todorov, E. (2012). Synthesis and stabilization of complex behaviors through online trajectory optimization. IEEE International Conference on Intelligent Robots and Systems, 4906–4913. https://doi.org/10.1109/IROS.2012.6386025 - -- Li, W., & Todorov, E. (n.d.). Iterative Linear Quadratic Regulator Design for Nonlinear Biological Movement Systems. Retrieved from https://homes.cs.washington.edu/~todorov/papers/LiICINCO04.pdf \ No newline at end of file diff --git a/iLQR/animation.py b/iLQR/animation.py deleted file mode 100644 index d6970d7..0000000 --- a/iLQR/animation.py +++ /dev/null @@ -1,294 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt -import matplotlib.animation as ani -import matplotlib.font_manager as fon -import sys -import math - -# default setting of figures -plt.rcParams["mathtext.fontset"] = 'stix' # math fonts -plt.rcParams['xtick.direction'] = 'in' # x axis in -plt.rcParams['ytick.direction'] = 'in' # y axis in -plt.rcParams["font.size"] = 10 -plt.rcParams['axes.linewidth'] = 1.0 # axis line width -plt.rcParams['axes.grid'] = True # make grid - -def coordinate_transformation_in_angle(positions, base_angle): - ''' - Transformation the coordinate in the angle - - Parameters - ------- - positions : numpy.ndarray - this parameter is composed of xs, ys - should have (2, N) shape - base_angle : float [rad] - - Returns - ------- - traslated_positions : numpy.ndarray - the shape is (2, N) - - ''' - if positions.shape[0] != 2: - raise ValueError('the input data should have (2, N)') - - positions = np.array(positions) - positions = positions.reshape(2, -1) - - rot_matrix = [[np.cos(base_angle), np.sin(base_angle)], - [-1*np.sin(base_angle), np.cos(base_angle)]] - - rot_matrix = np.array(rot_matrix) - - translated_positions = np.dot(rot_matrix, positions) - - return translated_positions - -def square_make_with_angles(center_x, center_y, size, angle): - ''' - Create square matrix with angle line matrix(2D) - - Parameters - ------- - center_x : float in meters - the center x position of the square - center_y : float in meters - the center y position of the square - size : float in meters - the square's half-size - angle : float in radians - - Returns - ------- - square xs : numpy.ndarray - lenght is 5 (counterclockwise from right-up) - square ys : numpy.ndarray - length is 5 (counterclockwise from right-up) - angle line xs : numpy.ndarray - angle line ys : numpy.ndarray - ''' - - # start with the up right points - # create point in counterclockwise - square_xys = np.array([[size, 0.5 * size], [-size, 0.5 * size], [-size, -0.5 * size], [size, -0.5 * size], [size, 0.5 * size]]) - trans_points = coordinate_transformation_in_angle(square_xys.T, -angle) # this is inverse type - trans_points += np.array([[center_x], [center_y]]) - - square_xs = trans_points[0, :] - square_ys = trans_points[1, :] - - angle_line_xs = [center_x, center_x + math.cos(angle) * size] - angle_line_ys = [center_y, center_y + math.sin(angle) * size] - - 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 - - Attributes - ------------ - cars : - anim_fig : figure of matplotlib - axis : axis of matplotlib - - """ - def __init__(self, objects): - """ - Parameters - ------------ - objects : list of objects - - Notes - --------- - lead_history_states, lead_history_predict_states, traj_ref, history_traj_ref, history_angle_ref - """ - self.car_history_state = objects[0] - self.traget = objects[1] - - self.history_xs = [self.car_history_state[:, 0]] - self.history_ys = [self.car_history_state[:, 1]] - self.history_ths = [self.car_history_state[:, 2]] - - # setting up figure - self.anim_fig = plt.figure(dpi=150) - self.axis = self.anim_fig.add_subplot(111) - - # imgs - self.car_imgs = [] - self.traj_imgs = [] - - def draw_anim(self, interval=50): - """draw the animation and save - - Parameteres - ------------- - interval : int, optional - animation's interval time, you should link the sampling time of systems - default is 50 [ms] - """ - self._set_axis() - self._set_img() - - self.skip_num = 1 - frame_num = int((len(self.history_xs[0])-1) / self.skip_num) - - animation = ani.FuncAnimation(self.anim_fig, self._update_anim, interval=interval, frames=frame_num) - - # self.axis.legend() - print('save_animation?') - shuold_save_animation = int(input()) - - if shuold_save_animation: - print('animation_number?') - num = int(input()) - animation.save('animation_{0}.mp4'.format(num), writer='ffmpeg') - # animation.save("Sample.gif", writer = 'imagemagick') # gif保存 - - plt.show() - - def _set_axis(self): - """ initialize the animation axies - """ - # (1) set the axis name - self.axis.set_xlabel(r'$\it{x}$ [m]') - self.axis.set_ylabel(r'$\it{y}$ [m]') - self.axis.set_aspect('equal', adjustable='box') - - 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 - """ - # object imgs - obj_color_list = ["k", "k", "m", "m"] - obj_styles = ["solid", "solid", "solid", "solid"] - - for i in range(len(obj_color_list)): - temp_img, = self.axis.plot([], [], color=obj_color_list[i], linestyle=obj_styles[i]) - self.car_imgs.append(temp_img) - - 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="b") - self.traj_imgs.append(temp_img) - - def _update_anim(self, i): - """the update animation - this function should be used in the animation functions - - Parameters - ------------ - i : int - time step of the animation - the sampling time should be related to the sampling time of system - - Returns - ----------- - object_imgs : list of img - traj_imgs : list of img - """ - i = int(i * self.skip_num) - - # self._draw_set_axis(i) - self._draw_car(i) - self._draw_traj(i) - # self._draw_prediction(i) - - return self.car_imgs, self.traj_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 - - Parameters - ------------ - i : int - time step of the animation - the sampling time should be related to the sampling time of system - """ - # cars - object_x, object_y, angle_x, angle_y = square_make_with_angles(self.history_xs[0][i], - self.history_ys[0][i], - 1.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]) - - def _draw_traj(self, i): - """ - This private function is just divided thing of - the _update_anim to see the code more clear - - Parameters - ------------ - i : int - time step of the animation - the sampling time should be related to the sampling time of system - """ - # car - self.traj_imgs[0].set_data(self.history_xs[0][:i], self.history_ys[0][:i]) - - # goal - self.traj_imgs[-1].set_data(self.traget[0], self.traget[1]) - - # traj_ref - # self.traj_imgs[2].set_data(self.history_traj_ref[i][0, :], self.history_traj_ref[i][1, :]) \ No newline at end of file diff --git a/iLQR/ilqr.py b/iLQR/ilqr.py deleted file mode 100644 index e4edf26..0000000 --- a/iLQR/ilqr.py +++ /dev/null @@ -1,410 +0,0 @@ -import numpy as np -from copy import copy, deepcopy - -from model import TwoWheeledCar - -class iLQRController(): - """ - A controller that implements iterative Linear Quadratic Gaussian control. - Controls the (x, y, th) of the two wheeled car - """ - - def __init__(self, N=100, max_iter=100, dt=0.016): - ''' - n int: length of the control sequence - max_iter int: limit on number of optimization iterations - ''' - self.old_target = [None, None] - - self.tN = N # number of timesteps - self.STATE_SIZE = 3 - self.INPUT_SIZE = 2 - self.dt = dt - - self.max_iter = max_iter - self.lamb_factor = 10 - self.lamb_max = 1e4 - self.eps_converge = 0.001 # exit if relative improvement below threshold - - def calc_input(self, car, x_target, changed=False): - """Generates a control signal to move the - arm to the specified target. - - car : the arm model being controlled NOTE:これが実際にコントロールされるやつ - des list : the desired system position - x_des np.array: desired task-space force, - irrelevant here. - """ - - # if the target has changed, reset things and re-optimize - # for this movement、目標が変わっている場合があるので確認 - if changed: - self.reset(x_target) - - # Reset k if at the end of the sequence - if self.t >= self.tN - 1: # 最初のSTEPのみ計算 - self.t = 0 - - # Compute the optimization - """ - NOTE : ここに条件を追加してもいいかもしれない、何サイクルも回す必要ないし、理想軌道とずれたらとか - """ - if self.t % 1 == 0: - x0 = np.zeros(self.STATE_SIZE) # 初期化、速度は0 - - self.simulator, x0 = self.initialize_simulator(car) # 前の時刻のものを確保 - - U = np.copy(self.U[self.t:]) # 初期入力かなこれ - - self.X, self.U[self.t:], cost = self.ilqr(x0, U) # 入力列が入ってくる - - self.u = self.U[self.t] - - # move us a step forward in our control sequence - self.t += 1 - - return self.u - - def initialize_simulator(self, car): - """ make a copy of the car model, to make sure that the - actual car model isn't affected during the iLQR process - """ - # need to make a copy the real car - simulator = TwoWheeledCar(deepcopy(car.xs)) - - return simulator, deepcopy(simulator.xs) - - def cost(self, xs, us): - """ the immediate state cost function - - Parameters - ------------ - xs : shape(STATE_SIZE, tN + 1) - us : shape(STATE_SIZE, tN) - """ - - """ - NOTE : 拡張する説ありますがとりあえず飛ばします - """ - # total cost - # quadratic のもののみ計算 - R_11 = 1. # terminal u thorottle cost weight - R_22 = 0.01 # terminal u steering cost weight - - l = np.dot(us.T, np.dot(np.diag([R_11, R_22]), us)) - - # compute derivatives of cost - l_x = np.zeros(self.STATE_SIZE) - l_xx = np.zeros((self.STATE_SIZE, self.STATE_SIZE)) - - l_u1 = 2. * us[0] * R_11 - l_u2 = 2. * us[1] * R_22 - - l_u = np.array([l_u1, l_u2]) - - l_uu = 2. * np.diag([R_11, R_22]) - - l_ux = np.zeros((self.INPUT_SIZE, self.STATE_SIZE)) - - # returned in an array for easy multiplication by time step - return l, l_x, l_xx, l_u, l_uu, l_ux - - def cost_final(self, x): - """ the final state cost function - - Parameters - ------------- - xs : shape(STATE_SIZE,) - - Notes : - --------- - l_x = np.zeros((self.STATE_SIZE)) - l_xx = np.zeros((self.STATE_SIZE, self.STATE_SIZE)) - """ - Q_11 = 10. # terminal x cost weight - Q_22 = 10. # terminal y cost weight - Q_33 = 0.0001 # terminal theta cost weight - - error = self.simulator.xs - self.target - - l = np.dot(error.T, np.dot(np.diag([Q_11, Q_22, Q_33]), error)) - - # about L_x - l_x1 = 2. * (x[0] - self.target[0]) * Q_11 - l_x2 = 2. * (x[1] - self.target[1]) * Q_22 - l_x3 = 2. * (x[2] -self.target[2]) * Q_33 - l_x = np.array([l_x1, l_x2, l_x3]) - - # about l_xx - l_xx = 2. * np.diag([Q_11, Q_22, Q_33]) - - # Final cost only requires these three values - return l, l_x, l_xx - - def finite_differences(self, x, u): - """ calculate gradient of plant dynamics using finite differences - - x np.array: the state of the system - u np.array: the control signal - """ - - A = np.zeros((self.STATE_SIZE, self.STATE_SIZE)) - A_ideal = np.zeros((self.STATE_SIZE, self.STATE_SIZE)) - - B = np.zeros((self.STATE_SIZE, self.INPUT_SIZE)) - B_ideal = np.zeros((self.STATE_SIZE, self.INPUT_SIZE)) - - - eps = 1e-4 # finite differences epsilon - - for ii in range(self.STATE_SIZE): - # calculate partial differential w.r.t. x - inc_x = x.copy() - inc_x[ii] += eps - state_inc,_ = self.plant_dynamics(inc_x, u.copy()) - dec_x = x.copy() - dec_x[ii] -= eps - state_dec,_ = self.plant_dynamics(dec_x, u.copy()) - A[:, ii] = (state_inc - state_dec) / (2 * eps) - - - A_ideal[0, 2] = -np.sin(x[2]) * u[1] - A_ideal[1, 2] = np.cos(x[2]) * u[1] - - # print("A = \n{}".format(A)) - # print("ideal A = \n{}".format(A_ideal)) - - - for ii in range(self.INPUT_SIZE): - # calculate partial differential w.r.t. u - inc_u = u.copy() - inc_u[ii] += eps - state_inc,_ = self.plant_dynamics(x.copy(), inc_u) - dec_u = u.copy() - dec_u[ii] -= eps - state_dec,_ = self.plant_dynamics(x.copy(), dec_u) - B[:, ii] = (state_inc - state_dec) / (2 * eps) - - B_ideal[0, 0] = np.cos(x[2]) - B_ideal[1, 0] = np.sin(x[2]) - B_ideal[2, 1] = 1. - - # print("B = \n{}".format(B)) - # print("ideal B = \n{}".format(B_ideal)) - - # input() - - return A_ideal, B_ideal - - def ilqr(self, x0, U=None): - """ use iterative linear quadratic regulation to find a control - sequence that minimizes the cost function - - x0 np.array: the initial state of the system - U np.array: the initial control trajectory dimensions = [dof, time] - """ - - U = self.U if U is None else U - - lamb = 1.0 # regularization parameter これが正規化項の1つ - sim_new_trajectory = True - tN = U.shape[0] # number of time steps - - for ii in range(self.max_iter): - - if sim_new_trajectory == True: - # simulate forward using the current control trajectory - """ - NOTE: 単純な計算でpredictする - """ - X, cost = self.simulate(x0, U) - oldcost = np.copy(cost) # copy for exit condition check - - # now we linearly approximate the dynamics, and quadratically - # approximate the cost function so we can use LQR methods - - # for storing linearized dynamics - # x(t+1) = f(x(t), u(t)) - """ - NOTE: Gradiantの取得 - """ - f_x = np.zeros((tN, self.STATE_SIZE, self.STATE_SIZE)) # df / dx - f_u = np.zeros((tN, self.STATE_SIZE, self.INPUT_SIZE)) # df / du - # for storing quadratized cost function - - l = np.zeros((tN,1)) # immediate state cost - l_x = np.zeros((tN, self.STATE_SIZE)) # dl / dx - l_xx = np.zeros((tN, self.STATE_SIZE, self.STATE_SIZE)) # d^2 l / dx^2 - l_u = np.zeros((tN, self.INPUT_SIZE)) # dl / du - l_uu = np.zeros((tN, self.INPUT_SIZE, self.INPUT_SIZE)) # d^2 l / du^2 - l_ux = np.zeros((tN, self.INPUT_SIZE, self.STATE_SIZE)) # d^2 l / du / dx - # for everything except final state - for t in range(tN-1): - # x(t+1) = f(x(t), u(t)) = x(t) + dx(t) * dt - # linearized dx(t) = np.dot(A(t), x(t)) + np.dot(B(t), u(t)) - # f_x = np.eye + A(t) - # f_u = B(t) - A, B = self.finite_differences(X[t], U[t]) - f_x[t] = np.eye(self.STATE_SIZE) + A * self.dt - f_u[t] = B * self.dt - - (l[t], l_x[t], l_xx[t], l_u[t], l_uu[t], l_ux[t]) = self.cost(X[t], U[t]) - l[t] *= self.dt - l_x[t] *= self.dt - l_xx[t] *= self.dt - l_u[t] *= self.dt - l_uu[t] *= self.dt - l_ux[t] *= self.dt - - # and for final state - l[-1], l_x[-1], l_xx[-1] = self.cost_final(X[-1]) - - sim_new_trajectory = False - - # optimize things! - # initialize Vs with final state cost and set up k, K - V = l[-1].copy() # value function - V_x = l_x[-1].copy() # dV / dx - V_xx = l_xx[-1].copy() # d^2 V / dx^2 - k = np.zeros((tN, self.INPUT_SIZE)) # feedforward modification - K = np.zeros((tN, self.INPUT_SIZE, self.STATE_SIZE)) # feedback gain - - # NOTE: they use V' to denote the value at the next timestep, - # they have this redundant in their notation making it a - # function of f(x + dx, u + du) and using the ', but it makes for - # convenient shorthand when you drop function dependencies - - # work backwards to solve for V, Q, k, and K - for t in range(self.tN-2, -1, -1): - - # NOTE: we're working backwards, so V_x = V_x[t+1] = V'_x - - # 4a) Q_x = l_x + np.dot(f_x^T, V'_x) - Q_x = l_x[t] + np.dot(f_x[t].T, V_x) - # 4b) Q_u = l_u + np.dot(f_u^T, V'_x) - Q_u = l_u[t] + np.dot(f_u[t].T, V_x) - - # NOTE: last term for Q_xx, Q_uu, and Q_ux is vector / tensor product - # but also note f_xx = f_uu = f_ux = 0 so they're all 0 anyways. - - # 4c) Q_xx = l_xx + np.dot(f_x^T, np.dot(V'_xx, f_x)) + np.einsum(V'_x, f_xx) - Q_xx = l_xx[t] + np.dot(f_x[t].T, np.dot(V_xx, f_x[t])) - # 4d) Q_ux = l_ux + np.dot(f_u^T, np.dot(V'_xx, f_x)) + np.einsum(V'_x, f_ux) - Q_ux = l_ux[t] + np.dot(f_u[t].T, np.dot(V_xx, f_x[t])) - # 4e) Q_uu = l_uu + np.dot(f_u^T, np.dot(V'_xx, f_u)) + np.einsum(V'_x, f_uu) - Q_uu = l_uu[t] + np.dot(f_u[t].T, np.dot(V_xx, f_u[t])) - - # Calculate Q_uu^-1 with regularization term set by - # Levenberg-Marquardt heuristic (at end of this loop) - Q_uu_evals, Q_uu_evecs = np.linalg.eig(Q_uu) - Q_uu_evals[Q_uu_evals < 0] = 0.0 - Q_uu_evals += lamb - Q_uu_inv = np.dot(Q_uu_evecs, np.dot(np.diag(1.0/Q_uu_evals), Q_uu_evecs.T)) - - # 5b) k = -np.dot(Q_uu^-1, Q_u) - k[t] = -np.dot(Q_uu_inv, Q_u) - # 5b) K = -np.dot(Q_uu^-1, Q_ux) - K[t] = -np.dot(Q_uu_inv, Q_ux) - - # 6a) DV = -.5 np.dot(k^T, np.dot(Q_uu, k)) - # 6b) V_x = Q_x - np.dot(K^T, np.dot(Q_uu, k)) - V_x = Q_x - np.dot(K[t].T, np.dot(Q_uu, k[t])) - # 6c) V_xx = Q_xx - np.dot(-K^T, np.dot(Q_uu, K)) - V_xx = Q_xx - np.dot(K[t].T, np.dot(Q_uu, K[t])) - - U_new = np.zeros((tN, self.INPUT_SIZE)) - # calculate the optimal change to the control trajectory - x_new = x0.copy() # 7a) - for t in range(tN - 1): - # use feedforward (k) and feedback (K) gain matrices - # calculated from our value function approximation - # to take a stab at the optimal control signal - U_new[t] = U[t] + k[t] + np.dot(K[t], x_new - X[t]) # 7b) - # given this u, find our next state - _,x_new = self.plant_dynamics(x_new, U_new[t]) # 7c) - - # evaluate the new trajectory - X_new, cost_new = self.simulate(x0, U_new) - - # Levenberg-Marquardt heuristic - if cost_new < cost: - # decrease lambda (get closer to Newton's method) - lamb /= self.lamb_factor - - X = np.copy(X_new) # update trajectory - U = np.copy(U_new) # update control signal - oldcost = np.copy(cost) - cost = np.copy(cost_new) - - sim_new_trajectory = True # do another rollout - - # print("iteration = %d; Cost = %.4f;"%(ii, costnew) + - # " logLambda = %.1f"%np.log(lamb)) - # check to see if update is small enough to exit - if ii > 0 and ((abs(oldcost-cost)/cost) < self.eps_converge): - print("Converged at iteration = %d; Cost = %.4f;"%(ii,cost_new) + - " logLambda = %.1f"%np.log(lamb)) - break - - else: - # increase lambda (get closer to gradient descent) - lamb *= self.lamb_factor - # print("cost: %.4f, increasing lambda to %.4f")%(cost, lamb) - if lamb > self.lamb_max: - print("lambda > max_lambda at iteration = %d;"%ii + - " Cost = %.4f; logLambda = %.1f"%(cost, - np.log(lamb))) - break - - return X, U, cost - - def plant_dynamics(self, x, u): - """ simulate a single time step of the plant, from - initial state x and applying control signal u - - x np.array: the state of the system - u np.array: the control signal - """ - - # set the arm position to x - self.simulator.initialize_state(x) - - # apply the control signal - x_next = self.simulator.update_state(u, self.dt) - - # calculate the change in state - xdot = ((x_next - x) / self.dt).squeeze() - - return xdot, x_next - - def reset(self, target): - """ reset the state of the system """ - - # Index along current control sequence - self.t = 0 - self.U = np.zeros((self.tN, self.INPUT_SIZE)) - self.target = target.copy() - - def simulate(self, x0, U): - """ do a rollout of the system, starting at x0 and - applying the control sequence U - - x0 np.array: the initial state of the system - U np.array: the control sequence to apply - """ - tN = U.shape[0] - X = np.zeros((tN, self.STATE_SIZE)) - X[0] = x0 - cost = 0 - - # Run simulation with substeps - for t in range(tN-1): - _,X[t+1] = self.plant_dynamics(X[t], U[t]) - l, _ , _, _ , _ , _ = self.cost(X[t], U[t]) - cost = cost + self.dt * l - - # Adjust for final cost, subsample trajectory - l_f, _, _ = self.cost_final(X[-1]) - cost = cost + l_f - - return X, cost diff --git a/iLQR/main.py b/iLQR/main.py deleted file mode 100644 index 65a0f96..0000000 --- a/iLQR/main.py +++ /dev/null @@ -1,58 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt - -from model import TwoWheeledCar -from ilqr import iLQRController -from animation import AnimDrawer - -def main(): - """ - """ - # iteration parameters - NUM_ITERARIONS = 1000 - dt = 0.01 - - # make plant - init_x = np.array([0., 0., 0.]) - car = TwoWheeledCar(init_x) - - # make goal - target = np.array([5., 3., 0.]) - - # controller - controller = iLQRController() - - - for iteration in range(NUM_ITERARIONS): - print("iteration num = {} / {}".format(iteration, NUM_ITERARIONS)) - - if iteration == 0: - changed = True - - u = controller.calc_input(car, target, changed=changed) - - car.update_state(u, dt) - - # figures and animation - history_states = np.array(car.history_xs) - - time_fig = plt.figure() - - x_fig = time_fig.add_subplot(311) - y_fig = time_fig.add_subplot(312) - th_fig = time_fig.add_subplot(313) - - time = len(history_states) - x_fig.plot(np.arange(time), history_states[:, 0]) - y_fig.plot(np.arange(time), history_states[:, 1]) - th_fig.plot(np.arange(time), history_states[:, 2]) - - plt.show() - - history_states = np.array(car.history_xs) - - animdrawer = AnimDrawer([history_states, target]) - animdrawer.draw_anim() - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/iLQR/model.py b/iLQR/model.py deleted file mode 100644 index a09fd40..0000000 --- a/iLQR/model.py +++ /dev/null @@ -1,177 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt -import math -import copy - - -""" -このWheeled modelはコントローラー用 -ホントはbase作って、継承すべきですが省略 -""" -class TwoWheeledCar(): - """SampleSystem, this is the simulator - Attributes - ----------- - xs : numpy.ndarray - system states, [x, y, theta] - history_xs : list - time history of state - """ - def __init__(self, init_states=None): - """ - Palameters - ----------- - init_state : float, optional, shape(3, ) - initial state of system default is None - """ - self.STATE_SIZE = 3 - self.INPUT_SIZE = 2 - - self.xs = np.zeros(3) - - if init_states is not None: - self.xs = copy.deepcopy(init_states) - - self.history_xs = [init_states] - self.history_predict_xs = [] - - def update_state(self, us, dt): - """ - Palameters - ------------ - us : numpy.ndarray - inputs of system in some cases this means the reference - 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(3)] - k1 = [0.0 for _ in range(3)] - k2 = [0.0 for _ in range(3)] - k3 = [0.0 for _ in range(3)] - - functions = [self._func_x_1, self._func_x_2, self._func_x_3] - - # solve Runge-Kutta - for i, func in enumerate(functions): - k0[i] = dt * func(self.xs[0], self.xs[1], self.xs[2], us[0], us[1]) - - for i, func in enumerate(functions): - k1[i] = dt * func(self.xs[0] + k0[0]/2., self.xs[1] + k0[1]/2., self.xs[2] + k0[2]/2., us[0], us[1]) - - for i, func in enumerate(functions): - k2[i] = dt * func(self.xs[0] + k0[0]/2., self.xs[1] + k0[1]/2., self.xs[2] + k0[2]/2., us[0], us[1]) - - for i, func in enumerate(functions): - k3[i] = dt * func(self.xs[0] + k2[0], self.xs[1] + k2[1], self.xs[2] + k2[2], us[0], us[1]) - - self.xs[0] += (k0[0] + 2. * k1[0] + 2. * k2[0] + k3[0]) / 6. - self.xs[1] += (k0[1] + 2. * k1[1] + 2. * k2[1] + k3[1]) / 6. - self.xs[2] += (k0[2] + 2. * k1[2] + 2. * k2[2] + k3[2]) / 6. - - # save - save_states = copy.deepcopy(self.xs) - self.history_xs.append(save_states) - - return self.xs.copy() - - def predict_state(self, init_xs, 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] - """ - ## test - # assert us.shape[0] == 2 and us.shape[1] == 15, "wrong shape" - - xs = copy.deepcopy(init_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] - - # solve Runge-Kutta - for i, func in enumerate(functions): - k0[i] = dt * func(xs[0], xs[1], xs[2], 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., 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., 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], 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. - - predict_xs.append(copy.deepcopy(xs)) - - self.history_predict_xs.append(np.array(predict_xs)) - - return np.array(predict_xs) - - def initialize_state(self, init_xs): - """ - initialize the state - - Parameters - ------------ - init_xs : numpy.ndarray - """ - self.xs = init_xs.flatten() - - def _func_x_1(self, y_1, y_2, y_3, u_1, u_2): - """ - Parameters - ------------ - y_1 : float - y_2 : float - y_3 : float - u_1 : float - system input - u_2 : float - system input - """ - y_dot = math.cos(y_3) * u_1 - return y_dot - - def _func_x_2(self, y_1, y_2, y_3, u_1, u_2): - """ - Parameters - ------------ - y_1 : float - y_2 : float - y_3 : float - u_1 : float - system input - u_2 : float - system input - """ - y_dot = math.sin(y_3) * u_1 - return y_dot - - def _func_x_3(self, y_1, y_2, y_3, u_1, u_2): - """ - Parameters - ------------ - y_1 : float - y_2 : float - y_3 : float - u_1 : float - system input - u_2 : float - system input - """ - y_dot = u_2 - return y_dot \ No newline at end of file