Python qpoases.PyOptions類代碼示例

# 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

    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)

    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.'

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;

    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)

    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,))

#.........這裏部分代碼省略.........

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
#.........這裏部分代碼省略.........

    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)

    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)

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)

    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)

    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)
#.........這裏部分代碼省略.........

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)
#.........這裏部分代碼省略.........