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 _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 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)
