From bc9a2ebe9f7e76fdc76ec8147cf2e589f1131b42 Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Wed, 26 Dec 2018 10:44:57 +0900 Subject: [PATCH] add constraints of mpc.py --- mpc/__pycache__/mpc_func.cpython-36.pyc | Bin 3196 -> 3566 bytes mpc/main_example.py | 66 +++++++++++++++--------- mpc/mpc_func.py | 65 ++++++++++++++--------- 3 files changed, 82 insertions(+), 49 deletions(-) diff --git a/mpc/__pycache__/mpc_func.cpython-36.pyc b/mpc/__pycache__/mpc_func.cpython-36.pyc index cb3f01cbf8cb014b0a7649f82fa511cc09a33a9c..73049d59e817d8c7b27133bd9f77c349d4a6f696 100644 GIT binary patch delta 1281 zcmZux&uO$Hy14CFdU@K}w|^3{;koru)!K3$^`DWxOBqkmZPQDh8s zkKQJEy=#e?;a8LtqCX?#UG;Sx+0T>R{oXl_OviLz^Bk&}`Hq zElVOjOM2X6!w93E8qUm!W3CL@`L+kxw)TyrZlD=9uqMb3eMX+qD)Tf?e`59wk9)=w z(!*2&Spwo@hzA~h+A~97OJqWsR%F%Po`{5(La(PRM_}ZhlOq$Phf%yf5&_33iEQ9g zx=Z)ed-HGWmS6Ry->mrRlR3ubg?eQbw$2>7jeb(|HrxyriA8V29iWyakcf8E`@}?k z=Jmn*&Oi$J6F^MwUA*O|zem(^GM3Y}5~XL|H# Jj9PVO{sW{`2#Wv! delta 897 zcmY*Y%WD%s7@wKVJ~G*jZ5o?b8&WMotOlwcq6G1QUPKY>DHOtzhfPS6IJ;2{yLgDu z_GTHpirzeVQP7M3f=Imu58eg!tRDQnP143)=C|K`@0stnKQe!l{JdrH=dqQ|*XBO= zsj|QOV9_As2fqx~by&`pJVtHpKJiG!l+Wlgwd5OGN|$-F)9bqxr#YyP_*T<#yW(-B z@8k^`VL4f5OEe}QvD?Ww=2T2Nel@01jYVmcz5T|%Fz|V&u4LxHMZ^#m0DkjQ8}e1V z+r7&-Ap+4FECrVH0l+0K>X8=n z=#cr!WT${8RMS!fbRkc^GU89vmNqf!o;qYh7AJet;8`1K1y)F4B*-4!Av?6eYDz8i z)bKGw7%Y59ecg?Cnx{hn!!!J_7p|!(m=l!MQzSurj&jTNz?Tw_YxhXr7#zrr@PWL` zSFf$kw2po4!XdD$K*r<1E=eGW4lJIVd8VVt%8xvmKAhVq+U>c`Z6HLg(&%@pw*1LU zn}R|ak(xr@8jmrHiY0`T2y+P7kXS=FoYF`g)J4}3c;^B+DRKb$$ar@0qPa#3vSpsS zRo0+5@GqbdP~!~xo<%@Pjd;~=S1SQ!@4((cdJrC^ab~hCJHeu=$v6wK7%)|SH8U{k yKjyh2l2@EYSVstsdIXqRAPew^aS#kO#Q3~Xkag=4P0N>7cJ9c1d{Od~mHP+x->ww^ diff --git a/mpc/main_example.py b/mpc/main_example.py index 145d870..fcf66f7 100644 --- a/mpc/main_example.py +++ b/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)) - k1 = dt * (np.dot(self.A, temp + k0/2.) + np.dot(self.B, us)) - k2 = dt * (np.dot(self.A, temp + k1/2.) + np.dot(self.B, us)) - k3 = dt * (np.dot(self.A, temp + k2) + 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_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() + # 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 - A = np.array([[1./tau, 0., 0., 0.], - [0., 1./tau, 0., 0.], + tau = 0.63 + A = np.array([[-1./tau, 0., 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 - # controller.calc_input + history_states = np.array(plant.history_xs) + + 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() diff --git a/mpc/mpc_func.py b/mpc/mpc_func.py index 443da34..3b4ddc8 100644 --- a/mpc/mpc_func.py +++ b/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_2 = np.dot(self.gamma_mat, self.history_us[-1]) + 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 - 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) - return opt_us - - - - - - - - - - - - - + self.history_us.append(opt_u) + return opt_u \ No newline at end of file