update cvxpy to v1.0

Author: AtsushiSakai

Diff for: inverted_pendulum_mpc_control/inverted_pendulum_mpc_control.py

@@ -77,6 +77,7 @@ def mpc_control(x0):
         cost += cvxpy.quad_form(x[:, t + 1], Q)
         cost += cvxpy.quad_form(u[:, t], R)
         constr += [x[:, t + 1] == A * x[:, t] + B * u[:, t]]
+    # print(x0)
     constr += [x[:, 0] == x0[:, 0]]
     prob = cvxpy.Problem(cvxpy.Minimize(cost), constr)
 

Diff for: mpc_modeling/mpc_modeling.py

@@ -15,6 +15,8 @@
 
 DEBUG_ = False
 
+print("cvxpy version:", cvxpy.__version__)
+
 
 def use_modeling_tool(A, B, N, Q, R, P, x0, umax=None, umin=None, xmin=None, xmax=None):
     """
@@ -23,8 +25,8 @@ def use_modeling_tool(A, B, N, Q, R, P, x0, umax=None, umin=None, xmin=None, xma
     (nx, nu) = B.shape
 
     # mpc calculation
-    x = cvxpy.Variable(nx, N + 1)
-    u = cvxpy.Variable(nu, N)
+    x = cvxpy.Variable((nx, N + 1))
+    u = cvxpy.Variable((nu, N))
 
     costlist = 0.0
     constrlist = []
@@ -36,23 +38,24 @@ def use_modeling_tool(A, B, N, Q, R, P, x0, umax=None, umin=None, xmin=None, xma
         constrlist += [x[:, t + 1] == A * x[:, t] + B * u[:, t]]
 
         if xmin is not None:
-            constrlist += [x[:, t] >= xmin]
+            constrlist += [x[:, t] >= xmin[:, 0]]
         if xmax is not None:
-            constrlist += [x[:, t] <= xmax]
+            constrlist += [x[:, t] <= xmax[:, 0]]
 
     costlist += 0.5 * cvxpy.quad_form(x[:, N], P)  # terminal cost
     if xmin is not None:
-        constrlist += [x[:, N] >= xmin]
+        constrlist += [x[:, N] >= xmin[:, 0]]
     if xmax is not None:
-        constrlist += [x[:, N] <= xmax]
-
-    prob = cvxpy.Problem(cvxpy.Minimize(costlist), constrlist)
+        constrlist += [x[:, N] <= xmax[:, 0]]
 
-    prob.constraints += [x[:, 0] == x0]  # inital state constraints
     if umax is not None:
-        prob.constraints += [u <= umax]  # input constraints
+        constrlist += [u <= umax]  # input constraints
     if umin is not None:
-        prob.constraints += [u >= umin]  # input constraints
+        constrlist += [u >= umin]  # input constraints
+
+    constrlist += [x[:, 0] == x0[:, 0]]  # inital state constraints
+
+    prob = cvxpy.Problem(cvxpy.Minimize(costlist), constrlist)
 
     prob.solve(verbose=True)
 
@@ -74,25 +77,25 @@ def opt_mpc_with_input_const(A, B, N, Q, R, P, x0, umax=None, umin=None):
     Ai = A
     AA = Ai
     for i in range(2, N + 1):
-        Ai = A * Ai
+        Ai = A @ Ai
         AA = np.vstack((AA, Ai))
     #  print(AA)
 
     # calc BB
     AiB = B
     BB = np.kron(np.eye(N), AiB)
     for i in range(1, N):
-        AiB = A * AiB
+        AiB = A @ AiB
         BB += np.kron(np.diag(np.ones(N - i), -i), AiB)
     #  print(BB)
 
     RR = np.kron(np.eye(N), R)
     QQ = scipy.linalg.block_diag(np.kron(np.eye(N - 1), Q), P)
 
-    H = (BB.T * QQ * BB + RR)
+    H = (BB.T @ QQ @ BB + RR)
     #  print(H)
 
-    gx0 = BB.T * QQ * AA * x0
+    gx0 = BB.T @ QQ @ AA @ x0
     #  print(gx0)
     P = matrix(H)
     q = matrix(gx0)
@@ -121,10 +124,10 @@ def opt_mpc_with_input_const(A, B, N, Q, R, P, x0, umax=None, umin=None):
 
         sol = cvxopt.solvers.qp(P, q, G, h)
 
-    u = np.matrix(sol["x"])
+    u = np.array(sol["x"])
 
     # recover x
-    xx = AA * x0 + BB * u
+    xx = AA @ x0 + BB @ u
     x = np.vstack((x0.T, xx.reshape(N, nx)))
 
     return x, u
@@ -191,7 +194,7 @@ def opt_mpc_with_state_constr(A, B, N, Q, R, P, x0, xmin=None, xmax=None, umax=N
     #  print(Ae.shape)
 
     # calc be
-    be = np.vstack((A, np.zeros(((N - 1) * nx, nx)))) * x0
+    be = np.vstack((A, np.zeros(((N - 1) * nx, nx)))) @ x0
     #  print(be)
 
     np.set_printoptions(precision=3)
@@ -221,7 +224,7 @@ def opt_mpc_with_state_constr(A, B, N, Q, R, P, x0, xmin=None, xmax=None, umax=N
         sol = cvxopt.solvers.qp(P, q, G, h, A=A, b=b)
 
     #  print(sol)
-    fx = np.matrix(sol["x"])
+    fx = np.array(sol["x"])
     #  print(fx)
 
     u = fx[0:N * nu].reshape(N, nu).T
@@ -236,9 +239,9 @@ def opt_mpc_with_state_constr(A, B, N, Q, R, P, x0, xmin=None, xmax=None, umax=N
 
 def test1():
     print("start!!")
-    A = np.matrix([[0.8, 1.0], [0, 0.9]])
+    A = np.array([[0.8, 1.0], [0, 0.9]])
     #  print(A)
-    B = np.matrix([[-1.0], [2.0]])
+    B = np.array([[-1.0], [2.0]])
     #  print(B)
     (nx, nu) = B.shape
     #  print(nx, nu)
@@ -252,7 +255,8 @@ def test1():
     #  print(P)
     #  umax = 0.7
 
-    x0 = np.matrix([[1.0], [2.0]])  # init state
+    x0 = np.array([[1.0],
+                   [2.0]])  # init state
 
     x, u = use_modeling_tool(A, B, N, Q, R, P, x0)
 
@@ -274,7 +278,6 @@ def test1():
     u = np.array(u).flatten()
 
     if DEBUG_:
-        #  flg, ax = plt.subplots(1)
         plt.plot(x1, '*r', label="x1")
         plt.plot(x2, '*b', label="x2")
         plt.plot(u, '*k', label="u")
@@ -288,8 +291,8 @@ def test1():
 
 def test2():
     print("start!!")
-    A = np.matrix([[0.8, 1.0], [0, 0.9]])
-    B = np.matrix([[-1.0], [2.0]])
+    A = np.array([[0.8, 1.0], [0, 0.9]])
+    B = np.array([[-1.0], [2.0]])
     (nx, nu) = B.shape
 
     N = 10  # number of horizon
@@ -299,10 +302,9 @@ def test2():
     umax = 0.7
     umin = -0.7
 
-    x0 = np.matrix([[1.0], [2.0]])  # init state
+    x0 = np.array([[1.0], [2.0]])  # init state
 
     x, u = use_modeling_tool(A, B, N, Q, R, P, x0, umax=umax, umin=umin)
-    #  x, u = use_modeling_tool(A, B, N, Q, R, P, x0, umin=umin)
 
     rx1 = np.array(x[0, :]).flatten()
     rx2 = np.array(x[1, :]).flatten()
@@ -336,8 +338,8 @@ def test2():
 
 def test3():
     print("start!!")
-    A = np.matrix([[0.8, 1.0], [0, 0.9]])
-    B = np.matrix([[-1.0], [2.0]])
+    A = np.array([[0.8, 1.0], [0, 0.9]])
+    B = np.array([[-1.0], [2.0]])
     (nx, nu) = B.shape
 
     N = 10  # number of horizon
@@ -347,7 +349,7 @@ def test3():
     umax = 0.7
     umin = -0.7
 
-    x0 = np.matrix([[1.0], [2.0]])  # init state
+    x0 = np.array([[1.0], [2.0]])  # init state
     x, u = use_modeling_tool(A, B, N, Q, R, P, x0, umax=umax, umin=umin)
 
     rx1 = np.array(x[0, :]).flatten()
@@ -383,16 +385,16 @@ def test3():
 
 def test4():
     print("start!!")
-    A = np.matrix([[0.8, 1.0], [0, 0.9]])
-    B = np.matrix([[-1.0], [2.0]])
+    A = np.array([[0.8, 1.0], [0, 0.9]])
+    B = np.array([[-1.0], [2.0]])
     (nx, nu) = B.shape
 
     N = 10  # number of horizon
     Q = np.eye(nx)
     R = np.eye(nu)
     P = np.eye(nx)
 
-    x0 = np.matrix([[1.0], [2.0]])  # init state
+    x0 = np.array([[1.0], [2.0]])  # init state
 
     x, u = use_modeling_tool(A, B, N, Q, R, P, x0)
 
@@ -429,16 +431,16 @@ def test4():
 
 def test5():
     print("start!!")
-    A = np.matrix([[0.8, 1.0], [0, 0.9]])
-    B = np.matrix([[-1.0], [2.0]])
+    A = np.array([[0.8, 1.0], [0, 0.9]])
+    B = np.array([[-1.0], [2.0]])
     (nx, nu) = B.shape
 
     N = 10  # number of horizon
     Q = np.eye(nx)
     R = np.eye(nu)
     P = np.eye(nx)
 
-    x0 = np.matrix([[1.0], [2.0]])  # init state
+    x0 = np.array([[1.0], [2.0]])  # init state
     umax = 0.7
 
     x, u = use_modeling_tool(A, B, N, Q, R, P, x0, umax=umax)
@@ -475,23 +477,23 @@ def test5():
 
 def test6():
     print("start!!")
-    A = np.matrix([[0.8, 1.0], [0, 0.9]])
-    B = np.matrix([[-1.0], [2.0]])
+    A = np.array([[0.8, 1.0], [0, 0.9]])
+    B = np.array([[-1.0], [2.0]])
     (nx, nu) = B.shape
 
     N = 10  # number of horizon
     Q = np.eye(nx)
     R = np.eye(nu)
     P = np.eye(nx)
 
-    x0 = np.matrix([[1.0], [2.0]])  # init state
+    x0 = np.array([[1.0], [2.0]])  # init state
     umax = 0.7
     umin = -0.7
 
-    x0 = np.matrix([[1.0], [2.0]])  # init state
+    x0 = np.array([[1.0], [2.0]])  # init state
 
-    xmin = np.matrix([[-3.5], [-0.5]])  # state constraints
-    xmax = np.matrix([[3.5], [2.0]])  # state constraints
+    xmin = np.array([[-3.5], [-0.5]])  # state constraints
+    xmax = np.array([[3.5], [2.0]])  # state constraints
 
     x, u = use_modeling_tool(A, B, N, Q, R, P, x0,
                              umax=umax, umin=umin, xmin=xmin, xmax=xmax)
@@ -529,23 +531,23 @@ def test6():
 
 def test7():
     print("start!!")
-    A = np.matrix([[0.8, 1.0], [0, 0.9]])
-    B = np.matrix([[-1.0], [2.0]])
+    A = np.array([[0.8, 1.0], [0, 0.9]])
+    B = np.array([[-1.0], [2.0]])
     (nx, nu) = B.shape
 
     N = 3  # number of horizon
     Q = np.eye(nx)
     R = np.eye(nu)
     P = np.eye(nx)
 
-    x0 = np.matrix([[1.0], [2.0]])  # init state
+    x0 = np.array([[1.0], [2.0]])  # init state
     umax = 0.7
     umin = -0.7
 
-    x0 = np.matrix([[1.0], [2.0]])  # init state
+    x0 = np.array([[1.0], [2.0]])  # init state
 
-    #  xmin = np.matrix([[-3.5], [-0.5]])  # state constraints
-    #  xmax = np.matrix([[3.5], [2.0]])  # state constraints
+    #  xmin = np.array([[-3.5], [-0.5]])  # state constraints
+    #  xmax = np.array([[3.5], [2.0]])  # state constraints
 
     x, u = use_modeling_tool(A, B, N, Q, R, P, x0, umax=umax, umin=umin)
     #  x, u = use_modeling_tool(A, B, N, Q, R, P, x0, umax=umax, umin=umin, xmin=xmin, xmax=xmax)
@@ -590,23 +592,23 @@ def test_output_check(rx1, rx2, ru, x1, x2, u):
     print("test x1")
     for (i, j) in zip(rx1, x1):
         print(i, j)
-        assert (i - j) <= 0.0001, "Error"
+        assert (i - j) <= 0.01, "Error"
     print("test x2")
     for (i, j) in zip(rx2, x2):
         print(i, j)
-        assert (i - j) <= 0.0001, "Error"
+        assert (i - j) <= 0.01, "Error"
     print("test u")
     for (i, j) in zip(ru, u):
         print(i, j)
-        assert (i - j) <= 0.0001, "Error"
+        assert (i - j) <= 0.01, "Error"
 
 
 if __name__ == '__main__':
     DEBUG_ = True
-    #  test1()
-    #  test2()
-    #  test3()
-    #  test4()
-    #  test5()
-    #  test6()
+    test1()
+    test2()
+    test3()
+    test4()
+    test5()
+    test6()
     test7()