def solve(self, q, dq, pd, vd, A=None, lbA=None, ubA=None):
''' 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
# 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.PyQProblem(ni, A.shape[0] if A is not None else 0)
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]))
u = np.zeros(ni)
qp.getPrimalSolution(u)
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_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_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_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(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 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 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)
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)