Figure 2.12:

The region of attraction for terminal constraint x(N) \in \mathbb {X}_f and terminal penalty V_f(x)=(1/2)x' \Pi x and the estimate of \bar {\mathcal {X}}_N for Exercise \ref {exer:NoTerminal}.

Code for Figure 2.12

Text of the GNU GPL.

linearmpc.py

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
# [Xk, Uk, phi, status, matrices] = linearmpc(model, constraint, penalty, terminal, [matrices])
#
# Solves the following optimization problem:
#
#      N-1
# min  sum [ 1/2(x_k' Q x_k + u_k' R u_k ) ] + 1/2(x_N' P x_n)
# u_k  k=1
#
#    s.t    x_k+1 = A x_k + B u_k
#           G x_k + H u_k <= psi
#           A_f x_N <= b_f
#
# Linear time-invariant MPC.
#
# Input 'model' has the following fields:
# - A: A matrix for system.
# - B: B matrix for system.
# - N: horizon length in number of stemps.
# - x0: initial state of system.
#
# Input 'constraint' has the following fields:
# - G: G matrix for constraints.
# - H: G matrix for constraints.
# - psi: psi vector for constraints.
# Either all or none must be specified.
#
# Input 'penalty' has the following fields:
# - Q: Penalty matricies for state.
# - R: Penalty matrix for input.
# - P: Terminal penalty matrix.
#
# Input terminal can be a vector of length x to specify a terminal equality
# constraint, or a struct with fields A and b (corresponding to A_f and b_f in
# the optimization given above).
#
# The final output matrices is a struct with fields H, f, Alt, blt, Aeq, beq,
# lb, and ub that define the QP formulation. If these are supplied in a struct
# as a fifth argument, only the initial condition model.x0 is updated, and the
# matrices are passed directly to the QP solver. This can save a lot of time if
# you're solving the same problem over and over but with different initial
# conditions. Note that minimal error checking is done
# Check arguments.
import numpy as np
from scipy import sparse
from scipy.optimize import minimize
import warnings

def linearmpc(model, constraint, penalty, terminal, matrices=None):
    """
    Solves linear time-invariant MPC optimization problem.

    Args:
        model: dict with fields A, B, N, x0
        constraint: dict with fields G, H, psi
        penalty: dict with fields Q, R, P
        terminal: vector or dict with fields A, b
        matrices: optional precomputed matrices

    Returns:
        Xk: optimal state trajectory
        Uk: optimal input trajectory
        phi: optimal cost
        status: solver status
        matrices: QP matrices
    """

    # Check arguments
    if not (4 <= len([model, constraint, penalty, terminal, matrices]) <= 5):
        raise ValueError("Wrong number of arguments")

    # Build matrices if not supplied
    if matrices is None:
        matrices = buildmatrices(model, constraint, penalty, terminal)

    # Get sizes
    numx = model.B.shape[0]
    numu = model.B.shape[1]
    N = model.N
    numVar = N*(numx + numu) + numx

    # Update initial condition
    matrices['lb'][0:numx] = model.x0
    matrices['ub'][0:numx] = model.x0

    # Get vectors to extract x and u
    allinds = np.mod(np.arange(numVar), numx + numu)
    xinds = allinds < numx
    uinds = allinds >= numx

    # Setup QP problem
    H = 0.5*(matrices['H'] + matrices['H'].T) # Make exactly symmetric

    def objective(x):
        return 0.5*x.T @ H @ x + matrices['f'].T @ x

    def constraint_eq(x):
        return matrices['Aeq'] @ x - matrices['beq']

    def constraint_ineq(x):
        return matrices['blt'] - matrices['Alt'] @ x

    bounds = list(zip(matrices['lb'], matrices['ub']))

    # Initial guess
    x0 = np.zeros(numVar)

    # Solve QP
    with warnings.catch_warnings():
        warnings.simplefilter("ignore")
        result = minimize(objective, x0,
                        method='SLSQP',
                        bounds=bounds,
                        constraints=[{'type': 'eq', 'fun': constraint_eq},
                                   {'type': 'ineq', 'fun': constraint_ineq}],
                        options={'disp': False})

    # Process results
    status = {'solver': 'SLSQP', 'flag': result.status}
    status['optimal'] = result.success

    if status['optimal']:
        X = result.x
        phi = result.fun
    else:
        X = np.nan * np.ones(numVar)
        phi = np.nan

    # Extract x and u trajectories
    Xk = X[xinds].reshape((numx, N+1), order='F')
    Uk = X[uinds].reshape((numu, N), order='F')

    return Xk, Uk, phi, status, matrices

