def con2vert(A, b): """ Convert sets of constraints to a list of vertices (of the feasible region). If the shape is open, con2vert returns False for the closed property. """ # Python implementation of con2vert.m by Michael Kleder (July 2005), # available: http://www.mathworks.com/matlabcentral/fileexchange/7894 # -con2vert-constraints-to-vertices # Author: Michael Kelder (Original) # Andre Campher (Python implementation) c = linalg.lstsq(mat(A), mat(b))[0] btmp = mat(b)-mat(A)*c D = mat(A)/matlib.repmat(btmp, 1, A.shape[1]) fmatv = qhull(D, "Ft") #vertices on facets G = zeros((fmatv.shape[0], D.shape[1])) for ix in range(0, fmatv.shape[0]): F = D[fmatv[ix, :], :].squeeze() G[ix, :] = linalg.lstsq(F, ones((F.shape[0], 1)))[0].transpose() V = G + matlib.repmat(c.transpose(), G.shape[0], 1) ux = uniqm(V) eps = 1e-13 Av = dot(A, ux.T) bv = tile(b, (1, ux.shape[0])) closed = sciall(Av - bv <= eps) return ux, closed
def remredcons(A, b, verts): """Reduce a constraint set by removing unnecessary constraints.""" eps = 10e-9 #1 Co-planar constraints; # Remove as not to affect 3rd check Ab = c_[A, b] Abnorms = ones((Ab.shape[0], 1)) for i in range(Ab.shape[0]): Abnorms[i] = linalg.norm(Ab[i, :]) Abn = Ab/Abnorms Abkeep = ones((0, Ab.shape[1])) Abtest = ones((0, Ab.shape[1])) for r1 in range(Abn.shape[0]): noocc = ones((1, 0)) for r2 in range(Abn.shape[0]): #print abs(Abn[r1, :] - Abn[r2, :]) if numpy.all(abs(Abn[r1, :] - Abn[r2, :]) < eps): noocc = c_[noocc, r2] if noocc.size == 1: Abtest = vstack([Abtest, Ab[r1, :]]) else: Abkeep = vstack([Abkeep, Ab[r1, :]]) if Abkeep.shape[0] > 1: Abkeep = uniqm(Abkeep, eps) #2 Vert subset satisfying; no action needed (redundancy uncertain) #3 All vert satisfying constraints; A, b = splitAb(array(Abtest).ravel(), verts.shape[1]) keepA = ones((0, A.shape[1])) keepb = ones((0, 1)) bt = tile(b, (1, verts.shape[0])) k = mat(A)*mat(verts.T) - bt kk = sum(k > eps, axis=1) for i in range(len(kk)): if kk[i] != 0: keepA = vstack([keepA, A[i, :]]) keepb = vstack([keepb, b[i, :]]) outAb = vstack([c_[keepA, keepb], Abkeep]) return splitAb(outAb.ravel(), verts.shape[1])