From d1ff16bfbab7048734aff68960ffcc7de23a96cd Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Tue, 5 Feb 2019 22:36:56 +0900 Subject: [PATCH] add mpc with noise --- mpc/with_disturbance/animation.py | 233 ++++++++++++++ mpc/{basic => with_disturbance}/main_track.py | 85 ++--- mpc/with_disturbance/mpc_func_with_cvxopt.py | 297 ++++++++++++++++++ 3 files changed, 577 insertions(+), 38 deletions(-) create mode 100755 mpc/with_disturbance/animation.py rename mpc/{basic => with_disturbance}/main_track.py (77%) create mode 100644 mpc/with_disturbance/mpc_func_with_cvxopt.py diff --git a/mpc/with_disturbance/animation.py b/mpc/with_disturbance/animation.py new file mode 100755 index 0000000..6ece541 --- /dev/null +++ b/mpc/with_disturbance/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/main_track.py b/mpc/with_disturbance/main_track.py similarity index 77% rename from mpc/basic/main_track.py rename to mpc/with_disturbance/main_track.py index 39ca699..fbbafdd 100644 --- a/mpc/basic/main_track.py +++ b/mpc/with_disturbance/main_track.py @@ -9,10 +9,12 @@ from control import matlab class WheeledSystem(): """SampleSystem, this is the simulator + Kinematic model of car + Attributes ----------- xs : numpy.ndarray - system states, [x, y, theta] + system states, [x, y, phi, beta] history_xs : list time history of state """ @@ -23,7 +25,11 @@ class WheeledSystem(): init_state : float, optional, shape(3, ) initial state of system default is None """ - self.xs = np.zeros(3) + self.NUM_STATE = 4 + self.xs = np.zeros(self.NUM_STATE) + + self.FRONT_WHEELE_BASE = 1.0 + self.REAR_WHEELE_BASE = 1.0 if init_states is not None: self.xs = copy.deepcopy(init_states) @@ -40,36 +46,37 @@ class WheeledSystem(): 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)] + 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] + 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(self.xs[0], self.xs[1], self.xs[2], us[0], us[1]) + k0[i] = dt * func(self.xs[0], self.xs[1], self.xs[2], self.xs[3], 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]) + k1[i] = dt * func(self.xs[0] + k0[0]/2., self.xs[1] + k0[1]/2., self.xs[2] + k0[2]/2., self.xs[3] + k0[3]/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]) + k2[i] = dt * func(self.xs[0] + k1[0]/2., self.xs[1] + k1[1]/2., self.xs[2] + k1[2]/2., self.xs[3] + k1[3]/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]) + k3[i] = dt * func(self.xs[0] + k2[0], self.xs[1] + k2[1], self.xs[2] + k2[2], self.xs[3] + k2[3], 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. + self.xs[3] += (k0[3] + 2. * k1[3] + 2. * k2[3] + k3[3]) / 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): + def _func_x_1(self, y_1, y_2, y_3, y_4, u_1, u_2): """ Parameters ------------ @@ -81,10 +88,10 @@ class WheeledSystem(): u_2 : float system input """ - y_dot = math.cos(y_3) * u_1 + y_dot = u_1 * math.cos(y_3 + y_4) return y_dot - def _func_x_2(self, y_1, y_2, y_3, u_1, u_2): + def _func_x_2(self, y_1, y_2, y_3, y_4, u_1, u_2): """ Parameters ------------ @@ -96,10 +103,10 @@ class WheeledSystem(): u_2 : float system input """ - y_dot = math.sin(y_3) * u_1 + y_dot = u_1 * math.sin(y_3 + y_4) return y_dot - def _func_x_3(self, y_1, y_2, y_3, u_1, u_2): + def _func_x_3(self, y_1, y_2, y_3, y_4, u_1, u_2): """ Parameters ------------ @@ -111,11 +118,18 @@ class WheeledSystem(): u_2 : float system input """ - y_dot = u_2 + y_dot = u_1 / self.REAR_WHEELE_BASE * math.sin(y_4) + return y_dot + + def _func_x_4(self, y_1, y_2, y_3, y_4, u_1, u_2): + """ + """ + y_dot = math.atan2(self.REAR_WHEELE_BASE / (self.REAR_WHEELE_BASE + self.FRONT_WHEELE_BASE) * math.tan(u_2)) + return y_dot def main(): - dt = 0.05 + dt = 0.016 simulation_time = 10 # in seconds iteration_num = int(simulation_time / dt) @@ -123,38 +137,33 @@ def main(): # 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.]]) + Ad = np.array([[1., 0., 0., 0.], + [0., 1, V, 0.], + [0., 0., 1., 0.], + [0., 0., 1., 0.]]) * dt - C = np.eye(2) - D = np.zeros((2, 1)) + Bd = np.array([[0.], [0.], [0.], [0.3]]) * dt + + W_D = np.array([[V], [0.], [0.], [0.]]) * dt # 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 + init_xs_lead = np.array([5., 0., 0. ,0.]) + init_xs_follow = np.array([0., 0., 0., 0.]) + lead_car = WheeledSystem(init_states=init_xs_lead) + follow_car = WheeledSystem(init_states=init_xs_follow) # evaluation function weight - Q = np.diag([1., 1.]) + Q = np.diag([1., 1., 1., 1.]) R = np.diag([5.]) - pre_step = 15 + pre_step = 2 # 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, + lead_controller = MpcController_cvxopt(Ad, Bd, W_D, 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, + follow_controller = MpcController_cvxopt(Ad, Bd, W_D, 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.])) diff --git a/mpc/with_disturbance/mpc_func_with_cvxopt.py b/mpc/with_disturbance/mpc_func_with_cvxopt.py new file mode 100644 index 0000000..d3f8708 --- /dev/null +++ b/mpc/with_disturbance/mpc_func_with_cvxopt.py @@ -0,0 +1,297 @@ +import numpy as np +np.set_printoptions(threshold=np.inf) + +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 + W_D : numpy.ndarray + distubance matrix in state equation + 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, W_D, 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 + W_D : numpy.ndarray + distubance matrix in state equation + 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.W_D = np.array(W_D) + 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)) + + # disturbance + 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)) + + 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)) + + 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)) + + # 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 - self.dist_mat + + 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 + + def update_system_model(self, A, B, W_D): + """update system model + A : numpy.ndarray + system matrix + B : numpy.ndarray + input matrix + W_D : numpy.ndarray + distubance matrix in state equation + """ + + self.A = A + self.B = B + self.W_D = W_D + + self.initialize_controller()