add ACC.py of mpc

mpc/README.md

@@ -1,50 +1,95 @@
 # Model Predictive Control Tool
-This program is about template, function of linear model predictive control
+This program is about template, generic function of linear model predictive control
 
-# Documentation of this function
+# 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
 
-Parameters :
+**class MpcController()**
+
+Attributes :
 
 - A : numpy.ndarray
      - system matrix
 - B : numpy.ndarray
     - input matrix
 - Q : numpy.ndarray
-    - evaluation function weight
+    - evaluation function weight for states
+- Qs : numpy.ndarray
+    - concatenated evaluation function weight for states
 - R : numpy.ndarray
-    - evaluation function weight
+    - evaluation function weight for inputs
+- Rs : numpy.ndarray
+    - concatenated evaluation function weight for inputs
 - pre_step : int
     - prediction step
-- dt_input_upper : numpy.ndarray
+- state_size : int
-    - constraints of input dt
+    - state size of the plant
-- dt_input_lower : numpy.ndarray
+- input_size : int
-    - constraints of input dt
+    - input size of the plant
-- input_upper : numpy.ndarray
+- dt_input_upper : numpy.ndarray, shape(input_size, ), optional
-    - constraints of input
+    - constraints of input dt, default is None
-- input_lower : numpy.ndarray
+- dt_input_lower : numpy.ndarray, shape(input_size, ), optional
-    - constraints of input
+    - 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 function have same variable and member function. however the solver is different. 
+Both functions have same variable and member function. However the solver is different. 
-Plese choose the right method for your environment
+Plese choose the right method for your environment.
 
-## Example
+- example of import
-# Problem Formulation and Expected results
+
+```py
+from mpc_func_with_scipy import MpcController as MpcController_scipy
+from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt
+```
+
+# Examples
+## Problem Formulation
+
+** updating soon !!
+
+- first order system
+
+
+- ACC (Adaptive cruise control)
+
+
+
+## Expected Results
+
+- first order system
+
+
+- ACC (Adaptive cruise control)
 
-- updating soon!!
 
 # Usage
 
-- for example
+- for example(first order system)
 
 ```
 $ python main_example.py
 ```
 
-- for comparing two methods
+- for example(ACC (Adaptive cruise control))
+
+```
+$ python main_ACC.py
+```
+
+- for comparing two methods of optimization solvers
 
 ```
 $ python test_compare_methods.py
@@ -57,6 +102,7 @@ $ python test_compare_methods.py
 - matplotlib
 - cvxopt
 - scipy1.2.0 or more
+- python-control
 
 # Reference
 I`m sorry that main references are written in Japanese

mpc/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 = 3
+        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])

mpc/main_ACC.py

@@ -0,0 +1,247 @@
+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()
+    time_history_fig.legend()
+
+    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()

mpc/main_example.py

@@ -12,7 +12,7 @@ class FirstOrderSystem():
 
     Attributes
     -----------
-    states : float
+    xs : numpy.ndarray
         system states
     A : numpy.ndarray
         system matrix
@@ -20,7 +20,7 @@ class FirstOrderSystem():
         control matrix
     C : numpy.ndarray
         observation matrix
-    history_state : list
+    history_xs : list
         time history of state
     """
     def __init__(self, A, B, C, D=None, init_states=None):
@@ -33,7 +33,7 @@ class FirstOrderSystem():
             control matrix
         C : numpy.ndarray
             observation matrix
-        C : numpy.ndarray
+        D : numpy.ndarray
             directly matrix
         init_state : float, optional
             initial state of system default is None
@@ -59,8 +59,8 @@ class FirstOrderSystem():
         """calculating input
         Parameters
         ------------
