def get_input_helper(x0,
ssys,
P1,
P3,
N,
R,
r,
Q,
ord=1,
closed_loop=True,
solver=None):
"""Calculate the sequence u_seq such that:
- x(t+1) = A x(t) + B u(t) + K
- x(k) \in P1 for k = 0,...N
- x(N) \in P3
- [u(k); x(k)] \in PU
and minimize:
|Rx|_{ord} + |Qu|_{ord} + r'x +
mid_weight * |xc - x(N)|_{ord}
"""
n = ssys.A.shape[1]
m = ssys.B.shape[1]
list_P = []
if closed_loop:
temp_part = P3
list_P.append(P3)
for i in range(N - 1, 0, -1):
temp_part = solve_feasible(P1,
temp_part,
ssys,
N=1,
closed_loop=False,
trans_set=P1)
list_P.insert(0, temp_part)
list_P.insert(0, P1)
L, M = createLM(ssys, N, list_P, disturbance_ind=[1])
else:
list_P.append(P1)
for i in range(N - 1, 0, -1):
list_P.append(P1)
list_P.append(P3)
L, M = createLM(ssys, N, list_P)
# Remove first constraint on x(0)
L = L[range(list_P[0].A.shape[0], L.shape[0]), :]
M = M[range(list_P[0].A.shape[0], M.shape[0]), :]
# Separate L matrix
Lx = L[:, range(n)]
Lu = L[:, range(n, L.shape[1])]
M = M - Lx.dot(x0).reshape(Lx.shape[0], 1)
B_diag = ssys.B
for i in range(N - 1):
B_diag = _block_diag2(B_diag, ssys.B)
K_hat = np.tile(ssys.K, (N, 1))
A_it = ssys.A.copy()
A_row = np.zeros([n, n * N])
A_K = np.zeros([n * N, n * N])
A_N = np.zeros([n * N, n])
for i in range(N):
A_row = ssys.A.dot(A_row)
A_row[np.ix_(range(n), range(i * n, (i + 1) * n))] = np.eye(n)
A_N[np.ix_(range(i * n, (i + 1) * n), range(n))] = A_it
A_K[np.ix_(range(i * n, (i + 1) * n), range(A_K.shape[1]))] = A_row
A_it = ssys.A.dot(A_it)
Ct = A_K.dot(B_diag)
if ord == 1:
# f(\epsilon,u) = sum(\epsilon)
c_LP = np.hstack((np.ones((1, N * (n + m))), r.T.dot(Ct)))
# Constraints -\epsilon_r < R*x <= \epsilon_r
# Constraints -\epsilon_u < Q*u <= \epsilon_u
# x = A_N*x0 + Ct*u --> ignore the first constant part
# x = Ct*u
G_LP = np.vstack(
(np.hstack((-np.eye(N * n), np.zeros((N * n, N * m)), -R.dot(Ct))),
np.hstack((-np.eye(N * n), np.zeros((N * n, N * m)), R.dot(Ct))),
np.hstack((np.zeros((N * m, N * n)), -np.eye(N * m), -Q)),
np.hstack((np.zeros((N * m, N * n)), -np.eye(N * m), Q)),
np.hstack((np.zeros((Lu.shape[0], N * n + N * m)), Lu))))
h_LP = np.vstack((np.zeros((2 * N * (n + m), 1)), M))
elif ord == 2:
assert_cvxopt()
# symmetrize
Q2 = Q.T.dot(Q)
R2 = R.T.dot(R)
# constraints
G = matrix(Lu)
h = matrix(M)
P = matrix(Q2 + Ct.T.dot(R2).dot(Ct))
q = matrix(
np.dot(
np.dot(x0.reshape(1, x0.size), A_N.T) +
A_K.dot(K_hat).T, R2.dot(Ct)) + 0.5 * r.T.dot(Ct)).T
if solver != None:
raise Exception(
"_get_input_helper: ",
"solver specified but only 'None' allowed for ord = 2")
sol = solvers.qp(P, q, G, h)
if sol['status'] != "optimal":
raise _InputHelperQPException("getInputHelper: "
"QP solver finished with status " +
str(sol['status']))
u = np.array(sol['x']).flatten()
cost = sol['primal objective']
return u.reshape(N, m), cost
elif ord == np.inf:
c_LP = np.hstack((np.ones((1, 2)), r.T.dot(Ct)))
G_LP = np.vstack(
(np.hstack((-np.ones((N * n, 1)), np.zeros(
(N * n, 1)), -R.dot(Ct))),
np.hstack((-np.ones((N * n, 1)), np.zeros(
(N * n, 1)), R.dot(Ct))),
np.hstack((np.zeros((N * m, 1)), -np.ones((N * m, 1)), -Q)),
np.hstack((np.zeros((N * m, 1)), -np.ones(
(N * m, 1)), Q)), np.hstack((np.zeros(
(Lu.shape[0], 2)), Lu))))
h_LP = np.vstack((np.zeros((2 * N * (n + m), 1)), M))
sol = pc.polytope.lpsolve(c_LP.flatten(), G_LP, h_LP, solver=solver)
if sol['status'] != 0:
raise _InputHelperLPException("getInputHelper: "
"LP solver finished with error code " +
str(sol['status']))
var = np.array(sol['x']).flatten()
u = var[-N * m:]
cost = sol['fun']
return u.reshape(N, m), cost
def get_input_helper(
x0, ssys, P1, P3, N, R, r, Q, ord=1,
closed_loop=True
):
"""Calculate the sequence u_seq such that:
- x(t+1) = A x(t) + B u(t) + K
- x(k) \in P1 for k = 0,...N
- x(N) \in P3
- [u(k); x(k)] \in PU
and minimize:
|Rx|_{ord} + |Qu|_{ord} + r'x +
mid_weight * |xc - x(N)|_{ord}
"""
n = ssys.A.shape[1]
m = ssys.B.shape[1]
list_P = []
if closed_loop:
temp_part = P3
list_P.append(P3)
for i in range(N - 1, 0, -1):
temp_part = solve_feasible(
P1, temp_part, ssys, N=1,
closed_loop=False, trans_set=P1
)
list_P.insert(0, temp_part)
list_P.insert(0, P1)
L, M = createLM(ssys, N, list_P, disturbance_ind=[1])
else:
list_P.append(P1)
for i in range(N - 1, 0, -1):
list_P.append(P1)
list_P.append(P3)
L, M = createLM(ssys, N, list_P)
# Remove first constraint on x(0)
L = L[range(list_P[0].A.shape[0], L.shape[0]), :]
M = M[range(list_P[0].A.shape[0], M.shape[0]), :]
# Separate L matrix
Lx = L[:, range(n)]
Lu = L[:, range(n, L.shape[1])]
M = M - Lx.dot(x0).reshape(Lx.shape[0], 1)
B_diag = ssys.B
for i in range(N - 1):
B_diag = _block_diag2(B_diag, ssys.B)
K_hat = np.tile(ssys.K, (N, 1))
A_it = ssys.A.copy()
A_row = np.zeros([n, n * N])
A_K = np.zeros([n * N, n * N])
A_N = np.zeros([n * N, n])
for i in range(N):
A_row = ssys.A.dot(A_row)
A_row[np.ix_(
range(n),
range(i * n, (i + 1) * n)
)] = np.eye(n)
A_N[np.ix_(
range(i * n, (i + 1) * n),
range(n)
)] = A_it
A_K[np.ix_(
range(i * n, (i + 1) * n),
range(A_K.shape[1])
)] = A_row
A_it = ssys.A.dot(A_it)
Ct = A_K.dot(B_diag)
if ord == 1:
# f(\epsilon,u) = sum(\epsilon)
c_LP = np.hstack((np.ones((1, N * (n + m))), r.T.dot(Ct)))
# Constraints -\epsilon_r < R*x <= \epsilon_r
# Constraints -\epsilon_u < Q*u <= \epsilon_u
# x = A_N*x0 + Ct*u --> ignore the first constant part
# x = Ct*u
G_LP = np.vstack((
np.hstack((- np.eye(N * n),
np.zeros((N * n, N * m)),
- R.dot(Ct))),
np.hstack((- np.eye(N * n),
np.zeros((N * n, N * m)),
R.dot(Ct))),
np.hstack((np.zeros((N * m, N * n)),
- np.eye(N * m), -Q)),
np.hstack((np.zeros((N * m, N * n)),
- np.eye(N * m), Q)),
np.hstack((np.zeros((Lu.shape[0], N * n + N * m)), Lu))
))
h_LP = np.vstack((np.zeros((2 * N * (n + m), 1)), M))
elif ord == 2:
assert_cvxopt()
# symmetrize
Q2 = Q.T.dot(Q)
R2 = R.T.dot(R)
# constraints
G = matrix(Lu)
h = matrix(M)
P = matrix(Q2 + Ct.T.dot(R2).dot(Ct))
q = matrix(
np.dot(
np.dot(x0.reshape(1, x0.size), A_N.T) +
A_K.dot(K_hat).T,
R2.dot(Ct)
) +
0.5 * r.T.dot(Ct)
).T
sol = solvers.qp(P, q, G, h)
if sol['status'] != "optimal":
raise Exception(
"getInputHelper: "
"QP solver finished with status " +
str(sol['status']))
u = np.array(sol['x']).flatten()
cost = sol['primal objective']
return u.reshape(N, m), cost
elif ord == np.inf:
c_LP = np.hstack((np.ones((1, 2)), r.T.dot(Ct)))
G_LP = np.vstack((
np.hstack((-np.ones((N * n, 1)),
np.zeros((N * n, 1)),
-R.dot(Ct))),
np.hstack((-np.ones((N * n, 1)),
np.zeros((N * n, 1)),
R.dot(Ct))),
np.hstack((np.zeros((N * m, 1)),
-np.ones((N * m, 1)), -Q)),
np.hstack((np.zeros((N * m, 1)),
-np.ones((N * m, 1)), Q)),
np.hstack((np.zeros((Lu.shape[0], 2)), Lu))
))
h_LP = np.vstack((np.zeros((2 * N * (n + m), 1)), M))
sol = pc.polytope.lpsolve(c_LP.flatten(), G_LP, h_LP)
if sol['status'] != 0:
raise Exception(
"getInputHelper: "
"LP solver finished with message " +
str(sol['message']))
var = np.array(sol['x']).flatten()
u = var[-N * m:]
cost = sol['fun']
return u.reshape(N, m), cost