本文整理汇总了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；未经允许，请勿转载。 |
客服电话
APP下载
官方微信
请发表评论