当前位置: 首页>>代码示例>>Python>>正文


Python qpoases.PyOptions类代码示例

本文整理汇总了Python中qpoases.PyOptions的典型用法代码示例。如果您正苦于以下问题:Python PyOptions类的具体用法?Python PyOptions怎么用?Python PyOptions使用的例子?那么, 这里精选的类代码示例或许可以为您提供帮助。


在下文中一共展示了PyOptions类的13个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的Python代码示例。

示例1: qpoases_solve_qp

# A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more
# details.
#
# You should have received a copy of the GNU Lesser General Public License
# along with qpsolvers. If not, see <http://www.gnu.org/licenses/>.

from numpy import array, hstack, ones, vstack, zeros
from qpoases import PyOptions as Options
from qpoases import PyPrintLevel as PrintLevel
from qpoases import PyQProblem as QProblem
from qpoases import PyQProblemB as QProblemB
from qpoases import PyReturnValue as ReturnValue


__infty = 1e10
options = Options()
options.printLevel = PrintLevel.NONE


def qpoases_solve_qp(P, q, G=None, h=None, A=None, b=None, initvals=None,
                     max_wsr=1000):
    """
    Solve a Quadratic Program defined as:

        minimize
            (1/2) * x.T * P * x + q.T * x

        subject to
            G * x <= h
            A * x == b
开发者ID:stephane-caron,项目名称:oqp,代码行数:30,代码来源:qpoases_.py

示例2: QProblem

    def test_example7(self):
        H   = np.array([ 0.8514828085899353, -0.15739890933036804, -0.081726007163524628, -0.530426025390625, 0.16773293912410736,
                        -0.15739890933036804, 1.1552412509918213, 0.57780224084854126, -0.0072606131434440613, 0.010559185408055782,
                        -0.081726007163524628, 0.57780224084854126, 0.28925251960754395, 5.324830453901086e-006, -3.0256599075073609e-006,
                        -0.530426025390625, -0.0072606131434440613, 5.324830453901086e-006, 0.35609596967697144, -0.15124998986721039,
                         0.16773293912410736, 0.010559185408055782, -3.0256599075073609e-006, -0.15124998986721039,
                         0.15129712224006653], dtype=float).reshape((5, 5))
        g   = np.array([0.30908384919166565, 0.99325823783874512, 0.49822014570236206, -0.26309865713119507, 0.024296050891280174], dtype=float).reshape((5,))
        A   = np.array([1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1], dtype=float).reshape((5, 5))
        lb  = np.array([-0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359938621520996], dtype=float).reshape((5,))
        ub  = np.array([ 0.052359879016876221, 0.052359879016876221, 0.052359879016876221, 0, 0], dtype=float).reshape((5,))
        lbA = np.array([-0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359879016876221, -0.052359938621520996], dtype=float).reshape((5,))
        ubA = np.array([0.052359879016876221, 0.052359879016876221, 0.052359879016876221, 0, 0], dtype=float).reshape((5,))

        # Setting up QProblem object.
        qp = QProblem(5, 5)
        options = Options()
        options.printLevel = PrintLevel.NONE
        qp.setOptions(options)

        # Solve first QP.
        nWSR = 100
        qp.init(H, g, A, lb, ub, lbA, ubA,  nWSR)

        result = np.zeros((5,))
        qp.getPrimalSolution(result)
开发者ID:bmagyar,项目名称:OpenSoT,代码行数:26,代码来源:test_examples.py

示例3: get_nfail

    def test_m44_reliable_sparse(self):
        test_name = 'mm44_reliable_sparse.txt'
        print(test_name)

        # QP Options
        options = Options()
        options.setToReliable()
        options.printLevel = PrintLevel.NONE

        isSparse = True
        useHotstarts = False

        # run QP benchmarks
        results = run_benchmarks(benchmarks, options, isSparse, useHotstarts,
                                 self.nWSR, self.cpu_time, self.TOL)

        # print and write results
        string = results2str(results)
        print(string)
        write_results(test_name, string)

        assert get_nfail(results) <= 0, 'One ore more tests failed.'
开发者ID:Ch-Sh,项目名称:StructuralInspectionPlanner,代码行数:22,代码来源:test_testbench.py

示例4: solveLeastSquare

def solveLeastSquare(A, b, lb=None, ub=None, A_in=None, lb_in=None, ub_in=None):
    n                   = A.shape[1];
    m_in                = 0;
    if(A_in!=None):
        m_in = A_in.shape[0];
        if(lb_in==None):
            lb_in           = np.array(m_in*[-1e99]);
        if(ub_in==None):
            ub_in           = np.array(m_in*[1e99]);
            
    if(lb==None):
        lb                  = np.array(n*[-1e99]);
    if(ub==None):
        ub                  = np.array(n*[1e99]);
        
    Hess                = np.dot(A.transpose(),A);
    grad                = -np.dot(A.transpose(),b);
    maxActiveSetIter    = np.array([100+2*m_in+2*n]);
    maxComputationTime  = np.array([600.0]);
    options             = Options();
    options.printLevel  = PrintLevel.LOW; #NONE, LOW, MEDIUM
    options.enableRegularisation = True;
    print 'Gonna solve QP...';
    if(m_in==0):
        qpOasesSolver       = QProblemB(n); #, HessianType.SEMIDEF);
        qpOasesSolver.setOptions(options);
        imode = qpOasesSolver.init(Hess, grad, lb, ub, maxActiveSetIter, maxComputationTime);
    else:
        qpOasesSolver       = SQProblem(n, m_in); #, HessianType.SEMIDEF);
        qpOasesSolver.setOptions(options);
        imode = qpOasesSolver.init(Hess, grad, A_in, lb, ub, lb_in, ub_in, maxActiveSetIter, maxComputationTime);
#    print 'QP solved in %f seconds and %d iterations' % (maxComputationTime[0],maxActiveSetIter[0]);
    if(imode!=0 and imode!=63):
        print "ERROR Qp oases %d " % (imode);    
    x_norm  = np.zeros(n);  # solution of the normalized problem
    qpOasesSolver.getPrimalSolution(x_norm);
    return x_norm;
开发者ID:andreadelprete,项目名称:sot-torque-control,代码行数:37,代码来源:identify_motor_dynamic.py

示例5: test_id_hessian

    def test_id_hessian(self):
        """Very simple example for testing qpOASES (using QProblem class)."""

        path = os.path.join(testing_path, "dev_idhessian_data")

        #Setup data for QP.
        H   = np.loadtxt(os.path.join(path, "H.txt"))
        g   = np.loadtxt(os.path.join(path, "g.txt"))
        A   = np.loadtxt(os.path.join(path, "A.txt"))
        lb  = np.loadtxt(os.path.join(path, "lb.txt"))
        ub  = np.loadtxt(os.path.join(path, "ub.txt"))
        lbA = np.loadtxt(os.path.join(path, "lbA.txt"))
        ubA = np.loadtxt(os.path.join(path, "ubA.txt"))

        #Setting up QProblem object.
        qp = QProblem(72,144)

        options = Options()
        options.numRefinementSteps   = 1
        options.printLevel = PrintLevel.NONE

        #options.setToMPC()
        #options.setToReliable()
        #options.enableFlippingBounds = BooleanType.FALSE
        options.enableRamping = BooleanType.FALSE
        #options.enableRamping = BooleanType.TRUE
        #options.enableFarBounds = BooleanType.FALSE
        #options.enableRamping = BooleanType.FALSE
        #options.printLevel = PL_LOW
        #options.enableFullLITests = BooleanType.FALSE
        #options.boundRelaxation = 1.0e-1
        qp.setOptions( options )

        #Solve QP.
        nWSR = 1200

        qp.init(H, g, A, lb, ub, lbA, ubA, nWSR)
开发者ID:SemRoCo,项目名称:qpOASES,代码行数:37,代码来源:test_idhessian.py

示例6: __init__

    def __init__(
        self, N=16, T=0.1, T_step=0.8,
        fsm_state='D', fsm_sl=1
    ):
        """
        Initialize pattern generator matrices through base class
        and allocate two QPs one for optimzation of orientation and
        one for position of CoM and feet.

        """
        super(ClassicGenerator, self).__init__(
            N, T, T_step, fsm_state, fsm_sl
        )
        # TODO for speed up one can define members of BaseGenerator as
        #      direct views of QP data structures according to walking report
        #      Maybe do that later!

        # The pattern generator has to solve the following kind of
        # problem in each iteration

        # min_x 1/2 * x^T * H(w0) * x + x^T g(w0)
        # s.t.   lbA(w0) <= A(w0) * x <= ubA(w0)
        #         lb(w0) <=         x <= ub(wo)

        # Because of varying H and A, we have to use the
        # SQPProblem class, which supports this kind of QPs

        # rename for convenience
        N  = self.N
        nf = self.nf

        # define some qpOASES specific things
        self.cpu_time = 0.1 # upper bound on CPU time, 0 is no upper limit
        self.nwsr     = 100      # number of working set recalculations
        self.options = Options()
        self.options.setToMPC()
        self.options.printLevel = PrintLevel.LOW

        # FOR ORIENTATIONS
        # define dimensions
        self.ori_nv = 2*self.N
        self.ori_nc = (
              self.nc_fvel_eq
            + self.nc_fpos_ineq
            + self.nc_fvel_ineq
        )

        # setup problem
        self.ori_dofs = numpy.zeros(self.ori_nv)
        self.ori_qp = SQProblem(self.ori_nv, self.ori_nc)
        self.ori_qp.setOptions(self.options) # load NMPC options

        self.ori_H   =  numpy.zeros((self.ori_nv,self.ori_nv))
        self.ori_A   =  numpy.zeros((self.ori_nc,self.ori_nv))
        self.ori_g   =  numpy.zeros((self.ori_nv,))
        self.ori_lb  = -numpy.ones((self.ori_nv,))*1e+08
        self.ori_ub  =  numpy.ones((self.ori_nv,))*1e+08
        self.ori_lbA = -numpy.ones((self.ori_nc,))*1e+08
        self.ori_ubA =  numpy.ones((self.ori_nc,))*1e+08

        self._ori_qp_is_initialized = False

        # save computation time and working set recalculations
        self.ori_qp_nwsr    = 0.0
        self.ori_qp_cputime = 0.0

        # FOR POSITIONS
        # define dimensions
        self.pos_nv = 2*(self.N + self.nf)
        self.pos_nc = (
            self.nc_cop
            + self.nc_foot_position
            + self.nc_fchange_eq
        )

        # setup problem
        self.pos_dofs = numpy.zeros(self.pos_nv)
        self.pos_qp = SQProblem(self.pos_nv, self.pos_nc)
        self.pos_qp.setOptions(self.options)

        self.pos_H   = numpy.zeros((self.pos_nv,self.pos_nv))
        self.pos_A   = numpy.zeros((self.pos_nc,self.pos_nv))
        self.pos_g   = numpy.zeros((self.pos_nv,))
        self.pos_lb  = -numpy.ones((self.pos_nv,))*1e+08
        self.pos_ub  =  numpy.ones((self.pos_nv,))*1e+08
        self.pos_lbA = -numpy.ones((self.pos_nc,))*1e+08
        self.pos_ubA =  numpy.ones((self.pos_nc,))*1e+08

        self._pos_qp_is_initialized = False

        # save computation time and working set recalculations
        self.pos_qp_nwsr    = 0.0
        self.pos_qp_cputime = 0.0

        # dummy matrices
        self._ori_Q = numpy.zeros((2*self.N, 2*self.N))
        self._ori_p = numpy.zeros((2*self.N,))
        self._pos_Q = numpy.zeros((self.N + self.nf, self.N + self.nf))
        self._pos_p = numpy.zeros((self.N + self.nf,))

#.........这里部分代码省略.........
开发者ID:laas,项目名称:nmpc_walkgen,代码行数:101,代码来源:classic.py

示例7: ClassicGenerator

class ClassicGenerator(BaseGenerator):
    """
    Reimplementation of current state-of-the-art pattern
    generator for HRP-2 of CNRS-LAAS, Toulouse.

    Solve QPs for position and orientation of CoM and feet
    independently of each other in each timestep.
    First solve  for orientations, then solve for the postions.
    """
    def __init__(
        self, N=16, T=0.1, T_step=0.8,
        fsm_state='D', fsm_sl=1
    ):
        """
        Initialize pattern generator matrices through base class
        and allocate two QPs one for optimzation of orientation and
        one for position of CoM and feet.

        """
        super(ClassicGenerator, self).__init__(
            N, T, T_step, fsm_state, fsm_sl
        )
        # TODO for speed up one can define members of BaseGenerator as
        #      direct views of QP data structures according to walking report
        #      Maybe do that later!

        # The pattern generator has to solve the following kind of
        # problem in each iteration

        # min_x 1/2 * x^T * H(w0) * x + x^T g(w0)
        # s.t.   lbA(w0) <= A(w0) * x <= ubA(w0)
        #         lb(w0) <=         x <= ub(wo)

        # Because of varying H and A, we have to use the
        # SQPProblem class, which supports this kind of QPs

        # rename for convenience
        N  = self.N
        nf = self.nf

        # define some qpOASES specific things
        self.cpu_time = 0.1 # upper bound on CPU time, 0 is no upper limit
        self.nwsr     = 100      # number of working set recalculations
        self.options = Options()
        self.options.setToMPC()
        self.options.printLevel = PrintLevel.LOW

        # FOR ORIENTATIONS
        # define dimensions
        self.ori_nv = 2*self.N
        self.ori_nc = (
              self.nc_fvel_eq
            + self.nc_fpos_ineq
            + self.nc_fvel_ineq
        )

        # setup problem
        self.ori_dofs = numpy.zeros(self.ori_nv)
        self.ori_qp = SQProblem(self.ori_nv, self.ori_nc)
        self.ori_qp.setOptions(self.options) # load NMPC options

        self.ori_H   =  numpy.zeros((self.ori_nv,self.ori_nv))
        self.ori_A   =  numpy.zeros((self.ori_nc,self.ori_nv))
        self.ori_g   =  numpy.zeros((self.ori_nv,))
        self.ori_lb  = -numpy.ones((self.ori_nv,))*1e+08
        self.ori_ub  =  numpy.ones((self.ori_nv,))*1e+08
        self.ori_lbA = -numpy.ones((self.ori_nc,))*1e+08
        self.ori_ubA =  numpy.ones((self.ori_nc,))*1e+08

        self._ori_qp_is_initialized = False

        # save computation time and working set recalculations
        self.ori_qp_nwsr    = 0.0
        self.ori_qp_cputime = 0.0

        # FOR POSITIONS
        # define dimensions
        self.pos_nv = 2*(self.N + self.nf)
        self.pos_nc = (
            self.nc_cop
            + self.nc_foot_position
            + self.nc_fchange_eq
        )

        # setup problem
        self.pos_dofs = numpy.zeros(self.pos_nv)
        self.pos_qp = SQProblem(self.pos_nv, self.pos_nc)
        self.pos_qp.setOptions(self.options)

        self.pos_H   = numpy.zeros((self.pos_nv,self.pos_nv))
        self.pos_A   = numpy.zeros((self.pos_nc,self.pos_nv))
        self.pos_g   = numpy.zeros((self.pos_nv,))
        self.pos_lb  = -numpy.ones((self.pos_nv,))*1e+08
        self.pos_ub  =  numpy.ones((self.pos_nv,))*1e+08
        self.pos_lbA = -numpy.ones((self.pos_nc,))*1e+08
        self.pos_ubA =  numpy.ones((self.pos_nc,))*1e+08

        self._pos_qp_is_initialized = False

        # save computation time and working set recalculations
#.........这里部分代码省略.........
开发者ID:laas,项目名称:nmpc_walkgen,代码行数:101,代码来源:classic.py

示例8: SQProblem

    def test_example2(self):
        # Example for qpOASES main function using the SQProblem class.
        #  Setup data of first QP.
        H = np.array([ 1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
        A = np.array([ 1.0, 1.0 ]).reshape((2,1))
        g = np.array([ 1.5, 1.0 ])
        lb = np.array([ 0.5, -2.0 ])
        ub = np.array([ 5.0, 2.0 ])
        lbA = np.array([ -1.0 ])
        ubA = np.array([ 2.0 ])

        #  Setup data of second QP.
        H_new = np.array([ 1.0, 0.5, 0.5, 0.5 ]).reshape((2,2))
        A_new = np.array([ 1.0, 5.0 ]).reshape((2,1))
        g_new = np.array([ 1.0, 1.5 ])
        lb_new = np.array([ 0.0, -1.0 ])
        ub_new = np.array([ 5.0, -0.5 ])
        lbA_new = np.array([ -2.0 ])
        ubA_new = np.array([ 1.0 ])

        #  Setting up SQProblem object and solution analyser.
        qp = SQProblem(2, 1)
        options = Options()
        options.printLevel = PrintLevel.NONE
        qp.setOptions(options)

        analyser = SolutionAnalysis()

        # get c++ solution from std
        cmd = os.path.join(bin_path, "example2")
        p = Popen(cmd, shell=True, stdout=PIPE)
        stdout, stderr = p.communicate()
        stdout = str(stdout).replace('\\n', '\n')
        stdout = stdout.replace("'", '')
        print(stdout)

        #  Solve first QP ...
        nWSR = 10
        qp.init(H, g, A, lb, ub, lbA, ubA, nWSR)

        #  ... and analyse it.
        maxKKTviolation = np.zeros(1)
        analyser.getMaxKKTviolation(qp, maxKKTviolation)
        print("maxKKTviolation: %e\n"%maxKKTviolation)
        actual = np.asarray(maxKKTviolation)

        pattern = re.compile(r'maxKKTviolation: (?P<maxKKTviolation>[0-9+-e.]*)')

        match = pattern.findall(stdout)
        expected = np.asarray(match[0], dtype=float)

        assert_almost_equal(actual, expected, decimal=7)

        #  Solve second QP ...
        nWSR = 10
        qp.hotstart(H_new, g_new, A_new,
                              lb_new, ub_new,
                              lbA_new, ubA_new, nWSR)

        #  ... and analyse it.
        analyser.getMaxKKTviolation(qp, maxKKTviolation)
        print("maxKKTviolation: %e\n"%maxKKTviolation)
        actual = np.asarray(maxKKTviolation)

        expected = np.asarray(match[1], dtype=float)

        assert_almost_equal(actual, expected, decimal=7)


        #  ------------ VARIANCE-COVARIANCE EVALUATION --------------------

        Var             = np.zeros(5*5)
        Primal_Dual_Var = np.zeros(5*5)

        Var.reshape((5,5))[0,0] = 1.
        Var.reshape((5,5))[1,1] = 1.

        #                  (  1   0   0   0   0   )
        #                  (  0   1   0   0   0   )
        #     Var     =    (  0   0   0   0   0   )
        #                  (  0   0   0   0   0   )
        #                  (  0   0   0   0   0   )


        analyser.getVarianceCovariance(qp, Var, Primal_Dual_Var)
        print('Primal_Dual_Var=\n', Primal_Dual_Var.reshape((5,5)))
        actual = Primal_Dual_Var.reshape((5,5))

        pattern = re.compile(r'Primal_Dual_VAR = (?P<VAR>.*)',
                             re.DOTALL)

        print(stdout)
        match = pattern.search(stdout)
        expected = match.group('VAR').strip().split("\n")
        expected = [x.strip().split() for x in expected]
        print(expected)
        expected = np.asarray(expected, dtype=float)

        assert_almost_equal(actual, expected, decimal=7)
开发者ID:bmagyar,项目名称:OpenSoT,代码行数:99,代码来源:test_examples.py

示例9: test_qp_setup_with_toy_example_hack_from_qpoases_manual

    def test_qp_setup_with_toy_example_hack_from_qpoases_manual(self):
        gen = ClassicGenerator()

        options = Options()
        options.printLevel = PrintLevel.LOW


        # define matrices from qpoases manual
        H   = numpy.array([ 1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
        A   = numpy.array([ 1.0, 1.0 ]).reshape((1,2))
        g   = numpy.array([ 1.5, 1.0 ])
        lb  = numpy.array([ 0.5, -2.0 ])
        ub  = numpy.array([ 5.0, 2.0 ])
        lbA = numpy.array([ -1.0 ])
        ubA = numpy.array([ 2.0 ])

        x = numpy.array([0.5, -1.5])
        f = -6.25e-02

        # hack pattern generator
        gen.ori_nv = 2
        gen.ori_nc = 1
        gen.ori_dofs = numpy.zeros((2,))
        gen.ori_qp = SQProblem(gen.ori_nv, gen.ori_nc)
        gen.ori_qp.setOptions(options)

        gen.ori_H   = H
        gen.ori_A   = A
        gen.ori_g   = g
        gen.ori_lb  = lb
        gen.ori_ub  = ub
        gen.ori_lbA = lbA
        gen.ori_ubA = ubA

        gen.pos_nv = 2
        gen.pos_nc = 1
        gen.pos_dofs = numpy.zeros((2,))
        gen.pos_qp = SQProblem(gen.pos_nv, gen.pos_nc)
        gen.pos_qp.setOptions(options)

        gen.pos_H   = H
        gen.pos_A   = A
        gen.pos_g   = g
        gen.pos_lb  = lb
        gen.pos_ub  = ub
        gen.pos_lbA = lbA
        gen.pos_ubA = ubA

        # test first qp solution
        gen._solve_qp()

        # get solution
        # NOTE post_process put entries into array that dont match anymore
        gen.pos_qp.getPrimalSolution(gen.pos_dofs)
        gen.ori_qp.getPrimalSolution(gen.ori_dofs)

        assert_allclose(gen.pos_dofs, x, rtol=RTOL, atol=ATOL)
        assert_allclose(gen.pos_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
        assert_allclose(gen.ori_dofs, x, rtol=RTOL, atol=ATOL)
        assert_allclose(gen.ori_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)

        # define matrices for warmstart
        H_new   = numpy.array([ 1.0, 0.5, 0.5, 0.5 ]).reshape((2,2))
        A_new   = numpy.array([ 1.0, 5.0 ]).reshape((1,2))
        g_new   = numpy.array([ 1.0, 1.5 ])
        lb_new  = numpy.array([ 0.0, -1.0 ])
        ub_new  = numpy.array([ 5.0, -0.5 ])
        lbA_new = numpy.array([ -2.0 ])
        ubA_new = numpy.array([ 1.0 ])

        x = numpy.array([0.5, -0.5])
        f = -1.875e-01

        # hack pattern generator
        gen.ori_H   = H_new
        gen.ori_A   = A_new
        gen.ori_g   = g_new
        gen.ori_lb  = lb_new
        gen.ori_ub  = ub_new
        gen.ori_lbA = lbA_new

        gen.pos_H   = H_new
        gen.pos_A   = A_new
        gen.pos_g   = g_new
        gen.pos_lb  = lb_new
        gen.pos_ub  = ub_new
        gen.pos_lbA = lbA_new

        # test qp warmstart
        gen._solve_qp()

        # get solution
        # NOTE post_process put entries into array that dont match anymore
        gen.pos_qp.getPrimalSolution(gen.pos_dofs)
        gen.ori_qp.getPrimalSolution(gen.ori_dofs)

        assert_allclose(gen.pos_dofs, x, rtol=RTOL, atol=ATOL)
        assert_allclose(gen.pos_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
        assert_allclose(gen.ori_dofs, x, rtol=RTOL, atol=ATOL)
        assert_allclose(gen.ori_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
开发者ID:laas,项目名称:nmpc_walkgen,代码行数:100,代码来源:test_classic.py

示例10: QProblemB

H   = np.array([1.0, 0.0, 0.0, 0.5 ]).reshape((2,2))
g   = np.array([1.5, 1.0 ])
lb  = np.array([0.5, -2.0])
ub  = np.array([5.0, 2.0 ])

# Setup data of second QP.

g_new   = np.array([1.0, 1.5])
lb_new  = np.array([0.0, -1.0])
ub_new  = np.array([5.0, -0.5])


# Setting up QProblemB object.
example = QProblemB(2)

options = Options()
options.enableFlippingBounds = BooleanType.FALSE
options.initialStatusBounds  = SubjectToStatus.INACTIVE
options.numRefinementSteps   = 1

example.setOptions(options)

# Solve first QP.
nWSR = np.array([10])
example.init(H, g, lb, ub, nWSR)
print("\nnWSR = %d\n\n"%nWSR)

# Solve second QP.
nWSR = np.array([10])
example.hotstart(g_new, lb_new, ub_new, nWSR)
print("\nnWSR = %d\n\n"% nWSR)
开发者ID:RobotXiaoFeng,项目名称:acado,代码行数:31,代码来源:example1b.py

示例11: QProblemB

    def test_example1b(self):
        """Example for qpOASES main function using the QProblemB class."""
        # Setup data of first QP.
        H = np.array([1.0, 0.0, 0.0, 0.5]).reshape((2, 2))
        g = np.array([1.5, 1.0])
        lb = np.array([0.5, -2.0])
        ub = np.array([5.0, 2.0])

        # Setup data of second QP.

        g_new = np.array([1.0, 1.5])
        lb_new = np.array([0.0, -1.0])
        ub_new = np.array([5.0, -0.5])

        # Setting up QProblemB object.
        qp = QProblemB(2)

        options = Options()
        # options.enableFlippingBounds = BooleanType.FALSE
        options.initialStatusBounds = SubjectToStatus.INACTIVE
        options.numRefinementSteps = 1
        options.enableCholeskyRefactorisation = 1
        options.printLevel = PrintLevel.NONE
        qp.setOptions(options)

        # Solve first QP.
        nWSR = 10
        qp.init(H, g, lb, ub, nWSR)

        xOpt_actual = np.zeros(2)
        qp.getPrimalSolution(xOpt_actual)
        xOpt_actual = np.asarray(xOpt_actual, dtype=float)
        objVal_actual = qp.getObjVal()
        objVal_actual = np.asarray(objVal_actual, dtype=float)
        print 'xOpt_actual:', xOpt_actual
        print 'objVal_actual:', objVal_actual

        # Solve second QP.
        nWSR = 10
        qp.hotstart(g_new, lb_new, ub_new, nWSR)

        xOpt_actual = np.zeros(2)
        qp.getPrimalSolution(xOpt_actual)
        xOpt_actual = np.asarray(xOpt_actual, dtype=float)
        objVal_actual = qp.getObjVal()
        objVal_actual = np.asarray(objVal_actual, dtype=float)
        print 'xOpt_actual:', xOpt_actual
        print 'objVal_actual:', objVal_actual

        # Get and print solution of second QP.
        xOpt_actual = np.zeros(2)
        qp.getPrimalSolution(xOpt_actual)
        xOpt_actual = np.asarray(xOpt_actual, dtype=float)
        objVal_actual = qp.getObjVal()
        objVal_actual = np.asarray(objVal_actual, dtype=float)

        cmd = os.path.join(bin_path, "example1b")
        p = Popen(cmd, shell=True, stdout=PIPE)
        stdout, stderr = p.communicate()
        stdout = str(stdout).replace('\\n', '\n')
        stdout = stdout.replace("'", '')

        # get c++ solution from std
        pattern = re.compile(r'xOpt\s*=\s*\[\s+(?P<xOpt>([0-9., e+-])*)\];')
        match = pattern.findall(stdout)
        xOpt_expected = match[-1][0]
        xOpt_expected = xOpt_expected.split(",")
        xOpt_expected = np.asarray(xOpt_expected, dtype=float)

        pattern = re.compile(r'objVal = (?P<objVal>[0-9-+e.]*)')
        match = pattern.findall(stdout)
        print match
        objVal_expected = match[-1]
        objVal_expected = np.asarray(objVal_expected, dtype=float)

        print("xOpt_actual =", xOpt_actual)
        print("xOpt_expected =", xOpt_expected)
        print("objVal_actual = ", objVal_actual)
        print("objVal_expected = ", objVal_expected)

        assert_almost_equal(xOpt_actual, xOpt_expected, decimal=7)
        assert_almost_equal(objVal_actual, objVal_expected, decimal=7)
开发者ID:RobotXiaoFeng,项目名称:acado,代码行数:82,代码来源:test_examples.py

示例12: __init__

    def __init__(
        self, N=16, T=0.1, T_step=0.8,
        fsm_state='D', fsm_sl=1
    ):
        """
        Initialize pattern generator matrices through base class
        and allocate two QPs one for optimzation of orientation and
        one for position of CoM and feet.

        """
        super(NMPCGenerator, self).__init__(
            N, T, T_step, fsm_state, fsm_sl
        )
        # The pattern generator has to solve the following kind of
        # problem in each iteration

        # min_x 1/2 * x.T * H(w0) * x + x.T g(w0)
        # s.t.   lbA(w0) <= A(w0) * x <= ubA(w0)
        #         lb(w0) <=         x <= ub(wo)

        # Because of varying H and A, we have to use the
        # SQPProblem class, which supports this kind of QPs

        # rename for convenience
        N  = self.N
        nf = self.nf

        # define some qpOASES specific things
        self.cpu_time = 0.1 # upper bound on CPU time, 0 is no upper limit
        self.nwsr     = 100 # # of working set recalculations
        self.options = Options()
        self.options.setToMPC()
        #self.options.printLevel = PrintLevel.LOW

        # define variable dimensions
        # variables of:     position + orientation
        self.nv = 2*(self.N+self.nf) + 2*N

        # define constraint dimensions
        self.nc_pos = (
              self.nc_cop
            + self.nc_foot_position
            + self.nc_fchange_eq
        )
        self.nc_ori = (
              self.nc_fvel_eq
            + self.nc_fpos_ineq
            + self.nc_fvel_ineq
        )
        self.nc = (
            # position
              self.nc_pos
            # orientation
            + self.nc_ori
        )

        # setup problem
        self.dofs = numpy.zeros(self.nv)
        self.qp   = SQProblem(self.nv, self.nc)

        # load NMPC options
        self.qp.setOptions(self.options)

        self.qp_H   =  numpy.eye(self.nv,self.nv)
        self.qp_A   =  numpy.zeros((self.nc,self.nv))
        self.qp_g   =  numpy.zeros((self.nv,))
        self.qp_lb  = -numpy.ones((self.nv,))*1e+08
        self.qp_ub  =  numpy.ones((self.nv,))*1e+08
        self.qp_lbA = -numpy.ones((self.nc,))*1e+08
        self.qp_ubA =  numpy.ones((self.nc,))*1e+08

        self._qp_is_initialized = False

        # save computation time and working set recalculations
        self.qp_nwsr    = 0.0
        self.qp_cputime = 0.0

        # setup analyzer for solution analysis
        analyser = SolutionAnalysis()

        # helper matrices for common expressions
        self.Hx     = numpy.zeros((1, 2*(N+nf)), dtype=float)
        self.Q_k_x  = numpy.zeros((N+nf, N+nf),  dtype=float)
        self.p_k_x  = numpy.zeros((N+nf,),       dtype=float)
        self.p_k_y  = numpy.zeros((N+nf,),       dtype=float)

        self.Hq     = numpy.zeros((1, 2*N), dtype=float)
        self.Q_k_qR = numpy.zeros((N, N),   dtype=float)
        self.Q_k_qL = numpy.zeros((N, N),   dtype=float)
        self.p_k_qR = numpy.zeros((N),      dtype=float)
        self.p_k_qL = numpy.zeros((N),      dtype=float)

        self.A_pos_x   = numpy.zeros((self.nc_pos, 2*(N+nf)), dtype=float)
        self.A_pos_q   = numpy.zeros((self.nc_pos, 2*N), dtype=float)
        self.ubA_pos = numpy.zeros((self.nc_pos,), dtype=float)
        self.lbA_pos = numpy.zeros((self.nc_pos,), dtype=float)

        self.A_ori   = numpy.zeros((self.nc_ori, 2*N), dtype=float)
        self.ubA_ori = numpy.zeros((self.nc_ori,),     dtype=float)
        self.lbA_ori = numpy.zeros((self.nc_ori,),     dtype=float)
#.........这里部分代码省略.........
开发者ID:laas,项目名称:nmpc_walkgen,代码行数:101,代码来源:combinedqp.py

示例13: NMPCGenerator

class NMPCGenerator(BaseGenerator):
    """
    Implementation of the combined problems using NMPC techniques.

    Solve QP for position and orientation of CoM and feet simultaneously in
    each timestep. Calculates derivatives and updates states in each step.
    """
    def __init__(
        self, N=16, T=0.1, T_step=0.8,
        fsm_state='D', fsm_sl=1
    ):
        """
        Initialize pattern generator matrices through base class
        and allocate two QPs one for optimzation of orientation and
        one for position of CoM and feet.

        """
        super(NMPCGenerator, self).__init__(
            N, T, T_step, fsm_state, fsm_sl
        )
        # The pattern generator has to solve the following kind of
        # problem in each iteration

        # min_x 1/2 * x.T * H(w0) * x + x.T g(w0)
        # s.t.   lbA(w0) <= A(w0) * x <= ubA(w0)
        #         lb(w0) <=         x <= ub(wo)

        # Because of varying H and A, we have to use the
        # SQPProblem class, which supports this kind of QPs

        # rename for convenience
        N  = self.N
        nf = self.nf

        # define some qpOASES specific things
        self.cpu_time = 0.1 # upper bound on CPU time, 0 is no upper limit
        self.nwsr     = 100 # # of working set recalculations
        self.options = Options()
        self.options.setToMPC()
        #self.options.printLevel = PrintLevel.LOW

        # define variable dimensions
        # variables of:     position + orientation
        self.nv = 2*(self.N+self.nf) + 2*N

        # define constraint dimensions
        self.nc_pos = (
              self.nc_cop
            + self.nc_foot_position
            + self.nc_fchange_eq
        )
        self.nc_ori = (
              self.nc_fvel_eq
            + self.nc_fpos_ineq
            + self.nc_fvel_ineq
        )
        self.nc = (
            # position
              self.nc_pos
            # orientation
            + self.nc_ori
        )

        # setup problem
        self.dofs = numpy.zeros(self.nv)
        self.qp   = SQProblem(self.nv, self.nc)

        # load NMPC options
        self.qp.setOptions(self.options)

        self.qp_H   =  numpy.eye(self.nv,self.nv)
        self.qp_A   =  numpy.zeros((self.nc,self.nv))
        self.qp_g   =  numpy.zeros((self.nv,))
        self.qp_lb  = -numpy.ones((self.nv,))*1e+08
        self.qp_ub  =  numpy.ones((self.nv,))*1e+08
        self.qp_lbA = -numpy.ones((self.nc,))*1e+08
        self.qp_ubA =  numpy.ones((self.nc,))*1e+08

        self._qp_is_initialized = False

        # save computation time and working set recalculations
        self.qp_nwsr    = 0.0
        self.qp_cputime = 0.0

        # setup analyzer for solution analysis
        analyser = SolutionAnalysis()

        # helper matrices for common expressions
        self.Hx     = numpy.zeros((1, 2*(N+nf)), dtype=float)
        self.Q_k_x  = numpy.zeros((N+nf, N+nf),  dtype=float)
        self.p_k_x  = numpy.zeros((N+nf,),       dtype=float)
        self.p_k_y  = numpy.zeros((N+nf,),       dtype=float)

        self.Hq     = numpy.zeros((1, 2*N), dtype=float)
        self.Q_k_qR = numpy.zeros((N, N),   dtype=float)
        self.Q_k_qL = numpy.zeros((N, N),   dtype=float)
        self.p_k_qR = numpy.zeros((N),      dtype=float)
        self.p_k_qL = numpy.zeros((N),      dtype=float)

        self.A_pos_x   = numpy.zeros((self.nc_pos, 2*(N+nf)), dtype=float)
#.........这里部分代码省略.........
开发者ID:laas,项目名称:nmpc_walkgen,代码行数:101,代码来源:combinedqp.py


注:本文中的qpoases.PyOptions类示例由纯净天空整理自Github/MSDocs等开源代码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。