Skip to content

Commit

Permalink
code cleanup
Browse files Browse the repository at this point in the history
  • Loading branch information
@AtsushiSakai
AtsushiSakai committed Feb 6, 2017
1 parent 62a1d42 commit 9bfbbc0
Show file tree
Hide file tree
Showing 2 changed files with 123 additions and 221 deletions.
196 changes: 114 additions & 82 deletions mpc_modeling/mpc_modeling.py
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,11 @@
#! /usr/bin/python
# -*- coding: utf-8 -*
"""
Model predictive control sample code without modeling tool (cvxpy)
author: Atsushi Sakai
"""

import cvxpy
import numpy as np
Expand All @@ -13,6 +19,9 @@


def use_modeling_tool(A, B, N, Q, R, P, x0, umax=None, umin=None, xmin=None, xmax=None):
"""
solve MPC with modeling tool for test
"""
(nx, nu) = B.shape

# mpc calculation
Expand Down Expand Up @@ -52,15 +61,23 @@ def use_modeling_tool(A, B, N, Q, R, P, x0, umax=None, umin=None, xmin=None, xma
return x.value, u.value


def hand_modeling(A, B, N, Q, R, P, x0, umax=None, umin=None):
def opt_mpc_with_input_const(A, B, N, Q, R, P, x0, umax=None, umin=None):
"""
optimize model predictive control only with input constraints
(if you want to solve a problem with state constraints, you can use opt_mpc_with_state_const())
return
x: state
u: state
"""
(nx, nu) = B.shape

# calc AA
Ai = A
AA = Ai
for i in range(2, N + 1):
Ai = A * Ai
AA = np.concatenate((AA, Ai), axis=0)
AA = np.vstack((AA, Ai))
# print(AA)

# calc BB
Expand All @@ -79,30 +96,27 @@ def hand_modeling(A, B, N, Q, R, P, x0, umax=None, umin=None):

gx0 = BB.T * QQ * AA * x0
# print(gx0)
P = matrix(H)
q = matrix(gx0)

if umax is None and umin is None:
P = matrix(H)
q = matrix(gx0)
sol = cvxopt.solvers.qp(P, q)
# print(sol)
else:
P = matrix(H)
q = matrix(gx0)

G = np.matrix([])
h = np.matrix([])
G = np.zeros((0, nu * N))
h = np.zeros((0, 1))

if umax is not None:
G = np.eye(N)
h = np.ones((N, 1)) * umax
tG = np.eye(N * nu)
th = np.kron(np.ones((N * nu, 1)), umax)
G = np.vstack([G, tG])
h = np.vstack([h, th])

if umin is not None:
if umax is None:
G = np.eye(N) * -1.0
h = np.ones((N, 1)) * umin * -1.0
else:
G = np.concatenate((G, np.eye(N) * -1.0), axis=0)
h = np.concatenate((h, np.ones((N, 1)) * umin * -1.0), axis=0)
tG = np.eye(N * nu) * -1.0
th = np.kron(np.ones((N * nu, 1)), umin * -1.0)
G = np.vstack([G, tG])
h = np.vstack([h, th])

G = matrix(G)
h = matrix(h)
Expand All @@ -113,12 +127,54 @@ def hand_modeling(A, B, N, Q, R, P, x0, umax=None, umin=None):

# recover x
xx = AA * x0 + BB * u
x = np.concatenate((x0.T, xx.reshape(N, nx)), axis=0)
x = np.vstack((x0.T, xx.reshape(N, nx)))

return x, u


def hand_modeling2(A, B, N, Q, R, P, x0, xmin=None, xmax=None, umax=None, umin=None):
def generate_inequalities_constraints_mat(N, nx, nu, xmin, xmax, umin, umax):
"""
generate matrices of inequalities constrints
return G, h
"""
G = np.zeros((0, (nx + nu) * N))
h = np.zeros((0, 1))
if umax is not None:
tG = np.hstack([np.eye(N * nu), np.zeros((N * nu, nx * N))])
th = np.kron(np.ones((N * nu, 1)), umax)
G = np.vstack([G, tG])
h = np.vstack([h, th])

if umin is not None:
tG = np.hstack([np.eye(N * nu) * -1.0, np.zeros((N * nu, nx * N))])
th = np.kron(np.ones((N, 1)), umin * -1.0)
G = np.vstack([G, tG])
h = np.vstack([h, th])

if xmax is not None:
tG = np.hstack([np.zeros((N * nx, nu * N)), np.eye(N * nx)])
th = np.kron(np.ones((N, 1)), xmax)
G = np.vstack([G, tG])
h = np.vstack([h, th])

if xmin is not None:
tG = np.hstack([np.zeros((N * nx, nu * N)), np.eye(N * nx) * -1.0])
th = np.kron(np.ones((N, 1)), xmin * -1.0)
G = np.vstack([G, tG])
h = np.vstack([h, th])

return G, h


def opt_mpc_with_state_constr(A, B, N, Q, R, P, x0, xmin=None, xmax=None, umax=None, umin=None):
"""
optimize MPC problem with state and (or) input constraints
return
x: state
u: input
"""
(nx, nu) = B.shape

H = scipy.linalg.block_diag(np.kron(np.eye(N), R), np.kron(np.eye(N - 1), Q), np.eye(P.shape[0]))
Expand All @@ -132,11 +188,11 @@ def hand_modeling2(A, B, N, Q, R, P, x0, xmin=None, xmax=None, umax=None, umin=N
Aex -= np.kron(np.diag([1.0] * (N - 1), k=-1), A)
# print(Aex)
# print(Aex.shape)
Ae = np.concatenate((Aeu, Aex), axis=1)
Ae = np.hstack((Aeu, Aex))
# print(Ae.shape)

# calc be
be = np.concatenate((A, np.zeros(((N - 1) * nx, nx))), axis=0) * x0
be = np.vstack((A, np.zeros(((N - 1) * nx, nx)))) * x0
# print(be)

# === optimization ===
Expand All @@ -148,31 +204,7 @@ def hand_modeling2(A, B, N, Q, R, P, x0, xmin=None, xmax=None, umax=None, umin=N
if umax is None and umin is None:
sol = cvxopt.solvers.qp(P, q, A=A, b=b)
else:
G = np.zeros((0, (nx + nu) * N))
h = np.zeros((0, 1))
if umax is not None:
tG = np.hstack([np.eye(N * nu), np.zeros((N * nu, nx * N))])
th = np.kron(np.ones((N, 1)), umax)
G = np.vstack([G, tG])
h = np.vstack([h, th])

if umin is not None:
tG = np.hstack([np.eye(N * nu) * -1.0, np.zeros((N * nu, nx * N))])
th = np.kron(np.ones((N, 1)), umin * -1.0)
G = np.vstack([G, tG])
h = np.vstack([h, th])

if xmax is not None:
tG = np.hstack([np.zeros((N * nx, nu * N)), np.eye(N * nx)])
th = np.kron(np.ones((N, 1)), xmax)
G = np.vstack([G, tG])
h = np.vstack([h, th])

if xmin is not None:
tG = np.hstack([np.zeros((N * nx, nu * N)), np.eye(N * nx) * -1.0])
th = np.kron(np.ones((N, 1)), xmin * -1.0)
G = np.vstack([G, tG])
h = np.vstack([h, th])
G, h = generate_inequalities_constraints_mat(N, nx, nu, xmin, xmax, umin, umax)

G = matrix(G)
h = matrix(h)
Expand All @@ -184,31 +216,36 @@ def hand_modeling2(A, B, N, Q, R, P, x0, xmin=None, xmax=None, umax=None, umin=N
# print(fx)

u = fx[0:N * nu].reshape(N, nu).T

x = fx[-N * nx:].reshape(N, nx).T
x = np.concatenate((x0, x), axis=1)
x = np.hstack((x0, x))
# print(x)
# print(u)

return x, u


def test2():
def test1():
print("start!!")
A = np.matrix([[0.8, 1.0], [0, 0.9]])
# print(A)
B = np.matrix([[-1.0], [2.0]])
# print(B)
(nx, nu) = B.shape
# print(nx, nu)

N = 10 # number of horizon
Q = np.eye(nx)
# print(Q)
R = np.eye(nu)
# print(R)
P = np.eye(nx)
umax = 0.7
umin = -0.7
# print(P)
# umax = 0.7

x0 = np.matrix([[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)
x, u = use_modeling_tool(A, B, N, Q, R, P, x0)

rx1 = np.array(x[0, :]).flatten()
rx2 = np.array(x[1, :]).flatten()
Expand All @@ -221,10 +258,10 @@ def test2():
plt.legend()
plt.grid(True)

x, u = hand_modeling(A, B, N, Q, R, P, x0, umax=umax, umin=umin)
# x, u = hand_modeling(A, B, N, Q, R, P, x0, umin=umin)
x, u = opt_mpc_with_input_const(A, B, N, Q, R, P, x0)
x1 = np.array(x[:, 0]).flatten()
x2 = np.array(x[:, 1]).flatten()
u = np.array(u).flatten()

# flg, ax = plt.subplots(1)
plt.plot(x1, '*r', label="x1")
Expand All @@ -233,30 +270,29 @@ def test2():
plt.legend()
plt.grid(True)

plt.show()
if DEBUG_:
plt.show()

test_output_check(rx1, rx2, ru, x1, x2, u)

def test1():

def test2():
print("start!!")
A = np.matrix([[0.8, 1.0], [0, 0.9]])
print(A)
B = np.matrix([[-1.0], [2.0]])
print(B)
(nx, nu) = B.shape
print(nx, nu)

N = 10 # number of horizon
Q = np.eye(nx)
print(Q)
R = np.eye(nu)
print(R)
P = np.eye(nx)
print(P)
# umax = 0.7
umax = 0.7
umin = -0.7

x0 = np.matrix([[1.0], [2.0]]) # init state

x, u = use_modeling_tool(A, B, N, Q, R, P, x0)
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 All @@ -269,9 +305,10 @@ def test1():
plt.legend()
plt.grid(True)

x, u = hand_modeling(A, B, N, Q, R, P, x0)
x, u = opt_mpc_with_input_const(A, B, N, Q, R, P, x0, umax=umax, umin=umin)
x1 = np.array(x[:, 0]).flatten()
x2 = np.array(x[:, 1]).flatten()
u = np.array(u).flatten()

# flg, ax = plt.subplots(1)
plt.plot(x1, '*r', label="x1")
Expand All @@ -280,7 +317,10 @@ def test1():
plt.legend()
plt.grid(True)

plt.show()
if DEBUG_:
plt.show()

test_output_check(rx1, rx2, ru, x1, x2, u)


def test3():
Expand All @@ -297,13 +337,7 @@ def test3():
umin = -0.7

x0 = np.matrix([[1.0], [2.0]]) # init state

# xmin = np.matrix([[-3.5], [-0.5]]) # state constraints
# xmax = np.matrix([[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)
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)

rx1 = np.array(x[0, :]).flatten()
rx2 = np.array(x[1, :]).flatten()
Expand All @@ -316,9 +350,7 @@ def test3():
plt.legend()
plt.grid(True)

# x, u = hand_modeling2(A, B, N, Q, R, P, x0)
x, u = hand_modeling2(A, B, N, Q, R, P, x0, umax=umax, umin=umin)
# x, u = hand_modeling(A, B, N, Q, R, P, x0, umin=umin)
x, u = opt_mpc_with_state_constr(A, B, N, Q, R, P, x0, umax=umax, umin=umin)
x1 = np.array(x[0, :]).flatten()
x2 = np.array(x[1, :]).flatten()
u = np.array(u).flatten()
Expand All @@ -330,11 +362,11 @@ def test3():
plt.legend()
plt.grid(True)

test_output_check(rx1, rx2, ru, x1, x2, u)

if DEBUG_:
plt.show()

test_output_check(rx1, rx2, ru, x1, x2, u)


def test4():
print("start!!")
Expand Down Expand Up @@ -362,7 +394,7 @@ def test4():
plt.legend()
plt.grid(True)

x, u = hand_modeling2(A, B, N, Q, R, P, x0)
x, u = opt_mpc_with_state_constr(A, B, N, Q, R, P, x0)
x1 = np.array(x[0, :]).flatten()
x2 = np.array(x[1, :]).flatten()
u = np.array(u).flatten()
Expand Down Expand Up @@ -407,7 +439,7 @@ def test5():
plt.legend()
plt.grid(True)

x, u = hand_modeling2(A, B, N, Q, R, P, x0, umax=umax)
x, u = opt_mpc_with_state_constr(A, B, N, Q, R, P, x0, umax=umax)
x1 = np.array(x[0, :]).flatten()
x2 = np.array(x[1, :]).flatten()
u = np.array(u).flatten()
Expand Down Expand Up @@ -458,7 +490,7 @@ def test6():
plt.legend()
plt.grid(True)

x, u = hand_modeling2(A, B, N, Q, R, P, x0, umax=umax, umin=umin, xmin=xmin, xmax=xmax)
x, u = opt_mpc_with_state_constr(A, B, N, Q, R, P, x0, umax=umax, umin=umin, xmin=xmin, xmax=xmax)
x1 = np.array(x[0, :]).flatten()
x2 = np.array(x[1, :]).flatten()
u = np.array(u).flatten()
Expand Down Expand Up @@ -494,8 +526,8 @@ def test_output_check(rx1, rx2, ru, x1, x2, u):
if __name__ == '__main__':
DEBUG_ = True
# test1()
# test2()
test2()
# test3()
# test4()
# test5()
test6()
# test6()
Loading

0 comments on commit 9bfbbc0

Please sign in to comment.