How to use the osqp.OSQP function in osqp

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)