add constraints of mpc.py

mpc/main_example.py

@@ -1,6 +1,7 @@
 import numpy as np
 import matplotlib.pyplot as plt
 import math
+import copy
 
 from mpc_func import MpcController
 from control import matlab
@@ -39,9 +40,6 @@ class FirstOrderSystem():
             time history of system states
         """
 
-        if init_states is not None:
-            self.states = init_states
-        
         self.A = A
         self.B = B
         self.C = C
@@ -51,9 +49,12 @@ class FirstOrderSystem():
 
         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, us, dt=0.01):
+    def update_state(self, u, dt=0.01):
         """calculating input
         Parameters
         ------------
@@ -62,34 +63,39 @@ class FirstOrderSystem():
         dt : float in seconds, optional
             sampling time of simulation, default is 0.01 [s]
         """
-        temp = self.xs.reshape(-1, 1)
+        temp_x = self.xs.reshape(-1, 1)
+        temp_u = u.reshape(-1, 1)
 
         # solve Runge-Kutta
-        k0 = dt * (np.dot(self.A, temp) + np.dot(self.B, us)) 
+        k0 = dt * (np.dot(self.A, temp_x) + np.dot(self.B, temp_u)) 
-        k1 = dt * (np.dot(self.A, temp + k0/2.) + np.dot(self.B, us))
+        k1 = dt * (np.dot(self.A, temp_x + k0/2.) + np.dot(self.B, temp_u))
-        k2 = dt * (np.dot(self.A, temp + k1/2.) + np.dot(self.B, us))
+        k2 = dt * (np.dot(self.A, temp_x + k1/2.) + np.dot(self.B, temp_u))
-        k3 = dt * (np.dot(self.A, temp + k2) + np.dot(self.B, us))
+        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()
+        # self.xs +=  ((k0 + 2 * k1 + 2 * k2 + k3) / 6.).flatten()
 
         # for oylar
-        # self.state += k0
+        self.xs += k0.flatten()
 
+        # print("xs = {0}".format(self.xs))
+        # a = input()
         # save
-        self.history_xs.append(self.xs)
+        save_states = copy.deepcopy(self.xs)
+        self.history_xs.append(save_states)
+        # print(self.history_xs)
 
 def main():
     dt = 0.01
-    simulation_time = 100 # in seconds
+    simulation_time = 300 # 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.53
+    tau = 0.63
-    A = np.array([[1./tau, 0., 0., 0.],
+    A = np.array([[-1./tau, 0., 0., 0.],
-                  [0., 1./tau, 0., 0.],
+                  [0., -1./tau, 0., 0.],
                   [1., 0., 0., 0.], 
-                  [1., 0., 0., 0.]])
+                  [0., 1., 0., 0.]])
     B = np.array([[1./tau, 0.],
                   [0., 1./tau],
                   [0., 0.],
@@ -99,7 +105,8 @@ def main():
     D = np.zeros((4, 2))
 
     # make simulator with coninuous matrix
-    plant = FirstOrderSystem(A, B, C)
+    init_xs = np.array([0., 0., -3000., 50.])
+    plant = FirstOrderSystem(A, B, C, init_states=init_xs)
 
     # create system
     sysc = matlab.ss(A, B, C, D)
@@ -111,20 +118,31 @@ def main():
 
     # evaluation function weight
     Q = np.diag([1., 1., 1., 1.])
-    R = np.diag([1., 1.])
+    R = np.diag([100., 100.])
     pre_step = 3
 
     # make controller with discreted matrix
     controller = MpcController(Ad, Bd, Q, R, pre_step)
     controller.initialize_controller()
 
-    xs = np.array([0., 0., 0., 0.])
-
     for i in range(iteration_num):
-        controller.calc_input(xs)
+        print("simulation time = {0}".format(i))
+        reference = np.array([0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.])
+        controller.calc_input(plant.xs, reference)
+        
+        states = plant.xs
+        opt_u = controller.calc_input(states, reference)
+        plant.update_state(opt_u)
 
-    # states = plant.states
+    history_states = np.array(plant.history_xs)
-    # controller.calc_input
+
+    print(history_states[:, 2])
+
+    plt.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 0])
+    plt.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 1])
+    plt.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 2], linestyle="dashed")
+    plt.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 3])
+    plt.show()
 
 if __name__ == "__main__":
     main()

mpc/mpc_func.py

@@ -12,7 +12,7 @@ class MpcController():
 
     """
 
-    def __init__(self, A, B, Q, R, pre_step, input_upper=None, input_lower=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):
         """
         """
         self.A = np.array(A)
@@ -27,8 +27,25 @@ class MpcController():
         self.state_size = self.A.shape[0]
         self.input_size = self.B.shape[1]
 
-        self.history_us = []
+        self.history_us = [np.zeros(self.input_size)]
 
+        # initial state
+        if initial_input is not None:
+            self.history_us = [initial_input]
+
+        # constraints
+        if dt_input_lower in not None:
+            self.dt_input_lower = dt_input_lower
+
+        if dt_input_upper in not None:
+            self.dt_input_upper = dt_input_upper
+        
+        if input_upper in not None:
+            self.input_upper = input_upper
+        
+        if input_lower in not None:
+            self.input_lower = input_lower
+        
     def initialize_controller(self):
         """
         make matrix to calculate optimal controller
@@ -66,6 +83,7 @@ class MpcController():
 
         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)])
         
@@ -75,6 +93,16 @@ class MpcController():
         print("Qs = {0}".format(self.Qs))
         print("Rs = {0}".format(self.Rs))
 
+        # constraints
+        # dt U
+        F = np.array([[], [], []])
+
+        # u
+
+
+        # state
+
+
     def calc_input(self, states, references):
         """
         Parameters
@@ -89,10 +117,10 @@ class MpcController():
 
 
         """
-        temp_1 = np.dot(self.phi_mat, states)
+        temp_1 = np.dot(self.phi_mat, states.reshape(-1, 1))
-        temp_2 = np.dot(self.gamma_mat, self.history_us[-1])
+        temp_2 = np.dot(self.gamma_mat, self.history_us[-1].reshape(-1, 1))
 
-        error = references - temp_1 - temp_2
+        error = references.reshape(-1, 1) - temp_1 - temp_2
 
         G = 2. * np.dot(self.theta_mat.T, np.dot(self.Qs, error) )
 
@@ -101,35 +129,22 @@ class MpcController():
         def optimized_func(dt_us):
             """
             """
-            return np.dot(dt_us.T, np.dot(H, dt_us)) - np.dot(G.T, dt_us)
+            return np.dot(dt_us.flatten(), np.dot(H, dt_us)) - np.dot(G.T, dt_us)
 
         def constraint_func():
             """
             """
-            return 
+            return None
 
-        init_dt_us = np.zeros(self.pre_step)
+        init_dt_us = np.zeros((self.input_size * self.pre_step, 1))
 
         opt_result = minimize(optimized_func, init_dt_us)
 
-        opt_dt_us = opt_result
+        opt_dt_us = opt_result.x
 
-        opt_us = opt_dt_us[0] + self.history_us[-1]
+        opt_u = opt_dt_us[:self.input_size] + self.history_us[-1]
 
         # save
-        self.history_us.append(opt_us)
+        self.history_us.append(opt_u)
-        return opt_us
-
-
-    
-
-
-
-
-
-        
-
-        
-
-
 
+        return opt_u