def test_initialization(self):
# 1 or 2d right hand sides
A = np.eye(3)
C = np.eye(2)
b = np.ones(3)
b_1d = np.ones(3)
d = np.ones(2)
d_1d = np.ones(2)
p = Polyhedron(A, b, C, d)
p_1d = Polyhedron(A, b_1d, C, d_1d)
np.testing.assert_array_equal(p.b, p_1d.b)
np.testing.assert_array_equal(p.d, p_1d.d)
# wrong initializations
self.assertRaises(ValueError, Polyhedron, A, b, C)
self.assertRaises(ValueError, Polyhedron, A, d)
self.assertRaises(ValueError, Polyhedron, A, b, C, b)
A = np.eye(3)
b = np.ones((3,1))
C = np.eye(2)
d = np.ones((2,1))
self.assertRaises(ValueError, Polyhedron, A, b)
b = np.ones(3)
self.assertRaises(ValueError, Polyhedron, A, b, C, d)
def test_vertices(self):
# basic eample
A = np.array([[-1, 0.],[0., -1.],[2., 1.],[-0.5, 1.]])
b = np.array([0.,0.,4.,2.])
p = Polyhedron(A,b)
u_list = [
np.array([0.,0.]),
np.array([2.,0.]),
np.array([0.,2.]),
np.array([.8,2.4])
]
self.assertTrue(same_vectors(p.vertices, u_list))
# 1d example
A = np.array([[-1],[1.]])
b = np.array([1.,1.])
p = Polyhedron(A,b)
u_list = [
np.array([-1.]),
np.array([1.])
]
self.assertTrue(same_vectors(p.vertices, u_list))
# unbounded
x_min = np.zeros(2)
p = Polyhedron.from_lower_bound(x_min)
self.assertTrue(p.vertices is None)
# lower dimensional (because of the inequalities)
x_max = np.array([1.,0.])
p.add_upper_bound(x_max)
self.assertTrue(p.vertices is None)
# empty
x_max = - np.ones(2)
p.add_upper_bound(x_max)
self.assertTrue(p.vertices is None)
# 3d case
x_min = - np.ones(3)
x_max = - x_min
p = Polyhedron.from_bounds(x_min, x_max)
u_list = [np.array(v) for v in product([1., -1.], repeat=3)]
self.assertTrue(same_vectors(p.vertices, u_list))
# 3d case with equalities
x_min = np.array([-1.,-2.])
x_max = - x_min
p = Polyhedron.from_bounds(x_min, x_max, [0,1], 3)
C = np.array([[1., 0., -1.]])
d = np.zeros(1)
p.add_equality(C, d)
u_list = [
np.array([1.,2.,1.]),
np.array([-1.,2.,-1.]),
np.array([1.,-2.,1.]),
np.array([-1.,-2.,-1.])
]
self.assertTrue(same_vectors(p.vertices, u_list))
def test_add_functions(self):
# add inequalities
A = np.eye(2)
b = np.ones(2)
p = Polyhedron(A, b)
A = np.ones((1, 2))
b = np.ones(1)
p.add_inequality(A, b)
A = np.ones((1, 1))
b = 3. * np.ones(1)
p.add_inequality(A, b, [1])
A = np.array([[1., 0.], [0., 1.], [1., 1.], [0., 1.]])
b = np.array([1., 1., 1., 3.])
self.assertTrue(
same_rows(np.column_stack((A, b)), np.column_stack((p.A, p.b))))
c = np.ones(2)
self.assertRaises(ValueError, p.add_inequality, A, c)
# add equalities
A = np.eye(2)
b = np.ones(2)
p.add_equality(A, b)
A = 3. * np.ones((1, 1))
b = np.ones(1)
p.add_equality(A, b, [0])
A = np.array([[1., 0.], [0., 1.], [3., 0.]])
b = np.array([[1.], [1.], [1.]])
self.assertTrue(
same_rows(np.column_stack((A, b)), np.column_stack((p.C, p.d))))
b = np.ones(2)
self.assertRaises(ValueError, p.add_equality, A, b)
# add lower bounds
A = np.zeros((0, 2))
b = np.zeros(0)
p = Polyhedron(A, b)
p.add_lower_bound(-np.ones(2))
p.add_upper_bound(2 * np.ones(2))
p.add_bounds(-3 * np.ones(2), 3 * np.ones(2))
p.add_lower_bound(-4 * np.ones(1), [0])
p.add_upper_bound(5 * np.ones(1), [0])
p.add_bounds(-6 * np.ones(1), 6 * np.ones(1), [1])
A = np.array([[-1., 0.], [0., -1.], [1., 0.], [0., 1.], [-1., 0.],
[0., -1.], [1., 0.], [0., 1.], [-1., 0.], [1., 0.],
[0., -1.], [0., 1.]])
b = np.array([1., 1., 2., 2., 3., 3., 3., 3., 4., 5., 6., 6.])
self.assertTrue(
same_rows(np.column_stack((A, b)), np.column_stack((p.A, p.b))))
# wrong size bounds
A = np.eye(3)
b = np.ones(3)
p = Polyhedron(A, b)
x = np.zeros(1)
indices = [0, 1]
self.assertRaises(ValueError, p.add_lower_bound, x, indices)
self.assertRaises(ValueError, p.add_upper_bound, x, indices)
self.assertRaises(ValueError, p.add_bounds, x, x, indices)
def test_convex_hull_method(self):
np.random.seed(3)
# cube from n-dimensions to n-1-dimensions
for n in range(2, 6):
x_min = - np.ones(n)
x_max = - x_min
p = Polyhedron.from_bounds(x_min, x_max)
E = np.vstack((
np.eye(n-1),
- np.eye(n-1)
))
f = np.ones(2*(n-1))
vertices = [np.array(v) for v in product([1., -1.], repeat=n-1)]
E_chm, f_chm, vertices_chm = convex_hull_method(p.A, p.b, range(n-1))
p_chm = Polyhedron(E_chm, f_chm)
p_chm.remove_redundant_inequalities()
self.assertTrue(same_rows(
np.column_stack((E, f)),
np.column_stack((p_chm.A, p_chm.b))
))
self.assertTrue(same_vectors(vertices, vertices_chm))
# test with random polytopes wiith m facets
m = 10
# dimension of the higher dimensional polytope
for n in range(3, 6):
# dimension of the lower dimensional polytope
for n_proj in range(2, n):
# higher dimensional polytope
A = np.random.randn(m, n)
b = np.random.rand(m)
p = Polyhedron(A, b)
# if not empty or unbounded
if p.vertices is not None:
points = [v[:n_proj] for v in p.vertices]
p = Polyhedron.from_convex_hull(points)
# check half spaces
p.remove_redundant_inequalities()
E_chm, f_chm, vertices_chm = convex_hull_method(A, b, range(n_proj))
p_chm = Polyhedron(E_chm, f_chm)
p_chm.remove_redundant_inequalities()
self.assertTrue(same_rows(
np.column_stack((p.A, p.b)),
np.column_stack((p_chm.A, p_chm.b))
))
# check vertices
self.assertTrue(same_vectors(p.vertices, vertices_chm))
def test_intialization(self):
np.random.seed(1)
# different number of systems and domains
A = np.ones((3, 3))
B = np.ones((3, 2))
c = np.ones(3)
S = AffineSystem(A, B, c)
affine_systems = [S] * 5
F = np.ones((9, 5))
g = np.ones(9)
D = Polyhedron(F, g)
domains = [D] * 4
self.assertRaises(ValueError, PieceWiseAffineSystem, affine_systems,
domains)
# incopatible number of states in affine systems
domains += [D, D]
A = np.ones((2, 2))
B = np.ones((2, 2))
c = np.ones(2)
affine_systems.append(AffineSystem(A, B, c))
self.assertRaises(ValueError, PieceWiseAffineSystem, affine_systems,
domains)
# incopatible number of inputs in affine systems
del affine_systems[-1]
A = np.ones((3, 3))
B = np.ones((3, 1))
c = np.ones(3)
affine_systems.append(AffineSystem(A, B, c))
self.assertRaises(ValueError, PieceWiseAffineSystem, affine_systems,
domains)
# different dimensinality of the domains and the systems
del affine_systems[-1]
affine_systems += [S, S]
F = np.ones((9, 4))
g = np.ones(9)
domains.append(Polyhedron(F, g))
self.assertRaises(ValueError, PieceWiseAffineSystem, affine_systems,
domains)
# different dimensinality of the domains and the systems
F = np.ones((9, 4))
g = np.ones(9)
D = Polyhedron(F, g)
domains = [D] * 7
self.assertRaises(ValueError, PieceWiseAffineSystem, affine_systems,
domains)
def test_normalize(self):
# construct polyhedron
A = np.array([[0.,2.],[0.,-3.]])
b = np.array([2.,0.])
C = np.ones((1, 2))
d = np.ones(1)
p = Polyhedron(A, b, C, d)
# normalize
p.normalize()
A = np.array([[0., 1.], [0., -1.]])
b = np.array([1., 0.])
C = np.ones((1, 2))/np.sqrt(2.)
d = np.ones(1)/np.sqrt(2.)
self.assertTrue(same_rows(
np.column_stack((A, b)),
np.column_stack((p.A, p.b)),
normalize=False
))
self.assertTrue(same_rows(
np.column_stack((C, d)),
np.column_stack((p.C, p.d)),
normalize=False
))
def mcais(self, K, D, **kwargs):
"""
Returns the maximal constraint-admissible invariant set O_inf for the closed-loop system X(t+1) = (A + B K) x(t).
It holds that x(0) in O_inf <=> (x(t), u(t) = K x(t)) in D for all t >= 0.
Arguments
----------
K : numpy.ndarray
Stabilizing feedback gain for the linear system.
D : instance of Polyhedron
Constraint set in the state and input space.
Returns
----------
O_inf : instance of Polyhedron
Maximal constraint-admissible (positive) ivariant.
t : int
Determinedness index.
"""
# closed loop dynamics
A_cl = self.A + self.B.dot(K)
# state-space constraint set
X_cl = Polyhedron(D.A[:, :self.nx] + D.A[:, self.nx:].dot(K), D.b)
O_inf = mcais(A_cl, X_cl, **kwargs)
return O_inf
def _voronoi_1d(points):
"""
Given a list of 1-dimensional points, returns the Voronoi partition of the space as a list of Polyhedron (pympc class).
Arguments
----------
points : list of numpy.ndarray
Points for the Voronoi partition.
Returns
----------
partition : list of Polyhedron
Halfspace representation of all the cells of the partiotion.
"""
# order point from smallest to biggest
points = sorted(points)
# loop from the smaller point
polyhedra = []
for i, point in enumerate(points):
# h-rep of the cell for the point i
A = []
b = []
# get previous and next point (if any)
tips = []
if i > 0:
tips.append(points[i-1])
if i < len(points)-1:
tips.append(points[i+1])
# vector from point i to the next/previous point
for tip in tips:
bottom = point
center = bottom + (tip - bottom) / 2.
# hyperplane that separates point i and its neighbor
Ai = tip - point
bi = Ai.dot(center)
A.append(Ai)
b.append(bi)
# assemble cell i and add to partition
polyhedron = Polyhedron(np.vstack(A), np.array(b))
polyhedron.normalize()
polyhedra.append(polyhedron)
return polyhedra
def _voronoi_nd(points):
"""
Given a list of n-dimensional points, returns the Voronoi partition of the space as a list of Polyhedron (pympc class).
Uses the scipy wrapper of Voronoi from Qhull.
Does not work in case of 1-dimensional points.
Arguments
----------
points : list of numpy.ndarray
Points for the Voronoi partition.
Returns
----------
partition : list of Polyhedron
Halfspace representation of all the cells of the partiotion.
"""
# loop over points
partition = []
for i, point in enumerate(points):
# h-rep of the cell for the point i
A = []
b = []
# loop over the points that share a facet with point i
for ridge in Voronoi(points).ridge_points:
if i in ridge:
# vector from point i to the neighbor
bottom = point
tip = points[ridge[1 - ridge.tolist().index(i)]]
# hyperplane that separates point i and its neighbor
Ai = tip - point
center = bottom + (tip - bottom) / 2.
bi = Ai.dot(center)
A.append(Ai)
b.append(bi)
# assemble cell i and add to partition
cell = Polyhedron(np.vstack(A), np.array(b))
cell.normalize()
partition.append(cell)
return partition
def get_feasible_set(self):
"""
Returns the feasible set of the mqQP, i.e. {x | exists u: Au u + Ax x <= b}.
Returns
----------
instance of Polyhedron
Feasible set.
"""
# constraint set
C = Polyhedron(
np.hstack((self.A['x'], self.A['u'])),
self.b
)
# feasible set
return C.project_to(range(self.A['x'].shape[1]))
def graph_representation(S):
'''
For the PWA system S
x+ = Ai x + Bi u + ci if Fi x + Gi u <= hi,
returns the graphs of the dynamics (list of Polyhedron)
[ Fi Gi 0] [ x] [ hi]
[ Ai Bi -I] [ u] <= [-ci]
[-Ai -Bi I] [x+] [ ci]
'''
P = []
for i in range(S.nm):
Di = S.domains[i]
Si = S.affine_systems[i]
Ai = np.vstack((
np.hstack((Di.A, np.zeros((Di.A.shape[0], S.nx)))),
np.hstack((Si.A, Si.B, -np.eye(S.nx))),
np.hstack((-Si.A, -Si.B, np.eye(S.nx))),
))
bi = np.concatenate((Di.b, -Si.c, Si.c))
P.append(Polyhedron(Ai, bi))
return P
def constrained_voronoi(points, X=None):
"""
Given a list of n-dimensional points, returns the Voronoi partition of the Polyhedron X as a list of Polyhedron.
If X is None, returns the partition of the whole space.
Arguments
----------
points : list of numpy.ndarray
Points for the Voronoi partition.
X : Polyhedron
Set we want to partition.
Returns
----------
partition : list of Polyhedron
Halfspace representation of all the cells of the partiotion.
"""
# get indices of non-coincident coordinates
nx = min(p.size for p in points)
assert nx == max(p.size for p in points)
indices = [i for i in range(nx) if not np.isclose(min([p[i] for p in points]), max([p[i] for p in points]))]
# get voronoi partition without boundaries
points_lower_dimensional = [p[indices] for p in points]
if len(indices) == 1:
vor = _voronoi_1d(points_lower_dimensional)
else:
vor = _voronoi_nd(points_lower_dimensional)
# go back to the higher dimensional space
partition = [Polyhedron(np.zeros((0,nx)), np.zeros(0)) for i in points]
for i, cell in enumerate(vor):
partition[i].add_inequality(cell.A, cell.b, indices=indices)
# intersect with X is provided
if X is not None:
partition[i].add_inequality(X.A, X.b)
return partition
def test_remove_redundant_inequalities(self):
# minimal facets only inequalities
A = np.array([[1., 1.], [-1., 1.], [0., -1.], [0., 1.], [0., 1.],
[2., 2.]])
b = np.array([[1.], [1.], [1.], [1.], [2.], [2.]])
p = Polyhedron(A, b)
mf = set(p.minimal_facets())
self.assertTrue(mf == set([1, 2, 0]) or mf == set([1, 2, 5]))
# add nasty redundant inequality
A = np.zeros((1, 2))
b = np.ones((1, 1))
p.add_inequality(A, b)
mf = set(p.minimal_facets())
self.assertTrue(mf == set([1, 2, 0]) or mf == set([1, 2, 5]))
# remove redundant facets
p.remove_redundant_inequalities()
A_min = np.array([[-1., 1.], [0., -1.], [1., 1.]])
b_min = np.array([[1.], [1.], [1.]])
self.assertTrue(
same_rows(np.hstack((A_min, b_min)), np.hstack((p.A, p.b))))
# both inequalities and equalities
x_min = -np.ones((2, 1))
x_max = -x_min
p = Polyhedron.from_bounds(x_min, x_max)
C = np.ones((1, 2))
d = np.ones((1, 1))
p.add_equality(C, d)
self.assertEqual([2, 3], sorted(p.minimal_facets()))
p.remove_redundant_inequalities()
A_min = np.array([[1., 0.], [0., 1.]])
b_min = np.array([[1.], [1.]])
self.assertTrue(
same_rows(np.hstack((A_min, b_min)), np.hstack((p.A, p.b))))
# add (redundant) inequality coincident with the equality
p.add_inequality(C, d)
p.remove_redundant_inequalities()
self.assertTrue(
same_rows(np.hstack((A_min, b_min)), np.hstack((p.A, p.b))))
# add (redundant) inequality
p.add_inequality(np.array([[-1., 1.]]), np.array([[1.1]]))
p.remove_redundant_inequalities()
self.assertTrue(
same_rows(np.hstack((A_min, b_min)), np.hstack((p.A, p.b))))
# add unfeasible equality (raises: 'empty polyhedron, cannot remove redundant inequalities.')
C = np.ones((1, 2))
d = -np.ones((1, 1))
p.add_equality(C, d)
self.assertRaises(ValueError, p.remove_redundant_inequalities)
# empty polyhderon (raises: 'empty polyhedron, cannot remove redundant inequalities.')
x_min = np.ones((2, 1))
x_max = np.zeros((2, 1))
p = Polyhedron.from_bounds(x_min, x_max)
self.assertRaises(ValueError, p.remove_redundant_inequalities)
def test_add_functions(self):
# add inequalities
A = np.eye(2)
b = np.ones(2)
p = Polyhedron(A, b)
A = np.ones((1, 2))
b = np.ones(1)
p.add_inequality(A, b)
A = np.ones((1, 1))
b = 3.*np.ones(1)
p.add_inequality(A, b, [1])
A = np.array([[1., 0.], [0., 1.], [1., 1.], [0., 1.]])
b = np.array([1.,1.,1.,3.])
self.assertTrue(same_rows(
np.column_stack((A, b)),
np.column_stack((p.A, p.b))
))
c = np.ones(2)
self.assertRaises(ValueError, p.add_inequality, A, c)
# add equalities
A = np.eye(2)
b = np.ones(2)
p.add_equality(A, b)
A = 3.*np.ones((1, 1))
b = np.ones(1)
p.add_equality(A, b, [0])
A = np.array([[1., 0.], [0., 1.], [3., 0.]])
b = np.array([[1.], [1.], [1.]])
self.assertTrue(same_rows(
np.column_stack((A, b)),
np.column_stack((p.C, p.d))
))
b = np.ones(2)
self.assertRaises(ValueError, p.add_equality, A, b)
# add lower bounds
A = np.zeros((0, 2))
b = np.zeros(0)
p = Polyhedron(A, b)
p.add_lower_bound(-np.ones(2))
p.add_upper_bound(2*np.ones(2))
p.add_bounds(-3*np.ones(2), 3*np.ones(2))
p.add_lower_bound(-4*np.ones(1), [0])
p.add_upper_bound(5*np.ones(1), [0])
p.add_bounds(-6*np.ones(1), 6*np.ones(1), [1])
A =np.array([[-1., 0.], [0., -1.], [1., 0.], [0., 1.], [-1., 0.], [0., -1.], [1., 0.], [0., 1.], [-1., 0.], [1., 0.], [0., -1.], [0., 1.] ])
b = np.array([1.,1.,2.,2.,3.,3.,3.,3.,4.,5.,6.,6.])
self.assertTrue(same_rows(
np.column_stack((A, b)),
np.column_stack((p.A, p.b))
))
# wrong size bounds
A = np.eye(3)
b = np.ones(3)
p = Polyhedron(A, b)
x = np.zeros(1)
indices = [0, 1]
self.assertRaises(ValueError, p.add_lower_bound, x, indices)
self.assertRaises(ValueError, p.add_upper_bound, x, indices)
self.assertRaises(ValueError, p.add_bounds, x, x, indices)
# add symbolic
n = 5
m = 3
A = np.random.rand(m, n)
b = np.random.rand(m)
C = np.random.rand(m, n)
d = np.random.rand(m)
P = Polyhedron(A, b)
x = sp.Matrix([sp.Symbol('x'+str(i), real=True) for i in range(n)])
ineq = sp.Matrix(A)*x - sp.Matrix(b)
P.add_symbolic_inequality(x, ineq)
np.testing.assert_array_almost_equal(P.A, np.vstack((A, A)))
np.testing.assert_array_almost_equal(P.b, np.concatenate((b, b)))
eq = sp.Matrix(C)*x - sp.Matrix(d)
P.add_symbolic_equality(x, eq)
np.testing.assert_array_almost_equal(P.C, C)
np.testing.assert_array_almost_equal(P.d, d)
# throw some errors here
P = Polyhedron(A, b)
x = sp.Matrix([sp.Symbol('x'+str(i), real=True) for i in range(n+1)])
A1 = np.random.rand(m, n+1)
ineq = sp.Matrix(A1)*x - sp.Matrix(b)
self.assertRaises(ValueError, P.add_symbolic_inequality, x, ineq)
P = Polyhedron(A, b, C, d)
C1 = np.random.rand(m, n+1)
eq = sp.Matrix(C1)*x - sp.Matrix(b)
self.assertRaises(ValueError, P.add_symbolic_equality, x, eq)
def explicit_solve_given_active_set(self, active_set):
"""
Returns the explicit solution of the mpQP for a given active set.
The solution turns out to be an affine function of x, i.e. u(x) = ux x + u0, p(x) = px x + p0, where p are the Lagrange multipliers for the inequality constraints.
Math
----------
Given an active set A and a set of multipliers p, the KKT conditions for the mpQP are a set of linear equations that can be solved for u(x) and p(x)
Huu u + Hux + fu + Aua' pa = 0, (stationarity of the Lagrangian),
Aua u + Axa x = ba, (primal feasibility),
pi = 0, (dual feasibility),
where the subscripts a and i dentote active and inactive inequalities respectively.
The inactive (primal and dual) constraints define the region of space where the given active set is optimal
Aui u + Axi x < bi, (primal feasibility),
pa > 0, (dual feasibility).
Arguments
----------
active_set : list of int
Indices of the active inequalities.
Reuturns
----------
instance of CriticalRegion
Critical region for the given active set.
"""
# ensure that LICQ will hold
Aua = self.A['u'][active_set]
if len(active_set) > 0 and np.linalg.matrix_rank(Aua) < Aua.shape[0]:
return None
# split active and inactive
inactive_set = [i for i in range(self.A['x'].shape[0]) if i not in active_set]
Aui = self.A['u'][inactive_set]
Axa = self.A['x'][active_set]
Axi = self.A['x'][inactive_set]
ba = self.b[active_set]
bi = self.b[inactive_set]
# multipliers
M = np.linalg.inv(Aua.dot(self.Huu_inv).dot(Aua.T))
pax = M.dot(Axa - Aua.dot(self.Huu_inv).dot(self.H['ux']))
pa0 = - M.dot(ba + Aua.dot(self.Huu_inv).dot(self.f['u']))
px = np.zeros(self.A['x'].shape)
p0 = np.zeros(self.A['x'].shape[0])
px[active_set] = pax
p0[active_set] = pa0
p = {'x': px, '0':p0}
# primary variables
ux = - self.Huu_inv.dot(self.H['ux'] + Aua.T.dot(pax))
u0 = - self.Huu_inv.dot(self.f['u'] + Aua.T.dot(pa0))
u = {'x':ux, '0':u0}
# critical region
Acr = np.vstack((
- pax,
Aui.dot(ux) + Axi
))
bcr = np.concatenate((
pa0,
bi - Aui.dot(u0)
))
cr = Polyhedron(Acr, bcr)
cr.normalize()
# optimal value function V(x) = 1/2 x' Vxx x + Vx' x + V0
Vxx = ux.T.dot(self.H['uu']).dot(ux) + 2.*self.H['ux'].T.dot(ux) + self.H['xx']
Vx = (ux.T.dot(self.H['uu'].T) + self.H['ux'].T).dot(u0) + ux.T.dot(self.f['u']) + self.f['x']
V0 = .5*u0.dot(self.H['uu']).dot(u0) + self.f['u'].dot(u0) + self.g
V = {'xx':Vxx, 'x':Vx, '0':V0}
return CriticalRegion(active_set, u, p, V, cr)