def random_state(self, X=None, controller=None):
"""
Sample a random state within the lower and upper bounds of the piecewise
affine system, and further restrict that state to some polytope X. By
default, X is the polytope defined by the robot's kinematic limits.
If `controller` is not None, use the given controller to check if there
is a feasible input sequence from the given sample before returning it
"""
if X is None:
A = self.kinematic_limits.polytope.A
b = self.kinematic_limits.polytope.b
x_eq, u_eq = self.equilibrium_point()
X = Polytope(A, b - A.dot(x_eq)).assemble()
while True:
x = np.random.rand(self.pwa_system.n_x, 1)
x = np.multiply(x, (self.pwa_system.x_max - self.pwa_system.x_min)) + self.pwa_system.x_min
if X.applies_to(x):
if controller is not None:
u, xtraj, ss, cost = controller.feedforward(x)
if np.any(np.isnan(u[0])):
continue
return x
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(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))
# optimal value function explicit solution: V_star = .5 x' V_quadratic x + V_linear x + V_offset
self.V_quadratic = self.z_linear.T.dot(qp.H).dot(
self.z_linear) + qp.F_xx_q
self.V_linear = self.z_offset.T.dot(qp.H).dot(
self.z_linear) + qp.F_x_q.T
self.V_offset = qp.F_q + .5 * self.z_offset.T.dot(qp.H).dot(
self.z_offset)
# primal original variables explicit solution
self.u_offset = self.z_offset - qp.H_inv.dot(qp.F_u)
self.u_linear = self.z_linear - qp.H_inv.dot(qp.F_xu.T)
# 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 = self.polytope.applies_to(x)
return is_inside