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.
