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

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()

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")
 

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
 

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 <https://en.wikipedia.org/wiki/Inverted_pendulum#From_Lagrange's_equations>`__ 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 <https://en.wikipedia.org/wiki/Inverted_pendulum#From_Lagrange's_equations>`__  (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

docs/modules/control/inverted_pendulum_mpc_control/inverted_pendulum_mpc_control.rst

@@ -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

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__)

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__':