From 040e12dbcb88ac5a3cfa7097ceca8a5e5562ee6d Mon Sep 17 00:00:00 2001 From: Trung Kien Date: Sat, 29 Jan 2022 14:16:34 +0700 Subject: [PATCH] Add inverted_pendulum_lqr_control (#635) * Add inverted_pendulum_lqr_control * reorganize document of inverted pendulum control module * Refactor inverted_pendulum_lqr_control.py * Add doccument for inverted pendulum control * Corrected inverted pedulum LQR control doccument * Refactor inverted pendulum control by mpc and lqr * Add unit test for inverted_pendulum_lqr_control.py --- .../inverted_pendulum_lqr_control.py | 192 ++++++++++++++++++ .../inverted_pendulum_mpc_control.py | 30 ++- docs/modules/control/control_main.rst | 3 +- .../inverted-pendulum.png | Bin 0 -> 6127 bytes .../inverted_pendulum_control.rst | 97 +++++++++ .../inverted_pendulum_mpc_control.rst | 6 - tests/test_inverted_pendulum_lqr_control.py | 11 + tests/test_inverted_pendulum_mpc_control.py | 12 +- 8 files changed, 328 insertions(+), 23 deletions(-) create mode 100644 Control/inverted_pendulum/inverted_pendulum_lqr_control.py rename Control/{InvertedPendulumMPCControl => inverted_pendulum}/inverted_pendulum_mpc_control.py (84%) create mode 100644 docs/modules/control/inverted_pendulum_control/inverted-pendulum.png create mode 100644 docs/modules/control/inverted_pendulum_control/inverted_pendulum_control.rst delete mode 100644 docs/modules/control/inverted_pendulum_mpc_control/inverted_pendulum_mpc_control.rst create mode 100644 tests/test_inverted_pendulum_lqr_control.py diff --git a/Control/inverted_pendulum/inverted_pendulum_lqr_control.py b/Control/inverted_pendulum/inverted_pendulum_lqr_control.py new file mode 100644 index 00000000..fb68c91c --- /dev/null +++ b/Control/inverted_pendulum/inverted_pendulum_lqr_control.py @@ -0,0 +1,192 @@ +""" +Inverted Pendulum LQR control +author: Trung Kien - letrungkien.k53.hut@gmail.com +""" + +import math +import time + +import matplotlib.pyplot as plt +import numpy as np +from numpy.linalg import inv, eig + +# Model parameters + +l_bar = 2.0 # length of bar +M = 1.0 # [kg] +m = 0.3 # [kg] +g = 9.8 # [m/s^2] + +nx = 4 # number of state +nu = 1 # number of input +Q = np.diag([0.0, 1.0, 1.0, 0.0]) # state cost matrix +R = np.diag([0.01]) # input cost matrix + +delta_t = 0.1 # time tick [s] +sim_time = 5.0 # simulation time [s] + +show_animation = True + + +def main(): + x0 = np.array([ + [0.0], + [0.0], + [0.3], + [0.0] + ]) + + x = np.copy(x0) + time = 0.0 + + while sim_time > time: + time += delta_t + + # calc control input + u = lqr_control(x) + + # simulate inverted pendulum cart + x = simulation(x, u) + + if show_animation: + plt.clf() + px = float(x[0]) + theta = float(x[2]) + plot_cart(px, theta) + plt.xlim([-5.0, 2.0]) + plt.pause(0.001) + + print("Finish") + print(f"x={float(x[0]):.2f} [m] , theta={math.degrees(x[2]):.2f} [deg]") + if show_animation: + plt.show() + + +def simulation(x, u): + A, B = get_model_matrix() + x = A @ x + B @ u + + return x + + +def solve_DARE(A, B, Q, R, maxiter=150, eps=0.01): + """ + Solve a discrete time_Algebraic Riccati equation (DARE) + """ + P = Q + + for i in range(maxiter): + Pn = A.T @ P @ A - A.T @ P @ B @ \ + inv(R + B.T @ P @ B) @ B.T @ P @ A + Q + if (abs(Pn - P)).max() < eps: + break + P = Pn + + return Pn + + +def dlqr(A, B, Q, R): + """ + Solve the discrete time lqr controller. + x[k+1] = A x[k] + B u[k] + cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k] + # ref Bertsekas, p.151 + """ + + # first, try to solve the ricatti equation + P = solve_DARE(A, B, Q, R) + + # compute the LQR gain + K = inv(B.T @ P @ B + R) @ (B.T @ P @ A) + + eigVals, eigVecs = eig(A - B @ K) + return K, P, eigVals + + +def lqr_control(x): + A, B = get_model_matrix() + start = time.time() + K, _, _ = dlqr(A, B, Q, R) + u = -K @ x + elapsed_time = time.time() - start + print(f"calc time:{elapsed_time:.6f} [sec]") + return u + + +def get_numpy_array_from_matrix(x): + """ + get build-in list from matrix + """ + return np.array(x).flatten() + + +def get_model_matrix(): + A = np.array([ + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, m * g / M, 0.0], + [0.0, 0.0, 0.0, 1.0], + [0.0, 0.0, g * (M + m) / (l_bar * M), 0.0] + ]) + A = np.eye(nx) + delta_t * A + + B = np.array([ + [0.0], + [1.0 / M], + [0.0], + [1.0 / (l_bar * M)] + ]) + B = delta_t * B + + return A, B + + +def flatten(a): + return np.array(a).flatten() + + +def plot_cart(xt, theta): + cart_w = 1.0 + cart_h = 0.5 + radius = 0.1 + + cx = np.array([-cart_w / 2.0, cart_w / 2.0, cart_w / + 2.0, -cart_w / 2.0, -cart_w / 2.0]) + cy = np.array([0.0, 0.0, cart_h, cart_h, 0.0]) + cy += radius * 2.0 + + cx = cx + xt + + bx = np.array([0.0, l_bar * math.sin(-theta)]) + bx += xt + by = np.array([cart_h, l_bar * math.cos(-theta) + cart_h]) + by += radius * 2.0 + + angles = np.arange(0.0, math.pi * 2.0, math.radians(3.0)) + ox = np.array([radius * math.cos(a) for a in angles]) + oy = np.array([radius * math.sin(a) for a in angles]) + + rwx = np.copy(ox) + cart_w / 4.0 + xt + rwy = np.copy(oy) + radius + lwx = np.copy(ox) - cart_w / 4.0 + xt + lwy = np.copy(oy) + radius + + wx = np.copy(ox) + bx[-1] + wy = np.copy(oy) + by[-1] + + plt.plot(flatten(cx), flatten(cy), "-b") + plt.plot(flatten(bx), flatten(by), "-k") + plt.plot(flatten(rwx), flatten(rwy), "-k") + plt.plot(flatten(lwx), flatten(lwy), "-k") + plt.plot(flatten(wx), flatten(wy), "-k") + plt.title(f"x: {xt:.2f} , theta: {math.degrees(theta):.2f}") + + # for stopping simulation with the esc key. + plt.gcf().canvas.mpl_connect( + 'key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) + + plt.axis("equal") + + +if __name__ == '__main__': + main() diff --git a/Control/InvertedPendulumMPCControl/inverted_pendulum_mpc_control.py b/Control/inverted_pendulum/inverted_pendulum_mpc_control.py similarity index 84% rename from Control/InvertedPendulumMPCControl/inverted_pendulum_mpc_control.py rename to Control/inverted_pendulum/inverted_pendulum_mpc_control.py index 3764f295..6b966b13 100644 --- a/Control/InvertedPendulumMPCControl/inverted_pendulum_mpc_control.py +++ b/Control/inverted_pendulum/inverted_pendulum_mpc_control.py @@ -17,14 +17,16 @@ M = 1.0 # [kg] m = 0.3 # [kg] g = 9.8 # [m/s^2] -Q = np.diag([0.0, 1.0, 1.0, 0.0]) -R = np.diag([0.01]) nx = 4 # number of state nu = 1 # number of input +Q = np.diag([0.0, 1.0, 1.0, 0.0]) # state cost matrix +R = np.diag([0.01]) # input cost matrix + T = 30 # Horizon length delta_t = 0.1 # time tick +sim_time = 5.0 # simulation time [s] -animation = True +show_animation = True def main(): @@ -36,8 +38,10 @@ def main(): ]) x = np.copy(x0) + time = 0.0 - for i in range(50): + while sim_time > time: + time += delta_t # calc control input opt_x, opt_delta_x, opt_theta, opt_delta_theta, opt_input = \ @@ -49,7 +53,7 @@ def main(): # simulate inverted pendulum cart x = simulation(x, u) - if animation: + if show_animation: plt.clf() px = float(x[0]) theta = float(x[2]) @@ -57,10 +61,14 @@ def main(): plt.xlim([-5.0, 2.0]) plt.pause(0.001) + print("Finish") + print(f"x={float(x[0]):.2f} [m] , theta={math.degrees(x[2]):.2f} [deg]") + if show_animation: + plt.show() + def simulation(x, u): A, B = get_model_matrix() - x = np.dot(A, x) + np.dot(B, u) return x @@ -85,7 +93,7 @@ def mpc_control(x0): start = time.time() prob.solve(verbose=False) elapsed_time = time.time() - start - print("calc time:{0} [sec]".format(elapsed_time)) + print(f"calc time:{elapsed_time:.6f} [sec]") if prob.status == cvxpy.OPTIMAL: ox = get_numpy_array_from_matrix(x.value[0, :]) @@ -165,8 +173,12 @@ def plot_cart(xt, theta): plt.plot(flatten(rwx), flatten(rwy), "-k") plt.plot(flatten(lwx), flatten(lwy), "-k") plt.plot(flatten(wx), flatten(wy), "-k") - plt.title("x:" + str(round(xt, 2)) + ",theta:" + - str(round(math.degrees(theta), 2))) + plt.title(f"x: {xt:.2f} , theta: {math.degrees(theta):.2f}") + + # for stopping simulation with the esc key. + plt.gcf().canvas.mpl_connect( + 'key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) plt.axis("equal") diff --git a/docs/modules/control/control_main.rst b/docs/modules/control/control_main.rst index 9eeba434..d6d36a72 100644 --- a/docs/modules/control/control_main.rst +++ b/docs/modules/control/control_main.rst @@ -3,6 +3,7 @@ Control ================= -.. include:: inverted_pendulum_mpc_control/inverted_pendulum_mpc_control.rst +.. include:: inverted_pendulum_control/inverted_pendulum_control.rst + .. include:: move_to_a_pose_control/move_to_a_pose_control.rst diff --git a/docs/modules/control/inverted_pendulum_control/inverted-pendulum.png b/docs/modules/control/inverted_pendulum_control/inverted-pendulum.png new file mode 100644 index 0000000000000000000000000000000000000000..841aed922078f2e72aeec6dab3779c177b011fd8 GIT binary patch literal 6127 zcmbVQcQ{<%+8rrEh$umb5+x=|5JVqBh(0=r8qq}=B0|)Nh!%_yy#zrddQ6njYog2O zqm2^1m(hLO@7{m!bMN!b^PD+n&Y8W>yZ2u2TI=1RT5zRnR~fHDAdqV+$_hH*JP(dh z5@PUeN~y04PNWFsr!OH8isti!=o2r+EeM1@Lq$PO&ogCp++VN%Rc+g*t<=T4A|yOy zCfZcGeUB~fJI>_Wi5(+l|CILLRWtEl3EfkMVw-A~l0I1S52lG=p5wGJ*|tM#6LBv) z@2V?~tKiVI9Cu7@p1xjt-?tJjEWy*sM~JHImT!-G`}Ux7!|xc?8MR*h$b5b~wTH0n z>%ZmzKJp|PDXjY#2wQWiiuil2*dSdJ#s79OF?l(p-U@A0>2n~Ykwh$tlT8nam}Ny0 zlLsqLPfwFvzfLem60Pw+HRi3(uxCtXmJTt{kzYuIkrN$WK-L$83=$1`uVrYOv1I)y z6=eMA_gySnMO^ViVwa72QC)Y4*u=9Nlmp$%J&<{*2D`LCR9=)!n3(|7VImq4MsYv6 z%jc&vvoK-BUMzN!VBsU(S9g4efsEcm&8b1Pk$Y7C>C@k#w7j0jy=+DAygH5^HrM)g zdQOv?$I4Ub2opUi$P6qNn^@2hbV*QziBBW`5Xb-7(C^p+e!DCMrm)yZTg?^ z>ZhM=M)uezJ=WHyQi^1<`SV4kGgXQn^;heKlIT(gPs^qIMbyim4s};EC za@aCl8pdOl@yc};tATF=)yG?JgoK3l7d8Fc|73Uk>M?JqaGkl!!_(-1KCIw*04D3Z zL)~do?Kr|@s3rV0IkdhjS zb{4U|x=#dd^?)NGQfPz$sSJR37n}JDX)2H7cnww$IY2CXIU(3IaAOYO@oerlP{H>lnS7{beDuKXnme z)04KEIPBV)5<2djTT+6Ij;54<^oW#<42Ar2WmYoRF22jga=hx5<4Eb{cMbLBlYYFD ztlj~ggXgH?!PU-e;G52$A<{=x1-*lVbcx`(sF;`-1DC8Vs0MZz{em8nn3!l~YkO3g zIq?W2nx28dV-*!rh(KNSL|w~{&^cIgvA<*Ps6n2TJilyjKXYQDz0QumQ7~F;anS>l zy1(yP`s0Up*ZA#k(IZBR30*eX9&i1gX<_UXN^^dL{^czEF6bWAH_Xye57`Wj@AuIG~i15sz6%LxCWPSc(I zbl#efeH83E4sNE-UViX>&#@*g4a_wEi)Ih!+$YglhF;hT( zem;NK{-~p93<{N3P+;8>M(a3SELZsAFfju36dAe4lfZ=v%~p%o*41sIH}{cqc6N>z z-5)8nCPSmq&24SMuh*DJb1rbETt!Hxu8f{U8p47Gi_CjgMt+P`xN@|#v~b?JNFnzWWfyUyG-9Q$+cc(=qAk>stukd{X6Z|2mGZniJr$ z92R84KY#YYo|G^q90-6%;iK2y#_GSa>$qO9o@e~CmMwLdmIJS+@c8lL59YYzeqlB? zB80!~$r@%|(uZ6F>v{6(wIB@*&C~rIU1qLoT2mkH?!uX$NlaVBvl z3JNCidNqNXemp^J9Z_uClV+e~0w7oGdvwl@Hp`I(?usw|HJjCEr$=VHl;JT#Efam1{pCxk6Hc_J zv!KszW#e_Lal8pN4UNg@1K#h3MXk>SA%b?jx2cJ*TRfSje0+p3epg^z{=6Kg0*BiS=09~)OK1eq_Ac+v z{{2f$Pd}fSP-0X7XnAXQXkNmH_?;gkbl$YIx%Y3fyI_1ft%SVj znDUQW-aJK6LjEomcT=$%$lSQ4T0Sb4=+ziakC#PR2MX~Jq z42{*E$V$RUZ!j{Bq66O>a;6Y~W!1_+yRcmYO z^Gr_*5ET)Lj*o|;RL7MKNVwT~c-og);WKunN?Lum&xdgE!!ySPfTCVw0^9;tGy%0An{IJM~52B z`p$8+$EuM;>NPFu?%P@RjNXK`%VWkod}h_dJ+z_SfHxi%4GFu=hENbF9n`iduG|uJ zj24Y2QvYeG6(nZ5Ju2B+@FH76Lc$UEQ)94~7<5gGM~H)LbcZev7F?`4{naSy^*Qrb|Smw$Z~?uXqD&)h;JyT=N0ZAkO9Lbl(z^ zqnXZ6MiCekBwRn@DB2Tu6(Uf*5tz+? zUlRO2(7rX-!OhLh1|TI+cdS$Rf|v>fsIBcfy1Yu!-q8UgqY!l(zWNkVY}3VA@z=mrK}08GOnrbD0xkj%`;z(>M@ zu9A_#jCnef9;gM5gaZstxl4Mj*ASeS+4PKzmKG7W$td2ilICWLXk?X@#R5o9&&)ji z>6lojMh#PM%=?OhYMz4{0<=m~UjBU9U>c2ixBtoYR_F4I8#`Fdqe^!Cb+7^pyXR(B z8Wt4E0s~!d^eM6a=F}3n6R&e4%RUMEr8HZ9or@!v9jG@EA@CxiDBZNmJ@nEKdX#bN z4G`GS%n8yB z;s2JO|E0K?5;2+l0&N6?g)A>E4VPMXFnd#%D>ZT}(65eT;>$z`Z`Q);1;Bke&Fj+( z3wDvrQmw#1Rm&8DK0H9LbR6c}$dc01n0HsMmp*H_P!H1?92|7poL1zcBeSruu(Gyx zJw3i$Bx~5n9hlVeyunHcjk)h?%mq&w&b)O7>`LCZZ(B^VJUmxx%%Xsy-rt;Il5h(W zoAf~eLIjGJDC)$M?z@*b^x)E}DYtyU5!W6tM94&8+nBhx)?Y}D1hB3}M)m#u{k^@t zQwJl?BDs>kiDuZBS1Psg{IrzJV~w@lP{bMx_W0zUf0Yj{}QqPmS7#3QuK&DR;6q)h61zEPR=ExN}Z6I>+R zH#i`F*3U?{OJ9;)yY?s-t8Hl5QCJ?HcQjc?H8@nSOZE~bX8GN2tl|dOSbSigonB6B zaj~EuX5AO4YCrqu(9qC`oA(1lX`oxbIDSZpy!pw+DH3sbct{sDUg4V6(!(E*0(R)R zplA7F2^dFr8mt4vM?`Qx9*z~!ghYK}{cBiMJMHCk7<47prH-0`A#bb@M}_w-!L4#~ zaS3^CSv`(o=>i4kulDw(4P}XE^^o=`7R#ynw@flV!@>-gdSxZUfHC<4mPx|Q)RcL? zCmkv&nFMfEX!Iy^ceg2q;elI_ zZD6bMvR_G>|5^D3ia6itgVQ{0WtOntvFC)(w#JHw6dsRnZfQXxk#erC_p`IJ^Gzya z+S^rGJg10?WQ`m10(N%X!QblM?9y(Tn|wqU#+8-|{lQ{^6L)%LpI|pxLtw(kcZ%HxG?d8DIot8=G(_ofb>$0!vIl{0LNjVbay+29-*8G)o>pCZiL zL3Ud=29ZwqJI}Y%*6wXYJk?7(2&6ZVzETf#OGU*ypyw2@x2dWs&wO_(X67XW1A~>7 zmHSdfsk|~();jY%gIoj5TM981fU`MOSZ4zgQRjE!>0rSR0geNI8letU=~x5)g7zHd z5th2*p0k%qTsOhEUaox?KIK>2M}mbn`M5S`B36VEh+cND!WCK%_V-tqzih%}o3n4I zh@Ujixt08#>&o}?dKuo99tDbyGzb2T52mpSQ5Kuq!3n!`JR`T_j^)tKdcE~4-|MjaDy%t9`)O*(Y2}Nb5O$$ zD`itWt$8n^X&h~AEt#6OwB!I!m24UACp~{uZpgnqFPoU~-~}BO)i_Rf@L32aH+P<~ zY*snxH>9w%G*edJ@FKyuz`&giZ>7=B$>#fHxZPh+=Pob4qvjgJ!x0rN%svt(4ZrHX&)y|NSL>_e3->PGU~sEz`LU7f?aE zWgX8;Jem14uup1zD(8JM9(7Hivc+Uoe+Hqi9(I`X3wmxm%Ymo17mlw_A;d?0HG!K3 zYqv6rCPV#h4vruC;V_^E({kE~)*rlT@94PFD|?&Nn!finfTK9u1ntc?=#JzG=S0Frs|2}?imlJS`#y)Pic4De2@0@ zxBAzUUHH9EQxX$@XO6X~x2Gs8C|m*+MnSF1>L^-0MI5=iJcLS5*904Jpg@FayJ>-K zOON-#!6l%!*xK56|?-`ozTBc zQeJdx>*En?bx&YnGlrJ$Gkz@XNZ6?OHd{^*Mq VGO#d4OR$3vQBi~|6v{sf{6G7W$mjq7 literal 0 HcmV?d00001 diff --git a/docs/modules/control/inverted_pendulum_control/inverted_pendulum_control.rst b/docs/modules/control/inverted_pendulum_control/inverted_pendulum_control.rst new file mode 100644 index 00000000..5a447f7c --- /dev/null +++ b/docs/modules/control/inverted_pendulum_control/inverted_pendulum_control.rst @@ -0,0 +1,97 @@ +Inverted Pendulum Control +----------------------------- + +An inverted pendulum on a cart consists of a mass :math:`m` at the top of a pole of length :math:`l` pivoted on a +horizontally moving base as shown in the adjacent. + +The objective of the control system is to balance the inverted pendulum by applying a force to the cart that the pendulum is attached to. + +Modeling +~~~~~~~~~~~~ + +.. image:: inverted_pendulum_control/inverted-pendulum.png + :align: center + +- :math:`M`: mass of the cart +- :math:`m`: mass of the load on the top of the rod +- :math:`l`: length of the rod +- :math:`u`: force applied to the cart +- :math:`x`: cart position coordinate +- :math:`\theta`: pendulum angle from vertical + +Using Lagrange's equations: + +.. math:: + & (M + m)\ddot{x} - ml\ddot{\theta}cos{\theta} + ml\dot{\theta^2}\sin{\theta} = u \\ + & l\ddot{\theta} - g\sin{\theta} = \ddot{x}\cos{\theta} + +See this `link `__ for more details. + +So + +.. math:: + & \ddot{x} = \frac{m(gcos{\theta} - \dot{\theta}^2l)sin{\theta} + u}{M + m - mcos^2{\theta}} \\ + & \ddot{\theta} = \frac{g(M + m)sin{\theta} - \dot{\theta}^2lmsin{\theta}cos{\theta} + ucos{\theta}}{l(M + m - mcos^2{\theta})} + + +Linearlied model when :math:`\theta` small, :math:`cos{\theta} \approx 1`, :math:`sin{\theta} \approx \theta`, :math:`\dot{\theta}^2 \approx 0`. + +.. math:: + & \ddot{x} = \frac{gm}{M}\theta + \frac{1}{M}u\\ + & \ddot{\theta} = \frac{g(M + m)}{Ml}\theta + \frac{1}{Ml}u + +State space: + +.. math:: + & \dot{x} = Ax + Bu \\ + & y = Cx + Du + +where + +.. math:: + & x = [x, \dot{x}, \theta,\dot{\theta}]\\ + & A = \begin{bmatrix} 0 & 1 & 0 & 0\\0 & 0 & \frac{gm}{M} & 0\\0 & 0 & 0 & 1\\0 & 0 & \frac{g(M + m)}{Ml} & 0 \end{bmatrix}\\ + & B = \begin{bmatrix} 0 \\ \frac{1}{M} \\ 0 \\ \frac{1}{Ml} \end{bmatrix} + +If control only \theta + +.. math:: + & C = \begin{bmatrix} 0 & 0 & 1 & 0 \end{bmatrix}\\ + & D = [0] + +If control x and \theta + +.. math:: + & C = \begin{bmatrix} 1 & 0 & 0 & 0\\0 & 0 & 1 & 0 \end{bmatrix}\\ + & D = \begin{bmatrix} 0 \\ 0 \end{bmatrix} + +LQR control +~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The LQR cotroller minimize this cost function defined as: + +.. math:: J = x^T Q x + u^T R u +the feedback control law that minimizes the value of the cost is: + +.. math:: u = -K x +where: + +.. math:: K = (B^T P B + R)^{-1} B^T P A +and :math:`P` is the unique positive definite solution to the discrete time `algebraic Riccati equation `__ (DARE): + +.. math:: P = A^T P A - A^T P B ( R + B^T P B )^{-1} B^T P A + Q + +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Control/InvertedPendulumCart/animation_lqr.gif + +MPC control +~~~~~~~~~~~~~~~~~~~~~~~~~~~ +The MPC cotroller minimize this cost function defined as: + +.. math:: J = x^T Q x + u^T R u + +subject to: + +- Linearlied Inverted Pendulum model +- Initial state + +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Control/InvertedPendulumCart/animation.gif diff --git a/docs/modules/control/inverted_pendulum_mpc_control/inverted_pendulum_mpc_control.rst b/docs/modules/control/inverted_pendulum_mpc_control/inverted_pendulum_mpc_control.rst deleted file mode 100644 index 328f9f6b..00000000 --- a/docs/modules/control/inverted_pendulum_mpc_control/inverted_pendulum_mpc_control.rst +++ /dev/null @@ -1,6 +0,0 @@ -Inverted Pendulum MPC Control ------------------------------ - -Bipedal Walking with modifying designated footsteps - -.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Control/InvertedPendulumCart/animation.gif diff --git a/tests/test_inverted_pendulum_lqr_control.py b/tests/test_inverted_pendulum_lqr_control.py new file mode 100644 index 00000000..cbbabb93 --- /dev/null +++ b/tests/test_inverted_pendulum_lqr_control.py @@ -0,0 +1,11 @@ +import conftest +from Control.inverted_pendulum import inverted_pendulum_lqr_control as m + + +def test_1(): + m.show_animation = False + m.main() + + +if __name__ == '__main__': + conftest.run_this_test(__file__) diff --git a/tests/test_inverted_pendulum_mpc_control.py b/tests/test_inverted_pendulum_mpc_control.py index 9519b45e..800aefd7 100644 --- a/tests/test_inverted_pendulum_mpc_control.py +++ b/tests/test_inverted_pendulum_mpc_control.py @@ -1,13 +1,11 @@ import conftest -import sys -if 'cvxpy' in sys.modules: # pragma: no cover - from Control.InvertedPendulumMPCControl \ - import inverted_pendulum_mpc_control as m +from Control.inverted_pendulum import inverted_pendulum_mpc_control as m - def test1(): - m.show_animation = False - m.main() + +def test1(): + m.show_animation = False + m.main() if __name__ == '__main__':