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 00000000..841aed92 Binary files /dev/null and b/docs/modules/control/inverted_pendulum_control/inverted-pendulum.png differ 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__':