def buildmatrices(model, constraint, penalty, terminal):
    """Helper function to build QP matrices"""

    N = model.N
    A = model.A

    if A.shape[0] != A.shape[1]:
        raise ValueError('model.A must be square')

    numx = A.shape[0]
    B = model.B
    numu = B.shape[1]

    if hasattr(constraint, 'G'):
        G = constraint.G
        H = constraint.H
        psi = constraint.psi
    else:
        G = np.zeros((0, numx))
        H = np.zeros((0, numu))
        psi = np.zeros((0, 1))

    Q = penalty.Q
    R = penalty.R
    P = penalty.P
    littleH = sparse.block_diag([sparse.csr_matrix(Q), sparse.csr_matrix(R)])
    bigH = sparse.kron(sparse.eye(N), littleH)
    bigH = sparse.block_diag([bigH, sparse.csr_matrix(P)])

    bigf = np.zeros(bigH.shape[0])

    littleAeq1 = sparse.hstack([sparse.csr_matrix(A), sparse.csr_matrix(B)])
    littleAeq2 = sparse.hstack([-sparse.eye(A.shape[0]), sparse.csr_matrix(B.shape)])

    bigAeq = (sparse.kron(sparse.eye(N+1), littleAeq1) +
              sparse.kron(sparse.diags(np.ones(N), 1), littleAeq2)).tocsr()

    bigAeq = bigAeq[:-numx, :-numu]

    bigbeq = np.zeros(numx*N)

    littleAlt1 = sparse.hstack([sparse.csr_matrix(G), sparse.csr_matrix(H)])
    littleAlt2 = sparse.hstack([sparse.csr_matrix(G.shape), sparse.csr_matrix(G.shape)])

    bigAlt = (sparse.kron(sparse.eye(N+1), littleAlt1) +
              sparse.kron(sparse.diags(np.ones(N), 1), littleAlt2)).tocsr()

    bigAlt = bigAlt[:-littleAlt1.shape[0], :-numu]

    bigblt = np.tile(psi, N)

    numVar = len(bigf)
    LB = -np.inf * np.ones(numVar)
    UB = np.inf * np.ones(numVar)

    if isinstance(terminal, dict) and all(k in terminal for k in ['A','b']):
        Af = terminal['A']
        bf = terminal['b']
        bigAlt = sparse.vstack([bigAlt,
                              sparse.hstack([sparse.csr_matrix((Af.shape[0], numVar-numx)),
                                           sparse.csr_matrix(Af)])])
        bigblt = np.concatenate([bigblt, np.asarray(bf).flatten()])
    elif isinstance(terminal, np.ndarray) and len(terminal) == numx:
        LB[-numx:] = terminal
        UB[-numx:] = terminal
    elif terminal is None:
        pass
    else:
        raise ValueError('Invalid terminal constraint')

    matrices = {'H': bigH, 'f': bigf, 'Aeq': bigAeq, 'beq': bigbeq,
                'Alt': bigAlt, 'blt': bigblt, 'lb': LB, 'ub': UB}

    return matrices

main.py

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
# Exercise "Terminal penalty with and without terminal constraint"
# [depends] functions/linearmpc.py

import sys
import numpy as np
from control import dlqr
from types import SimpleNamespace

sys.path.append('functions/')
from findXn import findXn
from linearmpc import linearmpc

# Define model, cost function, and bounds.
A = np.array([[2.0, 1.0], [0.0, 2.0]])
B = np.array([[1.0, 0.0], [0.0, 1.0]])
N = 3

alpha = 1e-5
Q = alpha * np.eye(2)
R = np.eye(2)

# Bounds on variables.
xlb = np.array([-15.0, -np.inf])
xub = np.array([ 15.0,  np.inf])
ulb = np.array([-5.0, -5.0])
uub = np.array([ 5.0,  5.0])

