From dca46b0889e796c706754f33d37e4f454e0782ec Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Fri, 28 Dec 2018 15:53:40 +0900 Subject: [PATCH] write compare method of mpc --- mpc/__pycache__/mpc_func.cpython-36.pyc | Bin 3566 -> 6866 bytes .../mpc_func_with_cvxopt.cpython-36.pyc | Bin 0 -> 6142 bytes .../mpc_func_with_scipy.cpython-36.pyc | Bin 0 -> 6877 bytes mpc/main_example.py | 22 +- mpc/mpc_func_with_cvxopt.py | 257 ++++++++++++++++++ mpc/{mpc_func.py => mpc_func_with_scipy.py} | 32 +-- mpc/simulator_func.py | 70 ----- mpc/test_compare_methods.py | 211 ++++++++++++++ 8 files changed, 494 insertions(+), 98 deletions(-) create mode 100644 mpc/__pycache__/mpc_func_with_cvxopt.cpython-36.pyc create mode 100644 mpc/__pycache__/mpc_func_with_scipy.cpython-36.pyc create mode 100644 mpc/mpc_func_with_cvxopt.py rename mpc/{mpc_func.py => mpc_func_with_scipy.py} (79%) delete mode 100644 mpc/simulator_func.py create mode 100644 mpc/test_compare_methods.py diff --git a/mpc/__pycache__/mpc_func.cpython-36.pyc b/mpc/__pycache__/mpc_func.cpython-36.pyc index 73049d59e817d8c7b27133bd9f77c349d4a6f696..4179dd6d5ca41d62d710007a46029783c8b77327 100644 GIT binary patch literal 6866 zcmb_hO>7(25q@v~xLp2-mSl;N6?yGAshK+dYmqdm-rzj|SGck#>XN@oSFLM|>2;mIM%}E-%n-IUZ?1)i_xrFciUnycRvuD+|(EmyuTYQ|%XC~nkbZ1y{YPQSC`0k6E*8F&r< z?cpE@{YGaHHmTM0(ScmS8@vSIu_SfHm37t4yQ-_*laNSvje7zTE>z2OH*{EFXFM8t{-F$;nT-N_*utnZM>iQd8=^P-_!|!KO-0}-B%QxL zbMD;k1mV!%cDQh(S%9z6LtnU!cpGojW}m2r)6jd4BF(b90K zeU70SiLTPkyUL0H%|b~fiCus>7fv#gG<9OQ;Cu?8L!s4d0H zU=-`TLB{fGEZ>Ud%~;-w&8gyJC(}aGQr?dBVc*+m#PWJ9TeU*01zxY^n~>Sh6R-%-!uE>* z^%4S6*lF~V{m(y26^jIx2rL8qR`}21^TliD+8Fs<&>jyu&35Npf7@veT`xE{7!GwjRg~yQRQz_9 z_C>L?(}9{~1bS!^IlL6LgTH7(JQQ8AAty4FU4`&n`A|6O*0oSWNq1!xlxyO{ixVX@ zy17WYODo)GSDC1wm}sR&Y6}}1^^0UTA7NvYV)Mn@BD7qkTc8%Z*aW3Wc@(*AX|FWV zw%$6x@BBnh_(eMND}tX98IiUuCR)n36lp2n(&jIjo66iu`?#S5$;w+>2RO8t!yKeT zi-}5KR|_i<=DbWZJhF(H7sP!9d5LykY0(y}9;~ODY5gSux#rQcoav*y`EQm>XSvF$ zTt;`{foMtBaC7&}L@LrT)lPC4BdIl?Qfs1iG0Jt1?k09x;#@XUF8TOsVq5sg+J^W{ZhdcN z-Y-q`hwx~Z^zSbH`kco!UGArmY=j7SqL1~VV`}vKzWXJvzFPb z!sYeMUdP#9n*S%VmrJW@zoqO{L#wG}>~wBNNE2IbqHCJxB69O|HNVI9d}6doAsy=& zYrBw)l-QVUn`qRI3aLhI%=}-s1@xvop*N&0;Cf=Jqlc8ntbuZTPmhnZ%@0z&DbU;e zp?W(BJuR?a>`YIKTt1nVkMa)l{I0;cLPJLX+|HFOob?-by)?PD!=c@5^qS)yqKiEo zg`Iw*NAZ~w|0M2Z-F!Mt?373Cj)Q!~e#5qYdVaD~*=Y3pjTy>~ol4mD!dX(b-%>h= zT~;j6ep?0DKqB{H1^X>hLudnD{R?Fi@^a9qslI_WvF3If8ytFSO01kcA4{hu{WVl^ zGk+b}7uKa7($MCxKf1hyazLyMvGeNw2$04rMA2!u!RhAgrQQ%q2rdJej`6>V?AzeZ zak8fP6t?`608a?*KM(3Q-l??to557C`CC);F->IyFP?wCxno{Wo9q*jA{cA60)(|{ zPN2~lZ6+$+Cr)hOL^g6-dun8fKF1K5MAt7&B{Ewn#)-`TfxE@rIpH`U0i7165}3EA z0u;IFwDd}|?KSV(@U_&JPn@tjEqi-72CXx=Yx|whJ7bS}Uf|iIanQECLFoC9|AS|$ z%|B*{|1H)V0F=B(&gyHYI17rk=8#en*Yie<<waUf;c;4&c&+LV!`S!;d0X>t zY&38*9JsMQ@^PAsjn>eooS$;4IG?ScIG?SY*y6;_>2>aUv5G!E88$ZBJf5FJft@VQ zwXk$UZ{U}RR3<=qmp@P72!U0ASh;>7Ny?59iE}hse@V)=LM?Po3u{GQSoPvnr`2fU zScmK`VDGJ)Y%YjhpCgHrMeh(XmYsTH%nP&;)N{08otr_5+zRT6qams6rs(a&9o1wR zzd)kANPyOIeDtZy^nFwX5rEK4rK~KvzTvJx%l3bTnuC2(`q!uaW&hyW(p)_eV z(nI>EEsXQaKO^hpeNI7imtDGpk-G|Kj@fod=gQg>z5pIfhl|}K{l*ddRwWL~n^UH|p8)9PM&7o|$Dh|oNj|{{QlqhN- zqawTrz9p7|V!VQU5G@}`-$*%@LDStbOFNfHTcCDx=vSdKBsJ4L&E4#tH`oZ<_DlAKTE^G+TAuF>njQwth|fIG^dwrd zvc9*n0W;FfL|)E_$Rm;yVK$kAmw_b3G}~Zhok?ca3GCAs&P)&7Jg1rYPy&+f$l}V4 zeG!N@M2>%TAmpKZrjV|3&kU7^YFEXpQNEgGfH$+G?VI(zQ?5 z^!^(JE)XDx_Ae5+OkgjE!+uh?Xj6bIjeB7|PdJlt-vo3#O$Od1*{%>+Bf!akwdd=905SM3fG95^MlUMs z%1Kp|nqnd1FDgZO244#?y{uNuHDm?zY8BDmP^)TD*{L38*P4EVo?4Esng-aa9VfON zr$2PZJ;IBQvpH_`(wdy(xWlI7_{T}a?-TeT0d}Z8K(hPf@08;Obc1FtYtp)Cwwa2% zHJ$$_NqqDO(xy>w81_1MaOFil{)79mZ6%(Tpx2K5_ eo0pQgTt=`G0cD1!v?T7!pt7P;{#1M_sM_zY%HgQaPjWtl0zj+ZicD+5hn zikDRW65piB?=162YI+)^$_}>V_x47nUFX&`UarZ%><8EITqnkLSR&19*6ynq9Tu_G z^@vs-%1?qyU5}pv>Xmi1-4wpt>-4v7$*)o$S7$u%JlbM@9LDk1ynyskAAAv{w_ytW zDFzA5_eJqq@zeETV16*6_Wy<9EbzGeGjrC!Y;#?{o_&*=ayxsWjCqNSUE$Z<2OMKp zM@R8SyXO1#F1JyX0&o&4fQo&i+NimHkHha0ydYm>ucuX1hqpT=D|3si_uW5RJ1I8yU8j=8uVcQ42c~dGd@tAx~(7sYWn|hNpYRV=_cf7FZ-<+BxEZ#h4CF zKNT9G3D-)5iB@`Og_ajbt1p&WfdHXFm;VtIrEB=I##522c7;W(6a9>o~LKz!^NfbV|+yB)lSbFwCh{=qXa%2Cbg(Z IO=h$I0MlJIQUCw| diff --git a/mpc/__pycache__/mpc_func_with_cvxopt.cpython-36.pyc b/mpc/__pycache__/mpc_func_with_cvxopt.cpython-36.pyc new file mode 100644 index 0000000000000000000000000000000000000000..007107319c1953bc985e382adbf7d4eeb640b64c GIT binary patch literal 6142 zcmb_gO>7&-72erDE|))|Bub(sM_D_LYX*)TxkXZ>ZfYlv6BMlxs{srU4Hh(aDU#x! zW|y|4F4Lll5%*BQz4Vfz_a0mHQ1sB=dhW5TNqh|2mteN<$ z-AvD_S-os6E0p&XPxJIo6i+`;YqlqTq*RT^;H2EDs8W6E$Aupb8}AJIp&WF(zO0{D6~%V& zTn{1b?kMyFF1|YRB=ydlPJh%J?p^A8bt&t6Or@T|UJ&{{CsCKFZ=cw<-5-v^*`~Ko zZ0g^ucSrTGJ?J~lQNO{+eZRfc3R%YH*X=lzz6*ti=-=)qyl504d+kID)v^pPbXj3{ zG#vW!M1hS#KL}+V+yZCN1XmiPm$06)=nn3GO%`8Q1s+1u`Kt@(&aHM34&w8kchDnFl3zAhxJSAwtjXto%4K(=vLy#V7+f7i9rU5jeQ>%z1&~BEvGnC0=KG z{oTpTC+}ZB(YaLnwR#$953bLqw$ZKJY{z#*KYn4uaU8tie$b7pI1Z)Dduw@>gR%rLzVY}W<;-6fi ziZX#^0xJN&Q{)Tyeev$)7C2uHTBClu(Q04r?YWJC=LeVjgMPQ&_v;d6-fk}Uh7I~$ z3g_b8RS;~v#HC0oXYEcx`l4c1@Ly2CZ;@w|9 zvZGy^FlvxP=%E2+@X=-+{6klihe}7;5@QjHj!O7}c&NDA?u}4K$?!x4lpD%NSH@~+ zb~2IhmQQh?19hx{Vxg55X-xz<>KBPlHbRgS%Fg*aN@#m(Cr2#~5D(FAsTUWJdamGS*YMg-B237IuEk(lnMa66T;C zT8uUNdwN)kFy<8+;d~h*&nXX7Pxtf(YLmjTcD$ZyCiPbp=(T{J#i>1tJO5_A4A!fd z>ScBoKUJE-Gri0wR-zTzm}GZi1^QoAAon+ro6OF&C_5Pw=s&+hV_i#p=FuEqO?-r;Qd$o|sh-^gDct)`=%a#9Perk-)qvCWeucD=;bG|nf3&rXglG+Y1F%oQ5QRn zgMi!a)w{G;(;*<)1E<|2ohpsc>i87V_H;(QA>E4AMb+Yx~q;u(NcK{;M?x|udPq9W~JY}FDF zwyHITMQ5y;nDmG`v5C{!&}|-?ku~}ZET$UW+?Z%&HB*d}n*W1zo27H|aZH2X&uC!X znJAFwrjyd^jh5fob}-k{xqRk~({4I@gAr)${x}`WB%bdN8Ox;0AvR>azvXj^l+0Sq8-%fW4>??8 zJ2vY$CicD97)qQ4W3xGsl>bwH6=$a_D9%n-PHc0M=XTrMeypL7B*(^Pi}}e63Ovc; zOcP5t^!u_vq#^;z!Q=vgc>=2dv3heONzm4a#Q7Snza)J-MJ)_Y5vzG#Shf6hw^?uC zh=)us;MrTV*j-S&Hb)vKi{2$-EZViinHv-cY8hIv?(HB&HiKF+(U4$vQuIzT9kpZ` zzf7u}CqQdCUV7>>-9%Lo0Vukq7S*y?5DWO1kpAVlWQk=e0ndvYDsXLGtg9taqEFAO z>!Ktspic=cY|!3RE9$DM11qEFIcgFM_t4@WQH1UU&MjRnIj#Nm5z?j@T^nF5OG(N)aenfi#+V`2Wq5M zlt`~AW5Y|1(n<$8Xv)!w4B}N*#%4;HtwT$y1^+BE#>|#7w(#LV+_60k2j)LTRwqX# z+BG8NOu+Myn`14AM)@FGJ`!@mzX+P)QmH(Hk?9Q_Z+@kj)H z@S14)vv8@i9Oa`zWJOue_@zEBhAUCgGbv9Q&qZ^dM$#Yf)dI#ce@cOMN}wS_jY_O@ z347;4y#?1jxlQtdnQo6C)MdTrBf=!NWDcI=7iC1_GhHyfQ#g8GMhz&S(J{{nT7yy7 zb6WL#zH@=QIbFZM6}FsLoQ>*KuJoI}^!p7TOsB->W;8i_R_$ru%_+kvX=Wg=PKn4C zk~83RFvqX|NQ+5M!N$6njI13v7d9?Vj@Ue=nekAXk;Yx++AVnwMeO_Hy;!tjvCDz_ ztAlEVqq4jRZjx?B-GlTs7rX>b$3SfZd#LZU2o8EU5P7mMSh3C_X)7Zhk$khNy#GJL<9t^6M?Ee=}oaiC| literal 0 HcmV?d00001 diff --git a/mpc/__pycache__/mpc_func_with_scipy.cpython-36.pyc b/mpc/__pycache__/mpc_func_with_scipy.cpython-36.pyc new file mode 100644 index 0000000000000000000000000000000000000000..9a16a4c052e573f7d681d09cb171704439287989 GIT binary patch literal 6877 zcmb_hO^h5@5q_`#rl;p;XMemqyWVWtjuSk=@gFFG;21kGP9%bbok*5OM6L03&(4hJ zZ@YVv-K8gySX*`uNan&RLWmnzPDmWMapa8p3KFu!fddDW3w%}G(>uHNCQhJ7U0tu< zPt~ic_p08TZ34QL;FWhf1Fzw~ zH5>$?-{=g&CbgPAI*==PgO>q3mZYw@vaY&$S9P^}5)$dIaZf;^oNMB1xC(e#ap^~+ zCI$`tVXx=;jh7T5EE~_&F!Vcj#-SH*@s*h;skdLZ2jl)|=j_03_jL2mdGYv3_~+H$N6 zMzPKtWGt`7^37P@jODG^oGLz6ZU(V(D~R==-57bX6@-ltI}&#N*h=JKT;KKlVGvu> zH8>99a%Kru-%Ob;6BlN5GA$%6_PxzUEU(A1RV&0=;PqO*37P#o0gC`FY`+Ll zFCh?xoklO&|NLWAu|!~*zzV=`h5sBrpInS__IS`~wmawgJ5F=xdcnEDaM0rr zhBAknbNx}1UWc}G=c7*8c7kRH0cErkn+`a2LdQYa6~So$ajYnlpP?AiP|C8bR8&)% zDoXStDt^1m2LjsJ=|D}g2R$^O99|0G!Cy2X9*VBmloJ`su0r^pd?*}s`&y`>q`R^T z$~E!9#fcIc-CQKyJ@!|3Q0<*Cmhz82{N ze5=G4@yuebNEKJalxu1)$R1fMkpGeZ-_O7|@y_E>emW=6e|DSZdOWe2OFq7q*cLvr zz9~MH+uxa)_sbLgAw1e8Jv~RBNIoG2(LkvZh02fAt##(N_&~z0!3Wpug6E) z*88d66zFZ?NWGneo)%dzcBZE#E}zWG$9RW%VNc+Qp&_|{Zud$S+4_yUUYcjy;m~e2 zdd+bUfyN$=!cM=@qfkv}fFuNF-F!M&?3PFEj)T<2e#5qYdVaE7*=+RtjTy>~-AdT@ z!dX(b-%@6XT~;j8ep?0DL{9fX1^X?sL}&wE{R?Fa@^bvBslI_WvF3Ifn;eB|O01kc zA4{hu{dH7vGk*iv=hmejGScR-Kf1h)azLyMvHRM=2#|&@1k`Ed!9nNjrQQ%q2rdJe zj`6>V?AzeZA+x6V6t?`608a?*zX0kE-l??to557C`CC);F->I?FP?vXf7iU8HaQ?7 zMKIQC1qf@^oIs;9+Due>K%CgX;cVoz_SMJ|eTE@2iLPImN@TWDjB}d*19ywLbMA3S z0y;QMB`|MK1t@aUY3bEw+iTvn;cKZcpEzN6TK3Lx3|eP!*Y-Q1cg7y|yuhQn=>b*l6ICIB;Wq7$vzTvyiQvfPkWuC2(mq!uaW z?hDVbp)_eV(nI>EEsXQaKO^hpeNI7imtDG!k$VbfjNgAz@gQg>z5iX7PlqK=$l*ddRwWL~n^DBtzn_^<%&7o|$D$dKl zj|{{QlqhN-qawTrz9p7|V!VQU5G@}`-$*%@LDStbOFNfHTcCDx=vSdKBsJ4zA>kPnm=F0F7aW=<@sr)F%TdFQqJS{IdPfEj6KBCljb@#WO3!jfe1t!BFDcv5b{tyQ%G02XNJl{wX5RQC|^xa7L;-55yOYH8Vy{DsV7iMm@_VxO# zSZyuQKk3@18+-o^0v8C7L;DvATqdv|;W>1lB^rfpj`03#gmHA|UwZsO(mcvTIGhK~F75 zS4{(K)s7Qej?*8y;~wEf$JrV;dTCA0aol0kas1~=#P1RKApv%%eL%AN= 0 - so we should recalculate the A and B - """ - temp_dt_us = np.array([dt_us[i] for i in range(self.input_size * self.pre_step)]) - - constraint = [] - for i in range(self.pre_step * self.input_size): - sums = -1. * (np.dot(A[i], temp_dt_us) - b[i])[0] - constraint.append(sums) - return np.array(sums) + # 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 : - print("consider constraint!") - cons = ({'type' : 'ineq', 'fun' : constraint_func}) - opt_result = minimize(optimized_func, init_dt_us, constraints=cons) + # print("consider constraint!") + opt_result = minimize(optimized_func, init_dt_us, constraints=[linear_cons]) opt_dt_us = opt_result.x - print("opt_u = {0}".format(np.round(opt_dt_us, 5))) + # 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 diff --git a/mpc/simulator_func.py b/mpc/simulator_func.py deleted file mode 100644 index bf38e16..0000000 --- a/mpc/simulator_func.py +++ /dev/null @@ -1,70 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt -import math - -class FirstOrderSystem(): - """FirstOrderSystem - - Attributes - ----------- - state : float - system state, this system should have one input - one output - a : float - parameter of the system - b : float - parameter of the system - history_state : list - time history of state - """ - def __init__(self, a, b, init_state=0.0): - """ - Parameters - ----------- - a : float - parameter of the system - b : float - parameter of the system - init_state : float, optional - initial state of system default is 0.0 - """ - self.state = init_state - self.a = a - self.b = b - self.history_state = [init_state] - - 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] - """ - # solve Runge-Kutta - k0 = dt * self._func(self.state, u) - k1 = dt * self._func(self.state + k0/2.0, u) - k2 = dt * self._func(self.state + k1/2.0, u) - k3 = dt * self._func(self.state + k2, u) - - self.state += (k0 + 2 * k1 + 2 * k2 + k3) / 6.0 - - # for oylar - # self.state += k0 - - # save - self.history_state.append(self.state) - - def _func(self, y, u): - """ - Parameters - ------------ - y : float - state of system - u : float - input of system in some cases this means the reference - """ - y_dot = -self.a * y + self.b * u - - return y_dot - diff --git a/mpc/test_compare_methods.py b/mpc/test_compare_methods.py new file mode 100644 index 0000000..5ddaea4 --- /dev/null +++ b/mpc/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