def check_boundedness(self, tol=1.e-9):
"""
Checks if the polyhedron is bounded: a polyhedron is unbounded (i.e. a polytope) iff there exists an x != 0 in the recession cone (A*x <= 0). We also have that { exists x != 0 | A*x <= 0 } <=> { exists z < 0 | A^T*z = 0 }. The second condition is tested through a LP.
"""
self.bounded = True
# if the Chebyshev radius is infinite
if np.isinf(self.radius):
print('Infinite Chebyshev center or radius!')
self.bounded = False
return
# if ker(A) != 0
rank_A = np.linalg.matrix_rank(self.A, tol)
dim_null_A = self.A.shape[1] - rank_A
if dim_null_A > 0:
self.bounded = False
return
# else solve lp
f = np.zeros((self.A.shape[0],1))
A = np.eye(self.A.shape[0])
b = - np.ones((self.A.shape[0],1))
C = self.A.T
d = np.zeros((self.A.shape[1],1))
sol = linear_program(f, A, b, C, d)
if any(np.isnan(sol.argmin)):
self.bounded = False
print 'Boundedness test failed!'
return
def chebyshev_center(A, b, C=None, d=None, tol=1.e-10):
"""
Finds the Chebyshev center of the polytope P := {x | A x <= b, C x = d} solving the linear program
min e
s.t. F z <= g + F_{row_norm} e
where if an equality is not provided F=A, z=x, g=b; whereas if equalities are present F=AZ, g=b-AYy, with: Z basis of the nullspace of C, Y orthogonal complement to Z, y=(CY)^-1d and x is retrived as x=Zz+Yy. Here F_{row_norm} dentes the vector whose ith entry is the 2-norm of the ith row of F.
INPUTS:
A: left-hand side of the inequalities
b: right-hand side of the inequalities
C: left-hand side of the equalities
d: right-hand side of the equalities
OUTPUTS:
center: Chebyshev center of the polytope (nan if the P is empty, inf if P is unbounded and the center is at infinity)
radius: Chebyshev radius of the polytope (nan if the P is empty, inf if it is infinite)
"""
A_projected = A
b_projected = b
if C is not None and d is not None:
# project in the null space of the equalities
Z = nullspace_basis(C)
Y = nullspace_basis(Z.T)
A_projected = A.dot(Z)
CY_inv = np.linalg.inv(C.dot(Y))
y = CY_inv.dot(d)
y = np.reshape(y, (y.shape[0], 1))
b_projected = b - A.dot(Y.dot(y))
[n_facets, n_variables] = A_projected.shape
# check if the problem is trivially unbounded
A_row_norm = np.linalg.norm(A, axis=1)
A_zero_rows = np.where(A_row_norm < tol)[0]
if any(b[A_zero_rows] < 0.):
radius = np.nan
center = np.zeros((n_variables, 1))
center[:] = np.nan
return [center, radius]
f_lp = np.zeros((n_variables + 1, 1))
f_lp[-1] = 1.
A_row_norm = np.reshape(np.linalg.norm(A_projected, axis=1), (n_facets, 1))
A_lp = np.hstack((A_projected, -A_row_norm))
sol = linear_program(f_lp, A_lp, b_projected)
center = sol.argmin
radius = sol.min
center = center[0:-1]
radius = -radius
if C is not None and d is not None:
# go back to the original coordinates
center = np.hstack((Z, Y)).dot(np.vstack((center, y)))
if np.isnan(radius):
radius = np.inf
if any(np.isnan(center)):
center[:] = np.inf
if radius < tol:
radius = np.nan
center[:] = np.nan
return [center, radius]
def first_two_points(A, b, n_proj):
v_proj = []
a = np.zeros((A.shape[1], 1))
a[0, 0] = 1.
for a in [a, -a]:
sol = linear_program(a, A, b)
v_proj.append(sol.argmin[:n_proj, :])
return v_proj
def x_max(self):
if self._x_max is None:
self._x_max = []
for i in range(self.n_variables):
f = np.zeros((self.n_variables, 1))
f[i,:] += 1.
sol = linear_program(-f, self.lhs_min, self.rhs_min)
self._x_max.append([-sol.min])
self._x_max = np.array(self._x_max)
return self._x_max
def include_point(self, point, tol=1e-7):
if self.hull is None:
self._initialize(point)
# dimension of the projection space
n_proj = len(self.resiudal_dimensions)
# violation of the approximation boundaires
residuals = [(hs[0].T.dot(point) - hs[1])[0, 0]
for hs in self.hull.halfspaces] # my version
# residuals = (self.hull.equations[:,:-1].dot(point) + self.hull.equations[:,-1:]).flatten().tolist() # qhull version
# # for the plot on the paper
# import copy
# from pympc.geometry.polytope import Polytope
# p_inner_plot = Polytope(self.hull.A, self.hull.b)
# p_inner_plot.assemble()
# p_list = [p_inner_plot]
# expand the most violated boundary until inclusion
while max(residuals) > tol:
facet_to_expand = residuals.index(max(residuals))
a = np.zeros((self.A.shape[1], 1))
hs = self.hull.halfspaces[facet_to_expand] # my version
# hs = [self.hull.equations[facet_to_expand:facet_to_expand+1,:-1].T, - self.hull.equations[facet_to_expand:facet_to_expand+1,-1:]] # qhull version
a[:n_proj, :] = hs[0]
sol = linear_program(-a, self.A_switched, self.b)
# the point might be outside the projection...
inflation = -sol.min - hs[1][0, 0]
if inflation < tol:
raise ValueError(
'The given point lies outside the projection.')
break
# add vertex to the hull
self.hull.add_point(sol.argmin[:n_proj, :]) # my version
# self.hull.add_points(sol.argmin[:n_proj,:].T) # qhull version
# new residuals
residuals = [(hs[0].T.dot(point) - hs[1])[0, 0]
for hs in self.hull.halfspaces] # my version
# residuals = (self.hull.equations[:,:-1].dot(point) + self.hull.equations[:,-1:]).flatten().tolist() # qhull version
# # for the plot on the paper
# p_inner_plot = Polytope(self.hull.A, self.hull.b)
# p_inner_plot.assemble()
# p_list.append(p_inner_plot)
# return p_list
return
def included_in(self, p, tol=1.e-6):
"""
Checks if the polytope is a subset of the polytope p (returns True or False).
"""
inclusion = True
for i in range(p.lhs_min.shape[0]):
sol = linear_program(-p.lhs_min[i,:], self.lhs_min, self.rhs_min)
penetration = - sol.min - p.rhs_min[i]
if penetration > tol:
inclusion = False
break
return inclusion
def intersect_with(self, p):
"""
Checks if the polytope intersect the other polytope p (returns True or False).
"""
intersection = True
A = np.vstack((self.lhs_min, p.lhs_min))
b = np.vstack((self.rhs_min, p.rhs_min))
f = np.zeros((A.shape[1],1))
sol = linear_program(f, A, b)
if any(np.isnan(sol.argmin)):
intersection = False
return intersection
def is_inside_a_domain(self, x):
is_inside = False
for D in self.domains:
A_x = D.lhs_min[:, :self.n_x]
A_u = D.lhs_min[:, self.n_x:]
b_u = D.rhs_min - A_x.dot(x)
cost = np.zeros((self.n_u, 1))
sol = linear_program(cost, A_u, b_u)
if not np.isnan(sol.min):
is_inside = True
break
return is_inside
def inner_simplex(A, b, v_proj, x=None, tol=1.e-7):
n_proj = v_proj[0].shape[0]
for i in range(2, n_proj + 1):
a, d = plane_through_points([v[:i, :] for v in v_proj])
# pick the right sign for a
sign = 1.
if x is not None:
sign = np.sign((a.T.dot(x[:i, :]) - d)[0, 0])
a = np.vstack((a, np.zeros((A.shape[1] - i, 1))))
sol = linear_program(-sign * a, A, b)
if -sol.min - sign * d[0, 0] < tol:
#if np.linalg.norm(a.T.dot(sol.argmin) + d) < tol:
#sol = linear_program(-a, A, b)
#if -sol.min < d[0,0] + tol:
if x is not None:
print 'This is not supposed to happen!'
a = -a
sol = linear_program(-a, A, b)
v_proj.append(sol.argmin[:n_proj, :])
return v_proj
def expand_simplex(A, b, hull, tol=1.e-7):
n_proj = hull.points[0].shape[0]
tested_directions = []
convergence = False
residual = np.nan
while not convergence:
convergence = True
for hs in hull.halfspaces:
if all((hs[0] != a).all() for a in tested_directions):
tested_directions.append(hs[0])
a = np.zeros((A.shape[1], 1))
a[:n_proj, :] = hs[0]
sol = linear_program(-a, A, b)
if -sol.min - hs[1][0, 0] > tol:
residual = '%e' % (-sol.min - hs[1][0, 0])
convergence = False
hull.add_point(sol.argmin[:n_proj, :])
break
print('Facets found so far ' + str(hull.A.shape[0]) +
', vertices found so far ' + str(len(hull.points)) +
', length of the last inflation ' + str(residual) + '.\r'),
print('\n'),
return hull
def find_minimal_facets(self, tol=1e-9):
"""
Finds the non-redundant facets and derives a minimal representation of the polyhedron solving a LP for each facet. See "Fukuda - Frequently asked questions in polyhedral computation" Sec.2.21.
"""
# list of non-redundant facets
self.minimal_facets = range(self.n_facets)
for i in range(self.n_facets):
# remove redundant constraints
A_reduced = self.A[self.minimal_facets,:]
# relax the ith constraint
b_relaxation = np.zeros(np.shape(self.b))
b_relaxation[i] += 1.
b_relaxed = (self.b + b_relaxation)[self.minimal_facets];
# check redundancy
sol = linear_program(-self.A[i,:], A_reduced, b_relaxed)
cost_i = - sol.min
# remove redundant facets from the list
if cost_i - self.b[i] < tol or np.isnan(cost_i):
self.minimal_facets.remove(i)
# list of non redundant facets
self.lhs_min = self.A[self.minimal_facets,:]
self.rhs_min = self.b[self.minimal_facets]
return