From 24a2568de014e8d77a02e7e09372c27a2706408f Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Wed, 13 Feb 2019 18:06:50 +0900 Subject: [PATCH] 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