def constraint_condenser(sys, X_N, switching_sequence):
N = len(switching_sequence)
D_sequence = [sys.domains[switching_sequence[i]] for i in range(N)]
lhs_x = linalg.block_diag(*[D.lhs_min[:, :sys.n_x]
for D in D_sequence] + [X_N.lhs_min])
lhs_u = linalg.block_diag(*[D.lhs_min[:, sys.n_x:] for D in D_sequence])
lhs_u = np.vstack((lhs_u, np.zeros(
(X_N.lhs_min.shape[0], lhs_u.shape[1]))))
rhs = np.vstack([D.rhs_min for D in D_sequence] + [X_N.rhs_min])
A_bar, B_bar, c_bar = sys.condense(switching_sequence)
G = (lhs_x.dot(B_bar) + lhs_u)
W = rhs - lhs_x.dot(c_bar)
E = -lhs_x.dot(A_bar)
# # the following might be super slow (and is not necessary)
# n_ineq_before = G.shape[0]
# tic = time.time()
# p = Polytope(np.hstack((G, -E)), W)
# p.assemble()
# if not p.empty:
# G = p.lhs_min[:,:sys.n_u*N]
# E = - p.lhs_min[:,sys.n_u*N:]
# W = p.rhs_min
# n_ineq_after = G.shape[0]
# else:
# G = None
# W = None
# E = None
# print '\n' + str(n_ineq_before - n_ineq_after) + 'on' + str(n_ineq_before) + 'redundant inequalities removed in', str(time.time()-tic),'s,',
return G, W, E
def linear_objective_condenser(sys, Q_bar, R_bar, switching_sequence):
"""
\sum_i | (F_u u + F_x x + F)_i |
"""
A_bar, B_bar, c_bar = sys.condense(switching_sequence)
F_u = np.vstack((Q_bar.dot(B_bar), R_bar))
F_x = np.vstack((Q_bar.dot(A_bar), np.zeros((R_bar.shape[0], A_bar.shape[1]))))
F = np.vstack((Q_bar.dot(c_bar), np.zeros((R_bar.shape[0], 1))))
return F_u, F_x, F
def state_constraint_condenser(sys, X_N, switching_sequence):
N = len(switching_sequence)
X_sequence = [sys.state_domains[switching_sequence[i]] for i in range(N)]
lhs_x = linalg.block_diag(*[X.lhs_min for X in X_sequence] + [X_N.lhs_min])
rhs_x = np.vstack([X.rhs_min for X in X_sequence] + [X_N.rhs_min])
A_bar, B_bar, c_bar = sys.condense(switching_sequence)
G_x = lhs_x.dot(B_bar)
W_x = rhs_x - lhs_x.dot(c_bar)
E_x = - lhs_x.dot(A_bar)
return G_x, W_x, E_x
def quadratic_objective_condenser(sys, Q_bar, R_bar, switching_sequence):
"""
.5 u' F_{uu} u + x0' F_{xu} u + F_u' u + .5 x0' F_{xx} x0 + F_x' x0 + F
"""
A_bar, B_bar, c_bar = sys.condense(switching_sequence)
F_uu = 2 * (R_bar + B_bar.T.dot(Q_bar).dot(B_bar))
F_xu = 2 * A_bar.T.dot(Q_bar).dot(B_bar)
F_xx = 2. * A_bar.T.dot(Q_bar).dot(A_bar)
F_u = 2. * B_bar.T.dot(Q_bar).dot(c_bar)
F_x = 2. * A_bar.T.dot(Q_bar).dot(c_bar)
F = c_bar.T.dot(Q_bar).dot(c_bar)
return F_uu, F_xu, F_xx, F_u, F_x, F