# Find LQR.
K, P, _ = dlqr(A, B, Q, R)
K = -K  # Sign convention.

# Also calculate the control law with increased penalty.
def riciter(A, B, Q, R, P):
    P = Q + A.T @ P @ A - A.T @ P @ B @ np.linalg.solve(B.T @ P @ B + R, B.T @ P @ A)
    K = -np.linalg.solve(B.T @ P @ B + R, B.T @ P @ A)
    return P, K

P3 = 3 * P
for i in range(N):
    P3, K3 = riciter(A, B, Q, R, P3)

# Calculate Xn and Xf (maximum LQR-invariant set) using normal penalty.
Xn, V, Z = findXn(A, B, K, N, xlb, xub, ulb, uub, 'lqr', doplot=False)

# Also calculate Xf using increased penalty (N=0 gives just Xf).
Xn3, V3, _ = findXn(A, B, K3, 0, xlb, xub, ulb, uub, 'lqr', doplot=False)

# Set up MPC model/constraint/penalty objects.
model = SimpleNamespace(A=A, B=B, N=N)
constraint = SimpleNamespace(**Z)
penalty = SimpleNamespace(Q=Q, R=R, P=P)
terminal1 = Xn[0]   # Terminal set with normal penalty.
terminal3 = Xn3[0]  # Terminal set with increased penalty.

# Remove terminal constraint and check if MPC is still stabilizing.
x1g, x2g = np.meshgrid(np.linspace(-10, 10, 25), np.linspace(-5, 5, 25))
x0 = np.vstack([x1g.ravel(), x2g.ravel()])
Npts = x0.shape[1]
Nstab = 10  # Steps to try before declaring stabilizing.

Ps = [P, P3]
names = ['one', 'three']
Xf_list = [terminal1, terminal3]
x0stabilizing = {}

terminal = None  # Remove terminal constraint.
for n in range(len(names)):
    stab = np.zeros(Npts, dtype=bool)
    penalty.P = Ps[n]
    mpcmats = None  # Rebuild matrices for new penalty.
    for i in range(Npts):
        model.x0 = x0[:, i].copy()
        for k in range(Nstab + 1):
            # If inside Xf, trajectory is stabilized.
            if np.all(Xf_list[n]['A'] @ model.x0 <= Xf_list[n]['b'].flatten()):
                stab[i] = True
                break
            if k == Nstab:
                break
            xk, uk, _, status, mpcmats = linearmpc(model, constraint, penalty,
                                                    terminal, mpcmats)
            if not status['optimal']:
                break
            model.x0 = xk[:, 1]
    x0stabilizing[names[n]] = x0[:, stab]

# Check where MPC gives the infinite-horizon control law.
x1g, x2g = np.meshgrid(np.linspace(-10, 10, 50), np.linspace(-5, 5, 50))
x0 = np.vstack([x1g.ravel(), x2g.ravel()])
X3 = Xn[-1]  # Feasible set.
x0 = x0[:, np.all(X3['A'] @ x0 <= X3['b'].reshape(-1, 1), axis=0)]
Npts = x0.shape[1]

infhorizon = np.zeros(Npts, dtype=bool)
terminal = terminal1  # Use original terminal set.
penalty.P = P
mpcmats = None
for i in range(Npts):
    model.x0 = x0[:, i]
    xk, uk, _, status, mpcmats = linearmpc(model, constraint, penalty,
                                           terminal, mpcmats)
    if status['optimal']:
        infhorizon[i] = np.all(
            terminal['A'] @ xk[:, -1] < terminal['b'].flatten() - 1e-5
        )
x0infhorizon = x0[:, infhorizon]

# Save data files.
pa = V[0].T            # Xf vertices (Mx2)
pb = V[-1].T           # X_N vertices (Mx2)
pe = x0infhorizon.T    # inf-horizon control law points (Kx2)
pg = x0stabilizing['one'].T    # stabilizing with P (Kx2)
pf = x0stabilizing['three'].T  # stabilizing with 3P (Kx2)

with open('NoTerminal.dat', 'w') as f:
    for label, data in [('pa', pa), ('pb', pb), ('pe', pe), ('pg', pg), ('pf', pf)]:
        f.write(f'# {label}\n')
        np.savetxt(f, data, fmt='%.6f')
        f.write('\n\n')