def __init__(self,
nv,
nc,
objective,
constraints,
bounds,
num_iter=3,
num_wsr=100,
verbose=False):
"""Initialize the SQP."""
self.nv = nv
self.nc = nc
self.num_iter = num_iter
self.num_wsr = num_wsr
self.objective = objective
self.constraints = constraints
self.bounds = bounds
self.qp = qpoases.PySQProblem(nv, nc)
if not verbose:
options = qpoases.PyOptions()
options.printLevel = qpoases.PyPrintLevel.NONE
self.qp.setOptions(options)
self.qp_initialized = False
def solve(self, q, dq, pd, vd, f):
''' Solve for the optimal inputs. '''
ni = self.model.ni
# forward kinematics
p = self.model.forward(q)
J = self.model.jacobian(q)
# calculate velocity reference
v = self.K.dot(pd - p) + vd - self.Cinv.dot(f)
# setup the QP
H = J.T.dot(J) + self.W
g = -J.T.dot(v)
# bounds on the computed input
vel_ub = np.ones(ni) * self.vel_lim
vel_lb = -vel_ub
acc_ub = np.ones(ni) * self.acc_lim * self.dt + dq
acc_lb = -np.ones(ni) * self.acc_lim * self.dt + dq
ub = np.maximum(vel_ub, acc_ub)
lb = np.minimum(vel_lb, acc_lb)
qp = qpoases.PyQProblemB(ni)
if not self.verbose:
options = qpoases.PyOptions()
options.printLevel = qpoases.PyPrintLevel.NONE
qp.setOptions(options)
ret = qp.init(H, g, lb, ub, np.array([NUM_WSR]))
u = np.zeros(ni)
qp.getPrimalSolution(u)
return u
def solve(self, q, dq, pd, vd, ad):
''' Solve for the optimal inputs. '''
ni = self.model.ni
# forward kinematics
p = self.model.forward(q)
J = self.model.jacobian(q)
Jdot = self.model.dJdt(q, dq)
# calculate acceleration reference
v = J.dot(dq)
a_ref = self.Kp.dot(pd - p) + self.Kv.dot(vd - v) + ad
# setup the QP
d = Jdot.dot(dq) - a_ref
H = J.T.dot(J) + self.W
g = J.T.dot(d)
# bounds on the computed input
acc_ub = np.ones(ni) * self.acc_lim
acc_lb = -acc_ub
ub = acc_ub
lb = acc_lb
qp = qpoases.PyQProblemB(ni)
if not self.verbose:
options = qpoases.PyOptions()
options.printLevel = qpoases.PyPrintLevel.NONE
qp.setOptions(options)
ret = qp.init(H, g, lb, ub, np.array([NUM_WSR]))
u = np.zeros(ni)
qp.getPrimalSolution(u)
return u
def __init__(self, dim_a, dim_b):
self.qpProblem = qpoases.PySQProblem(dim_a, dim_b)
options = qpoases.PyOptions()
options.printLevel = qpoases.PyPrintLevel.NONE
self.qpProblem.setOptions(options)
self.xdot_full = np.zeros(dim_a)
self.started = False
def __init__(self, dim_a, dim_b):
"""
:param dim_a: number of joint constraints + number of soft constraints
:type int
:param dim_b: number of hard constraints + number of soft constraints
:type int
"""
self.qpProblem = qpoases.PySQProblem(dim_a, dim_b)
options = qpoases.PyOptions()
options.printLevel = qpoases.PyPrintLevel.NONE
self.qpProblem.setOptions(options)
self.xdot_full = np.zeros(dim_a)
self.started = False
def _iterate(self, q0, dq0, pr, u, N, pc):
ni = self.model.ni
# Create the QP, which we'll solve sequentially.
# num vars, num constraints (note that constraints only refer to matrix
# constraints rather than bounds)
# num constraints = N obstacle constraints and ni*N joint acceleration
# constraints
num_var = ni * N
num_constraints = 3 * N + ni * N
qp = qpoases.PySQProblem(num_var, num_constraints)
options = qpoases.PyOptions()
options.printLevel = qpoases.PyPrintLevel.NONE
qp.setOptions(options)
# Initial opt problem.
H, g, A_obs, lbA_obs = self._lookahead(q0, pr, u, N, pc)
ubA_obs = np.infty * np.ones_like(lbA_obs)
lb, ub = self._calc_vel_limits(u, ni, N)
A_acc, lbA_acc, ubA_acc = self._calc_acc_limits(u, dq0, ni, N)
A = np.vstack((A_obs, A_acc))
lbA = np.concatenate((lbA_obs, lbA_acc))
ubA = np.concatenate((ubA_obs, ubA_acc))
ret = qp.init(H, g, A, lb, ub, lbA, ubA, np.array([NUM_WSR]))
delta = np.zeros(ni * N)
qp.getPrimalSolution(delta)
u = u + delta
# Remaining sequence is hotstarted from the first.
for i in range(NUM_ITER - 1):
H, g, A_obs, lbA_obs = self._lookahead(q0, pr, u, N, pc)
lb, ub = self._calc_vel_limits(u, ni, N)
A_acc, lbA_acc, ubA_acc = self._calc_acc_limits(u, dq0, ni, N)
A = np.vstack((A_obs, A_acc))
lbA = np.concatenate((lbA_obs, lbA_acc))
ubA = np.concatenate((ubA_obs, ubA_acc))
qp.hotstart(H, g, A, lb, ub, lbA, ubA, np.array([NUM_WSR]))
qp.getPrimalSolution(delta)
u = u + delta
return u
def solve(self, q, pd, vd, C=None):
''' Solve for the optimal inputs. '''
ni = self.model.ni
no = self.model.no
# forward kinematics
p = self.model.forward(q)
J = self.model.jacobian(q)
# calculate velocity reference
v = self.K.dot(pd - p) + vd
# setup the QP
H = self.W
g = np.zeros(ni)
# optionally add additional constraints to decouple the system
if C is not None:
A = np.vstack((J, C))
lbA = ubA = np.concatenate((v, np.zeros(C.shape[0])))
nc = no + C.shape[0]
else:
A = J
lbA = ubA = v
nc = no
# bounds on the computed input
lb = np.ones(ni) * self.lb
ub = np.ones(ni) * self.ub
qp = qpoases.PyQProblem(ni, nc)
if not self.verbose:
options = qpoases.PyOptions()
options.printLevel = qpoases.PyPrintLevel.NONE
qp.setOptions(options)
ret = qp.init(H, g, A, lb, ub, lbA, ubA, np.array([NUM_WSR]))
dq = np.zeros(ni)
qp.getPrimalSolution(dq)
return dq
def solve(self, example):
'''
Solve problem
Args:
example: example object
Returns:
Results structure
'''
p = example.qp_problem
n, m = p['n'], p['m']
if p['P'] is not None:
P = np.ascontiguousarray(p['P'].todense())
if 'A_nobounds' in p:
A = np.ascontiguousarray(p['A_nobounds'].todense())
m = A.shape[0]
elif p['A'] is not None:
A = np.ascontiguousarray(p['A'].todense())
# Define contiguous array vectors
q = np.ascontiguousarray(p['q'])
if 'l_nobounds' in p:
l = np.ascontiguousarray(p['l_nobounds'])
else:
l = np.ascontiguousarray(p['l'])
if 'u_nobounds' in p:
u = np.ascontiguousarray(p['u_nobounds'])
else:
u = np.ascontiguousarray(p['u'])
if 'lx' in p:
lx = np.ascontiguousarray(p['lx'])
else:
lx = np.ascontiguousarray(-np.inf * np.ones(n))
if 'ux' in p:
ux = np.ascontiguousarray(p['ux'])
else:
ux = np.ascontiguousarray(np.inf * np.ones(n))
# Redirect output if verbose is False
if 'verbose' in self._settings:
if self._settings['verbose'] is False:
with stdout_redirected():
qpoases_m = qpoases.PyQProblem(n, m)
else:
qpoases_m = qpoases.PyQProblem(n, m)
else:
# Suppress output also if we do not specify verbose
with stdout_redirected():
qpoases_m = qpoases.PyQProblem(n, m)
options = qpoases.PyOptions()
for param, value in self._settings.items():
if param == 'verbose':
if value is False:
options.printLevel = qpoases.PyPrintLevel.NONE
elif param == 'time_limit':
qpoases_cpu_time = np.array([value])
elif param == 'nWSR':
qpoases_nWSR = np.array([value])
else:
exec("options.%s = %s" % (param, value))
qpoases_m.setOptions(options)
if 'time_limit' not in self._settings:
# Set default to max 10 seconds in runtime
qpoases_cpu_time = np.array([10.])
if 'nWSR' not in self._settings:
# Set default to max 1000000 working set recalculations
qpoases_nWSR = np.array([1000000])
# Solve problem
status = qpoases_m.init(P, q, A, lx, ux, l, u,
qpoases_nWSR, qpoases_cpu_time)
# Check status
status = self.STATUS_MAP.get(status, s.SOLVER_ERROR)
# run_time
run_time = qpoases_cpu_time[0]
# number of iterations
niter = qpoases_nWSR[0]
if status in s.SOLUTION_PRESENT:
x = np.zeros(n)
y_temp = np.zeros(n + m)
obj_val = qpoases_m.getObjVal()
qpoases_m.getPrimalSolution(x)
qpoases_m.getDualSolution(y_temp)
# Change sign
y = -y_temp[n:]
if 'A_nobounds' in p:
# If explicit bounds provided, reconstruct dual variable
# Bounds on all the variables
y_bounds = -y_temp[:n]
# Limit to only the actual bounds
y_bounds = y_bounds[p['bounds_idx']]
y = np.concatenate((y, y_bounds))
if not is_qp_solution_optimal(p, x, y):
status = s.SOLVER_ERROR
return Results(status, obj_val,
x, y,
run_time, niter)
else:
return Results(status, None, None, None,
run_time, niter)
def solve_loop(qp_matrices, solver='osqp', osqp_settings=None):
"""
Solve lasso optimization loop for all lambdas
"""
# Shorter name for qp_matrices
qp = qp_matrices
# Extract features
n = qp.n
print('n = %d and solver %s' % (n, solver))
# Get number of problems to solve
n_prob = qp.q.shape[1]
# Initialize time vector
time = np.zeros(n_prob)
# Initialize number of iterations vector
niter = np.zeros(n_prob)
if solver == 'osqp':
# Setup OSQP
m = osqp.OSQP()
m.setup(qp.P, qp.q[:, 0], qp.A, qp.l, qp.u, **osqp_settings)
for i in range(n_prob):
q = qp.q[:, i]
# Update linear cost
m.update(q=q)
# Solve
results = m.solve()
x = results.x
y = results.y
status = results.info.status_val
niter[i] = results.info.iter
time[i] = results.info.run_time
# Check if status correct
if status != m.constant('OSQP_SOLVED'):
import ipdb
ipdb.set_trace()
raise ValueError('OSQP did not solve the problem!')
# DEBUG
# solve with gurobi
# prob = mpbpy.QuadprogProblem(qp.P, q, Aosqp, losqp, uosqp)
# res = prob.solve(solver=mpbpy.GUROBI, verbose=False)
# print('Norm difference OSQP-GUROBI %.3e' %
# np.linalg.norm(x - res.x))
# import ipdb; ipdb.set_trace()
elif solver == 'osqp_coldstart':
# Setup OSQP
m = osqp.OSQP()
m.setup(qp.P, qp.q[:, 0], qp.A, qp.l, qp.u, **osqp_settings)
for i in range(n_prob):
q = qp.q[:, i]
# Update linear cost
m.update(q=q)
# Solve
results = m.solve()
x = results.x
y = results.y
status = results.info.status_val
niter[i] = results.info.iter
time[i] = results.info.run_time
# Check if status correct
if status != m.constant('OSQP_SOLVED'):
import ipdb
ipdb.set_trace()
raise ValueError('OSQP did not solve the problem!')
# DEBUG
# solve with gurobi
# prob = mpbpy.QuadprogProblem(qp.P, q, qp.A, qp.l, qp.u)
# res = prob.solve(solver=mpbpy.GUROBI, verbose=False)
# print('Norm difference OSQP-GUROBI %.3e' %
# np.linalg.norm(x - res.x))
# import ipdb; ipdb.set_trace()
# DEBUG print iterations per value of lambda
# lambda_vals = np.logspace(-2, 2, 101)[::-1]
#
# import matplotlib.pylab as plt
# plt.figure()
# ax = plt.gca()
# plt.plot(lambda_vals, niter)
# ax.set_xlabel(r'$\lambda$')
# ax.set_ylabel(r'iter')
# plt.show(block=False)
# import ipdb; ipdb.set_trace()
elif solver == 'osqp_no_caching':
for i in range(n_prob):
# Setup OSQP
m = osqp.OSQP()
m.setup(qp.P, qp.q[:, i], qp.A, qp.l, qp.u, **osqp_settings)
# Solve
results = m.solve()
x = results.x
y = results.y
status = results.info.status_val
niter[i] = results.info.iter
time[i] = results.info.run_time
# Check if status correct
if status != m.constant('OSQP_SOLVED'):
import ipdb
ipdb.set_trace()
raise ValueError('OSQP did not solve the problem!')
# DEBUG
# solve with gurobi
# prob = mpbpy.QuadprogProblem(qp.P, q, Aosqp, losqp, uosqp)
# res = prob.solve(solver=mpbpy.GUROBI, verbose=False)
# print('Norm difference OSQP-GUROBI %.3e' %
# np.linalg.norm(x - res.x))
# import ipdb; ipdb.set_trace()
elif solver == 'qpoases':
n_dim = qp.P.shape[0] # Number of variables
m_dim = qp.A.shape[0] # Number of constraints without bounds
# Initialize qpoases and set options
qpoases_m = qpoases.PyQProblem(n_dim, m_dim)
options = qpoases.PyOptions()
options.printLevel = qpoases.PyPrintLevel.NONE
qpoases_m.setOptions(options)
# Setup matrix P and A
P = np.ascontiguousarray(qp.P.todense())
A = np.ascontiguousarray(qp.A.todense())
for i in range(n_prob):
# Get linera cost as contiguous array
q = np.ascontiguousarray(qp.q[:, i])
# Reset cpu time
qpoases_cpu_time = np.array([10.])
# Reset number of of working set recalculations
nWSR = np.array([1000])
if i == 0:
# First iteration
res_qpoases = qpoases_m.init(P, q, A,
np.ascontiguousarray(qp.lx),
np.ascontiguousarray(qp.ux),
np.ascontiguousarray(qp.l),
np.ascontiguousarray(qp.u), nWSR,
qpoases_cpu_time)
else:
# Solve new hot started problem
res_qpoases = qpoases_m.hotstart(q,
np.ascontiguousarray(qp.lx),
np.ascontiguousarray(qp.ux),
np.ascontiguousarray(qp.l),
np.ascontiguousarray(qp.u),
nWSR, qpoases_cpu_time)
# # DEBUG Solve with gurobi
# qpoases solution
# sol_qpoases = np.zeros(n + k)
# qpoases_m.getPrimalSolution(sol_qpoases)
# import mathprogbasepy as mpbpy
# Agrb = spa.vstack((qp.A,
# spa.hstack((spa.eye(n), spa.csc_matrix((n, k)))
# ))).tocsc()
# lgrb = np.append(qp.l, qp.lx)
# ugrb = np.append(qp.u, qp.ux)
# prob = mpbpy.QuadprogProblem(spa.csc_matrix(qp.P), q,
# Agrb, lgrb, ugrb)
# res = prob.solve(solver=mpbpy.GUROBI, verbose=True)
# print("Norm difference x qpoases - GUROBI = %.4f" %
# np.linalg.norm(sol_qpoases - res.x))
# print("Norm difference objval qpoases - GUROBI = %.4f" %
# abs(qpoases_m.getObjVal() - res.obj_val))
# import ipdb; ipdb.set_trace()
# if res_qpoases != 0:
# raise ValueError('qpoases did not solve the problem!')
# Save time
time[i] = qpoases_cpu_time[0]
# Save number of iterations
niter[i] = nWSR[0]
elif solver == 'gurobi':
for i in range(n_prob):
# Get linera cost as contiguous array
q = qp.q[:, i]
# Solve with gurobi
prob = mpbpy.QuadprogProblem(qp.P, q, qp.A, qp.l, qp.u)
res = prob.solve(solver=mpbpy.GUROBI, verbose=False)
# Save time
time[i] = res.cputime
# Save number of iterations
niter[i] = res.total_iter
elif solver == 'mosek':
for i in range(n_prob):
# Get linera cost as contiguous array
q = qp.q[:, i]
# Solve with mosek
prob = mpbpy.QuadprogProblem(qp.P, q, qp.A, qp.l, qp.u)
res = prob.solve(solver=mpbpy.MOSEK, verbose=False)
# Save time
time[i] = res.cputime
# Save number of iterations
niter[i] = res.total_iter
elif solver == 'ecos':
n_var = qp_matrices.A_lasso.shape[1]
m_var = qp_matrices.A_lasso.shape[0]
for i in range(n_prob):
if n_var <= 60: # (problem becomes too big otherwise):
# Model with CVXPY
# minimize y' * y + lambda * 1' * t
# subject to y = Ax - b
# -t <= x <= t
lambda_i = qp_matrices.lambdas[i]
x = cvxpy.Variable(n_var)
y = cvxpy.Variable(m_var)
t = cvxpy.Variable(n_var)
objective = cvxpy.Minimize(
cvxpy.quad_form(y, spa.eye(m_var)) +
lambda_i * np.ones(n_var) * t)
constraints = [
y == qp_matrices.A_lasso * x - qp_matrices.b_lasso,
-t <= x, x <= t
]
problem = cvxpy.Problem(objective, constraints)
problem.solve(solver=cvxpy.ECOS, verbose=False)
# DEBUG: Solve with MOSEK
q = qp.q[:, i]
# Solve with mosek
# prob = mpbpy.QuadprogProblem(qp.P, q, qp.A, qp.l, qp.u)
# res = prob.solve(solver=mpbpy.MOSEK, verbose=False)
# x_mosek = res.x[:n_var]
# import ipdb; ipdb.set_trace()
# Obtain time and number of iterations
time[i] = problem.solver_stats.setup_time + \
problem.solver_stats.solve_time
niter[i] = problem.solver_stats.num_iters
else:
time[i] = 0
niter[i] = 0
else:
raise ValueError('Solver not understood')
# Return statistics
return utils.Statistics(time), utils.Statistics(niter)
def solve(self, q, pr, f, fd):
n = self.model.n
pe = self.model.forward(q)
d = pr - pe
J = self.model.jacobian(q)
H = self.dt**2 * J.T.dot(self.Q).dot(J) + self.R
g = -self.dt * d.T.dot(self.Q).dot(J)
lb = np.ones(n) * self.lb
ub = np.ones(n) * self.ub
# force control
Kp = 0.01
f_norm = np.linalg.norm(f)
# TODO we can actually set nf in the case we have f=0 but fd != 0
if f_norm > 0:
nf = f / f_norm # unit vector in direction of force
df = fd - f_norm
# only the first two rows of J are used since we only want to
# constrain position
A = nf.T.dot(J[:2, :])
lbA = ubA = np.array([Kp * df])
# lbA = np.array([0.0]) if df > 0 else None
# ubA = np.array([0.0]) if df < 0 else None
# A = A.reshape((1, 3))
# H += np.outer(A, A)
# g -= Kp * df * A
# # admittance control
Qf = np.eye(2)
Bf = 0 * np.eye(2)
Kf = 0.001 * np.eye(2)
Jp = J[:2, :]
K = -(self.dt * Kf + Bf).dot(Jp)
d = K.dot(pr - pe) - f
H += K.T.dot(Qf).dot(K)
g += d.T.dot(Qf).dot(K)
# Create the QP, which we'll solve sequentially.
# num vars, num constraints (note that constraints only refer to
# matrix constraints rather than bounds)
# qp = qpoases.PyQProblem(n, 1)
# options = qpoases.PyOptions()
# options.printLevel = qpoases.PyPrintLevel.NONE
# qp.setOptions(options)
# ret = qp.init(H, g, A, lb, ub, lbA, ubA, NUM_WSR)
qp = qpoases.PyQProblemB(n)
options = qpoases.PyOptions()
options.printLevel = qpoases.PyPrintLevel.NONE
qp.setOptions(options)
ret = qp.init(H, g, lb, ub, np.array([NUM_WSR]))
else:
qp = qpoases.PyQProblemB(n)
options = qpoases.PyOptions()
options.printLevel = qpoases.PyPrintLevel.NONE
qp.setOptions(options)
ret = qp.init(H, g, lb, ub, np.array([NUM_WSR]))
dq = np.zeros(n)
qp.getPrimalSolution(dq)
return dq
def solve_loop(qp_matrices, problem, nsim, solver='osqp'):
"""
Solve MPC loop
"""
# Shorter name for qp_matrices
qp = qp_matrices
# Get dimensions
(nx, nu) = problem.B.shape
N = problem.N
print('N = %d and solver %s' % (N, solver))
# Initialize time and number of iterations vectors
time = np.zeros(nsim)
niter = np.zeros(nsim)
if solver == 'osqp':
# Construct qp matrices
if len(problem.xmin) > 0:
# If the problem has state constraints
Aosqp = spa.vstack([
qp.A,
spa.eye((N + 1) * nx + N * nu),
]).tocsc()
else:
Aosqp = spa.vstack([
qp.A,
spa.hstack(
[spa.csc_matrix((N * nu, (N + 1) * nx)),
spa.eye(N * nu)]),
]).tocsc()
losqp = np.hstack([qp.l, qp.lx])
uosqp = np.hstack([qp.u, qp.ux])
# Initial state
x0 = problem.x0
# Setup OSQP
m = osqp.OSQP()
m.setup(
qp.P,
qp.q,
Aosqp,
losqp,
uosqp,
# auto_rho=False,
auto_rho=True,
max_iter=2500,
scaling=True,
scaling_iter=50,
polish=False,
verbose=False)
for i in range(nsim):
# Solve with osqp
res = m.solve()
# Save time and number of iterations
time[i] = res.info.run_time
niter[i] = res.info.iter
# Check if status is correct
status = res.info.status_val
if status != m.constant('OSQP_SOLVED'):
# # Dump file to 'bad_convergence/data'folder
# import pickle
# problem = {'P': qp.P,
# 'q': qp.q,
# 'A': Aosqp,
# 'l': losqp,
# 'u': uosqp}
# with open('bad_convergence/data/%s.pickle' % 'helicopter_scaling_large', 'wb') as f:
# pickle.dump(problem, f)
import ipdb
ipdb.set_trace()
raise ValueError('OSQP did not solve the problem!')
# Apply first control input to the plant
u = res.x[-N * nu:-(N - 1) * nu]
x0 = problem.A.dot(x0) + problem.B.dot(u)
# Update linear constraints
if len(problem.tmin) > 0:
losqp = np.hstack([b(x0, nx, N), problem.tmin, qp.lx])
uosqp = np.hstack([b(x0, nx, N), problem.tmax, qp.ux])
else:
losqp = np.hstack([b(x0, nx, N), qp.lx])
uosqp = np.hstack([b(x0, nx, N), qp.ux])
m.update(l=losqp, u=uosqp)
elif solver == 'osqp_coldstart':
# Construct qp matrices
if len(problem.xmin) > 0:
# If the problem has state constraints
Aosqp = spa.vstack([
qp.A,
spa.eye((N + 1) * nx + N * nu),
]).tocsc()
else:
Aosqp = spa.vstack([
qp.A,
spa.hstack(
[spa.csc_matrix((N * nu, (N + 1) * nx)),
spa.eye(N * nu)]),
]).tocsc()
losqp = np.hstack([qp.l, qp.lx])
uosqp = np.hstack([qp.u, qp.ux])
# Initial state
x0 = problem.x0
# Setup OSQP
m = osqp.OSQP()
m.setup(
qp.P,
qp.q,
Aosqp,
losqp,
uosqp,
warm_start=False,
auto_rho=True,
# auto_rho=False,
rho=0.1,
max_iter=2500,
scaling_iter=50,
polish=False,
verbose=False)
for i in range(nsim):
# Solve with osqp
res = m.solve()
# Save time and number of iterations
time[i] = res.info.run_time
niter[i] = res.info.iter
# Check if status is correct
status = res.info.status_val
if status != m.constant('OSQP_SOLVED'):
import ipdb
ipdb.set_trace()
raise ValueError('OSQP did not solve the problem!')
# Apply first control input to the plant
u = res.x[-N * nu:-(N - 1) * nu]
x0 = problem.A.dot(x0) + problem.B.dot(u)
# Update linear constraints
if len(problem.tmin) > 0:
losqp = np.hstack([b(x0, nx, N), problem.tmin, qp.lx])
uosqp = np.hstack([b(x0, nx, N), problem.tmax, qp.ux])
else:
losqp = np.hstack([b(x0, nx, N), qp.lx])
uosqp = np.hstack([b(x0, nx, N), qp.ux])
m.update(l=losqp, u=uosqp)
elif solver == 'osqp_no_caching':
# Construct qp matrices
if len(problem.xmin) > 0:
# If the problem has state constraints
Aosqp = spa.vstack([
qp.A,
spa.eye((N + 1) * nx + N * nu),
]).tocsc()
else:
Aosqp = spa.vstack([
qp.A,
spa.hstack(
[spa.csc_matrix((N * nu, (N + 1) * nx)),
spa.eye(N * nu)]),
]).tocsc()
losqp = np.hstack([qp.l, qp.lx])
uosqp = np.hstack([qp.u, qp.ux])
# Initial state
x0 = problem.x0
for i in range(nsim):
# Setup OSQP
m = osqp.OSQP()
m.setup(
qp.P,
qp.q,
Aosqp,
losqp,
uosqp,
warm_start=False,
auto_rho=True,
# auto_rho=False,
rho=0.1,
max_iter=2500,
scaling_iter=50,
polish=False,
verbose=False)
# Solve
res = m.solve()
# Save time and number of iterations
time[i] = res.info.run_time
niter[i] = res.info.iter
# Check if status is correct
status = res.info.status_val
if status != m.constant('OSQP_SOLVED'):
import ipdb
ipdb.set_trace()
raise ValueError('OSQP did not solve the problem!')
# Apply first control input to the plant
u = res.x[-N * nu:-(N - 1) * nu]
x0 = problem.A.dot(x0) + problem.B.dot(u)
# Update linear constraints
if len(problem.tmin) > 0:
losqp = np.hstack([b(x0, nx, N), problem.tmin, qp.lx])
uosqp = np.hstack([b(x0, nx, N), problem.tmax, qp.ux])
else:
losqp = np.hstack([b(x0, nx, N), qp.lx])
uosqp = np.hstack([b(x0, nx, N), qp.ux])
m.update(l=losqp, u=uosqp)
elif solver == 'qpoases':
n_dim = qp.P.shape[0] # Number of variables
m_dim = qp.A.shape[0] # Number of constraints without bounds
# Initialize qpoases and set options
qpoases_m = qpoases.PyQProblem(n_dim, m_dim)
options = qpoases.PyOptions()
options.printLevel = qpoases.PyPrintLevel.NONE
qpoases_m.setOptions(options)
# Construct bounds for qpoases
lx = np.append(-np.inf * np.ones((N + 1) * nx), qp.lx)
ux = np.append(np.inf * np.ones((N + 1) * nx), qp.ux)
# Setup matrix P and A
P = np.ascontiguousarray(qp.P.todense())
A = np.ascontiguousarray(qp.A.todense())
# Initial state
x0 = problem.x0
# RHS of the linear equality constraints
lqpoases = qp.l
uqpoases = qp.u
for i in range(nsim):
# Reset cpu time
qpoases_cpu_time = np.array([60.])
# Reset number of of working set recalculations
nWSR = np.array([1e6])
if i == 0:
# First iteration
res_qpoases = qpoases_m.init(P, np.ascontiguousarray(qp.q), A,
np.ascontiguousarray(lx),
np.ascontiguousarray(ux),
np.ascontiguousarray(lqpoases),
np.ascontiguousarray(uqpoases),
nWSR, qpoases_cpu_time)
else:
# Solve new hot started problem
res_qpoases = qpoases_m.hotstart(
np.ascontiguousarray(qp.q), np.ascontiguousarray(lx),
np.ascontiguousarray(ux), np.ascontiguousarray(lqpoases),
np.ascontiguousarray(uqpoases), nWSR, qpoases_cpu_time)
if res_qpoases != 0:
raise ValueError('qpoases did not solve the problem!')
# Save time and number of iterations
time[i] = qpoases_cpu_time[0]
niter[i] = nWSR[0]
# Get qpoases solution
sol_qpoases = np.zeros((N + 1) * nx + N * nu)
qpoases_m.getPrimalSolution(sol_qpoases)
# Apply first control input to the plant
u = sol_qpoases[-N * nu:-(N - 1) * nu]
x0 = problem.A.dot(x0) + problem.B.dot(u)
# Update linear constraints
if len(problem.tmin) > 0:
lqpoases = np.hstack([b(x0, nx, N), problem.tmin])
uqpoases = np.hstack([b(x0, nx, N), problem.tmax])
else:
lqpoases = np.hstack([b(x0, nx, N)])
uqpoases = np.hstack([b(x0, nx, N)])
elif solver == 'gurobi' or solver == 'mosek':
# Construct qp matrices
if len(problem.xmin) > 0:
# If the problem has state constraints
Agurobi = spa.vstack([
qp.A,
spa.eye((N + 1) * nx + N * nu),
]).tocsc()
else:
Agurobi = spa.vstack([
qp.A,
spa.hstack(
[spa.csc_matrix((N * nu, (N + 1) * nx)),
spa.eye(N * nu)]),
]).tocsc()
lgurobi = np.hstack([qp.l, qp.lx])
ugurobi = np.hstack([qp.u, qp.ux])
# Initial state
x0 = problem.x0
for i in range(nsim):
# Solve with gurobi
prob = mpbpy.QuadprogProblem(qp.P, qp.q, Agurobi, lgurobi, ugurobi)
if solver == 'gurobi':
res = prob.solve(solver=mpbpy.GUROBI, verbose=False)
else:
res = prob.solve(solver=mpbpy.MOSEK, verbose=False)
# Save time and number of iterations
time[i] = res.cputime
niter[i] = res.total_iter
# Check if status is correct
status = res.status
if status != 'optimal':
import ipdb
ipdb.set_trace()
raise ValueError('Gurobi did not solve the problem!')
# Apply first control input to the plant
u = res.x[-N * nu:-(N - 1) * nu]
x0 = problem.A.dot(x0) + problem.B.dot(u)
# Update QP problem
if len(problem.tmin) > 0:
lgurobi = np.hstack([b(x0, nx, N), problem.tmin, qp.lx])
ugurobi = np.hstack([b(x0, nx, N), problem.tmax, qp.ux])
else:
lgurobi = np.hstack([b(x0, nx, N), qp.lx])
ugurobi = np.hstack([b(x0, nx, N), qp.ux])
else:
raise ValueError('Solver not understood')
# Return statistics
return utils.Statistics(time), utils.Statistics(niter)
def solve_problem(qp_matrices, n_prob, solver='osqp'):
"""
Solve Huber fitting problem
"""
# Shorter name for qp_matrices
qp = qp_matrices
# Get dimensions
m = int(len(qp.lx) / 2)
n = len(qp.q) - 2 * m
print('n = %d and solver %s' % (n, solver))
# Initialize time vector
time = np.zeros(n_prob)
# Initialize number of iterations vector
niter = np.zeros(n_prob)
if solver == 'osqp':
# Construct qp matrices
Aosqp = spa.vstack(
[qp.A,
spa.hstack([spa.csc_matrix((2 * m, n)),
spa.eye(2 * m)])]).tocsc()
losqp = np.hstack([qp.l, qp.lx])
uosqp = np.hstack([qp.u, qp.ux])
for i in range(n_prob):
# Setup OSQP
m = osqp.OSQP()
m.setup(qp.P,
qp.q,
Aosqp,
losqp,
uosqp,
auto_rho=True,
polish=False,
verbose=False)
# Solve
results = m.solve()
x = results.x
status = results.info.status_val
niter[i] = results.info.iter
time[i] = results.info.run_time
# Check if status correct
if status != m.constant('OSQP_SOLVED'):
import ipdb
ipdb.set_trace()
raise ValueError('OSQP did not solve the problem!')
# DEBUG
# solve with gurobi
# import mathprogbasepy as mpbpy
# prob = mpbpy.QuadprogProblem(qp.P, qp.q, Aosqp, losqp, uosqp)
# res = prob.solve(solver=mpbpy.GUROBI, verbose=False)
# print('Norm difference OSQP-GUROBI %.3e' %
# np.linalg.norm(x - res.x))
# import ipdb; ipdb.set_trace()
elif solver == 'qpoases':
for i in range(n_prob):
n_dim = qp.P.shape[0] # Number of variables
m_dim = qp.A.shape[0] # Number of constraints without bounds
# Initialize qpoases and set options
qpoases_m = qpoases.PyQProblem(n_dim, m_dim)
options = qpoases.PyOptions()
options.printLevel = qpoases.PyPrintLevel.NONE
qpoases_m.setOptions(options)
# Construct bounds for qpoases
lx = np.append(-np.inf * np.ones(n), qp.lx)
ux = np.append(np.inf * np.ones(n), qp.ux)
# Setup matrix P and A
P = np.ascontiguousarray(qp.P.todense())
A = np.ascontiguousarray(qp.A.todense())
# Reset cpu time
qpoases_cpu_time = np.array([10.])
# Reset number of working set recalculations
nWSR = np.array([1000])
# Solve
res_qpoases = qpoases_m.init(P, np.ascontiguousarray(qp.q), A,
np.ascontiguousarray(lx),
np.ascontiguousarray(ux),
np.ascontiguousarray(qp.l),
np.ascontiguousarray(qp.u), nWSR,
qpoases_cpu_time)
# if res_qpoases != 0:
# raise ValueError('qpoases did not solve the problem!')
# Save time and number of iterations
time[i] = qpoases_cpu_time[0]
niter[i] = nWSR[0]
elif solver == 'gurobi':
for i in range(n_prob):
# Construct qp matrices
Agurobi = spa.vstack([
qp.A,
spa.hstack([spa.csc_matrix((2 * m, n)),
spa.eye(2 * m)])
]).tocsc()
lgurobi = np.hstack([qp.l, qp.lx])
ugurobi = np.hstack([qp.u, qp.ux])
# Solve with gurobi
prob = mpbpy.QuadprogProblem(qp.P, qp.q, Agurobi, lgurobi, ugurobi)
res = prob.solve(solver=mpbpy.GUROBI, verbose=False)
niter[i] = res.total_iter
time[i] = res.cputime
elif solver == 'mosek':
for i in range(n_prob):
# Construct qp matrices
Amosek = spa.vstack([
qp.A,
spa.hstack([spa.csc_matrix((2 * m, n)),
spa.eye(2 * m)])
]).tocsc()
lmosek = np.hstack([qp.l, qp.lx])
umosek = np.hstack([qp.u, qp.ux])
# Solve with mosek
prob = mpbpy.QuadprogProblem(qp.P, qp.q, Amosek, lmosek, umosek)
res = prob.solve(solver=mpbpy.MOSEK, verbose=False)
niter[i] = res.total_iter
time[i] = res.cputime
elif solver == 'ecos':
for i in range(n_prob):
# Model with CVXPY
# minimize 1/2 u.T * u + np.ones(m).T * v
# subject to -u - v <= Ax - b <= u + v
# 0 <= u <= 1
# v >= 0
n_var = qp.A_huber.shape[1]
m_var = qp.b_huber.shape[0]
x = cvxpy.Variable(n_var)
u = cvxpy.Variable(m_var)
v = cvxpy.Variable(m_var)
objective = cvxpy.Minimize(.5 *
cvxpy.quad_form(u, spa.eye(m_var)) +
np.ones(m_var) * v)
constraints = [
-u - v <= qp.A_huber * x - qp.b_huber,
qp.A_huber * x - qp.b_huber <= u + v, 0 <= u, u <= 1, v >= 0
]
problem = cvxpy.Problem(objective, constraints)
problem.solve(solver=cvxpy.ECOS, verbose=False)
# DEBUG: solve with MOSEK
# Amosek = spa.vstack([
# qp.A,
# spa.hstack([spa.csc_matrix((2*m, n)), spa.eye(2*m)])
# ]).tocsc()
# lmosek = np.hstack([qp.l, qp.lx])
# umosek = np.hstack([qp.u, qp.ux])
#
# # Solve with mosek
# prob = mpbpy.QuadprogProblem(qp.P, qp.q, Amosek, lmosek, umosek)
# res = prob.solve(solver=mpbpy.MOSEK, verbose=False)
# x_mosek = res.x[:n_var]
# Obtain time and number of iterations
time[i] = problem.solver_stats.setup_time + \
problem.solver_stats.solve_time
niter[i] = problem.solver_stats.num_iters
else:
raise ValueError('Solver not understood')
# Return statistics
return utils.Statistics(time), utils.Statistics(niter)
def solve(self, p):
if p.P is not None:
P = np.ascontiguousarray(p.P.todense())
if p.A is not None:
A = np.ascontiguousarray(p.A.todense())
if p.i_idx is not None:
raise ValueError('Cannot solve MIQPs with qpOASES')
# Define contiguous array vectors
q = np.ascontiguousarray(p.q)
l = np.ascontiguousarray(p.l)
u = np.ascontiguousarray(p.u)
# Create infinite arrays of bounds
lx = np.ascontiguousarray(-np.inf * np.ones(p.n))
ux = np.ascontiguousarray(np.inf * np.ones(p.n))
# Solve with qpOASES
qpoases_m = qpoases.PyQProblem(p.n, p.m)
options = qpoases.PyOptions()
if not self.options['verbose']:
options.printLevel = qpoases.PyPrintLevel.NONE
for param, value in self.options.items():
if param == 'verbose':
if value is False:
options.printLevel = qpoases.PyPrintLevel.NONE
elif param == 'cputime':
qpoases_cpu_time = np.array([value])
elif param == 'nWSR':
qpoases_nWSR = np.array([value])
else:
exec("options.%s = %s" % (param, value))
qpoases_m.setOptions(options)
if 'cputime' not in self.options:
# Set default to max 10 seconds in runtime
qpoases_cpu_time = np.array([10.])
if 'nWSR' not in self.options:
# Set default to max 1000 working set recalculations
qpoases_nWSR = np.array([1000])
# Set number of working set recalculations
status = qpoases_m.init(P, q, A, lx, ux, l, u, qpoases_nWSR,
qpoases_cpu_time)
# Check status
status = self.STATUS_MAP.get(status, qp.SOLVER_ERROR)
# run_time
run_time = qpoases_cpu_time[0]
# number of iterations
niter = qpoases_nWSR[0]
if status in qp.SOLUTION_PRESENT:
x = np.zeros(p.n)
y_temp = np.zeros(p.n + p.m)
obj_val = qpoases_m.getObjVal()
qpoases_m.getPrimalSolution(x)
qpoases_m.getDualSolution(y_temp)
# Change sign and take only last part of y (No bounds on x in our formulation)
y = -y_temp[p.n:]
return QuadprogResults(status, obj_val, x, y, run_time, niter)
else:
return QuadprogResults(status, None, None, None, run_time, niter)
def main():
n = 2 # state dimension
m = 1 # input dimension
p = 1 # output dimension
N = 10 # number of lookahead steps
dt = 0.1
tf = 10.0
num_steps = int(tf / dt)
pid = PID(Kp=1, Ki=0.1, Kd=1)
# x+ = Ax + Bu
# y = Cx
A = np.matrix([[1, dt], [0, 1]])
B = np.matrix([[0], [dt]])
C = np.matrix([1, 0])
x0 = np.matrix([[0], [0]])
sys1 = LinearSystem(x0, A, B, C)
sys2 = LinearSystem(x0, A, B, C)
# V = x'Qx + u'Ru
Q = matlib.eye(p) * 10
R = matlib.eye(p)
lb = np.ones(N) * -1.0
ub = np.ones(N) * 1.0
# nWSR = np.array([100])
nWSR = 100
t = np.zeros(num_steps)
x1 = np.zeros(sys1.n * num_steps)
x2 = np.zeros(sys2.n * num_steps)
yd = np.zeros(sys1.p * num_steps)
qp = qpoases.PyQProblemB(sys2.m * N)
options = qpoases.PyOptions()
options.printLevel = qpoases.PyPrintLevel.NONE
qp.setOptions(options)
Yd = np.matrix([[step(t[0] + dt * j)] for j in range(N)])
H, g = sys2.lookahead(x0, Yd, Q, R, N)
qp.init(H, g, lb, ub, nWSR)
for i in range(num_steps - 1):
# desired trajectory
yd[i] = step(t[i])
# PID control
x0 = np.matrix(x1[i * n:(i + 1) * n]).T
e = yd[i] - sys1.C * x0
u = pid.control(e, dt)
sys1.apply(u)
sys1.step()
# MPC
x0 = np.matrix(x2[i * n:(i + 1) * n]).T
Yd = np.matrix([[step(t[i] + dt * j)] for j in range(N)])
_, g = sys2.lookahead(x0, Yd, Q, R, N)
qp.hotstart(g, lb, ub, nWSR)
U = np.zeros(sys2.m * N)
qp.getPrimalSolution(U)
u = U[0]
sys2.apply(u)
sys2.step()
x1[(i + 1) * sys1.n:(i + 2) * sys1.n] = np.array(sys1.x).flatten()
x2[(i + 1) * sys2.n:(i + 2) * sys2.n] = np.array(sys2.x).flatten()
t[i + 1] = t[i] + dt
plt.subplot(211)
plt.plot(t, yd, label='desired')
plt.plot(t, x1[::2], label='actual')
plt.title('PID control')
plt.legend()
plt.subplot(212)
plt.plot(t, yd, label='desired')
plt.plot(t, x2[::2], label='actual')
plt.title('MPC')
plt.legend()
plt.xlabel('Time (s)')
plt.show()
def solve_problem(qp_matrices, n_prob, solver='osqp'):
"""
Solve equality constrained optimization
"""
# Shorter name for qp_matrices
qp = qp_matrices
# Get dimensions
n = len(qp.q)
m = len(qp.l)
print('n = %d and solver %s' % (n, solver))
# Initialize time vector
time = np.zeros(n_prob)
# Initialize number of iterations vector
niter = np.zeros(n_prob)
if solver == 'osqp':
for i in range(n_prob):
# Setup OSQP
m = osqp.OSQP()
m.setup(qp.P, qp.q, qp.A, qp.l, qp.u,
rho=1000, # Set high rho to enforce feasibility
auto_rho=False,
polish=False,
verbose=False)
# Solve
results = m.solve()
x = results.x
y = results.y
status = results.info.status_val
niter[i] = results.info.iter
time[i] = results.info.run_time
# Check if status correct
if status != m.constant('OSQP_SOLVED'):
import ipdb; ipdb.set_trace()
# raise ValueError('OSQP did not solve the problem!')
# DEBUG
# solve with gurobi
# prob = mpbpy.QuadprogProblem(qp.P, q, Aosqp, losqp, uosqp)
# res = prob.solve(solver=mpbpy.GUROBI, verbose=False)
# print('Norm difference OSQP-GUROBI %.3e' %
# np.linalg.norm(x - res.x))
# import ipdb; ipdb.set_trace()
elif solver == 'qpoases':
n_dim = qp.P.shape[0] # Number of variables
m_dim = qp.A.shape[0] # Number of constraints without bounds
for i in range(n_prob):
# Initialize qpoases and set options
qpoases_m = qpoases.PyQProblem(n_dim, m_dim)
options = qpoases.PyOptions()
options.printLevel = qpoases.PyPrintLevel.NONE
qpoases_m.setOptions(options)
# Reset cpu time
qpoases_cpu_time = np.array([60.])
# Reset number of of working set recalculations
nWSR = np.array([10000])
# First iteration
res_qpoases = qpoases_m.init(np.ascontiguousarray(qp.P.todense()),
np.ascontiguousarray(qp.q),
np.ascontiguousarray(qp.A.todense()),
np.ascontiguousarray(qp.lx),
np.ascontiguousarray(qp.ux),
np.ascontiguousarray(qp.l),
np.ascontiguousarray(qp.u),
nWSR, qpoases_cpu_time)
# # DEBUG Solve with gurobi
# qpoases solution
# sol_qpoases = np.zeros(n + k)
# qpoases_m.getPrimalSolution(sol_qpoases)
# import mathprogbasepy as mpbpy
# Agrb = spa.vstack((qp.A,
# spa.hstack((spa.eye(n), spa.csc_matrix((n, k)))
# ))).tocsc()
# lgrb = np.append(qp.l, qp.lx)
# ugrb = np.append(qp.u, qp.ux)
# prob = mpbpy.QuadprogProblem(spa.csc_matrix(qp.P), q,
# Agrb, lgrb, ugrb)
# res = prob.solve(solver=mpbpy.GUROBI, verbose=True)
# print("Norm difference x qpoases - GUROBI = %.4f" %
# np.linalg.norm(sol_qpoases - res.x))
# print("Norm difference objval qpoases - GUROBI = %.4f" %
# abs(qpoases_m.getObjVal() - res.obj_val))
# import ipdb; ipdb.set_trace()
if res_qpoases != 0:
raise ValueError('qpoases did not solve the problem!')
# Save time
time[i] = qpoases_cpu_time[0]
# Save number of iterations
niter[i] = nWSR[0]
elif solver == 'gurobi':
for i in range(n_prob):
# Solve with gurobi
prob = mpbpy.QuadprogProblem(qp.P, qp.q, qp.A, qp.l, qp.u)
res = prob.solve(solver=mpbpy.GUROBI, verbose=False)
# Save time
time[i] = res.cputime
# Save number of iterations
niter[i] = res.total_iter
elif solver == 'mosek':
for i in range(n_prob):
# Solve with mosek
prob = mpbpy.QuadprogProblem(qp.P, qp.q, qp.A, qp.l, qp.u)
res = prob.solve(solver=mpbpy.MOSEK, verbose=False)
# Save time
time[i] = res.cputime
# Save number of iterations
niter[i] = res.total_iter
elif solver == 'ecos':
for i in range(n_prob):
# Solve with ECOS (via CVXPY)
n_var = qp.P.shape[0]
x_var = cvxpy.Variable(n_var)
objective = cvxpy.Minimize(.5 * cvxpy.quad_form(x_var, qp.P) + qp.q * x_var)
constraints = [qp.A * x_var <= qp.u, qp.A * x_var >= qp.l]
problem = cvxpy.Problem(objective, constraints)
problem.solve(solver=cvxpy.ECOS)
# Obtain time and number of iterations
time[i] = problem.solver_stats.setup_time + \
problem.solver_stats.solve_time
niter[i] = problem.solver_stats.num_iters
else:
raise ValueError('Solver not understood')
# Return statistics
return utils.Statistics(time), utils.Statistics(niter)
def getVector_S(self):
R_top = np.zeros(shape=((self.M * self.N), (self.M + self.N)))
R_btm = np.zeros(shape=((self.M * self.N), (self.M + self.N)))
v = np.zeros(shape=(self.M * self.N))
grid_saliency = self.get_gridSM()
#print("GRID SALIENCY")
for k in range(0, (self.M * self.N)):
for l in range(0, (self.M + self.N)):
if l == (self.r(k)):
R_top[k][l] = (grid_saliency[
(self.r(k)) - 1][(self.c(k)) - 1]) * (self.M / self.H)
else:
R_top[k][l] = 0
for k in range(0, (self.M * self.N)):
for l in range(0, (self.M + self.N)):
if l == (self.M + (self.c(k))):
R_btm[k][l] = (grid_saliency[
(self.r(k)) - 1][(self.c(k)) - 1]) * (self.N / self.W)
else:
R_btm[k][l] = 0
for k in range(0, (self.M * self.N)):
v[k] = grid_saliency[(self.r(k)) - 1][(self.c(k)) - 1]
Q1 = np.concatenate((R_top, R_btm), axis=0)
Qt = Q1.transpose()
Q = np.matmul(Qt, Q1)
b1 = np.concatenate((R_top, R_btm), axis=0)
bt = b1.transpose()
v1 = np.concatenate((v, v), axis=0)
b = -2 * (np.matmul(bt, v1))
G1 = np.matrix(np.identity(self.M + self.N))
G1 = -1 * G1
Gq = -1 * G1
h1 = np.zeros(shape=(self.M + self.N))
hzero = h1
for i in range(self.M):
h1[i] = 0.1 * (self.H / self.M)
#h1[i] = (self.Hr/self.H)*(self.H/self.M)
for i in range(self.M, (self.M + self.N)):
h1[i] = 0.1 * (self.W / self.N)
#h1[i] = (self.Wr/self.W)*(self.W/self.N)
h1 = np.array([h1])
ht = -1 * (h1.T)
hq = h1.T
a_tmp = np.zeros(shape=((self.M + self.N - 2), (self.M + self.N)))
a_row = np.zeros(shape=(self.M + self.N))
a_col = np.ones(shape=(self.M + self.N))
for i in range((int)((self.M + self.N) / 2)):
a_row[i] = 1
a_col[i] = 0
a = np.vstack((a_row, a_col))
aq = np.vstack((a, a_tmp))
bq = np.zeros(shape=(self.M + self.N))
bq[0] = self.Hr
bq[1] = self.Wr
b1 = np.array([self.Hr, self.Wr])
b1 = np.array([b1])
bt = b1.T
q1 = b.transpose()
P = cvxopt.matrix(Q)
q = cvxopt.matrix(q1)
G = cvxopt.matrix(G1)
h = cvxopt.matrix(ht)
A = cvxopt.matrix(a)
b = cvxopt.matrix(bt)
sol = cvxopt.solvers.qp(P, q, G, h, A, b)
S1 = sol['x']
S = np.array(cvxopt.matrix(S1))
E1 = sol['primal objective']
hq = hq.reshape((8, ))
bq = bq.reshape((8, ))
#C = np.vstack([Gq,aq])
#C = aq
C = a
lbh = np.zeros(shape=(self.M + self.N))
for i in range(self.M):
lbh[i] = 0.1 * (self.H / self.M)
for i in range(self.M, (self.M + self.N)):
lbh[i] = 0.1 * (self.W / self.N)
bC = 10 * (np.ones(shape=(self.M + self.N)))
bC[0] = self.Hr / self.M
bC[1] = self.Wr / self.N
lbC = np.array([self.Hr, self.Wr])
ubC = lbC
lb = lbh.T
ub = hq
itr = np.array([50])
options = qpoases.PyOptions()
qp = qpoases.PyQProblem(Q.shape[0], C.shape[0])
qp.setOptions(options)
#print("P", Q)
#print("q", q1)
#print("C", C)
#print("lbC", lbC)
#print("ubC", ubC)
#print("lb", lb)
#print("ub", ub)
R = qp.init(Q, q1, C, lb, ub, lbC, ubC, itr)
x_opt = np.ones(shape=(Q.shape[0], ))
ret = qp.getPrimalSolution(x_opt)
objVal = qp.getObjVal()
vector = np.array(x_opt)
print(vector)
print("############################")
#print(vector[0],vector[1], vector[2], vector[3])
#print(vector[4],vector[5], vector[6], vector[7])
#print("************************")
#print(S)
print("############################")
#print(S[0][0], S[1][0], S[2][0], S[3][0])
#print(S[4][0], S[5][0], S[6][0], S[7][0])
print(S)
print(E1)
return (S)
def solve_loop(qp_matrices, solver='osqp'):
"""
Solve portfolio optimization loop for all gammas
"""
# Shorter name for qp_matrices
qp = qp_matrices
# Get dimensions
n = len(qp.lx)
k = len(qp.l) - 1
print('n = %d and solver %s' % (n, solver))
# Get number of problems to solve
n_prob = qp.q.shape[1]
# Initialize time vector
time = np.zeros(n_prob)
# Initialize number of iterations vector
niter = np.zeros(n_prob)
if solver == 'osqp':
# Construct qp matrices
Aosqp = spa.vstack(
(qp.A, spa.hstack((spa.eye(n), spa.csc_matrix((n, k)))))).tocsc()
losqp = np.append(qp.l, qp.lx)
uosqp = np.append(qp.u, qp.ux)
# Setup OSQP
m = osqp.OSQP()
m.setup(qp.P,
qp.q[:, 0],
Aosqp,
losqp,
uosqp,
auto_rho=True,
polish=False,
verbose=False)
for i in range(n_prob):
q = qp.q[:, i]
# Update linear cost
m.update(q=q)
# Solve
results = m.solve()
x = results.x
y = results.y
status = results.info.status_val
niter[i] = results.info.iter
time[i] = results.info.run_time
# Check if status correct
if status != m.constant('OSQP_SOLVED'):
import ipdb
ipdb.set_trace()
raise ValueError('OSQP did not solve the problem!')
# DEBUG
# solve with gurobi
# prob = mpbpy.QuadprogProblem(qp.P, q, Aosqp, losqp, uosqp)
# res = prob.solve(solver=mpbpy.GUROBI, verbose=False)
# print('Norm difference OSQP-GUROBI %.3e' %
# np.linalg.norm(x - res.x))
# import ipdb; ipdb.set_trace()
elif solver == 'osqp_coldstart':
# Construct qp matrices
Aosqp = spa.vstack(
(qp.A, spa.hstack((spa.eye(n), spa.csc_matrix((n, k)))))).tocsc()
losqp = np.append(qp.l, qp.lx)
uosqp = np.append(qp.u, qp.ux)
# Setup OSQP
m = osqp.OSQP()
m.setup(qp.P,
qp.q[:, 0],
Aosqp,
losqp,
uosqp,
warm_start=False,
auto_rho=True,
polish=False,
verbose=False)
for i in range(n_prob):
q = qp.q[:, i]
# Update linear cost
m.update(q=q)
# Solve
results = m.solve()
x = results.x
y = results.y
status = results.info.status_val
niter[i] = results.info.iter
time[i] = results.info.run_time
# Check if status correct
if status != m.constant('OSQP_SOLVED'):
import ipdb
ipdb.set_trace()
raise ValueError('OSQP did not solve the problem!')
# DEBUG
# solve with gurobi
# prob = mpbpy.QuadprogProblem(qp.P, q, Aosqp, losqp, uosqp)
# res = prob.solve(solver=mpbpy.GUROBI, verbose=False)
# print('Norm difference OSQP-GUROBI %.3e' %
# np.linalg.norm(x - res.x))
# import ipdb; ipdb.set_trace()
# DEBUG print iterations per value of gamma
# gamma_vals = np.logspace(-2, 2, 101)[::-1]
#
# import matplotlib.pylab as plt
# plt.figure()
# ax = plt.gca()
# plt.plot(gamma_vals, niter)
# ax.set_xlabel(r'$\gamma$')
# ax.set_ylabel(r'iter')
# plt.show(block=False)
# import ipdb; ipdb.set_trace()
elif solver == 'osqp_no_caching':
# Construct qp matrices
Aosqp = spa.vstack(
(qp.A, spa.hstack((spa.eye(n), spa.csc_matrix((n, k)))))).tocsc()
losqp = np.append(qp.l, qp.lx)
uosqp = np.append(qp.u, qp.ux)
for i in range(n_prob):
# Setup OSQP
m = osqp.OSQP()
m.setup(qp.P,
qp.q[:, i],
Aosqp,
losqp,
uosqp,
warm_start=False,
auto_rho=True,
polish=False,
verbose=False)
# Solve
results = m.solve()
x = results.x
y = results.y
status = results.info.status_val
niter[i] = results.info.iter
time[i] = results.info.run_time
# Check if status correct
if status != m.constant('OSQP_SOLVED'):
import ipdb
ipdb.set_trace()
raise ValueError('OSQP did not solve the problem!')
# DEBUG
# solve with gurobi
# prob = mpbpy.QuadprogProblem(qp.P, q, Aosqp, losqp, uosqp)
# res = prob.solve(solver=mpbpy.GUROBI, verbose=False)
# print('Norm difference OSQP-GUROBI %.3e' %
# np.linalg.norm(x - res.x))
# import ipdb; ipdb.set_trace()
# DEBUG print iterations per value of gamma
# gamma_vals = np.logspace(-2, 2, 101)[::-1]
#
# import matplotlib.pylab as plt
# plt.figure()
# ax = plt.gca()
# plt.plot(gamma_vals, niter)
# ax.set_xlabel(r'$\gamma$')
# ax.set_ylabel(r'iter')
# plt.show(block=False)
# import ipdb; ipdb.set_trace()
elif solver == 'qpoases':
n_dim = qp.P.shape[0] # Number of variables
m_dim = qp.A.shape[0] # Number of constraints without bounds
# Initialize qpoases and set options
qpoases_m = qpoases.PyQProblem(n_dim, m_dim)
options = qpoases.PyOptions()
options.printLevel = qpoases.PyPrintLevel.NONE
qpoases_m.setOptions(options)
# Construct bounds for qpoases
lx = np.append(qp.lx, -np.inf * np.ones(k))
ux = np.append(qp.ux, np.inf * np.ones(k))
# Setup matrix P and A
P = np.ascontiguousarray(qp.P.todense())
A = np.ascontiguousarray(qp.A.todense())
for i in range(n_prob):
# Get linera cost as contiguous array
q = np.ascontiguousarray(qp.q[:, i])
# Reset cpu time
qpoases_cpu_time = np.array([20.])
# Reset number of of working set recalculations
nWSR = np.array([1000])
if i == 0:
# First iteration
res_qpoases = qpoases_m.init(P, q, A, np.ascontiguousarray(lx),
np.ascontiguousarray(ux),
np.ascontiguousarray(qp.l),
np.ascontiguousarray(qp.u), nWSR,
qpoases_cpu_time)
else:
# Solve new hot started problem
res_qpoases = qpoases_m.hotstart(q, np.ascontiguousarray(lx),
np.ascontiguousarray(ux),
np.ascontiguousarray(qp.l),
np.ascontiguousarray(qp.u),
nWSR, qpoases_cpu_time)
# # DEBUG Solve with gurobi
# qpoases solution
# sol_qpoases = np.zeros(n + k)
# qpoases_m.getPrimalSolution(sol_qpoases)
# import mathprogbasepy as mpbpy
# Agrb = spa.vstack((qp.A,
# spa.hstack((spa.eye(n), spa.csc_matrix((n, k)))
# ))).tocsc()
# lgrb = np.append(qp.l, qp.lx)
# ugrb = np.append(qp.u, qp.ux)
# prob = mpbpy.QuadprogProblem(spa.csc_matrix(qp.P), q,
# Agrb, lgrb, ugrb)
# res = prob.solve(solver=mpbpy.GUROBI, verbose=True)
# print("Norm difference x qpoases - GUROBI = %.4f" %
# np.linalg.norm(sol_qpoases - res.x))
# print("Norm difference objval qpoases - GUROBI = %.4f" %
# abs(qpoases_m.getObjVal() - res.obj_val))
# import ipdb; ipdb.set_trace()
if res_qpoases != 0:
raise ValueError('qpoases did not solve the problem!')
# Save time
time[i] = qpoases_cpu_time[0]
# Save number of iterations
niter[i] = nWSR[0]
elif solver == 'gurobi':
# Construct qp matrices
Agurobi = spa.vstack(
(qp.A, spa.hstack((spa.eye(n), spa.csc_matrix((n, k)))))).tocsc()
lgurobi = np.append(qp.l, qp.lx)
ugurobi = np.append(qp.u, qp.ux)
for i in range(n_prob):
# Get linera cost as contiguous array
q = qp.q[:, i]
# Solve with gurobi
prob = mpbpy.QuadprogProblem(qp.P, q, Agurobi, lgurobi, ugurobi)
res = prob.solve(solver=mpbpy.GUROBI, verbose=False)
# Save time
time[i] = res.cputime
# Save number of iterations
niter[i] = res.total_iter
elif solver == 'mosek':
# Construct qp matrices
Amosek = spa.vstack(
(qp.A, spa.hstack((spa.eye(n), spa.csc_matrix((n, k)))))).tocsc()
lmosek = np.append(qp.l, qp.lx)
umosek = np.append(qp.u, qp.ux)
for i in range(n_prob):
# Get linera cost as contiguous array
q = qp.q[:, i]
# Solve with mosek
prob = mpbpy.QuadprogProblem(qp.P, q, Amosek, lmosek, umosek)
res = prob.solve(solver=mpbpy.MOSEK, verbose=False)
# Save time
time[i] = res.cputime
# Save number of iterations
niter[i] = res.total_iter
elif solver == 'ecos':
for i in range(n_prob):
# Construct the problem
# minimize x' D x + y' I y - (1/gamma) * mu' x
# subject to 1' x = 1
# F' x = y
# 0 <= x <= 1
n_var = qp.F.shape[0]
m_var = qp.F.shape[1]
x = cvxpy.Variable(n_var)
y = cvxpy.Variable(m_var)
objective = cvxpy.Minimize(
cvxpy.quad_form(x, qp.D) + cvxpy.quad_form(y, spa.eye(m_var)) +
-1 / qp.gammas[i] * qp.mu * x)
constraints = [
np.ones(n_var) * x == 1, qp.F.T * x == y, 0 <= x, x <= 1
]
problem = cvxpy.Problem(objective, constraints)
problem.solve(solver=cvxpy.ECOS, verbose=False)
# Obtain time and number of iterations
time[i] = problem.solver_stats.setup_time + \
problem.solver_stats.solve_time
niter[i] = problem.solver_stats.num_iters
# # DEBUG: Solve with MOSEK
# Amosek = spa.vstack((qp.A,
# spa.hstack((spa.eye(n), spa.csc_matrix((n, k)))
# ))).tocsc()
# lmosek = np.append(qp.l, qp.lx)
# umosek = np.append(qp.u, qp.ux)
# prob = mpbpy.QuadprogProblem(qp.P, qp.q[:, i],
# Amosek, lmosek, umosek)
# res = prob.solve(solver=mpbpy.MOSEK, verbose=False)
# x_mosek = res.x[:n_var]
# import ipdb; ipdb.set_trace()
else:
raise ValueError('Solver not understood')
# Return statistics
return utils.Statistics(time), utils.Statistics(niter)
