def moas_closed_loop(A, B, K, X, U):
# closed loop dynamics
A_cl = A + B.dot(K)
# constraints for the maximum output admissible set
lhs_cl = np.vstack((X.lhs_min, U.lhs_min.dot(K)))
rhs_cl = np.vstack((X.rhs_min, U.rhs_min))
X_cl = Polytope(lhs_cl, rhs_cl)
X_cl.assemble()
# compute maximum output admissible set
return moas(A_cl, X_cl)
def moas(A, X):
"""
Returns the maximum output admissible set (see Gilbert, Tan - Linear Systems with State and Control Constraints, The Theory and Application of Maximal Output Admissible Sets) for a non-actuated linear system with state constraints (the output vector is supposed to be the entire state of the system, i.e. y=x and C=I).
INPUTS:
A: state transition matrix
X: constraint polytope X.lhs * x <= X.rhs
OUTPUTS:
moas: maximum output admissible set (instatiated as a polytope)
"""
# ensure that the system is stable (otherwise the algorithm doesn't converge)
eig_max = np.max(np.absolute(np.linalg.eig(A)[0]))
if eig_max > 1:
raise ValueError('Cannot compute MOAS for unstable systems')
# Gilber and Tan algorithm
[n_constraints, n_variables] = X.lhs_min.shape
t = 0
convergence = False
while convergence == False:
# cost function gradients for all i
J = X.lhs_min.dot(np.linalg.matrix_power(A, t + 1))
# constraints to each LP
cons_lhs = np.vstack([
X.lhs_min.dot(np.linalg.matrix_power(A, k))
for k in range(0, t + 1)
])
cons_rhs = np.vstack([X.rhs_min for k in range(0, t + 1)])
# list of all minima
J_sol = []
for i in range(0, n_constraints):
J_sol_i = linear_program(np.reshape(-J[i, :], (n_variables, 1)),
cons_lhs, cons_rhs)[1]
J_sol.append(-J_sol_i - X.rhs_min[i])
# convergence check
if np.max(J_sol) < 0:
convergence = True
else:
t += 1
# define polytope
moas = Polytope(cons_lhs, cons_rhs)
moas.assemble()
return moas
def polytope(self, qp):
"""
Stores a polytope that describes the critical region in the parameter space.
"""
# multipliers explicit solution
[G_A, W_A, S_A] = [qp.G[self.active_set,:], qp.W[self.active_set,:], qp.S[self.active_set,:]]
[G_I, W_I, S_I] = [qp.G[self.inactive_set,:], qp.W[self.inactive_set,:], qp.S[self.inactive_set,:]]
H_A = np.linalg.inv(G_A.dot(qp.H_inv.dot(G_A.T)))
self.lambda_A_offset = - H_A.dot(W_A)
self.lambda_A_linear = - H_A.dot(S_A)
# primal variables explicit solution
self.z_offset = - qp.H_inv.dot(G_A.T.dot(self.lambda_A_offset))
self.z_linear = - qp.H_inv.dot(G_A.T.dot(self.lambda_A_linear))
# primal original variables explicit solution
self.u_offset = self.z_offset
self.u_linear = self.z_linear - np.linalg.inv(qp.H).dot(qp.F.T)
# optimal value function explicit solution
# V = .5*u_feedforward.T.dot(self.qp.H.dot(u_feedforward)) + x0.T.dot(self.qp.F.dot(u_feedforward)) + .5*x0.T.dot(self.qp.Q).dot(x0)
self.V_offset = .5*self.u_offset.T.dot(qp.H).dot(self.u_offset)
self.V_linear = self.u_offset.T.dot(qp.H).dot(self.u_linear) + self.u_offset.T.dot(qp.F.T)
self.V_quadratic = self.u_linear.T.dot(qp.H).dot(self.u_linear) + qp.Q + 2.*qp.F.dot(self.u_linear)
# equation (12) (modified: only inactive indices considered)
lhs_type_1 = G_I.dot(self.z_linear) - S_I
rhs_type_1 = - G_I.dot(self.z_offset) + W_I
# equation (13)
lhs_type_2 = - self.lambda_A_linear
rhs_type_2 = self.lambda_A_offset
# gather facets of type 1 and 2 to define the polytope (note the order: the ith facet of the cr is generated by the ith constraint)
lhs = np.zeros((self.n_constraints, self.n_parameters))
rhs = np.zeros((self.n_constraints, 1))
lhs[self.inactive_set + self.active_set, :] = np.vstack((lhs_type_1, lhs_type_2))
rhs[self.inactive_set + self.active_set] = np.vstack((rhs_type_1, rhs_type_2))
# construct polytope
self.polytope = Polytope(lhs, rhs)
self.polytope.assemble()
return
def backward_reachability_analysis(self, switching_sequence):
if self.X_N is None:
raise ValueError('A terminal constraint is needed for the backward reachability analysis!')
if len(switching_sequence) != self.N:
raise ValueError('Switching sequence not coherent with the controller horizon.')
print('Computing feasible set for the switching sequence ' + str(switching_sequence))
tic = time.time()
feasible_set = self.X_N
A_sequence = [self.sys.affine_systems[switch].A for switch in switching_sequence]
B_sequence = [self.sys.affine_systems[switch].B for switch in switching_sequence]
c_sequence = [self.sys.affine_systems[switch].c for switch in switching_sequence]
U_sequence = [self.sys.input_domains[switch] for switch in switching_sequence]
X_sequence = [self.sys.state_domains[switch] for switch in switching_sequence]
for i in range(self.N-1,-1,-1):
lhs_x = feasible_set.lhs_min.dot(A_sequence[i])
lhs_u = feasible_set.lhs_min.dot(B_sequence[i])
lhs = np.hstack((lhs_x, lhs_u))
rhs = feasible_set.rhs_min - feasible_set.lhs_min.dot(c_sequence[i])
feasible_set = Polytope(lhs, rhs)
lhs = linalg.block_diag(X_sequence[i].lhs_min, U_sequence[i].lhs_min)
rhs = np.vstack((X_sequence[i].rhs_min, U_sequence[i].rhs_min))
feasible_set.add_facets(lhs, rhs)
feasible_set.assemble()
feasible_set = feasible_set.orthogonal_projection(range(self.sys.n_x))
toc = time.time()
print('Feasible set computed in ' + str(toc-tic) + ' s')
return feasible_set
def constraint_condenser(sys, X_N, switching_sequence):
N = len(switching_sequence)
G_u, W_u, E_u = input_constraint_condenser(sys, switching_sequence)
G_x, W_x, E_x = state_constraint_condenser(sys, X_N, switching_sequence)
G = np.vstack((G_u, G_x))
W = np.vstack((W_u, W_x))
E = np.vstack((E_u, E_x))
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
else:
G = None
W = None
E = None
return G, W, E
B_1 = np.array([[1.]]) c_1 = np.array([[0.]]) sys_1 = ds.DTAffineSystem.from_continuous(A_1, B_1, c_1, t_s) A_2 = np.array([[-1.]]) B_2 = np.array([[1.]]) c_2 = np.array([[2.]]) sys_2 = ds.DTAffineSystem.from_continuous(A_2, B_2, c_2, t_s) sys = [sys_0, sys_1, sys_2] # domains x_min_0 = np.array([[-3.]]) x_max_0 = np.array([[-1.]]) X_0 = Polytope.from_bounds(x_min_0, x_max_0) X_0.assemble() x_min_1 = x_max_0 x_max_1 = -x_max_0 X_1 = Polytope.from_bounds(x_min_1, x_max_1) X_1.assemble() x_min_2 = x_max_1 x_max_2 = -x_min_0 X_2 = Polytope.from_bounds(x_min_2, x_max_2) X_2.assemble() u_max = np.array([[10.]]) u_min = -u_max
# numerical parameters
m = 1.
k = 1.
o = np.sqrt(k / m)
x_max = np.array([[1.], [1.]])
x_min = -x_max
for i, h in enumerate([.1, 1., 2., 3.5, 5., 10.]):
plt.figure()
# mode sequence 1
A = np.array([[-1., 0.], [-1., -h]])
b = np.zeros((2, 1))
D1 = Polytope(A, b)
D1.add_bounds(x_min, x_max)
D1.assemble()
D1.plot(facecolor=np.array([1., .5, .5]))
plt.text(D1.center[0], D1.center[1], '{1}')
# mode sequence 2
if h <= np.pi / o:
A = np.array([[1., 0.], [np.cos(o * h), np.sin(o * h) / o]])
b = np.zeros((2, 1))
D2 = Polytope(A, b)
D2.add_bounds(x_min, x_max)
D2.assemble()
D2.plot(facecolor=np.array([.5, 1., .5]))
plt.text(D2.center[0], D2.center[1], '{2}')
B_2 = np.array([[1.]]) c_2 = np.array([[0.]]) sys_2 = ds.DTAffineSystem.from_continuous(A_2, B_2, c_2, t_s) A_3 = np.array([[-1.]]) B_3 = np.array([[1.]]) c_3 = np.array([[2.]]) sys_3 = ds.DTAffineSystem.from_continuous(A_3, B_3, c_3, t_s) sys = [sys_1, sys_2, sys_3] # state domains x_min_1 = np.array([[-3.]]) x_max_1 = np.array([[-1.]]) X_1 = Polytope.from_bounds(x_min_1, x_max_1) X_1.assemble() x_min_2 = x_max_1 x_max_2 = - x_max_1 X_2 = Polytope.from_bounds(x_min_2, x_max_2) X_2.assemble() x_min_3 = x_max_2 x_max_3 = - x_min_1 X_3 = Polytope.from_bounds(x_min_3, x_max_3) X_3.assemble() X = [X_1, X_2, X_3] # inoput domains
# Station Positions and Shifts
quat_AB = np.array([0, 0, 0, 1])
Track_Contact_Dist = 15455
X_Shift = 17079
X_Shift_Site_Determined = 17088
X_Shift_RobotStudio = 17079
Rob_Wrist = np.array([0, 0, 200])
# Communication Parameters
Rob1_Addr = ('192.168.131.1', 8888)
Rob2_Addr = ('192.168.131.2', 8889)
# Setup the Ende Effector Polytope
Flange1 = Polytope([[250, 250, 0], [250, -250, 0], [-250, -250, 0],
[-250, 250, 0], [250, 250, 10], [250, -250, 10],
[-250, -250, 10], [-250, 250, 10]])
Flange2 = Polytope([[250, 250, 0], [250, -250, 0], [-250, -250, 0],
[-250, 250, 0], [250, 250, 10], [250, -250, 10],
[-250, -250, 10], [-250, 250, 10]])
RobA = ("Rob1", Flange1)
RobB = ("Rob2", Flange2)
# Setup the rearside walls
Wall1 = Polytope([[-1500, 5000, 5000], [-1500, 5000, -5000],
[-1500, -5000, -5000], [-1500, -5000, 5000],
[-1510, 5000, 5000], [-1510, 5000, -5000],
[-1510, -5000, -5000], [-1510, -5000, 5000]])
Wall2 = Polytope([[18230, 5000, 5000], [18230, 5000, -5000],
[18230, -5000, -5000], [18230, -5000, 5000],
[18250, 5000, 5000], [18250, 5000, -5000],
class CriticalRegion:
"""
Implements the algorithm from Tondel et al. "An algorithm for multi-parametric quadratic programming and explicit MPC solutions"
VARIABLES:
n_constraints: number of contraints in the qp
n_parameters: number of parameters of the qp
active_set: active set inside the critical region
inactive_set: list of indices of non active contraints inside the critical region
polytope: polytope describing the ceritical region in the parameter space
weakly_active_constraints: list of indices of constraints that are weakly active iside the entire critical region
candidate_active_sets: list of lists of active sets, its ith element collects the set of all the possible
active sets that can be found crossing the ith minimal facet of the polyhedron
z_linear: linear term in the piecewise affine primal solution z_opt = z_linear*x + z_offset
z_offset: offset term in the piecewise affine primal solution z_opt = z_linear*x + z_offset
u_linear: linear term in the piecewise affine primal solution u_opt = u_linear*x + u_offset
u_offset: offset term in the piecewise affine primal solution u_opt = u_linear*x + u_offset
lambda_A_linear: linear term in the piecewise affine dual solution (only active multipliers) lambda_A = lambda_A_linear*x + lambda_A_offset
lambda_A_offset: offset term in the piecewise affine dual solution (only active multipliers) lambda_A = lambda_A_linear*x + lambda_A_offset
"""
def __init__(self, active_set, qp):
# store active set
print 'Computing critical region for the active set ' + str(active_set)
[self.n_constraints, self.n_parameters] = qp.S.shape
self.active_set = active_set
self.inactive_set = sorted(list(set(range(0, self.n_constraints)) - set(active_set)))
# find the polytope
self.polytope(qp)
if self.polytope.empty:
return
# find candidate active sets for the neighboiring regions
minimal_coincident_facets = [self.polytope.coincident_facets[i] for i in self.polytope.minimal_facets]
self.candidate_active_sets = self.candidate_active_sets(active_set, minimal_coincident_facets)
# find weakly active constraints
self.find_weakly_active_constraints()
# expand the candidates if there are weakly active constraints
if self.weakly_active_constraints:
self.candidate_active_set = self.expand_candidate_active_sets(self.candidate_active_set, self.weakly_active_constraints)
return
def polytope(self, qp):
"""
Stores a polytope that describes the critical region in the parameter space.
"""
# multipliers explicit solution
[G_A, W_A, S_A] = [qp.G[self.active_set,:], qp.W[self.active_set,:], qp.S[self.active_set,:]]
[G_I, W_I, S_I] = [qp.G[self.inactive_set,:], qp.W[self.inactive_set,:], qp.S[self.inactive_set,:]]
H_A = np.linalg.inv(G_A.dot(qp.H_inv.dot(G_A.T)))
self.lambda_A_offset = - H_A.dot(W_A)
self.lambda_A_linear = - H_A.dot(S_A)
# primal variables explicit solution
self.z_offset = - qp.H_inv.dot(G_A.T.dot(self.lambda_A_offset))
self.z_linear = - qp.H_inv.dot(G_A.T.dot(self.lambda_A_linear))
# primal original variables explicit solution
self.u_offset = self.z_offset
self.u_linear = self.z_linear - np.linalg.inv(qp.H).dot(qp.F.T)
# optimal value function explicit solution
# V = .5*u_feedforward.T.dot(self.qp.H.dot(u_feedforward)) + x0.T.dot(self.qp.F.dot(u_feedforward)) + .5*x0.T.dot(self.qp.Q).dot(x0)
self.V_offset = .5*self.u_offset.T.dot(qp.H).dot(self.u_offset)
self.V_linear = self.u_offset.T.dot(qp.H).dot(self.u_linear) + self.u_offset.T.dot(qp.F.T)
self.V_quadratic = self.u_linear.T.dot(qp.H).dot(self.u_linear) + qp.Q + 2.*qp.F.dot(self.u_linear)
# equation (12) (modified: only inactive indices considered)
lhs_type_1 = G_I.dot(self.z_linear) - S_I
rhs_type_1 = - G_I.dot(self.z_offset) + W_I
# equation (13)
lhs_type_2 = - self.lambda_A_linear
rhs_type_2 = self.lambda_A_offset
# gather facets of type 1 and 2 to define the polytope (note the order: the ith facet of the cr is generated by the ith constraint)
lhs = np.zeros((self.n_constraints, self.n_parameters))
rhs = np.zeros((self.n_constraints, 1))
lhs[self.inactive_set + self.active_set, :] = np.vstack((lhs_type_1, lhs_type_2))
rhs[self.inactive_set + self.active_set] = np.vstack((rhs_type_1, rhs_type_2))
# construct polytope
self.polytope = Polytope(lhs, rhs)
self.polytope.assemble()
return
def find_weakly_active_constraints(self, toll=1e-8):
"""
Stores the list of constraints that are weakly active in the whole critical region
enumerated in the as in the equation G z <= W + S x ("original enumeration")
(by convention weakly active constraints are included among the active set,
so that only constraints of type 2 are anlyzed)
"""
# equation (13), again...
lhs_type_2 = - self.lambda_A_linear
rhs_type_2 = self.lambda_A_offset
# weakly active constraints are included in the active set
self.weakly_active_constraints = []
for i in range(0, len(self.active_set)):
# to be weakly active in the whole region they can only be in the form 0^T x <= 0
if np.linalg.norm(lhs_type_2[i,:]) + np.absolute(rhs_type_2[i,:]) < toll:
print('Weakly active constraint detected!')
self.weakly_active_constraints.append(self.active_set[i])
return
@staticmethod
def candidate_active_sets(active_set, minimal_coincident_facets):
"""
Computes one candidate active set for each non-redundant facet of a critical region
(Theorem 2 and Corollary 1).
INPUTS:
active_set: active set of the parent critical region
minimal_coincident_facets: list of facets coincident to the minimal facets
(i.e.: [coincident_facets[i] for i in minimal_facets])
OUTPUTS:
candidate_active_sets: list of the candidate active sets for each minimal facet
"""
# initialize list of condidate active sets
candidate_active_sets = []
# cross each non-redundant facet of the parent CR
for coincident_facets in minimal_coincident_facets:
# add or remove each constraint crossed to the active set of the parent CR
candidate_active_set = set(active_set).symmetric_difference(set(coincident_facets))
candidate_active_sets.append([sorted(list(candidate_active_set))])
return candidate_active_sets
@staticmethod
def expand_candidate_active_sets(candidate_active_sets, weakly_active_constraints):
"""
Expands the candidate active sets if there are some weakly active contraints (Theorem 5).
INPUTS:
candidate_active_sets: list of the candidate active sets for each minimal facet
weakly_active_constraints: list of weakly active constraints (in the "original enumeration")
OUTPUTS:
candidate_active_sets: list of the candidate active sets for each minimal facet
"""
# determine every possible combination of the weakly active contraints
wac_combinations = []
for n in range(1, len(weakly_active_constraints)+1):
wac_combinations_n = itertools.combinations(weakly_active_constraints, n)
wac_combinations += [list(c) for c in wac_combinations_n]
# for each minimal facet of the CR add or remove each combination of wakly active constraints
for i in range(0, len(candidate_active_sets)):
active_set = candidate_active_sets[i][0]
for combination in wac_combinations:
further_active_set = set(active_set).symmetric_difference(combination)
candidate_active_sets[i].append(sorted(list(further_active_set)))
return candidate_active_sets
def z_optimal(self, x):
"""
Returns the explicit solution of the mpQP as a function of the parameter.
INPUTS:
x: value of the parameter
OUTPUTS:
z_optimal: solution of the QP
"""
z_optimal = self.z_offset + self.z_linear.dot(x).reshape(self.z_offset.shape)
return z_optimal
def lambda_optimal(self, x):
"""
Returns the explicit value of the multipliers of the mpQP as a function of the parameter.
INPUTS:
x: value of the parameter
OUTPUTS:
lambda_optimal: optimal multipliers
"""
lambda_A_optimal = self.lambda_A_offset + self.lambda_A_linear.dot(x)
lambda_optimal = np.zeros(len(self.active_set + self.inactive_set))
for i in range(0, len(self.active_set)):
lambda_optimal[self.active_set[i]] = lambda_A_optimal[i]
return lambda_optimal
def applies_to(self, x):
"""
Determines is a given point belongs to the critical region.
INPUTS:
x: value of the parameter
OUTPUTS:
is_inside: flag (True if x is in the CR, False otherwise)
"""
# check if x is inside the polytope
is_inside = np.max(self.polytope.lhs_min.dot(x) - self.polytope.rhs_min) <= 0
return is_inside
def feasible_set(self):
if self._feasible_set is None:
augmented_polytope = Polytope(np.hstack((- self.C_x, self.C_u)), self.C)
augmented_polytope.assemble()
self._feasible_set = augmented_polytope.orthogonal_projection(range(self.C_x.shape[1]))
return self._feasible_set