-        u : float
+        u : numpy.ndarray
-            input of system in some cases this means the reference
+            inputs of system in some cases this means the reference
         dt : float in seconds, optional
             sampling time of simulation, default is 0.01 [s]
         """
@@ -77,13 +77,11 @@ class FirstOrderSystem():
 
         # 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
@@ -135,7 +133,7 @@ def main():
         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)
+        plant.update_state(opt_u, dt=dt)
 
     history_states = np.array(plant.history_xs)
 

mpc/mpc_func_with_cvxopt.py

@@ -14,41 +14,52 @@ class MpcController():
     B : numpy.ndarray
         input matrix
     Q : numpy.ndarray
-        evaluation function weight
+        evaluation function weight for states
+    Qs : numpy.ndarray
+        concatenated evaluation function weight for states
     R : numpy.ndarray
-        evaluation function weight
+        evaluation function weight for inputs
+    Rs : numpy.ndarray
+        concatenated evaluation function weight for inputs
     pre_step : int
         prediction step
-    dt_input_upper : numpy.ndarray
+    state_size : int
-        constraints of input dt
+        state size of the plant
-    dt_input_lower : numpy.ndarray
+    input_size : int
-        constraints of input dt
+        input size of the plant
-    input_upper : numpy.ndarray
+    dt_input_upper : numpy.ndarray, shape(input_size, ), optional
-        constraints of input
+        constraints of input dt, default is None
-    input_lower : numpy.ndarray
+    dt_input_lower : numpy.ndarray, shape(input_size, ), optional
-        constraints of input
+        constraints of input dt, default is None
-    history_
+    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
+            evaluation function weight for states
         R : numpy.ndarray
-            evaluation function weight
+            evaluation function weight for inputs
         pre_step : int
             prediction step
-        dt_input_upper : numpy.ndarray
+        dt_input_upper : numpy.ndarray, shape(input_size, ), optional
-            constraints of input dt
+            constraints of input dt, default is None
-        dt_input_lower : numpy.ndarray
+        dt_input_lower : numpy.ndarray, shape(input_size, ), optional
-            constraints of input dt
+            constraints of input dt, default is None
-        input_upper : numpy.ndarray
+        input_upper : numpy.ndarray, shape(input_size, ), optional
-            constraints of input
+            constraints of input, default is None
-        input_lower : numpy.ndarray
+        input_lower : numpy.ndarray, shape(input_size, ), optional
-            constraints of input
+            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)
@@ -73,7 +84,8 @@ class MpcController():
         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
@@ -82,6 +94,7 @@ class MpcController():
     def initialize_controller(self):
         """
         make matrix to calculate optimal control input
+        
         """
         A_factorials = [self.A]
 
@@ -187,22 +200,22 @@ class MpcController():
         """calculate optimal input
         Parameters
         -----------
-        states : numpy.array
+        states : numpy.ndarray, shape(state length, )
-            the size should have (state length * 1)
+            current state of system
-        references :
+        references : numpy.ndarray, shape(state length * pre_step, )
-            the size should have (state length * pre_step)
+            reference of the system, you should set this value as reachable goal
 
         References
         ------------
-        opt_input : numpy.ndarray
+        opt_input : numpy.ndarray, shape(input_length, )
-            optimal input, size is (1, 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) )
+        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
 
@@ -220,9 +233,8 @@ class MpcController():
             b.append(b_F)
 
         A = np.array(A).reshape(-1, self.input_size * self.pre_step)
-        # b = np.array(b).reshape(-1, 1)
+
         ub = np.array(b).flatten()
-        # print(np.dot(self.F1, self.history_us[-1].reshape(-1, 1)))
 
         # make cvxpy problem formulation
         P = 2*matrix(H)
@@ -232,26 +244,13 @@ class MpcController():
 
         # constraint
         if self.W is not None or self.F is not None :
-            # print("consider constraint!")
             opt_result = solvers.qp(P, q, G=A, h=b)
         
-        # print(list(opt_result['x']))
         opt_dt_us = list(opt_result['x'])
-        # print("current_u = {0}".format(self.history_us[-1]))
+        
-        # print("opt_dt_u = {0}".format(np.round(opt_dt_us, 5)))
         opt_u = opt_dt_us[:self.input_size] + self.history_us[-1]
-        # print("opt_u = {0}".format(np.round(opt_u, 5)))
+        
         # save
         self.history_us.append(opt_u)
-        # a = input()
-        return opt_u
 
