Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def test_primal_and_dual_infeasible_problem(self):
self.n = 2
self.m = 4
self.P = spspa.csc_matrix((2, 2))
self.q = np.array([-1., -1.])
self.A = spspa.csc_matrix([[1., -1.], [-1., 1.], [1., 0.], [0., 1.]])
self.l = np.array([1., 1., 0., 0.])
self.u = np.inf * np.ones(self.m)
self.model = osqp.OSQP()
self.model.setup(P=self.P, q=self.q, A=self.A, l=self.l, u=self.u,
**self.opts)
res = self.model.solve()
# Assert close
self.assertEqual(res.info.status_val,
self.model.constant('OSQP_PRIMAL_INFEASIBLE'))
def test_polish_unconstrained(self):
# Unconstrained QP problem
sp.random.seed(4)
self.n = 30
self.m = 0
P = sparse.diags(np.random.rand(self.n)) + 0.2*sparse.eye(self.n)
self.P = P.tocsc()
self.q = np.random.randn(self.n)
self.A = sparse.csc_matrix((self.m, self.n))
self.l = np.array([])
self.u = np.array([])
self.model = osqp.OSQP()
self.model.setup(P=self.P, q=self.q, A=self.A, l=self.l, u=self.u,
**self.opts)
# Solve problem
res = self.model.solve()
# Assert close
nptest.assert_array_almost_equal(
res.x, np.array([
-0.61981415, -0.06174194, 0.83824061, -0.0595013, -0.17810828,
2.90550031, -1.8901713, -1.91191741, -3.73603446, 1.7530356,
-1.67018181, 3.42221944, 0.61263403, -0.45838347, -0.13194248,
2.95744794, 5.2902277, -1.42836238, -8.55123842, -0.79093815,
0.43418189, -0.69323554, 1.15967924, -0.47821898, 3.6108927,
0.03404309, 0.16322926, -2.17974795, 0.32458796, -1.97553574]))
nptest.assert_array_almost_equal(res.y, np.array([]))
import numpy as np
import scipy.sparse as spa
import osqp
np.random.seed(3)
n = 10
m = 20
P = spa.rand(n, n, density=.2, format='csc')
P = (P.T).dot(P)
q = np.random.randn(n)
A = spa.rand(m, n, density=.2, format='csc')
l = np.random.randn(m) - 5
u = np.random.randn(m) + 5
m = osqp.OSQP()
m.setup(P, q, A, l, u, rho=0.001)
# Test workspace return
m.codegen("code", 'Unix Makefiles', embedded=1, python_ext_name='test')
Gx = sp.kron(sp.eye(N), constr_E)
Gu = sp.kron(sp.eye(N), constr_D)
G = sp.block_diag([Gx, constr_F, Gu])
gl = np.hstack([np.tile(constr_el, N), constr_fl, np.tile(constr_dl, N)])
gu = np.hstack([np.tile(constr_eu, N), constr_fu, np.tile(constr_du, N)])
# Pass the data to OSQP
x = np.array([0., 0., 0.15, 0.]) # initial state: [p, p_dot, theta, theta_dot]
P = H
q = np.zeros((N+1)*n + N*m)
A = sp.vstack([M, G])
l = np.hstack([b(x, n, N), gl])
u = np.hstack([b(x, n, N), gu])
m = osqp.OSQP()
m.setup(P, q, A, l, u, rho=1e-1, sigma=1e-5)
# Generate the code
m.codegen("code", project_type="Makefile", embedded=1,
python_ext_name='emosqp', force_rewrite=True)
'''
Apply MPC in closed loop
'''
import emosqp
# Apply MPC to the system
sim_steps = 20
B = dyn_B.T.toarray()[0]
l = -sp.rand(m)
u = sp.rand(m)
random_scaling = np.power(10, 5*np.random.randn())
P = random_scaling * sparse.random(n, n, density=0.4)
P = P.dot(P.T).tocsc()
q = sp.randn(n)
osqp_opts = {'adaptive_rho_interval': 100, # to make C code not depend on timing
'check_termination': 1, # Check termination every iteration
'linsys_solver': 'mkl pardiso'
}
# OSQP
model = osqp.OSQP()
model.setup(P=P, q=q, A=A, l=l, u=u, **osqp_opts)
res = model.solve()
# OSQPPUREPY
model = osqppurepy.OSQP()
model.setup(P=P, q=q, A=A, l=l, u=u, linsys_solver=2, **osqp_opts)
res_purepy = model.solve()
print("Norm difference x OSQP and OSQPPUREPY %.4e" %
np.linalg.norm(res.x - res_purepy.x))
print("Norm difference y OSQP and OSQPPUREPY %.4e" %
np.linalg.norm(res.y - res_purepy.y))
# Apply first control input to the plant
u_sys[:, i] = res.x[-N*nu:-(N-1)*nu]
# x_{k+1} = Ax_{t} + Bu_{t}
x_sys[:, i+1] = problem.A.dot(x_sys[:, i]) + \
problem.B.dot(u_sys[:, i])
# Update linear constraints
self.update_initial_state(qp, x_sys[:, i+1])
# Change l and u
m.update(l=qp.l, u=qp.u)
elif solver == 'osqp_coldstart':
# Setup OSQP
m = osqp.OSQP()
m.setup(qp.P, qp.q, qp.A, qp.l, qp.u,
warm_start=False, **osqp_settings)
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
# Check if status correct
if status != m.constant('OSQP_SOLVED'):
print('OSQP did not solve the problem!')
uSS_PointSelectedTot = np.append(uSS_PointSelectedTot, uSS_PointSelected, axis=1)
SS_glob_PointSelectedTot = np.append(SS_glob_PointSelectedTot, SS_glob_PointSelected, axis=1)
Qfun_SelectedTot = np.append(Qfun_SelectedTot, Qfun_Selected, axis=0)
self.SS_PointSelectedTot = SS_PointSelectedTot
self.uSS_PointSelectedTot = uSS_PointSelectedTot
self.SS_glob_PointSelectedTot = SS_glob_PointSelectedTot
self.Qfun_SelectedTot = Qfun_SelectedTot
# Solve QP
solver = "osqp"
if solver == "osqp":
startTimer = datetime.datetime.now()
osqp = OSQP()
# Create Equality Constraints: G*x = E*x0 + L
OnesZeros = np.hstack(( np.ones(SS_PointSelectedTot.shape[1]), np.zeros(n) ))
G = np.vstack(( np.hstack((SS_PointSelectedTot, np.eye(n) )), OnesZeros ))
E = np.zeros((G.shape[0], n))
E[0:n,0:n] = np.eye(6)
L = np.zeros(( G.shape[0], 1 ))
L[n,:] = 1
# Create Inequality Constraints
numVariables = G.shape[1]
numLambda = numVariables-n
F = np.eye(numLambda, numVariables)
b_ub = np.ones(numLambda)
def __init__(self, data, settings, qp_settings=None):
self.data = data
self.settings = settings
# Setup OSQP solver instance
self.solver = osqp.OSQP()
self.qp_settings = qp_settings
if self.qp_settings is None:
self.qp_settings = {}
self.solver.setup(self.data.P, self.data.q, self.data.A,
self.data.l, self.data.u, **qp_settings)
# Define other internal variables
self.first_run = 1
self.iter_num = 1
self.osqp_solve_time = 0.
self.osqp_iter = 0
self.osqp_iter_avg = 0
self.lower_glob = -np.inf
self.status = MI_UNSOLVED
# Define root node
G : scipy.sparse.csc_matrix Linear inequality constraint matrix.
h : numpy.array Linear inequality constraint vector.
A : scipy.sparse.csc_matrix, optional Linear equality constraint matrix.
b : numpy.array, optional Linear equality constraint vector.
initvals : numpy.array, optional Warm-start guess vector.
Returns
-------
x : array, shape=(n,)
Solution to the QP, if found, otherwise ``None``.
Note
----
OSQP requires `P` to be symmetric, and won't check for errors otherwise.
Check out for this point if you e.g. `get nan values
`_ in your solutions.
"""
osqp = OSQP()
if G is not None:
l = -inf * ones(len(h))
if A is not None:
qp_A = vstack([G, A]).tocsc()
qp_l = hstack([l, b])
qp_u = hstack([h, b])
else: # no equality constraint
qp_A = G
qp_l = l
qp_u = h
osqp.setup(P=P, q=q, A=qp_A, l=qp_l, u=qp_u, verbose=False, polish=True)
else:
osqp.setup(P=P, q=q, A=None, l=None, u=None, verbose=False)
if initvals is not None:
osqp.warm_start(x=initvals)
res = osqp.solve()
# Load one problem
with open('./data/%s.pickle' % 'helicopter_scaling_small', 'rb') as f:
problem = pickle.load(f)
# OSQP settings
osqp_settings = {'verbose': True,
'scaling': True,
'scaling_iter': 50,
'early_terminate_interval': 1,
'auto_rho': True,
'rho': 0.1,
'polish': False}
# Solve with OSQP
model = osqp.OSQP()
model.setup(problem['P'], problem['q'], problem['A'],
problem['l'], problem['u'], **osqp_settings)
res_osqp = model.solve()
# Solve with GUROBI
import mathprogbasepy as mpbpy
qp = mpbpy.QuadprogProblem(problem['P'], problem['q'], problem['A'],
problem['l'], problem['u'])
res_gurobi = qp.solve(solver=mpbpy.GUROBI, verbose=False)
print("GUROBI time = %.4e" % res_gurobi.cputime)
print("OSQP time = %.4e" % res_osqp.info.run_time)