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 fit(self, X):
q, n = len(self.alphas), X.shape[0]
self.X = X
if self.kernel == 'gauss':
H = self.kernel_fun(X, gamma=self.gamma)
else:
H = self.kernel_fun(X, X)
H = 1 / 2 * (H + np.transpose(H))
H = repmat(H, q, q)
g = np.zeros(q * n)
A = np.zeros((q, q * n))
lbA = np.ones(q)
lb = np.zeros(q * n)
ub = np.ones(q * n)
for i in range(q):
start = i * n + 1
end = start + n - 1
ub[start:end] = 1 / (n * (1 - self.alphas[i]))
A[i, start:end] = 1
qp = QProblem(q * n, q)
if not self.verbose:
options = Options()
options.printLevel = PrintLevel.NONE
qp.setOptions(options)
suc = qp.init(H, g, A, lb, ub, lbA, lbA, self.max_iter)
if suc == ReturnValue.MAX_NWSR_REACHED:
msg = "qPOASES reached the maximum number of iterations ({}). ".format(
self.max_iter)
msg += "\nThe resulting regions may not be reliable"
print(msg)
etas = np.zeros(q * n)
qp.getPrimalSolution(etas)
etas = etas.reshape(q, n)
etastars = etas.sum(axis=0)
nus = 1 - self.alphas
SVidx = np.arange(len(etastars))[etastars > self.tol]
nSV = len(SVidx)
ub = 1 / n * nus
rhos = np.zeros(q)
for j, eta in enumerate(etas):
choose = np.logical_and(eta > self.tol, eta < ub[j])
hyperSVidx = np.arange(len(eta))[choose]
if len(hyperSVidx) == 0:
hyperSVidx = np.arange(len(eta))[eta > self.tol]
rhos[j] = max(
np.dot(H[hyperSVidx][:, SVidx], etastars[SVidx]) / q)
else:
rhos[j] = np.median(
np.dot(H[hyperSVidx][:, SVidx], etastars[SVidx]) / q)
self.rhos = rhos
self.etastars = etastars
tmp = np.dot(H[:, SVidx], etastars[SVidx]) / q
self.rhobounds = tmp.max(), tmp.min()
return self
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 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
using qpOASES <https://projects.coin-or.org/qpOASES>.
Parameters
----------
P : numpy.array
Symmetric quadratic-cost matrix.
q : numpy.array
Quadratic-cost vector.
G : numpy.array
Linear inequality constraint matrix.
h : numpy.array
Linear inequality constraint vector.
A : numpy.array, optional
Linear equality constraint matrix.
b : numpy.array, optional
Linear equality constraint vector.
initvals : numpy.array, optional
Warm-start guess vector.
max_wsr : integer, optional
Maximum number of Working-Set Recalculations given to qpOASES.
Returns
-------
x : numpy.array
Solution to the QP, if found, otherwise ``None``.
Note
----
This function relies on some updates from the standard distribution of
qpOASES (details below). A fully compatible repository is published at
<https://github.com/stephane-caron/qpOASES>. (Quick install instructions:
run ``make`` from the cloned repository, then go to interfaces/python and
run ``sudo python setup.py install``.)
Note
----
This function allows empty bounds (lb, ub, lbA or ubA). This was
provisioned by the C++ API but not by the Python API of qpOASES (as of
version 3.2.0). Be sure to update the Cython file (qpoases.pyx) to convert
``None`` to the null pointer.
"""
if initvals is not None:
print("qpOASES: note that warm-start values ignored by wrapper")
n = P.shape[0]
lb, ub = None, None
has_cons = G is not None or A is not None
if G is not None and A is None:
C = G
lb_C = None # NB:
ub_C = h
elif G is None and A is not None:
C = A
lb_C = b
ub_C = b
elif G is not None and A is not None:
C = vstack([G, A, A])
lb_C = hstack([-__infty * ones(h.shape[0]), b, b])
ub_C = hstack([h, b, b])
if has_cons:
qp = QProblem(n, C.shape[0])
qp.setOptions(options)
return_value = qp.init(P, q, C, lb, ub, lb_C, ub_C, array([max_wsr]))
if return_value == ReturnValue.MAX_NWSR_REACHED:
print("qpOASES reached the maximum number of WSR (%d)" % max_wsr)
else:
qp = QProblemB(n)
qp.setOptions(options)
qp.init(P, q, lb, ub, max_wsr)
x_opt = zeros(n)
ret = qp.getPrimalSolution(x_opt)
if ret != 0: # 0 == SUCCESSFUL_RETURN code of qpOASES
print("qpOASES failed with return code %d" % ret)
return x_opt
def __init__(self, model, ntime, T, T_step, future_steps, feet):
#linear system model
self.model = model
#feet dimensions
self.feet = feet
#walk parameters
self.T = T
self.T_step = T_step
self.samples_per_step = int(self.T_step / self.T)
self.future_steps = future_steps
self.N = int(self.future_steps * self.samples_per_step)
self.ntime = ntime
#constraints (x, y)
self.feet_ubounds = np.array([0.2, -0.1569, 0.2, 0.25])
self.feet_lbounds = np.array([-0.2, -0.25, -0.2, 0.1569])
#compute the restrained CoP zone
self.zone = (np.min(self.feet) / np.sqrt(2.)) / 2.
self.CoP_ubounds = np.array([self.zone, self.zone])
self.CoP_lbounds = np.array([-self.zone, -self.zone])
#angle between feet bound
self.feet_angle_ubound = 0.05
self.feet_angle_lbound = -0.05
#number of decision variables (3*N for x, y, theta jerks + 3*future_steps
#for x, y, theta for each of two steps)
self.n_dec = 3 * self.N + 3 * self.future_steps
#number of constraints
self.n_constraints = 2 * self.N + 3 * self.future_steps
#QP
self.QP = QProblem(self.n_dec, self.n_constraints)
#initial state
self.x = np.array([[0., 0., 0., 0., 0., 0., 0., 0., 0.]])
#initial CoP
self.CoP = np.dot(self.model.D(self.T), self.x.T)
#initial and future footstep position and orientation
self.f_current = np.array([[0., 0., 0.]])
#we record next step after each iteration and change it in the right moment
self.f_future = np.array([[0., 0., 0.]])
#controls
self.controls = np.array([[0., 0., 0.]])
#rotation matrix of the current footstep
self.current_rotation = np.array(
[[np.cos(self.f_current[0, 2]),
np.sin(self.f_current[0, 2])],
[-np.sin(self.f_current[0, 2]),
np.cos(self.f_current[0, 2])]])
#initial step pattern - long vector of 1s and 0s which we roll over at each iteration
self.cols_step0 = np.hstack((np.ones(
(1, self.samples_per_step)), np.zeros((1, self.samples_per_step))))
self.cols_step1 = np.hstack((np.zeros(
(1, self.samples_per_step)), np.ones((1, self.samples_per_step))))
self.cols_step2 = np.hstack((np.zeros(
(1, self.samples_per_step)), np.zeros((1, self.samples_per_step))))
self.steps = np.hstack(
(self.cols_step0, self.cols_step1, self.cols_step2))
#weights for QP
self.alpha = 1e-5
self.beta = 1e3
self.gamma = 2.5
def test_example1(self):
return 0
# Example for qpOASES main function using the QProblem 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.
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 QProblemB object.
qp = QProblem(2, 1)
options = Options()
options.printLevel = PrintLevel.NONE
qp.setOptions(options)
# Solve first QP.
nWSR = 10
qp.init(H, g, A, lb, ub, lbA, ubA, nWSR)
# Solve second QP.
nWSR = 10
qp.hotstart(g_new, lb_new, ub_new, lbA_new, ubA_new, nWSR)
# 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, "example1")
p = Popen(cmd, shell=True, stdout=PIPE)
stdout, stderr = p.communicate()
stdout = str(stdout).replace('\\n', '\n')
stdout = stdout.replace("'", '')
print(stdout)
# get c++ solution from std
pattern = re.compile(r'xOpt\s*=\s*\[\s+(?P<xOpt>([0-9., e+-])*)\];')
match = pattern.search(stdout)
xOpt_expected = match.group('xOpt')
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.search(stdout)
objVal_expected = match.group('objVal')
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)
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.
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 QProblem object.
example = QProblem(2, 1)
options = Options()
#options.printLevel = PrintLevel.NONE
example.setOptions(options)
# Solve first QP.
nWSR = np.array([10])
example.init(H, g, A, lb, ub, lbA, ubA, nWSR)
xOpt = np.zeros(2)
example.getPrimalSolution(xOpt)
print("\nxOpt = [ %e, %e ]; objVal = %e\n\n" %
(xOpt[0], xOpt[1], example.getObjVal()))
# Solve second QP.
nWSR = np.array([10])