def cl1m_socp(A, m, n, b, e): c = matrix(0.0, (3 * n, 1)) c[range(0, 3 * n, 3)] = 1.0 Ga = [] ha = [] for i in range(n): G = matrix(0.0, (3, 3 * n)) G[0, 3 * i] = G[1, 3 * i + 1] = G[2, 3 * i + 2] = -1.0 Ga.append(G) ha.append(matrix(0.0, (3, 1))) sol = None Aa = matrix(0.0, (2 * m, 3 * n)) Aa[0:m, range(1, 3 * n, 3)] = A.real() Aa[0:m, range(2, 3 * n, 3)] = -A.imag() Aa[m : 2 * m, range(1, 3 * n, 3)] = A.imag() Aa[m : 2 * m, range(2, 3 * n, 3)] = A.real() y = matrix(0.0, (2 * m, 1)) y[0:m] = b.real() y[m : 2 * m] = b.imag() if e == 0: sol = solvers.socp(c, Gq=Ga, hq=ha, A=Aa, b=y) else: G = matrix(0.0, (2 * m + 1, 3 * n)) G[1 : 2 * m + 1, :] = Aa Ga.append(G) h = matrix(0.0, (2 * m + 1, 1)) h[0] = e h[1 : 2 * m + 1] = y ha.append(h) sol = solvers.socp(c, Gq=Ga, hq=ha) s = sol["x"] z = matrix(complex(0, 0), (n, 1)) for i in range(n): z[i] = complex(s[3 * i + 1], s[3 * i + 2]) return z
def testsocp(opts): c = matrix([-2., 1., 5.]) G = [matrix( [[12., 13., 12.], [ 6., -3., -12.], [-5., -5., 6.]] ) ] G += [matrix( [[ 3., 3., -1., 1.], [-6., -6., -9., 19.], [10., -2., -2., -3.]] ) ] h = [ matrix( [-12., -3., -2.] ), matrix( [27., 0., 3., -42.] ) ] solvers.options.update(opts) sol = solvers.socp(c, Gq = G, hq = h) print "x = \n", helpers.str2(sol['x'], "%.9f") print "zq[0] = \n", helpers.str2(sol['zq'][0], "%.9f") print "zq[1] = \n", helpers.str2(sol['zq'][1], "%.9f") print "\n *** running GO test ***" helpers.run_go_test("../testsocp", {'x': sol['x'], 'sq0': sol['sq'][0], 'sq1': sol['sq'][1], 'zq0': sol['zq'][0], 'zq1': sol['zq'][1]})
def test_socp(self): from cvxopt import matrix, msk, solvers c = matrix([-2., 1., 5.]) G = [ matrix( [[12., 13., 12.], [6., -3., -12.], [-5., -5., 6.]] ) ] G += [ matrix( [[3., 3., -1., 1.], [-6., -6., -9., 19.], [10., -2., -2., -3.]] ) ] h = [ matrix( [-12., -3., -2.] ), matrix( [27., 0., 3., -42.] ) ] self.assertAlmostEqualLists(list(solvers.socp(c, Gq = G, hq = h)['x']),list(msk.socp(c, Gq = G, hq = h)[1]))
def quality(frames): M = np.zeros((frames, n)) A = np.zeros((frames, n, n)) for i in range(frames): #preallocate space for matrix of frames p = peak(x) + peak(x - 1) + np.random.normal(0, noise, n) shift = np.random.randint(-16, 16) M[i, :] = np.roll( np.repeat(np.dot((np.roll(C, shift, axis=0))[::srf, :], p), srf), -shift) A[i, :, :] = np.roll(np.repeat((np.roll(C, shift, axis=0))[::srf, :], srf, axis=0), -shift, axis=0) #input for the solver D = sp.linalg.circulant(np.append(np.array([1., -1.]), np.zeros(n - 2))).T I = np.eye(n) G0 = matrix( np.append(np.append(D, -I, axis=1), np.append(-D, -I, axis=1), axis=0)) epsilon = frames * noise h1 = [matrix(np.append(np.array([epsilon]), -M.reshape(frames * n)))] G1 = [ matrix(-np.append(np.append( [np.zeros(n)], A.reshape((frames * n, n)), axis=0), np.zeros((frames * n + 1, n)), axis=1)) ] c = matrix(np.ones(2 * n)) sol = solvers.socp(c, Gl=G0, hl=matrix(np.zeros(2 * n)), Gq=G1, hq=h1) reconstruction = sol['x'] return 1 - np.sum( (np.squeeze(np.array(reconstruction[0:n])) - puresignal)**2) / np.sum( np.array(reconstruction[0:n])**2)
def Robust_opt_risktarget(mean, cov, sigma, eta, risk_level): N = len(mean) kappa = np.sqrt(chi2.ppf(eta, df=N)) X = np.linalg.cholesky(cov) Y = np.linalg.cholesky(sigma) c = matrix(np.append(-mean, [kappa])) A = matrix(np.array([[1.0 for i in range(N)] + [0.0]]), tc='d') b = matrix(np.array([1.0]), tc='d') Gl = matrix(np.diag([-1.0 for i in range(N + 1)]), tc='d') hl = matrix(np.array([0.0 for i in range(N + 1)]), tc='d') G_0 = np.hstack([(-1) * X.T, np.array([[0.0] for i in range(N)])]) G_0 = np.vstack([np.array([0.0 for i in range(N + 1)]), G_0]) Gq = [matrix(G_0)] hq = [matrix(np.array([risk_level] + [0.0 for i in range(N)]))] G_1 = np.hstack([(-1) * Y.T, np.array([[0.0] for i in range(N)])]) G_1 = np.vstack([np.array([0.0 for i in range(N)] + [-1.0]), G_1]) Gq += [matrix(G_1)] hq += [matrix(np.array([0.0 for i in range(N + 1)]))] sol = solvers.socp(c, Gl=Gl, hl=hl, Gq=Gq, hq=hq, A=A, b=b) w = sol['x'][:-1] return w
def quality(frames): G = [] h = [] for i in range(frames): p = peak(x) + peak(x - 1) + np.random.normal(0, noise, n) shift = np.random.randint(-8, 8) f = np.roll( np.repeat(np.dot((np.roll(C, shift, axis=0))[::srf, :], p), srf), -shift) epsilon = np.sqrt(frames) * noise h += [matrix(np.append(np.array([epsilon]), -f))] A = np.roll(np.repeat((np.roll(C, shift, axis=0))[::srf, :], srf, axis=0), -shift, axis=0) G += [ matrix(-np.append(np.append([np.zeros(n)], A, axis=0), np.zeros((n + 1, n)), axis=1)) ] D = sp.linalg.circulant(np.append(np.array([1., -1.]), np.zeros(n - 2))).T I = np.eye(n) G0 = matrix( np.append(np.append(D, -I, axis=1), np.append(-D, -I, axis=1), axis=0)) c = matrix(np.ones(2 * n)) sol = solvers.socp(c, Gl=G0, hl=matrix(np.zeros(2 * n)), Gq=G, hq=h) reconstruction = sol['x'] return 1 - np.sum( (np.squeeze(np.array(reconstruction[0:n])) - puresignal)[range(77) + range(94, 162) + range(179, n)]**2) / np.sum( np.array(reconstruction[0:n])**2)
def rl1m_socp(A, m, n, b, e): c = matrix(0.0, (2*n,1)) c[n:2*n] = 1.0 Gl = matrix(0.0, (3*n,2*n)) for i in range(n): Gl[i,i] = 1.0 Gl[n+i,i] = Gl[i,n+i] = Gl[n+i,n+i] = Gl[2*n+i,n+i] = -1.0 hl = matrix(0.0, (3*n,1)) Gq = [] hq = [] G = matrix(0.0, (m+1,2*n)) G[1:m+1,0:n] = A Gq.append(G) h = matrix(0.0, (m+1,1)) h[0] = e h[1:m+1] = b hq.append(h) sol = solvers.socp(c, Gl, hl, Gq, hq) s = sol['x'] return s[0:n]
def solve_socp(c, Gl=None, hl=None, Gql=[], hql=[], A=None, b=None): """ Sovle Second Order Cone Programming :param c: :param Gl: :param hl: :param Gql: :param hql: :param A: :param b: :return: """ if Gl is None: Gl = np.zeros((1, len(c))) hl = np.zeros((1, 1)) if not Gql: Gql = [np.zeros((1, len(c)))] hql = [np.zeros((1, 1))] args = [ matrix(c), matrix(Gl), matrix(hl), [matrix(Gq) for Gq in Gql], [matrix(hq) for hq in hql] ] if A is not None: args.extend([matrix(A), matrix(b)]) sol = solvers.socp(*args) if 'optimal' not in sol['status']: print("CVXOPT FAIL:", sol['status']) return None return np.array(sol['x']).reshape((len(c), ))
def testsocp(opts): c = matrix([-2., 1., 5.]) G = [matrix([[12., 13., 12.], [6., -3., -12.], [-5., -5., 6.]])] G += [ matrix([[3., 3., -1., 1.], [-6., -6., -9., 19.], [10., -2., -2., -3.]]) ] h = [matrix([-12., -3., -2.]), matrix([27., 0., 3., -42.])] solvers.options.update(opts) sol = solvers.socp(c, Gq=G, hq=h) print "x = \n", helpers.str2(sol['x'], "%.9f") print "zq[0] = \n", helpers.str2(sol['zq'][0], "%.9f") print "zq[1] = \n", helpers.str2(sol['zq'][1], "%.9f") helpers.run_go_test( "../testsocp", { 'x': sol['x'], 'sq0': sol['sq'][0], 'sq1': sol['sq'][1], 'zq0': sol['zq'][0], 'zq1': sol['zq'][1] })
def CVXOPT_SOCP_Solver(p, solverName): if solverName == 'native_CVXOPT_SOCP_Solver': solverName = None cvxopt_solvers.options['maxiters'] = p.maxIter cvxopt_solvers.options['feastol'] = p.contol cvxopt_solvers.options['abstol'] = p.ftol if p.iprint <= 0: cvxopt_solvers.options['show_progress'] = False cvxopt_solvers.options['LPX_K_MSGLEV'] = 0 cvxopt_solvers.options['MSK_IPAR_LOG'] = 0 xBounds2Matrix(p) #FIXME: if problem is search for MAXIMUM, not MINIMUM!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! f = copy(p.f).reshape(-1,1) # CVXOPT has some problems with x0 so currently I decided to avoid using the one Gq, hq = [], [] C, d, q, s = p.C, p.d, p.q, p.s for i in range(len(q)): Gq.append(Matrix(Vstack((-atleast_1d(q[i]),-atleast_1d(C[i]) if not isspmatrix(C[i]) else C[i])))) hq.append(matrix(hstack((atleast_1d(s[i]), atleast_1d(d[i]))), tc='d')) sol = cvxopt_solvers.socp(Matrix(p.f), Gl=Matrix(p.A), hl = Matrix(p.b), Gq=Gq, hq=hq, A=Matrix(p.Aeq), b=Matrix(p.beq), solver=solverName) p.msg = sol['status'] if p.msg == 'optimal' : p.istop = SOLVED_WITH_UNIMPLEMENTED_OR_UNKNOWN_REASON else: p.istop = -100 if sol['x'] is not None: p.xf = asarray(sol['x']).flatten() p.ff = sum(p.dotmult(p.f, p.xf)) else: p.ff = nan p.xf = nan*ones(p.n)
def solve_SOCP(f, A, b, c, d, F, g): '''solve SOCP of form min f^T x s.t. ||A_i x + b_i ||_2 \leq c_i^T x + d_i i=1,...m Fx = g using cvxopt''' c, G, h, A, b = convert_SOCP(f, A, b, c, d, F, g) sol = solvers.socp(c=c, Gq=G, hq=h, A=A, b=b) return sol
def cl1m_socp(A, m, n, b, e): c = matrix(0.0, (3*n,1)) c[range(0,3*n,3)] = 1.0 Ga=[] ha=[] for i in range(n): G = matrix(0.0, (3,3*n)) G[0,3*i] = G[1,3*i+1] = G[2,3*i+2] = -1.0 Ga.append(G) ha.append(matrix(0.0,(3,1))) sol = None Aa = matrix(0.0, (2*m,3*n)) Aa[0:m,range(1,3*n,3)] = A.real() Aa[0:m,range(2,3*n,3)] = -A.imag() Aa[m:2*m,range(1,3*n,3)] = A.imag() Aa[m:2*m,range(2,3*n,3)] = A.real() y = matrix(0.0,(2*m,1)) y[0:m] = b.real() y[m:2*m] = b.imag() if e == 0: sol = solvers.socp(c, Gq=Ga, hq=ha, A=Aa, b=y) else: G = matrix(0.0, (2*m+1,3*n)) G[1:2*m+1,:] = Aa Ga.append(G) h = matrix(0.0, (2*m+1,1)) h[0] = e h[1:2*m+1] = y ha.append(h) sol = solvers.socp(c, Gq=Ga, hq=ha) s = sol['x'] z = matrix(complex(0,0),(n,1)) for i in range(n): z[i] = complex(s[3*i+1], s[3*i+2]) return z
def frac_delay(delta, N, w_max=0.9, C=4): ''' Compute optimal fractionnal delay filter according to Design of Fractional Delay Filters Using Convex Optimization William Putnam and Julius Smith Arguments --------- delta: delay of filter in (fractionnal) samples N: number of taps w_max: Bandwidth of the filter (in fraction of pi) (default 0.9) C: sets the number of constraints to C*N (default 4) ''' # constraints N_C = int(C*N) w = np.linspace(0, w_max*np.pi, N_C)[:,np.newaxis] n = np.arange(N) from cvxopt import solvers, matrix f = np.concatenate((np.zeros(N), np.ones(1))) A = [] b = [] for i in range(N_C): Anp = np.concatenate(([np.cos(w[i]*n), -np.sin(w[i]*n)], [[0],[0]]), axis=1) Anp = np.concatenate(([-f], Anp), axis=0) A.append(matrix(Anp)) b.append(matrix(np.concatenate(([0], np.cos(w[i]*delta), -np.sin(w[i]*delta))))) solvers.options['show_progress'] = False sol = solvers.socp(matrix(f), Gq=A, hq=b) h = np.array(sol['x'])[:-1,0] ''' import matplotlib.pyplot as plt w = np.linspace(0, np.pi, 2*N_C) F = np.exp(-1j*w[:,np.newaxis]*n) Hd = np.exp(-1j*delta*w) plt.figure() plt.subplot(3,1,1) plt.plot(np.abs(np.dot(F,h) - Hd)) plt.subplot(3,1,2) plt.plot(np.diff(np.angle(np.dot(F,h)))) plt.subplot(3,1,3) plt.plot(h) ''' return h
def frac_delay(delta, N, w_max=0.9, C=4): """ Compute optimal fractionnal delay filter according to Design of Fractional Delay Filters Using Convex Optimization William Putnam and Julius Smith Arguments --------- delta: delay of filter in (fractionnal) samples N: number of taps w_max: Bandwidth of the filter (in fraction of pi) (default 0.9) C: sets the number of constraints to C*N (default 4) """ # constraints N_C = int(C * N) w = np.linspace(0, w_max * np.pi, N_C)[:, np.newaxis] n = np.arange(N) from cvxopt import solvers, matrix f = np.concatenate((np.zeros(N), np.ones(1))) A = [] b = [] for i in range(N_C): Anp = np.concatenate(([np.cos(w[i] * n), -np.sin(w[i] * n)], [[0], [0]]), axis=1) Anp = np.concatenate(([-f], Anp), axis=0) A.append(matrix(Anp)) b.append(matrix(np.concatenate(([0], np.cos(w[i] * delta), -np.sin(w[i] * delta))))) solvers.options["show_progress"] = False sol = solvers.socp(matrix(f), Gq=A, hq=b) h = np.array(sol["x"])[:-1, 0] """ import matplotlib.pyplot as plt w = np.linspace(0, np.pi, 2*N_C) F = np.exp(-1j*w[:,np.newaxis]*n) Hd = np.exp(-1j*delta*w) plt.figure() plt.subplot(3,1,1) plt.plot(np.abs(np.dot(F,h) - Hd)) plt.subplot(3,1,2) plt.plot(np.diff(np.angle(np.dot(F,h)))) plt.subplot(3,1,3) plt.plot(h) """ return h
def test_socp(self): from cvxopt import matrix, msk, solvers c = matrix([-2., 1., 5.]) G = [matrix([[12., 13., 12.], [6., -3., -12.], [-5., -5., 6.]])] G += [ matrix([[3., 3., -1., 1.], [-6., -6., -9., 19.], [10., -2., -2., -3.]]) ] h = [matrix([-12., -3., -2.]), matrix([27., 0., 3., -42.])] self.assertAlmostEqualLists(list(solvers.socp(c, Gq=G, hq=h)['x']), list(msk.socp(c, Gq=G, hq=h)[1]))
def testSecondOrderConeProgram(self): # min c # subject to: ||Q||^2 <= h c = matrix([-2., 1., 5.]) G = [matrix([[12., 13., 12.], [6., -3., -12.], [-5., -5., 6.]])] G += [ matrix([[3., 3., -1., 1.], [-6., -6., -9., 19.], [10., -2., -2., -3.]]) ] h = [matrix([-12., -3., -2.]), matrix([27., 0., 3., -42.])] sol = solvers.socp(c, Gq=G, hq=h) #print(sol['x']) self.assertTrue(sol)
def solve(A, b): c = numpy.zeros(A.shape[2]) c[-1] = 1 rhs = numpy.zeros_like(A[0][0]) rhs[-1] = 1 G = [matrix(-numpy.vstack((rhs.reshape(1, -1), A_i))) for A_i in A] h = [matrix(numpy.append(0, b_i)) for b_i in b] solvers.options["show_progress"] = False sol = solvers.socp(matrix(c), Gq=G, hq=h) return numpy.array(sol["x"]).flatten()
def socp(self, a, A1, A2, t, B_s, u_s): #Max a^T z ~ min -a^T z c = matrix(np.vstack([np.zeros((self.size_x(), 1)), -a])) A1_diag = block_diag(*([A1]*len(self.gravity_envelope))) A2 = np.vstack([self.computeA2(self.gravity+e) for e in self.gravity_envelope]) T = np.vstack([self.computeT(self.gravity+e) for e in self.gravity_envelope]) A = matrix(np.hstack([A1_diag, A2])) g_s, h_s = [], [] if self.L_s: g_s.append(np.vstack(self.L_s)) h_s.append(np.vstack(self.tb_s)) g_force = np.vstack([np.eye(self.size_x()), -np.eye(self.size_x())]) g_s.append(np.hstack([g_force, np.zeros((2*self.size_x(), self.size_z()))])) h_s.append(self.force_lim*self.mass*9.81*np.ones((2*self.size_x(), 1))) gl = np.vstack(g_s) hl = np.vstack(h_s) #Compute com friction cone g_com = np.zeros((self.size_z()+1, self.nrVars())) g_com[1:, -3:] = -np.eye(self.size_z()) h_com = np.zeros((self.size_z()+1, 1)) h_com[0, 0] = self.radius G = [g_com] H = [h_com] #For G : compute all cones for i, (b, u) in enumerate(list(zip(B_s, u_s))*len(self.gravity_envelope)): block = -np.vstack([u.T, b]) g = np.hstack([np.zeros((4, 3*i)), block, np.zeros((4, 3*(len(self.contacts)*len(self.gravity_envelope)-1-i))), np.zeros((4, self.size_z()))]) G.append(g) H.append(np.zeros((4, 1))) sol = solvers.socp(c, Gl=matrix(gl), hl=matrix(hl), Gq=list(map(matrix, G)), hq=list(map(matrix, H)), A=A, b=matrix(T)) return sol
def optimizer_socp_cvxopt(u0, linear_objective, socp_constraints): """ Solve the optimization problem min_u A u + b s.t. h₀ - (G u)₀ ≻ |h₁ - (Gu)₁ |₂ u0: reference control signal linear_objective: ( convert to cvxopt format and pass to cvxopt min cᵀu s.t Gₖ x + sₖ = hₖ, k = 0, ..., M A x = b s₀ ⪰ 0 # Component wise inequalities sₖ₀ ⪰ | sₖ₁ |₂, k = 1, ..., M min cᵀu s.t Gₖ x + sₖ = hₖ, k = 0, ..., M A x = b h₀ - G₀ x ⪰ 0 # Component wise inequalities hₖ[0] - Gₖ[0, :] x ⪰ | hₖ[1:] - Gₖ[1:, :] x |₂, k = 1, ..., M """ from cvxopt import solvers, matrix c, Gqs, hqs = convert_socp_to_cvxopt_format(linear_objective, socp_constraints) inputs_socp = dict(c=matrix(c), Gq=list(map(matrix, Gqs)), hq=list(map(matrix, hqs))) #print("optimizers.py:72", inputs_socp) sol = solvers.socp(**inputs_socp) if sol['status'] != 'optimal': if sol['status'] == 'primal infeasible': y_uopt = sol.get('z', u0) else: y_uopt = u0 print("{c}.T [y, u]\n".format(c=c) + "s.t. " + "".join( (" sq = {hq} - {Gq} [{y_uopt}]\n".format(hq=np.asarray(hq), Gq=np.asarray(Gq), y_uopt=np.asarray(y_uopt)) for Gq, hq in zip(Gqs, hqs)))) raise InfeasibleProblemError("Infeasible problem: %s" % sol['status']) return np.asarray(sol['x']).astype(u0.dtype).reshape(-1)
def Markovitz_opt_risktarget(mean, cov, risk_level): N = len(mean) X = np.linalg.cholesky(cov) c = matrix(-mean) A = matrix(np.array([[1.0 for i in range(N)]]), tc='d') b = matrix(np.array([1.0]), tc='d') Gl = matrix(np.diag([-1.0 for i in range(N)]), tc='d') hl = matrix(np.array([0.0 for i in range(N)]), tc='d') Gq = [matrix(np.vstack([np.array([0.0 for i in range(N)]), (-1) * X.T]))] hq = [matrix(np.array([risk_level] + [0.0 for i in range(N)]))] sol = solvers.socp(c, Gl=Gl, hl=hl, Gq=Gq, hq=hq, A=A, b=b) w = sol['x'] #CE_0 = -sol['primal objective'] return w
def CVXOPT_SOCP_Solver(p, solverName): if solverName == 'native_CVXOPT_SOCP_Solver': solverName = None cvxopt_solvers.options['maxiters'] = p.maxIter cvxopt_solvers.options['feastol'] = p.contol cvxopt_solvers.options['abstol'] = p.ftol if p.iprint <= 0: cvxopt_solvers.options['show_progress'] = False cvxopt_solvers.options['LPX_K_MSGLEV'] = 0 cvxopt_solvers.options['MSK_IPAR_LOG'] = 0 xBounds2Matrix(p) #FIXME: if problem is search for MAXIMUM, not MINIMUM!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! f = copy(p.f).reshape(-1, 1) # CVXOPT has some problems with x0 so currently I decided to avoid using the one Gq, hq = [], [] C, d, q, s = p.C, p.d, p.q, p.s for i in range(len(q)): Gq.append( Matrix( Vstack((-atleast_1d(q[i]), -atleast_1d(C[i]) if not isspmatrix(C[i]) else C[i])))) hq.append(matrix(hstack((atleast_1d(s[i]), atleast_1d(d[i]))), tc='d')) sol = cvxopt_solvers.socp(Matrix(p.f), Gl=Matrix(p.A), hl=Matrix(p.b), Gq=Gq, hq=hq, A=Matrix(p.Aeq), b=Matrix(p.beq), solver=solverName) p.msg = sol['status'] if p.msg == 'optimal': p.istop = SOLVED_WITH_UNIMPLEMENTED_OR_UNKNOWN_REASON else: p.istop = -100 if sol['x'] is not None: p.xf = asarray(sol['x']).flatten() p.ff = sum(p.dotmult(p.f, p.xf)) else: p.ff = nan p.xf = nan * ones(p.n)
def testfun(noise=None, gap=None, randstate=None): if gap is None: gap = param.gap if randstate is None: randstate = np.random.RandomState(param.seed) if noise is None: noise = param.noise A, M = datagen.datagen(noise, gap, randstate) G0 = matrix( np.append(np.append(param.I, -param.I, axis=1), np.append(-param.I, -param.I, axis=1), axis=0)) epsilon = param.frames * noise h1 = [ matrix( np.append(np.array([epsilon]), -M.reshape(param.frames * param.n))) ] G1 = [ matrix(-np.append(np.append([np.zeros(param.n)], A.reshape((param.frames * param.n, param.n)), axis=0), np.zeros((param.frames * param.n + 1, param.n)), axis=1)) ] sol = solvers.socp(param.c, Gl=G0, hl=matrix(np.zeros(2 * param.n)), Gq=G1, hq=h1) reconstruction = sol['x'][0:param.n] res = min( 1, max(reconstruction[param.n / 3 - 2:param.n / 3 + 3]), max(reconstruction[int(param.n * (1 + gap) / 3) - 2:int(param.n * (1 + gap) / 3) + 3]) ) - max(0, min(reconstruction[param.n / 3:int(param.n * (1 + gap) / 3)])) print('Resolution factor: %f' % res) return reconstruction
def socp(self, a, A1, A2, t, B_s, u_s): #Max a^T z ~ min -a^T z c = matrix(np.vstack([np.zeros((self.size_x(), 1)), -a])) A = matrix(np.hstack([A1, A2])) #For G : compute all cones G = [] H = [] for i, (b, u) in enumerate(zip(B_s, u_s)): block = -np.vstack([u.T, b]) g = np.hstack([np.zeros((4, 3*i)), block, np.zeros((4, 3*(len(self.contacts)-1-i))), np.zeros((4, self.size_z()))]) G.append(g) H.append(np.zeros((4, 1))) sol = solvers.socp(c, Gq=map(matrix, G), hq=map(matrix, H), A=A, b=matrix(t)) return sol
def test_cvxopt_example(): from cvxopt import matrix, solvers c = matrix([-2., 1., 5.]) G = [ matrix( [[12., 13., 12.], [6., -3., -12.], [-5., -5., 6.]] ) ] G += [ matrix( [[3., 3., -1., 1.], [-6., -6., -9., 19.], [10., -2., -2., -3.]] ) ] h = [ matrix( [-12., -3., -2.] ), matrix( [27., 0., 3., -42.] ) ] sol = solvers.socp(c, Gq = G, hq = h) assert sol['status'] == 'optimal' assert np.asarray(sol['x']) == pytest.approx(np.array([ [-5.02e+00], [-5.77e+00], [-8.52e+00]]), abs=1e-3, rel=1e-2) assert np.asarray(sol['zq'][0]) == pytest.approx(np.array([ [ 1.34e+00], [-7.63e-02], [-1.34e+00]]), abs=1e-3, rel=1e-2) assert np.asarray(sol['zq'][1]) == pytest.approx(np.array([ [ 1.02e+00], [ 4.02e-01], [ 7.80e-01], [-5.17e-01]]), abs=1e-3, rel=1e-2)
def solver(data): print "Calling the solver socp...", data.shape clip_size, _ = data.shape threshold = matrix([10.00]) p_bar = -np.mean(data, axis=1) cov_matrix = np.cov(data) cov_matrix_sqrt = scipy.linalg.sqrtm(cov_matrix) c = matrix(p_bar) d = matrix([[0.0]] * clip_size) G = [d] G+= [matrix(-cov_matrix_sqrt.real)] h = [threshold, matrix(np.zeros(clip_size, dtype=float).T)] A = matrix([[1.0]] * clip_size) b = matrix([1.0]) n=clip_size Gl = matrix(0.0, (n,n)) Gl[::n+1] = -1.0 hl = matrix(0.0, (n,1)) # print G sol = solvers.socp(c, Gq=G, hq=h, A=A, b=b, Gl=Gl, hl=hl) result = sol['x'] resultArray = np.array(result) print sol['status'] var = np.dot(np.dot(resultArray.T, cov_matrix), resultArray) print "variance", var # Find the closest document dot_prod = np.dot(data.T, result) closest_doc = np.argmax(dot_prod) # print closest_doc return closest_doc
def testfun(noise=None, gap=None, randstate=None): if gap is None: gap = param.gap if randstate is None: randstate = np.random.RandomState(param.seed) if noise is None: noise = param.noise h = [] G = [] A, M = datagen.datagen(noise, gap, randstate) epsilon = 6 * noise for i in range(param.frames): h += [matrix(np.append(np.array([epsilon]), -M[i, :]))] G += [ matrix( -np.append(np.append([np.zeros(param.n)], A[i, :, :], axis=0), np.zeros((param.n + 1, param.n)), axis=1)) ] #input for the solver G0 = matrix( np.append(np.append(param.D, -param.I, axis=1), np.append(-param.D, -param.I, axis=1), axis=0)) sol = solvers.socp(param.c, Gl=G0, hl=matrix(np.zeros(2 * param.n)), Gq=G, hq=h) reconstruction = sol['x'][0:param.n] res = min([ 1, reconstruction[param.n / 3], reconstruction[int(param.n * (1 + gap) / 3)] ]) - max(0, min(reconstruction[param.n / 3:int(param.n * (1 + gap) / 3)])) print('Resolution factor: %f' % res) return reconstruction
def solve(self): solvers.options['show_progress'] = False # [print(i) for i in self._hq] sol = solvers.socp(self.c, Gq=self._G, hq=self._hq) return sol
def solve(r0, rr0, att0, T, N, g, max_thrust=1.0, min_thrust=0.1, maxT_angle=90, safe_angle=10, dt=0.1, maxLand_angle=10, min_height=20): T = float(T) # minimise SUM(t0..tN) # # divide time T into N equal portions # ti is magnitude of xi,yi # # ||(xi,yi)|| < Ti # # solution x = (t1..tN,x1,y1 ... xN,yN,z1...zN) # Minimise by this criteria c = np.array(np.zeros(4 * N)) # minimise SUM(t0...tN) c[0] = T / (N - 1) # t(0) c[1:N - 1] = T / N # t(1)...t(N-2) c[N - 1] = T / (N - 1) # t(N-1) # limit ||(Txi,Tyi,Tzi)|| < Ti ################################################################ # Constraints on magnitude of accelerations related to Ti ################################################################ Gq = [] for i in range(0, N): m = np.zeros((4, 4 * N)) m[0, i] = -1.0 # Ti m[1, N + i * 3] = 1.0 # xi m[2, N + i * 3 + 1] = 1.0 # yi m[3, N + i * 3 + 2] = 1.0 # zi Gq.append(m) # constants hq = [] for i in range(0, N): hq.append(np.zeros(4).transpose()) ################################################################ # Constraint for Ti < max_thrust ################################################################ for i in range(0, N): m = np.zeros((2, 4 * N)) m[1, i] = 1.0 # Ti weight Gq.append(m) # constants for i in range(0, N): hq.append(np.array([max_thrust, 0.]).transpose()) ################################################################ # Constraint for Ti > min_thrust ################################################################ for i in range(0, N): m = np.zeros((2, 4 * N)) # RHS m[0, i] = -1.0 # Ti weight Gq.append(m) # constants for i in range(0, N): hq.append(np.array([0., min_thrust]).transpose()) ################################################################ # Constraint for T within cone angle, maxT_angle, of vertial ################################################################ # |Ty,Tz| < k.Tx # m = sin(a)/cos(a) # Note T(0) is ignored since later this is tied to the initial attitude # of the craft for i in range(1, N): m = np.zeros((3, 4 * N)) if i < N - 1: a = math.radians(min(maxT_angle, 89)) else: a = math.radians(min(maxLand_angle, 89)) k = math.sin(a) / math.cos(a) # RHS, <= below m[0, N + i * 3] = -k # Tx weight (gradient) # LHS, weights on Tx,Ty,Tz m[1, N + i * 3 + 1] = 1.0 # Ty weight m[2, N + i * 3 + 2] = 1.0 # Tz weight Gq.append(m) # constants # Note that cone is slightly raised by min_Tx so we have a # minimum upwards thrust for i in range(1, N): hq.append(np.array([min_Tx, 0., 0.]).transpose()) ################################################################ # EQUALITY CONSTRAINTS ################################################################ A = np.zeros((8, 4 * N)) b = np.zeros(8) ################################################################ # Constraint that accelerations sum mean vel. vector=(0,0,0) (after g applied) # canceling out initial velocity ################################################################ for i in range(0, N): # Assume T=12 # N=3, dt=4 # Divide time into M steps and compute acceleration after this point sw = 0 t = 0.0 while (t < T): w = utils.basis_weights(t, T, N) sw = sw + w[i] * dt t = t + dt A[0, N + i * 3] = sw A[1, N + i * 3 + 1] = sw A[2, N + i * 3 + 2] = sw # constants = sum to 0 b[0] = -(g[0] * T + rr0[0]) b[1] = -(g[1] * T + rr0[1]) b[2] = -(g[2] * T + rr0[2]) ################################################################ # Constraint that final position is (0,0) ################################################################ for i in range(0, N): # Assume T=12 # N=3, dt=4 # Divide time into M steps and compute acceleration after this point sw = 0 t = 0 while (t < T): tr = T - t - dt w = utils.basis_weights(t, T, N) sw = sw + tr * w[i] * dt + 0.5 * w[i] * dt * dt t = t + dt A[3, N + i * 3] = sw A[4, N + i * 3 + 1] = sw A[5, N + i * 3 + 2] = sw # constants = sum to 0 b[3] = -(r0[0] + rr0[0] * T + 0.5 * g[0] * T * T) b[4] = -(r0[1] + rr0[1] * T + 0.5 * g[1] * T * T) b[5] = -(r0[2] + rr0[2] * T + 0.5 * g[2] * T * T) ####### EXTRA CONSTRAINT TO MAKE FIRST VECTOR ATT0 ######### # Ensure T1 is co-linear with att0 # by making sure U,V vectors orthogonal to att0 give got products # of 0 ensure on the 2 intersecting planes # TODO: Ensure T1 is also in correct direction U, V = utils.orthogonal_vectors(att0) A[6, N] = U[0] A[6, N + 1] = U[1] A[6, N + 2] = U[2] b[6] = 0.0 A[7, N] = V[0] A[7, N + 1] = V[1] A[7, N + 2] = V[2] b[7] = 0.0 # Ensure T(N-1)y and T(N-1)z is 0 (thrust is up) #A[8,N+i*3+1] = 1.0 #b[8] = 0 #A[9,N+i*3+2] = 1.0 #b[9] = 0 # Ensure T1(X),T1(Y),T1(Z) is in direction att0 m = np.zeros((2, 4 * N)) m[0, N] = -np.sign(att0[0]) Gq.append(m) hq.append(np.array([0., 0.]).transpose()) m = np.zeros((2, 4 * N)) m[0, N + 1] = -np.sign(att0[1]) Gq.append(m) hq.append(np.array([0., 0.]).transpose()) m = np.zeros((2, 4 * N)) m[0, N + 2] = -np.sign(att0[2]) Gq.append(m) hq.append(np.array([0., 0.]).transpose()) ################################################################ # Add constraint to keep trajectory above surface ################################################################ # m[1..].X + hq[1] > m[0].X + hq[0] # use k=-k to invert inequality posm = [] # proportion of total time we want position of prop = 0.1 while prop <= 0.9: mid_t = T * prop # Ensure T1(X),T1(Y),T1(Z) is in direction att0 m = np.zeros((3, 4 * N)) for i in range(0, N): # each thrust vector # Assume T=12 # N=3, dt=4 wa = 0 t = 0 while (t < mid_t): tr = mid_t - t - dt w = utils.basis_weights(t, T, N) wa = wa + tr * w[i] * dt + 0.5 * w[i] * dt * dt t = t + dt # RHS m[0, N + i * 3] = -wa # weight in x(i) acceleration # LHS weights on magnitude sw = math.tan(math.radians(safe_angle)) sw = 0 m[1, N + i * 3 + 1] = wa * sw m[2, N + i * 3 + 2] = wa * sw Gq.append(m) x_final = r0[0] + rr0[0] * mid_t + 0.5 * g[0] * mid_t * mid_t y_final = r0[1] + rr0[1] * mid_t + 0.5 * g[1] * mid_t * mid_t z_final = r0[2] + rr0[2] * mid_t + 0.5 * g[2] * mid_t * mid_t # x_final is on RHS as constant # |y_final*W,z_final*W| <= -( x_final + sum(accelerations)*weights ) hq.append(np.array([x_final, min_height, 0.]).transpose()) prop = prop + 0.1 posm.append((mid_t, -m[0, :], x_final)) ################################################################ # Convert matrices to CVXOPT ################################################################ c = matrix(c.transpose()) Gq = [matrix(m) for m in Gq] hq = [matrix(m) for m in hq] A = matrix(A) b = matrix(b) # SOLVER SETTINGS #solvers.options['maxiters'] = 5 #solvers.options['show_progress'] = False solvers.options['abstol'] = 0.5 #solvers.options['feastol'] = 0.1 # SOLVE! sol = solvers.socp(c, Gq=Gq, hq=hq, A=A, b=b) if sol['status'] != 'optimal': return float("inf"), None accels = np.zeros((N, 3)) for i in range(0, N): accels[i, 0] = sol['x'][N + i * 3] accels[i, 1] = sol['x'][N + i * 3 + 1] accels[i, 2] = sol['x'][N + i * 3 + 2] return sol['primal objective'], accels
def convex_solution_no_perm(positions: np.array, formation: np.array) -> np.array: """Uses Convex Optimization to solve Min-Max Pattern Formation Finds solution for the trivial reflection/assignment See Paper: https://academic.microsoft.com/paper/2025764057 Args: positions: Initial Robot Positions formation: Formation to form Returns: Destinations similar to formation that minimize the maximum \ distance any robot with initial positions "positions" \ must travel """ positions = np.copy(positions) formation = np.copy(formation) - formation[0] # adjust formation to origin angle = -math.atan2(formation[1][1], formation[1][0]) xs, ys = formation[:, 0].copy(), formation[:, 1].copy() formation[:, 0] = math.cos(angle) * xs - math.sin(angle) * ys formation[:, 1] = math.sin(angle) * xs + math.cos(angle) * ys assert (positions.shape == formation.shape) s2norm = np.linalg.norm(formation[1]) n = len(formation) # | q_i.x - p_i.x | # | | <= r # | q_i.y - p_i.y |_2 # # such that # Aq = 0 <--- this says the shape is similar Gs = [] hs = [] eqs = np.append(np.eye(2 * n), np.zeros((2 * n, 2)), axis=1) eqs[:, -1][::2] = -positions[:, 0] eqs[:, -1][1::2] = -positions[:, 1] eq_r = np.zeros(2 * n + 2) eq_r[-2] = 1.0 for i in range(n): G, h = format_eqs([eqs[2 * i].tolist(), eqs[2 * i + 1].tolist()], eq_r) Gs.append(G) hs.append(h) # A has 2 (n-2) equations with 2n + 1 variables A = np.zeros((2 * (n - 2), 2 * n + 1), dtype=float) b = np.zeros(2 * (n - 2), dtype=float) for i in range(2, n): x_idx = 2 * i A[x_idx - 4][0] = formation[i][0] - s2norm # q_1.x A[x_idx - 4][1] = -formation[i][1] # q_1.y A[x_idx - 4][2] = -formation[i][0] # q_2.x A[x_idx - 4][3] = formation[i][1] # q_2.y A[x_idx - 4][x_idx] = s2norm # q_i.x y_idx = x_idx + 1 A[y_idx - 4][0] = formation[i][1] # q_1.x A[y_idx - 4][1] = formation[i][0] - s2norm # q_1.y A[y_idx - 4][2] = -formation[i][1] # q_2.x A[y_idx - 4][3] = -formation[i][0] # q_2.y A[y_idx - 4][y_idx] = s2norm # q_i.y # c is what we are trying to minimize # we are trying to minimize t (the last element of (q, t)) c = np.zeros(2 * n + 1) c[-1] = 1 # Shape/data-type conversions for optimization function c = matrix(c.tolist()) A = matrix(A.T.tolist()) b = matrix([b.tolist()]) sol = solvers.socp(c, A=A, b=b, Gq=Gs, hq=hs) # second-order cone programming return np.array(sol['x'][:-1]).reshape((n, 2)), sol['x'][-1]
def socp_for_design(main_design_mat, model_list, estimation_list, kappa_weight_list, lambda_weight_list, filename): estimation_eq_index_list = estimation_list[0] estimation_pen_index_list = estimation_list[1] estimation_pen_weight_list = estimation_list[2] estimation_ineq_index_list = estimation_list[3] estimation_ineq_win_list = estimation_list[4] estimators_list = list(set(list(chain.from_iterable([estimation_eq_index_list, estimation_pen_index_list, estimation_ineq_index_list])))) estimators_list.sort() factors_num = len(levels_list) terms_num = len(model_list) estimators_num = len(estimators_list) candidates_num = np.shape(main_design_mat)[1] eq_const_num = len(estimation_eq_index_list) penalties_num = len(estimation_pen_index_list) ineq_const_num = len(estimation_ineq_index_list) if estimators_num != len(list(chain.from_iterable([estimation_eq_index_list, estimation_ineq_index_list, estimation_pen_index_list]))): print("Error!! 0") if len(estimation_ineq_index_list) != len(estimation_ineq_win_list): print("Error!! 10") if len(estimation_pen_index_list) != len(estimation_pen_weight_list): print("Error!! 20") if len(kappa_weight_list) != estimators_num: print("Error!! 30") if len(lambda_weight_list) != candidates_num: print("Error!! 40") if max(list(chain.from_iterable(model_list))) >= factors_num: print("Error!! 50") if estimation_eq_index_list != []: if max(estimation_eq_index_list) >= terms_num: print("Error!! 60") if estimation_pen_index_list != []: if max(estimation_pen_index_list) >= terms_num: print("Error!! 70") if estimation_ineq_index_list != []: if max(estimation_ineq_index_list) >= terms_num: print("Error!! 80") model_mat = gen_model_mat(main_design_mat, model_list) print("\n\n << SOCP >>\n") x_var_num = estimators_num * candidates_num all_var_num = x_var_num + 2*estimators_num + candidates_num + 2*penalties_num + ineq_const_num cvec = [0.]*x_var_num cvec.extend([0.]*estimators_num) cvec.extend(kappa_weight_list) cvec.extend(lambda_weight_list) cvec.extend([0.]*penalties_num) cvec.extend(estimation_pen_weight_list) cvec.extend([0.]*ineq_const_num) cvec = matrix(cvec) print("kappa_weight_list") print(kappa_weight_list) print("lambda_weight_list") print(lambda_weight_list) print("estimation_pen_weight_list") print(estimation_pen_weight_list) print("cvec") print(cvec) Gqmat = [] hqvec = [] # L2 for i in range(0, estimators_num): temp_val_list = [-1]+[1]*candidates_num temp_row_list = list(range(0,1+candidates_num)) temp_col_list = [x_var_num+i]+list(range(i*candidates_num, (i+1)*candidates_num)) Gqmat += [spmatrix(temp_val_list, temp_row_list, temp_col_list, (candidates_num+1, all_var_num))] hqvec += [matrix(spmatrix([], [], [], (candidates_num+1,1)))] temp_val_list = [-1, -1, 2] temp_row_list = [0, 1, 2] temp_index_num = x_var_num+estimators_num+i temp_col_list = [temp_index_num, temp_index_num, x_var_num+i] Gqmat += [spmatrix(temp_val_list, temp_row_list, temp_col_list, (3, all_var_num))] hqvec += [matrix(spmatrix([1,-1], [0,1], [0,0], (3,1)))] # L1 for i in range(0, candidates_num): temp_val_list = [-1]+[1]*estimators_num temp_row_list = list(range(0,1+estimators_num)) temp_col_list = [x_var_num+2*estimators_num+i]+list(range(i, x_var_num, candidates_num)) Gqmat += [spmatrix(temp_val_list, temp_row_list, temp_col_list, (estimators_num+1, all_var_num))] hqvec += [matrix(spmatrix([], [], [], (estimators_num+1,1)))] for i in range(0, penalties_num): temp_val_list = [-1] temp_row_list = [0] temp_col_list = [x_var_num+2*estimators_num+candidates_num+i] temp_index_num = estimators_list.index(estimation_pen_index_list[i]) for j in range(0, terms_num): for k in range(0, candidates_num): temp_val_list += [model_mat[j,k]] temp_row_list += [j+1] temp_col_list += [temp_index_num*candidates_num+k] Gqmat += [spmatrix(temp_val_list, temp_row_list, temp_col_list, (terms_num+1, all_var_num))] hqvec += [matrix(spmatrix([1], [estimation_pen_index_list[i]+1], [0], (terms_num+1,1)))] temp_val_list = [-1, -1, 2] temp_row_list = [0, 1, 2] temp_index_num = x_var_num+2*estimators_num+candidates_num+penalties_num+i temp_col_list = [temp_index_num, temp_index_num,x_var_num+2*estimators_num+candidates_num+i] Gqmat += [spmatrix(temp_val_list, temp_row_list, temp_col_list, (3, all_var_num))] hqvec += [matrix(spmatrix([1,-1], [0,1], [0,0], (3,1)))] Glmat=matrix([]) hlvec=matrix([]) if estimation_ineq_index_list != []: for i in range(0, ineq_const_num): temp_val_list = [-1] temp_row_list = [0] temp_col_list = [x_var_num+2*estimators_num+candidates_num+2*penalties_num+i] temp_index_num = estimators_list.index(estimation_ineq_index_list[i]) for j in range(0, terms_num): for k in range(0, candidates_num): temp_val_list += [model_mat[j,k]] temp_row_list += [j+1] temp_col_list += [temp_index_num*candidates_num+k] Gqmat += [spmatrix(temp_val_list, temp_row_list, temp_col_list, (terms_num+1, all_var_num))] hqvec += [matrix(spmatrix([1], [estimation_ineq_index_list[i]+1], [0], (terms_num+1,1)))] temp_val_list = [] temp_row_list = [] temp_col_list = [] for i in range(0, ineq_const_num): temp_val_list += [1] temp_row_list += [i] temp_col_list += [x_var_num+2*estimators_num+candidates_num+2*penalties_num+i] Glmat = spmatrix(temp_val_list, temp_row_list, temp_col_list, (ineq_const_num, all_var_num)) hlvec = matrix(estimation_ineq_win_list) print("Glmat") print(matrix(Glmat)) print("hlvec") print(matrix(hlvec)) print("Gqmat") print(Gqmat,"\n") for i in range(0, estimators_num): print(matrix(Gqmat[i])) print("hqvec") print(hqvec,"\n") for i in range(0, estimators_num): print(matrix(hqvec[i])) Amat = matrix([]) bvec = matrix([]) temp_val_list = [] temp_row_list = [] temp_col_list = [] for i in range(0, eq_const_num): temp_index_num = estimators_list.index(estimation_eq_index_list[i]) for j in range(0, terms_num): for k in range(0, candidates_num): temp_val_list += [model_mat[j,k]] temp_row_list += [i*terms_num+j] temp_col_list += [temp_index_num*candidates_num+k] Amat = spmatrix(temp_val_list, temp_row_list, temp_col_list, (eq_const_num*terms_num, all_var_num)) temp_val_list = [] temp_row_list = [] temp_col_list = [] for i in range(0, eq_const_num): temp_val_list += [1] temp_row_list += [i*terms_num+estimation_eq_index_list[i]] temp_col_list += [0] bvec = matrix(spmatrix(temp_val_list, temp_row_list, temp_col_list, (eq_const_num*terms_num, 1))) print("Amat") print(Amat,"\n") print(matrix(Amat)) print("bvec") print(matrix(bvec)) filename = filename+".check.csv" f = open(filename,"w") csvf = csv.writer(f, lineterminator='\n') csvf.writerow(["main_design_mat"]) temp_mat = np.array(main_design_mat) for i in range(0, factors_num): csvf.writerow(temp_mat[i,:]) csvf.writerow(["model_list"]) csvf.writerow(model_list) csvf.writerow(["model_mat"]) temp_mat = np.array(model_mat) for i in range(0, terms_num): csvf.writerow(temp_mat[i,:]) csvf.writerow(["estimation_eq_index_list"]) csvf.writerow(estimation_eq_index_list) csvf.writerow(np.array(model_list)[estimation_eq_index_list]) csvf.writerow(["estimation_pen_index_list"]) csvf.writerow(estimation_pen_index_list) csvf.writerow(np.array(model_list)[estimation_pen_index_list]) csvf.writerow(["estimation_pen_weight_list"]) csvf.writerow(estimation_pen_weight_list) csvf.writerow(["estimation_ineq_index_list"]) csvf.writerow(estimation_ineq_index_list) csvf.writerow(np.array(model_list)[estimation_ineq_index_list]) csvf.writerow(["estimation_ineq_win_list"]) csvf.writerow(estimation_ineq_win_list) csvf.writerow(["kappa_weight_list"]) csvf.writerow(kappa_weight_list) csvf.writerow(["lambda_weight_list"]) csvf.writerow(lambda_weight_list) csvf.writerow(["c"]) temp_mat = np.array(matrix(cvec)) for i in range(0, all_var_num): csvf.writerow(temp_mat[i]) csvf.writerow(["Gl"]) temp_mat = np.array(matrix(Glmat)) temp_index_num = np.shape(temp_mat)[0] for i in range(0, temp_index_num): csvf.writerow(temp_mat[i,:]) csvf.writerow(["hl"]) temp_mat = np.array(matrix(hlvec)) temp_index_num = np.shape(temp_mat)[0] for i in range(0, temp_index_num): csvf.writerow(temp_mat[i]) for i in range(0, len(Gqmat)): csvf.writerow(["Gq", i]) temp_mat = np.array(matrix(Gqmat[i])) temp_index_num = np.shape(temp_mat)[0] for j in range(0, temp_index_num): csvf.writerow(temp_mat[j,:]) csvf.writerow(["hq", i]) temp_mat = np.array(matrix(hqvec[i])) temp_index_num = np.shape(temp_mat)[0] for j in range(0, temp_index_num): csvf.writerow(temp_mat[j]) csvf.writerow(["Amat"]) temp_mat = np.array(matrix(Amat)) temp_index_num = np.shape(temp_mat)[0] for i in range(0, temp_index_num): csvf.writerow(temp_mat[i]) csvf.writerow(["bvec"]) temp_mat = np.array(matrix(bvec)) temp_index_num = np.shape(temp_mat)[0] for i in range(0, temp_index_num): csvf.writerow(temp_mat[i]) f.close() if estimation_ineq_index_list != []: sol = solvers.socp(c=cvec, Gl=Glmat, hl=hlvec, Gq=Gqmat, hq=hqvec, A=Amat, b=bvec) else: sol = solvers.socp(c=cvec, Gq=Gqmat, hq=hqvec, A=Amat, b=bvec) args = {"c":cvec, "Gl":Glmat, "hl":hlvec, "Gq":Gqmat, "hq":hqvec, "A":Amat, "b":bvec} model_mat = np.array(model_mat) return (model_mat, args, sol)
def solver(**kwargs): if all(valid_args(e) for e in kwargs.values()): # args contains *actual* dimensions (for variables) and parameter values args = mangle(kwargs) # only care about the ones that are used # args = dict( (k,v) for k,v in mangled.iteritems() if k in set(used) ) # make sure all keys are subset of needed variable list if variable_set.issubset(args) and set(params).issubset(args): # first, make sure all parameter arguments are sparse matrices for k in set(params): if isinstance(args[k], float) or isinstance(args[k], int): args[k] = o.spmatrix(args[k],[0],[0]) elif isinstance(args[k], o.matrix): args[k] = o.sparse(args[k]) # get the size lookup table using actual dimensions sizes = codegen.get_variable_sizes(args) # build the location of the start indices from the size table start_idxs, cum = {}, 0 for k,v in sizes.iteritems(): start_idxs[k] = cum cum += v # add parameter sizes to the dictionary (!!!hack) for k in codegen.parameters: if k in set(args): sizes[k] = args[k].size[0] # get objective vector c_obj = o.matrix(0, (cum,1), 'd') for k,v in c.iteritems(): # we ignore constant coefficients if k != '1': idx = start_idxs[k] row_height = sizes[k] c_obj[idx:idx+row_height] = eval_matrix_coeff(v, args, row_height, 1, transpose_output=True) # get matrices A_mat, b_mat = build_matrix(A, b, b_height, args, sizes, start_idxs, cum) Gl_mat, hl_vec = build_matrix(Gl, hl, hl_height, args, sizes, start_idxs, cum) Gq_mats, hq_vecs = [], [] # matrices in SOC for G, h, height in zip(Gq, hq, hq_height): mat, vec = build_matrix(G, h, height, args, sizes, start_idxs, cum) # ensure that sizes agree oldsize = mat.size mat.size = (oldsize[0], cum) Gq_mats.append(mat) hq_vecs.append(vec) for G, h, height in zip(Gblk, hblk, hblk_blocks): mats, vecs = build_block_matrices(G, h, height, args, sizes, start_idxs, cum) # ensure that sizes agree for m in mats: oldsize = m.size m.size = (oldsize[0], cum) Gq_mats += mats hq_vecs += vecs sol = solvers.socp(c_obj, Gl_mat, hl_vec, Gq_mats, hq_vecs, A_mat, b_mat) # print sol # # Gl_mat, hl_vec # # print sizes # print c_obj # print A_mat # print b_mat # print Gl_mat # print hl_vec # # # print c_obj solution = recover_variables(sol['x'], start_idxs, sizes, variable_set) return solution else: raise Exception("Not all variable dimensions or parameters have been specified.") else: raise Exception("Expected integer arguments for variables and matrix or float arguments for params.")
#===Form Block Matrices===# Gs = [] for a in range(len(As)): G1 = (numpy.r_[-cs[a].T, -As[a]]) Gs.append(matrix(G1)) G = (lambda x, y: x + y, Gs)[1] #===Form Block Vectors===# hs = [] for b in range(len(bs)): h1 = (numpy.r_[ds[b], bs[b]]) hs.append(matrix(h1)) h = (lambda x, y: x + y, hs)[1] #===Solve===# sol = solvers.socp(f, Gq=G, hq=h) print sol['status'] #===Extract Filter===# h_found = numpy.zeros(filter_length) h_found = numpy.asarray(numpy.asmatrix(sol['x'][0:filter_length]))[:, 0] #h_found *= -1.0; if (0): #===Ensure Symmetry===# for i in range(len(h_found) / 2): temp1 = h_found[i] temp2 = h_found[len(h_found) - i - 1] avg = 0.5 * (temp1 + temp2) h_found[i] = h_found[len(h_found) - i - 1] = avg
# print c.size d = matrix([[0.0]] * clip_size) # print d.size G = [d] # print G[0].size # print cov_matrix_sqrt.shape G+= [matrix(-cov_matrix_sqrt.real)] print G[1].size # print G h = [threshold, matrix(np.zeros(clip_size, dtype=float).T)] # print h A = matrix([[1.0]] * clip_size) b = matrix([1.0]) sol = solvers.socp(c, Gq=G, hq=h, A=A, b=b) # print sol['x'] result = sol['x'] # print sum(result) resultArray = np.array(result) # print resultArray print sol['status'] # print resultArray.shape var = np.dot(np.dot(resultArray.T, cov_matrix), resultArray) print var # Find the closest document dot_prod = np.dot(U, result)
def run(self, BC): """Function to call for optimizing a trajectory by the direct approach. Args: BC (utils.BoundaryConditions): constraints for two-point boundary value problem. Returns: (utils.ControlLaw): optimal control law. """ # pre-computations d = 2 * BC.half_dim z = self.dyn.compute_rhs(BC, self.prop_ana) # scaling the right-hand side of the moment equation scale = linalg.norm(z) for i in range(0, d): z[i] /= scale # building grid on possible impulses' location if self.p == 1: grid = numpy.linspace(BC.nu0, BC.nuf, direct_params["n_grid_1norm"]) else: # p = 2 grid = numpy.linspace(BC.nu0, BC.nuf, direct_params["n_grid_2norm"]) Y_grid = self.grid_Y(grid, BC.half_dim) if self.p == 1: # building matrix for linear program M = numpy.zeros((d, d * direct_params["n_grid_1norm"])) for k in range(0, direct_params["n_grid_1norm"]): inter = Y_grid[:, k * BC.half_dim:(k + 1) * BC.half_dim] M[:, d * k:d * k + BC.half_dim] = inter M[:, d * k + BC.half_dim:d * k + d] = -inter # solving for slack variables res = linprog(numpy.ones(d * direct_params["n_grid_1norm"]), A_eq=M, b_eq=z, options={ "disp": False, "tol": direct_params["tol_linprog"] }) sol = res.x if direct_params["verbose"]: print('direct cost 1-norm: ' + str(res.fun)) # extracting nus with non-zero impulses lost = 0.0 # variable to keep track of cost from deleted impulses n_components = 0 indices = [] nus = [] for k in range(0, direct_params["n_grid_1norm"]): DV = sol[d * k:d * k + BC.half_dim] - sol[d * k + BC.half_dim:d * k + d] if linalg.norm(DV, 1) > direct_params["DV_min"]: indices.append(k) nus.append(grid[k]) for component in DV: if math.fabs(component) > direct_params["DV_min"]: n_components += 1 else: # Delta-V is considered numerically negligible lost += linalg.norm(DV, 1) # reconstructing velocity jumps DVs = numpy.zeros((len(nus), BC.half_dim)) for k, index in enumerate(indices): if BC.half_dim == 1: DVs[k, 0] = sol[2 * index] - sol[2 * index + 1] else: # in-plane of complete dynamics for j in range(0, BC.half_dim): aux = sol[d * index + j] - sol[d * index + BC.half_dim + j] if math.fabs(aux) > direct_params["DV_min"]: DVs[k, j] = aux else: # Delta-V is considered numerically negligible lost += math.fabs(aux) if direct_params["verbose"]: print("lost impulse: " + str(lost)) else: # p = 2 # building matrix for linear constraints M = numpy.zeros((d, BC.half_dim * direct_params["n_grid_2norm"])) for k in range(0, direct_params["n_grid_2norm"]): M[:, BC.half_dim * k:BC.half_dim * (k + 1)] = Y_grid[:, k * BC.half_dim:(k + 1) * BC.half_dim] A = numpy.concatenate((numpy.zeros( (d, direct_params["n_grid_2norm"])), M), axis=1) A = matrix(A) # building matrix for linear cost function f = numpy.concatenate( (numpy.ones(direct_params["n_grid_2norm"]), numpy.zeros(BC.half_dim * direct_params["n_grid_2norm"])), axis=0) f = matrix(f) # building matrices for SDP constraints G = None h = None vec = numpy.zeros(BC.half_dim + 1) vec = matrix(vec) for j in range(0, direct_params["n_grid_2norm"]): mat = numpy.zeros( (BC.half_dim + 1, direct_params["n_grid_2norm"] * (BC.half_dim + 1))) mat[0, j] = -1.0 for i in range(0, BC.half_dim): mat[i + 1, direct_params["n_grid_2norm"] + BC.half_dim * j + i] = 1.0 if j == 0: G = [matrix(mat)] h = [vec] else: # not first loop G += [matrix(mat)] h += [vec] if not direct_params["verbose"]: solvers.options[ 'show_progress'] = False # turn off printed stuff solvers.options['abstol'] = direct_params["tol_cvx"] solution = solvers.socp(f, Gq=G, hq=h, A=A, b=matrix(z)) sol = [] for el in solution['x']: sol.append(el) if direct_params["verbose"]: print("direct cost 2-norm: " + str(solution['primal objective'])) # extracting nus with non-zero impulses lost = 0.0 # variable to keep track of cost from deleted impulses indices = [] nus = [] for k in range(0, direct_params["n_grid_2norm"]): DV = sol[direct_params["n_grid_2norm"] + BC.half_dim * k:direct_params["n_grid_2norm"] + BC.half_dim * k + BC.half_dim] if linalg.norm(DV, 2) > direct_params["DV_min"]: indices.append(k) nus.append(grid[k]) else: # Delta-V is considered numerically negligible lost += linalg.norm(DV, 2) if direct_params["verbose"]: print("lost impulse: " + str(lost)) # reconstructing velocity jumps DVs = numpy.zeros((len(nus), BC.half_dim)) for k in range(0, len(nus)): DVs[k, :] = sol[direct_params["n_grid_2norm"] + BC.half_dim * indices[k]:direct_params["n_grid_2norm"] + BC.half_dim * indices[k] + BC.half_dim] # un-scaling for j in range(0, len(nus)): for i in range(0, BC.half_dim): DVs[j, i] *= scale return utils.ControlLaw(BC.half_dim, nus, DVs)
def testfun(noise=None, gap=None, randstate=None): if gap is None: gap = param.gap if noise is None: noise = param.noise if randstate is None: randstate = np.random.RandomState(param.seed) A, M = datagen.datagen(noise, gap, randstate) #TV opt G0 = matrix( np.append(np.append(param.D, -param.I, axis=1), np.append(-param.D, -param.I, axis=1), axis=0)) #L1 opt #G0 = matrix(np.append(np.append(param.I,-param.I,axis=1),np.append(-param.I,-param.I,axis=1),axis = 0)) #socpTVsmall or socpL1small epsilon = param.frames * noise B = np.sum(A, axis=0) f = (np.sum(M, axis=0)).T h1 = [matrix(np.append(np.array([epsilon]), -f))] G1 = [ matrix(-np.append(np.append([np.zeros(param.n)], B, axis=0), np.zeros((param.n + 1, param.n)), axis=1)) ] sol = solvers.socp(param.c, Gl=G0, hl=matrix(np.zeros(2 * param.n)), Gq=G1, hq=h1) reconstruction = sol['x'][0:param.n] res1 = min([ 1, reconstruction[param.n / 3], reconstruction[int(param.n * (1 + gap) / 3)] ]) - max(0, min(reconstruction[param.n / 3:int(param.n * (1 + gap) / 3)])) #socpTVbig or socpL1big epsilon = param.frames * noise h1 = [ matrix( np.append(np.array([epsilon]), -M.reshape(param.frames * param.n))) ] G1 = [ matrix(-np.append(np.append([np.zeros(param.n)], A.reshape((param.frames * param.n, param.n)), axis=0), np.zeros((param.frames * param.n + 1, param.n)), axis=1)) ] sol = solvers.socp(param.c, Gl=G0, hl=matrix(np.zeros(2 * param.n)), Gq=G1, hq=h1) reconstruction = sol['x'][0:param.n] res2 = min([ 1, reconstruction[param.n / 3], reconstruction[int(param.n * (1 + gap) / 3)] ]) - max(0, min(reconstruction[param.n / 3:int(param.n * (1 + gap) / 3)])) #socpTVmany or socpL1many epsilon = 6 * noise G = [] h = [] for i in range(param.frames): h += [matrix(np.append(np.array([epsilon]), -M[i, :]))] G += [ matrix( -np.append(np.append([np.zeros(param.n)], A[i, :, :], axis=0), np.zeros((param.n + 1, param.n)), axis=1)) ] sol = solvers.socp(param.c, Gl=G0, hl=matrix(np.zeros(2 * param.n)), Gq=G, hq=h) reconstruction = sol['x'][0:param.n] res3 = min([ 1, reconstruction[param.n / 3], reconstruction[int(param.n * (1 + gap) / 3)] ]) - max(0, min(reconstruction[param.n / 3:int(param.n * (1 + gap) / 3)])) return res1, res2, res3
# The small SOCP of section 8.5 (Second-order cone programming). from cvxopt import matrix, solvers c = matrix([-2., 1., 5.]) G = [ matrix( [[12., 13., 12.], [6., -3., -12.], [-5., -5., 6.]] ) ] G += [ matrix( [[3., 3., -1., 1.], [-6., -6., -9., 19.], [10., -2., -2., -3.]] ) ] h = [ matrix( [-12., -3., -2.] ), matrix( [27., 0., 3., -42.] ) ] sol = solvers.socp(c, Gq = G, hq = h) print("\nx = \n") print(sol['x']) print("zq[0] = \n") print(sol['zq'][0]) print("zq[1] = \n") print(sol['zq'][1])
def test_controller_socp_cvxopt(): """ From cvxopt socp example: >>> from cvxopt import matrix, solvers >>> c = matrix([-2., 1., 5.]) >>> G = [ matrix( [[12., 13., 12.], [6., -3., -12.], [-5., -5., 6.]] ) ] >>> G += [ matrix( [[3., 3., -1., 1.], [-6., -6., -9., 19.], [10., -2., -2., -3.]] ) ] >>> h = [ matrix( [-12., -3., -2.] ), matrix( [27., 0., 3., -42.] ) ] >>> sol = solvers.socp(c, Gq = G, hq = h) >>> sol['status'] optimal >>> print(sol['x']) [-5.02e+00] [-5.77e+00] [-8.52e+00] >>> print(sol['zq'][0]) [ 1.34e+00] [-7.63e-02] [-1.34e+00] >>> print(sol['zq'][1]) [ 1.02e+00] [ 4.02e-01] [ 7.80e-01] [-5.17e-01] """ linear_objective = np.array([-2., 1., 5.]) # | A x + b |₂ ≺ cᵀ x + d A = list(map(np.array, [ [[-13., 3., 5.], [-12., 12., -6.]], [[-3., 6., 2.], [ 1., 9., 2.], [-1., -19., 3.]] ])) b = list(map(np.array, [ [-3, -2], [0., 3., -42] ])) c = list(map(np.array, [ [-12., -6., 5,], [-3., 6., -10] ])) d = list(map(np.array, [ -12, 27 ])) exp_uopt = np.array([ [-5.02e+00], [-5.77e+00], [-8.52e+00]]) names = ("1", "2") socp_constraints = zip(A, b, c, d) named_socp_constraints = list(zip(names, socp_constraints)) cvx_c, cvx_Gqs, cvx_hqs = convert_socp_to_cvxopt_format( linear_objective, named_socp_constraints) exp_Gqs = [ np.array( [[12., 13., 12.], [6., -3., -12.], [-5., -5., 6.]] ) ] exp_Gqs += [ np.array( [[3., 3., -1., 1.], [-6., -6., -9., 19.], [10., -2., -2., -3.]] ) ] exp_hqs = [ np.array( [-12., -3., -2.] ), np.array( [27., 0., 3., -42.] ) ] for Gq, hq, exp_Gq, exp_hq in zip(cvx_Gqs, cvx_hqs, exp_Gqs, exp_hqs): assert Gq.T == pytest.approx(exp_Gq) assert hq.flatten() == pytest.approx(exp_hq) from cvxopt import solvers, matrix inputs_socp = dict(c = matrix(cvx_c), Gq = list(map(matrix, cvx_Gqs)), hq = list(map(matrix, cvx_hqs))) #print("test_optimizers.py:72", inputs_socp) sol = solvers.socp(**inputs_socp) uopt = sol['x'] assert np.asarray(uopt) == pytest.approx(np.asarray(exp_uopt), rel=1e-2) uopt = optimizer_socp_cvxopt(np.random.rand(3), linear_objective, named_socp_constraints) assert np.asarray(uopt) == pytest.approx(np.asarray(exp_uopt).flatten(), rel=1e-2)
def testfun(noise=None, gap=None, randstate=None): if gap is None: gap = param.gap if noise is None: noise = param.noise if randstate is None: randstate = np.random.RandomState(param.seed) A, M = datagen.datagen(noise, gap, randstate) #L1 opt G0 = matrix( np.append(np.append(param.I, -param.I, axis=1), np.append(-param.I, -param.I, axis=1), axis=0)) #socpL1many epsilon = 6 * noise G = [] h = [] for i in range(param.frames): h += [matrix(np.append(np.array([epsilon]), -M[i, :]))] G += [ matrix( -np.append(np.append([np.zeros(param.n)], A[i, :, :], axis=0), np.zeros((param.n + 1, param.n)), axis=1)) ] sol = solvers.socp(param.c, Gl=G0, hl=matrix(np.zeros(2 * param.n)), Gq=G, hq=h) reconstruction = sol['x'][0:param.n] res = min( 1, max(reconstruction[param.n / 3 - 2:param.n / 3 + 3]), max(reconstruction[int(param.n * (1 + gap) / 3) - 2:int(param.n * (1 + gap) / 3) + 3]) ) - max(0, min(reconstruction[param.n / 3:int(param.n * (1 + gap) / 3)])) print(res) if res < param.resthr: #socpL1small epsilon = param.frames * noise B = np.sum(A, axis=0) f = (np.sum(M, axis=0)).T h1 = [matrix(np.append(np.array([epsilon]), -f))] G1 = [ matrix(-np.append(np.append([np.zeros(param.n)], B, axis=0), np.zeros((param.n + 1, param.n)), axis=1)) ] sol = solvers.socp(param.c, Gl=G0, hl=matrix(np.zeros(2 * param.n)), Gq=G1, hq=h1) reconstruction = sol['x'][0:param.n] res = min( 1, max(reconstruction[param.n / 3 - 2:param.n / 3 + 3]), max(reconstruction[int(param.n * (1 + gap) / 3) - 2:int(param.n * (1 + gap) / 3) + 3]) ) - max(0, min(reconstruction[param.n / 3:int(param.n * (1 + gap) / 3)])) if res < param.resthr: return 0. return 0.5 return 1.