Figure 2.11

Figure 2.11:

The region \mathbb {X}_f, in which the unconstrained LQR control law is feasible for Exercise \ref {exer:Xinf}.

Code for Figure 2.11

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
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 # Set computations for Ex. 2.6, 2.7, and 2.8.
# [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.
xlb = np.array([-5.0, -np.inf])
xub = np.array([5.0,  np.inf])
ulb = np.array([-1.0, -1.0])
uub = np.array([ 1.0,  1.0])

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


def vertices2lines(p, xlims, ylims):
    """
    Returns 2-column array of line segments corresponding to edges of the
    polygon with vertices in columns of p (2 x N, must be in proper order).
    Each line segment has 101 interpolation points, and there is a row of NaNs
    inserted between each line segment.
    """
    dp = np.diff(p, axis=1)
    n_edges = p.shape[1] - 1
    xarr = np.linspace(xlims[0], xlims[1], 101).reshape(-1, 1)
    yarr = np.linspace(ylims[0], ylims[1], 101).reshape(-1, 1)
    blocks = []
    for i in range(n_edges):
        slope = dp[1, i] / dp[0, i]
        if np.isinf(slope) or np.isnan(slope):
            x = p[0, i] * np.ones((101, 1))
            y = yarr
        else:
            x = xarr
            y = slope * (xarr - p[0, i]) + p[1, i]
        blocks.append(np.hstack((x, y)))
        blocks.append(np.array([[np.nan, np.nan]]))
    return np.vstack(blocks)


# Run the computation for a few different cases.
terminals = ['termeq', 'lqr']
Xn = {}
V = {}
Z = {}
for t in terminals:
    Xn[t], V[t], Z[t] = findXn(A, B, K, N, xlb, xub, ulb, uub, t, doplot=False)

# Simulate MPC from the initial starting point.
model = SimpleNamespace(A=A, B=B, N=N)
constraint = SimpleNamespace(**Z['lqr'])
penalty = SimpleNamespace(Q=Q, R=R, P=P)
terminal = Xn['lqr'][0]  # LQR terminal set.

Nsim = 25
xsim = np.zeros((2, Nsim + 1))
xsim[:, 0] = np.array([0.5, 0.5])  # Initial condition.
usim = np.zeros((2, Nsim))
mpcmats = None
for t in range(Nsim):
    model.x0 = xsim[:, t]
    xk, uk, _, status, mpcmats = linearmpc(model, constraint, penalty,
                                           terminal, mpcmats)
    usim[:, t] = uk[:, 0]
    xsim[:, t + 1] = xk[:, 1]

# Now check and see where the finite-horizon control law is the same as the
# infinite-horizon control law.
x1g, x2g = np.meshgrid(np.linspace(-1.86, 1.86, 50), np.linspace(-0.97, 0.97, 50))
x0 = np.vstack([x1g.ravel(), x2g.ravel()])
terminal = Xn['lqr'][0]
X3 = Xn['lqr'][-1]  # Feasible set.
okaypts = np.all(X3['A'] @ x0 <= X3['b'].reshape(-1, 1), axis=0)
x0 = x0[:, okaypts]

Npts = x0.shape[1]
ininterior = np.all(terminal['A'] @ x0 <= terminal['b'].reshape(-1, 1), axis=0)
mpcmats = None
for i in range(Npts):
    if not ininterior[i]:
        model.x0 = x0[:, i]
        xk, uk, _, status, mpcmats = linearmpc(model, constraint, penalty,
                                               terminal, mpcmats)
        if status['optimal']:
            ininterior[i] = np.all(
                terminal['A'] @ xk[:, -1] < terminal['b'].flatten() - 1e-3
            )
x0interior = x0[:, ininterior]

# Save data files.
lims = np.array([-10.0, 10.0])

pa      = V['lqr'][0].T        # X_f vertices for LQR (Nx2)
palines = vertices2lines(V['lqr'][0], lims, lims)   # X_f facets

pb      = V['lqr'][-1].T       # X_N vertices for LQR (Nx2)
pblines = vertices2lines(V['lqr'][-1], lims, lims)  # X_N facets

pc      = V['termeq'][-1].T    # X_N vertices for terminal equality (Nx2)

xtable  = np.column_stack((np.arange(Nsim + 1), xsim.T))   # (Nsim+1) x 3
utable  = np.column_stack((np.arange(Nsim),     usim.T))   # Nsim x 3

pe      = x0interior.T         # points where finite-horizon = inf-horizon (Kx2)

with open('Xinf.dat', 'w') as f:
    f.write('# pa\n')
    np.savetxt(f, pa, fmt='%.6f')
    f.write('\n\n')
    f.write('# palines\n')
    np.savetxt(f, palines, fmt='%.6f')
    f.write('\n\n')
    f.write('# pb\n')
    np.savetxt(f, pb, fmt='%.6f')
    f.write('\n\n')
    f.write('# pblines\n')
    np.savetxt(f, pblines, fmt='%.6f')
    f.write('\n\n')
    f.write('# pc\n')
    np.savetxt(f, pc, fmt='%.6f')
    f.write('\n\n')
    f.write('# xtable\n')
    np.savetxt(f, xtable, fmt='%.6f')
    f.write('\n\n')
    f.write('# utable\n')
    np.savetxt(f, utable, fmt='%.6f')
    f.write('\n\n')
    f.write('# pe\n')
    np.savetxt(f, pe, fmt='%.6f')
    f.write('\n\n')