def optimize(self, solver="socp"):
"""
Available Solvers: Linear Programming (of course including min infinity-norm/1-norm), Quadratic Prgramming,
Second-Order Cone Prgramming, Semi-Definite Progamming (or LMI)
:param solver:
:return:
"""
self.rho = mycvxopt.solve(solver, [self.c],
G=self.Gl,
h=self.hl,
Gql=self.Gql,
hql=self.hql,
Gsl=self.Gsl,
hsl=self.hsl,
A=self.A,
b=self.b,
MAX_ITER_SOL=1)
return self.rho
def optimize(self, solver="lp"):
"""
Available Solvers: Linear Programming (of course including min infinity-norm/1-norm), Quadratic Prgramming,
Second-Order Cone Prgramming, Semi-Definite Progamming (or LMI)
:param solver:
:return:
"""
rhovec = mycvxopt.solve(solver, [self.c],
G=self.Gl,
h=self.hl,
Gql=self.Gql,
hql=self.hql,
Gsl=self.Gsl,
hsl=self.hsl,
A=self.A,
b=self.b,
MAX_ITER_SOL=1)
self.rho0 = rhovec.reshape((self.NOP, 1))
self.rho = [
rhovec[self.sepidx[i]:self.sepidx[i + 1]]
for i in range(len(self.sepidx) - 1)
]
return self.rho
def min_2norm(fig=0):
"""
2 norm optimization
:return:
"""
"""
CONDITION AND CONSTRAINTS
"""
MIN = JER # What to be minimized
TS = 0.001 # Sampling Period
TINIT = (0, 1)
START, END = 0, 1
TSAMPLE = np.linspace(0, TINIT[END], TINIT[END] / TS + 1)
QC = (0, 1.0)
VC = (0, 0)
AC = (0, 0)
JC = (0, 0)
CONSTRAINTS = (*QC, *VC, *AC, *JC)
LABEL_CONSTRAINTS = (POS, VEL, ACC, JER)
NCONSTRAINTS = len(CONSTRAINTS)
assert NCONSTRAINTS == len(LABEL_CONSTRAINTS) * 2
VMAX = 10000
AMAX = 10000
nc = NCONSTRAINTS + 60 # number of control points == number of theta
p = 4 # jerk continous
u = [TINIT[END] * i / (nc - 1) for i in range(nc)] # knots
bspl = Bspline(u, nc, p, verbose=True)
"""
QUDRATIC PROGRAMMING
"""
M = np.array(bspl.basis(TSAMPLE, der=MIN))
P = np.dot(M.T, M)
q = np.zeros(nc)
A = None
for d in LABEL_CONSTRAINTS:
for x in bspl.basis(TINIT, der=d):
if A is None:
A = np.array(x)
else:
A = np.block([[A], [np.array(x)]])
b = np.matrix(CONSTRAINTS).reshape((NCONSTRAINTS, 1))
if True:
G = np.array([*bspl.basis(TSAMPLE, der=VEL), *bspl.basis(TSAMPLE, der=ACC)])
hv = np.ones((len(TSAMPLE), 1)) * VMAX
ha = np.ones((len(TSAMPLE), 1)) * AMAX
h = np.append(hv, ha, axis=0)
G, h = mycvxopt.constraints(G, h, -h)
else:
G, h = None, None # No ineq
theta = mycvxopt.solve("qp", [P, q], G=G, h=h, A=A, b=b)
print(theta)
"""
Plot
"""
for d in STATES:
fig += 1
text = None
myplot.time(TSAMPLE, bspl.bspline(theta, TSAMPLE, der=d), fig, text=text,
save_name="data/" + LABELS[d][:-1].split()[0], label=("Time (s)", LABELS[d]))
myplot.show()
def test():
"""
:return:
"""
"""
Linear Quadratic Programming Example
"""
M = np.array([[1., 2., 0.], [-8., 3., 2.], [0., 1., 1.]])
P = np.dot(M.T, M)
q = np.dot(np.array([3., 2., 3.]), M).reshape((3, ))
if True:
G = np.array([[1., 2., 1.], [2., 0., 1.], [-1., 2., -1.]])
h = np.array([3., 2., -2.]).reshape((3, ))
else:
G = None
h = None
print(mycvxopt.solve("qp", [P, q], G=G, h=h))
"""
Linear Programming
"""
c = np.array([-4., -5.])
G = np.array([[2., 1., -1., 0.], [1., 2., 0., -1.]]).reshape((4, 2))
h = np.array([3., 3., 0., 0.])
print(mycvxopt.solve("lp", [c], G=G, h=h))
"""
Second Order Cone Programming
min x1 + x2
s.t. ||x||2 <= 1 eqiv. x in circle with radius=1
:return:
"""
A0 = np.array([[1., 0], [0, 1]])
A0.reshape((2, 2))
b0 = np.array([0., 0])
b0.reshape((2, 1))
c0 = np.array([0., 0])
c0.reshape((2, 1))
d0 = np.array([1.])
gq0, hq0 = mycvxopt.qc2socp(A0, b0, c0, d0)
Gq = [gq0]
hq = [hq0]
c = np.array([1., 1])
print(mycvxopt.solve("socp", [c], Gql=Gq, hql=hq))
"""
Semi-Definite Progamming
"""
c = np.array([0., 0.])
hs0 = [[1., 0.], [0., 1.]]
Gs0 = [[0., 1, 1, 0], [0., 0, 0, 0]]
Gs = [Gs0]
hs = [hs0]
Gl = np.eye(2) * -1
hl = -0. * np.ones(2)
sol = mycvxopt.solve("sdp", [c],
G=Gl,
h=hl,
Gsl=Gs,
hsl=hs,
MAX_ITER_SOL=1)
print(sol)
"""
Semi-Definite Progamming
"""
c = np.array([0., 0.])
hs0 = [[-1., 0.], [0., -1.]]
Gs0 = [[0., 1, 1, 0], [0., 0, 0, 0]]
Gs = [Gs0]
hs = [hs0]
Gl = np.eye(2) * -1
hl = -0. * np.ones(2)
try:
sol = mycvxopt.solve("sdp", [c], G=Gl, h=hl, Gsl=Gs, hsl=hs)
except:
print("THIS PROBELM IS INFEASIBLE BECAUSE IT IS CONCAVE!!!")