def __init__(self,
n,
m_in,
solver='slsqp',
accuracy=1e-6,
maxIter=100,
verb=0):
self.name = "AbstractSolver"
self.n = n
self.iter = 0
self.solver = solver
self.accuracy = accuracy
self.maxIter = maxIter
self.verb = verb
self.qpOasesAnalyser = SolutionAnalysis()
self.changeInequalityNumber(m_in)
return
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)
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.
example = SQProblem(2, 1)
analyser = SolutionAnalysis()
# Solve first QP ...
nWSR = np.array([10])
example.init(H, g, A, lb, ub, lbA, ubA, nWSR)
# ... and analyse it.
maxStat = np.zeros(1)
maxFeas = np.zeros(1)
maxCmpl = np.zeros(1)
analyser.getKktViolation(example, maxStat, maxFeas, maxCmpl)
print("maxStat: %e, maxFeas:%e, maxCmpl: %e\n"%(maxStat, maxFeas, maxCmpl))
# Solve second QP ...
nWSR = np.array([10])
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)
self.derv_Acop_map = numpy.zeros((self.nc_cop, self.N), dtype=float)
self.derv_Afoot_map = numpy.zeros((self.nc_foot_position, self.N),
dtype=float)
self._update_foot_selection_matrix()
# add additional keys that should be saved
self._data_keys.append('qp_nwsr')
self._data_keys.append('qp_cputime')
# reinitialize plot data structure
self.data = PlotData(self)