Python PyOptions.setToMPC方法代码示例

# 需要导入模块: from qpoases import PyOptions [as 别名]
# 或者: from qpoases.PyOptions import setToMPC [as 别名]
    def test_m44_mpc_sparse(self):
        test_name ='mm44_mpc_sparse.txt'
        print("Test: ", test_name)

        # QP Options
        options = Options()
        options.setToMPC()
        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) <= 19, 'One ore more tests failed.'

# 需要导入模块: from qpoases import PyOptions [as 别名]
# 或者: from qpoases.PyOptions import setToMPC [as 别名]
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
#.........这里部分代码省略.........

# 需要导入模块: from qpoases import PyOptions [as 别名]
# 或者: from qpoases.PyOptions import setToMPC [as 别名]
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)
#.........这里部分代码省略.........