Skip to content

Commit

Permalink
update cvxpy to v1.0
Browse files Browse the repository at this point in the history
  • Loading branch information
@AtsushiSakai
AtsushiSakai committed Apr 14, 2019
1 parent 39aeb02 commit c704c67
Show file tree
Hide file tree
Showing 2 changed files with 60 additions and 57 deletions.
1 change: 1 addition & 0 deletions inverted_pendulum_mpc_control/inverted_pendulum_mpc_control.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down
116 changes: 59 additions & 57 deletions mpc_modeling/mpc_modeling.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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):
"""
Expand All @@ -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 = []
Expand All @@ -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)

Expand All @@ -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)
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand All @@ -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)
Expand All @@ -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)

Expand All @@ -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")
Expand All @@ -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
Expand All @@ -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()
Expand Down Expand Up @@ -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
Expand All @@ -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()
Expand Down Expand Up @@ -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)

Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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()

0 comments on commit c704c67

Please sign in to comment.