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)