-
+        return opt_u
-
-
-"""
-constraint = []
-for i in range(self.pre_step * self.input_size):
-    sums = -1. * (np.dot(A[i], init_dt_us) - b[i])[0]
-    constraint.append(sums)
-"""

mpc/mpc_func_with_scipy.py

@@ -15,41 +15,52 @@ class MpcController():
     B : numpy.ndarray
         input matrix
     Q : numpy.ndarray
-        evaluation function weight
+        evaluation function weight for states
+    Qs : numpy.ndarray
+        concatenated evaluation function weight for states
     R : numpy.ndarray
-        evaluation function weight
+        evaluation function weight for inputs
+    Rs : numpy.ndarray
+        concatenated evaluation function weight for inputs
     pre_step : int
         prediction step
-    dt_input_upper : numpy.ndarray
+    state_size : int
-        constraints of input dt
+        state size of the plant
-    dt_input_lower : numpy.ndarray
+    input_size : int
-        constraints of input dt
+        input size of the plant
-    input_upper : numpy.ndarray
+    dt_input_upper : numpy.ndarray, shape(input_size, ), optional
-        constraints of input
+        constraints of input dt, default is None
-    input_lower : numpy.ndarray
+    dt_input_lower : numpy.ndarray, shape(input_size, ), optional
-        constraints of input
+        constraints of input dt, default is None
-    history_
+    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
+            evaluation function weight for states
         R : numpy.ndarray
-            evaluation function weight
+            evaluation function weight for inputs
         pre_step : int
             prediction step
-        dt_input_upper : numpy.ndarray
+        dt_input_upper : numpy.ndarray, shape(input_size, ), optional
-            constraints of input dt
+            constraints of input dt, default is None
-        dt_input_lower : numpy.ndarray
+        dt_input_lower : numpy.ndarray, shape(input_size, ), optional
-            constraints of input dt
+            constraints of input dt, default is None
-        input_upper : numpy.ndarray
+        input_upper : numpy.ndarray, shape(input_size, ), optional
-            constraints of input
+            constraints of input, default is None
-        input_lower : numpy.ndarray
+        input_lower : numpy.ndarray, shape(input_size, ), optional
-            constraints of input
+            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)
@@ -188,15 +199,15 @@ class MpcController():
         """calculate optimal input
         Parameters
         -----------
-        states : numpy.array
+        states : numpy.ndarray, shape(state length, )
-            the size should have (state length * 1)
+            current state of system
-        references :
+        references : numpy.ndarray, shape(state length * pre_step, )
-            the size should have (state length * pre_step)
+            reference of the system, you should set this value as reachable goal
 
         References
         ------------
-        opt_input : numpy.ndarray
+        opt_input : numpy.ndarray, shape(input_length, )
-            optimal input, size is (1, 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))
@@ -221,9 +232,8 @@ class MpcController():
             b.append(b_F)
 
         A = np.array(A).reshape(-1, self.input_size * self.pre_step)
-        # b = np.array(b).reshape(-1, 1)
+
         ub = np.array(b).flatten()
-        # print(np.dot(self.F1, self.history_us[-1].reshape(-1, 1)))
 
         def optimized_func(dt_us):
             """
@@ -240,25 +250,13 @@ class MpcController():
 
         # constraint
         if self.W is not None or self.F is not None :
-            # print("consider constraint!")
             opt_result = minimize(optimized_func, init_dt_us, constraints=[linear_cons])
         
         opt_dt_us = opt_result.x
-        # print("current_u = {0}".format(self.history_us[-1]))
+
-        # print("opt_dt_u = {0}".format(np.round(opt_dt_us, 5)))
         opt_u = opt_dt_us[:self.input_size] + self.history_us[-1]
-        # print("opt_u = {0}".format(np.round(opt_u, 5)))
+
         # save
         self.history_us.append(opt_u)
         
-        return opt_u
+        return opt_u
-
-
-
-
-"""
-constraint = []
-for i in range(self.pre_step * self.input_size):
-    sums = -1. * (np.dot(A[i], init_dt_us) - b[i])[0]
-    constraint.append(sums)
-"""