def set_contacts(self,
contact_points,
contact_normals,
mu,
regularization=1e-5):
# compute matrix A, which maps the force generator coefficients into the centroidal wrench
(self.A,
self.G4) = compute_centroidal_cone_generators(contact_points,
contact_normals, mu)
# since the size of the problem may have changed we need to recreate the solver and all the problem matrices/vectors
self.n = contact_points.shape[0] * 4 + 1
self.qpOasesSolver = SQProblem(self.n, self.m_in)
#, HessianType.SEMIDEF);
self.qpOasesSolver.setOptions(self.options)
self.Hess = INITIAL_HESSIAN_REGULARIZATION * np.identity(self.n)
self.grad = np.ones(self.n) * regularization
self.grad[-1] = 1.0
self.constrMat = np.zeros((self.m_in, self.n))
self.constrMat[:, :-1] = self.A
self.constrMat[:3, -1] = self.mass * self.v
self.constrMat[3:, -1] = self.mass * np.cross(self.c0, self.v)
self.lb = np.zeros(self.n)
self.lb[-1] = -1e100
self.ub = np.array(self.n * [
1e100,
])
self.x = np.zeros(self.n)
self.y = np.zeros(self.n + self.m_in)
self.initialized = False
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 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, name, n, m_in, maxIter=1000, verb=1):
self.name = name
self.iter = 0
self.maxIter = maxIter
self.verb = verb
self.m_in = m_in
self.n = n
self.iter = 0
self.qpOasesSolver = SQProblem(self.n, self.m_in)
#, HessianType.SEMIDEF);
self.options = Options()
if (self.verb <= 1):
self.options.printLevel = PrintLevel.NONE
elif (self.verb == 2):
self.options.printLevel = PrintLevel.LOW
elif (self.verb == 3):
self.options.printLevel = PrintLevel.MEDIUM
elif (self.verb > 4):
self.options.printLevel = PrintLevel.DEBUG_ITER
print("set high print level")
self.options.enableRegularisation = True
self.qpOasesSolver.setOptions(self.options)
self.initialized = False
self.Hess = np.identity(self.n)
self.grad = np.zeros(self.n)
self.x_lb = np.array(self.n * [
-1e10,
])
self.x_ub = np.array(self.n * [
1e10,
])
self.B = np.zeros((self.m_in, self.n))
self.b_ub = np.zeros(self.m_in) + 1e10
self.b_lb = np.zeros(self.m_in) - 1e10
self.x = np.zeros(self.n)
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 compute_max_deceleration(self, alpha, maxIter=None, maxTime=100.0):
start = time.time()
self.alpha = alpha
if (self.NO_WARM_START):
self.qpOasesSolver = SQProblem(self.n, self.m_in)
self.qpOasesSolver.setOptions(self.options)
self.initialized = False
if (maxIter == None):
maxIter = self.maxIter
maxActiveSetIter = np.array([maxIter])
maxComputationTime = np.array(maxTime)
self.constrUB[:6] = np.dot(self.b, alpha) + self.d
self.constrLB[:6] = self.constrUB[:6]
while (True):
if (not self.initialized):
self.imode = self.qpOasesSolver.init(self.Hess, self.grad,
self.constrMat, self.lb,
self.ub, self.constrLB,
self.constrUB,
maxActiveSetIter,
maxComputationTime)
else:
self.imode = self.qpOasesSolver.hotstart(
self.grad, self.lb, self.ub, self.constrLB, self.constrUB,
maxActiveSetIter, maxComputationTime)
if (self.imode == 0):
self.initialized = True
if (self.imode == 0
or self.imode == PyReturnValue.INIT_FAILED_INFEASIBILITY or
self.imode == PyReturnValue.HOTSTART_STOPPED_INFEASIBILITY
or self.Hess[0, 0] >= MAX_HESSIAN_REGULARIZATION):
break
self.initialized = False
self.Hess *= 10.0
maxActiveSetIter = np.array([maxIter])
maxComputationTime = np.array(maxTime)
if (self.verb > -1):
print "[%s] WARNING %s. Increasing Hessian regularization to %f" % (
self.name, qpOasesSolverMsg(self.imode), self.Hess[0, 0])
self.qpTime = maxComputationTime
self.iter = 1 + maxActiveSetIter[0]
if (self.imode == 0):
self.qpOasesSolver.getPrimalSolution(self.x)
self.qpOasesSolver.getDualSolution(self.y)
if ((self.x < self.lb - self.INEQ_VIOLATION_THR).any()):
self.initialized = False
raise ValueError("[%s] ERROR lower bound violated" %
(self.name) + str(self.x) + str(self.lb))
if ((self.x > self.ub + self.INEQ_VIOLATION_THR).any()):
self.initialized = False
raise ValueError("[%s] ERROR upper bound violated" %
(self.name) + str(self.x) + str(self.ub))
if ((np.dot(self.constrMat, self.x) >
self.constrUB + self.INEQ_VIOLATION_THR).any()):
self.initialized = False
raise ValueError(
"[%s] ERROR constraint upper bound violated " %
(self.name) +
str(np.min(np.dot(self.constrMat, self.x) -
self.constrUB)))
if ((np.dot(self.constrMat, self.x) <
self.constrLB - self.INEQ_VIOLATION_THR).any()):
self.initialized = False
raise ValueError(
"[%s] ERROR constraint lower bound violated " %
(self.name) +
str(np.max(np.dot(self.constrMat, self.x) -
self.constrLB)))
(dx, alpha_min,
alpha_max) = self.compute_max_deceleration_derivative()
else:
self.initialized = False
dx = 0.0
alpha_min = 0.0
alpha_max = 0.0
if (self.verb > 0):
print "[%s] ERROR Qp oases %s" % (self.name,
qpOasesSolverMsg(self.imode))
if (self.qpTime >= maxTime):
if (self.verb > 0):
print "[%s] Max time reached %f after %d iters" % (
self.name, self.qpTime, self.iter)
self.imode = 9
self.computationTime = time.time() - start
return (self.imode, self.x[-1], dx, alpha_min, alpha_max)
class ComAccLP(object):
"""
LP solver dedicated to finding the maximum center of mass (CoM) deceleration in a
specified direction, subject to the friction cone constraints.
This is possible thanks to the simplifying assumption that the CoM acceleration
is going to be parallel to its velocity. This allows us to represent the 3d
com trajectory by means of a 1d trajectory alpha(t):
c(t) = c0 + alpha(t) v
v = c0 / ||c0||
dc = dAlpha(t) v
ddc(t) = ddAlpha(t) v
The operation amounts to solving the following parametric Linear Program:
minimize ddAlpha + w sum_i(f_i)
subject to A f = a ddAlpha + b alpha + d
f >= 0
where:
f are the contact forces generator coefficients
ddAlpha is the magnitude of the CoM acceleration
alpha is the magnitude of the CoM displacement (with respect to its initial position c0)
w regularization parameter
Given a CoM position (by means of a value of alpha), this class can compute the
minimum com acceleration in direction v (i.e. the maximum acceleration in direction -v).
Since this is a piecewise-linear function of alpha, it can also compute its derivative with respect
to alpha, and the boundaries of the alpha-region in which the derivative remains constant.
"""
NO_WARM_START = False
name = ""
# solver name
n = 0
# number of variables
m_in = 0
# number of constraints (i.e. 6)
Hess = []
# Hessian
grad = []
# gradient
A = None
# constraint matrix multiplying the contact force generators
b = None
# constraint vector multiplying the CoM position parameter alpha
d = None
# constraint vector
mass = 0.0
# robot mass
g = None
# 3d gravity vector
maxIter = 0
# max number of iterations
verb = 0
# verbosity level of the solver (0=min, 2=max)
iter = 0
# current iteration number
computationTime = 0.0
# total computation time
qpTime = 0.0
# time taken to solve the QP(s) only
initialized = False
# true if solver has been initialized
qpOasesSolver = []
options = []
# qp oases solver's options
epsilon = np.sqrt(np.finfo(float).eps)
INEQ_VIOLATION_THR = 1e-4
def __init__(self,
name,
c0,
v,
contact_points,
contact_normals,
mu,
g,
mass,
maxIter=10000,
verb=0,
regularization=1e-5):
''' Constructor
@param c0 Initial CoM position
@param v Opposite of the direction in which you want to maximize the CoM acceleration (typically that would be
the CoM velocity direction)
@param g Gravity vector
@param regularization Weight of the force minimization, the higher this value, the sparser the solution
'''
self.name = name
self.maxIter = maxIter
self.verb = verb
self.m_in = 6
self.initialized = False
self.options = Options()
self.options.setToReliable()
if (self.verb <= 1):
self.options.printLevel = PrintLevel.NONE
elif (self.verb == 2):
self.options.printLevel = PrintLevel.LOW
elif (self.verb == 3):
self.options.printLevel = PrintLevel.MEDIUM
elif (self.verb > 3):
self.options.printLevel = PrintLevel.DEBUG_ITER
self.options.enableRegularisation = False
self.options.enableEqualities = True
# self.qpOasesSolver.printOptions()
self.b = np.zeros(6)
self.d = np.empty(6)
self.c0 = np.empty(3)
self.v = np.empty(3)
self.constrUB = np.zeros(self.m_in) + 1e100
self.constrLB = np.zeros(self.m_in) - 1e100
self.set_problem_data(c0, v, contact_points, contact_normals, mu, g,
mass, regularization)
def set_com_state(self, c0, v):
assert np.asarray(
c0).squeeze().shape[0] == 3, "Com position vector has not size 3"
assert np.asarray(v).squeeze(
).shape[0] == 3, "Com acceleration direction vector has not size 3"
self.c0 = np.asarray(c0).squeeze()
self.v = np.asarray(v).squeeze().copy()
if (norm(v) == 0.0):
raise ValueError(
"[%s] Norm of com acceleration direction v is zero!" %
self.name)
self.v /= norm(self.v)
self.constrMat[:3, -1] = self.mass * self.v
self.constrMat[3:, -1] = self.mass * np.cross(self.c0, self.v)
self.b[3:] = self.mass * np.cross(v, self.g)
self.d[:3] = self.mass * self.g
self.d[3:] = self.mass * np.cross(c0, self.g)
def set_contacts(self,
contact_points,
contact_normals,
mu,
regularization=1e-5):
# compute matrix A, which maps the force generator coefficients into the centroidal wrench
(self.A,
self.G4) = compute_centroidal_cone_generators(contact_points,
contact_normals, mu)
# since the size of the problem may have changed we need to recreate the solver and all the problem matrices/vectors
self.n = contact_points.shape[0] * 4 + 1
self.qpOasesSolver = SQProblem(self.n, self.m_in)
#, HessianType.SEMIDEF);
self.qpOasesSolver.setOptions(self.options)
self.Hess = INITIAL_HESSIAN_REGULARIZATION * np.identity(self.n)
self.grad = np.ones(self.n) * regularization
self.grad[-1] = 1.0
self.constrMat = np.zeros((self.m_in, self.n))
self.constrMat[:, :-1] = self.A
self.constrMat[:3, -1] = self.mass * self.v
self.constrMat[3:, -1] = self.mass * np.cross(self.c0, self.v)
self.lb = np.zeros(self.n)
self.lb[-1] = -1e100
self.ub = np.array(self.n * [
1e100,
])
self.x = np.zeros(self.n)
self.y = np.zeros(self.n + self.m_in)
self.initialized = False
def set_problem_data(self,
c0,
v,
contact_points,
contact_normals,
mu,
g,
mass,
regularization=1e-5):
assert g.shape[0] == 3, "Gravity vector has not size 3"
assert mass > 0.0, "Mass is not positive"
self.mass = mass
self.g = np.asarray(g).squeeze()
self.set_contacts(contact_points, contact_normals, mu, regularization)
self.set_com_state(c0, v)
def compute_max_deceleration_derivative(self):
''' Compute the derivative of the max CoM deceleration (i.e. the solution of the last LP)
with respect to the parameter alpha (i.e. the CoM position parameter). Moreover,
it also computes the bounds within which this derivative is valid (alpha_min, alpha_max).
'''
act_set = np.where(self.y[:self.n - 1] != 0.0)[0]
# indexes of active bound constraints
n_as = act_set.shape[0]
if (n_as > self.n - 6):
raise ValueError(
"[%s] ERROR Too many active constraints: %d (rather than %d)" %
(self.name, n_as, self.n - 6))
if (self.verb > 0 and n_as < self.n - 6):
print "[%s] INFO Less active constraints than expected: %d (rather than %d)" % (
self.name, n_as, self.n - 6)
self.K = np.zeros((n_as + 6, self.n))
self.k1 = np.zeros(n_as + 6)
self.k2 = np.zeros(n_as + 6)
self.K[:n_as, :] = np.identity(self.n)[act_set, :]
self.K[-6:, :] = self.constrMat
self.k1[-6:] = self.b
self.k2[-6:] = self.d
U, s, VT = np.linalg.svd(self.K)
rank = (s > EPS).sum()
K_inv_k1 = np.dot(VT[:rank, :].T,
(1.0 / s[:rank]) * np.dot(U[:, :rank].T, self.k1))
K_inv_k2 = np.dot(VT[:rank, :].T,
(1.0 / s[:rank]) * np.dot(U[:, :rank].T, self.k2))
if (rank < self.n):
Z = VT[rank:, :].T
P = np.dot(
np.dot(Z, np.linalg.inv(np.dot(Z.T, np.dot(self.Hess, Z)))),
Z.T)
K_inv_k1 -= np.dot(P, np.dot(self.Hess, K_inv_k1))
K_inv_k2 -= np.dot(P,
np.dot(self.Hess, K_inv_k2) + self.grad)
# Check that the solution you get by solving the KKT is the same found by the solver
x_kkt = K_inv_k1 * self.alpha + K_inv_k2
if (norm(self.x - x_kkt) > 10 * EPS):
warnings.warn("[%s] ERROR x different from x_kkt. x=" %
(self.name) + str(self.x) + "\nx_kkt=" + str(x_kkt) +
" " + str(norm(self.x - x_kkt)))
# store the derivative of the solution w.r.t. the parameter alpha
dx = K_inv_k1[-1]
# act_set_mat * alpha >= act_set_vec
act_set_mat = K_inv_k1[:-1]
act_set_vec = -K_inv_k2[:-1]
for i in range(act_set_mat.shape[0]):
if (abs(act_set_mat[i]) > EPS):
act_set_vec[i] /= abs(act_set_mat[i])
act_set_mat[i] /= abs(act_set_mat[i])
if (act_set_mat * self.alpha < act_set_vec - EPS).any():
raise ValueError(
"ERROR: after normalization current alpha violates constraints "
+ str(act_set_mat * self.alpha - act_set_vec))
ind_pos = np.where(act_set_mat > EPS)[0]
if (ind_pos.shape[0] > 0):
alpha_min = np.max(act_set_vec[ind_pos])
else:
alpha_min = -1e10
# warnings.warn("[%s] alpha_min seems unbounded %.7f"%(self.name, np.max(act_set_mat)));
ind_neg = np.where(act_set_mat < -EPS)[0]
if (ind_neg.shape[0] > 0):
alpha_max = np.min(-act_set_vec[ind_neg])
else:
alpha_max = 1e10
# warnings.warn("[%s] alpha_max seems unbounded %.7f"%(self.name, np.min(act_set_mat)));
if (alpha_min > alpha_max):
raise ValueError("ERROR alpha_min %.3f > alpha_max %.3f" %
(alpha_min, alpha_max))
return (dx, alpha_min, alpha_max)
def compute_max_deceleration(self, alpha, maxIter=None, maxTime=100.0):
start = time.time()
self.alpha = alpha
if (self.NO_WARM_START):
self.qpOasesSolver = SQProblem(self.n, self.m_in)
self.qpOasesSolver.setOptions(self.options)
self.initialized = False
if (maxIter == None):
maxIter = self.maxIter
maxActiveSetIter = np.array([maxIter])
maxComputationTime = np.array(maxTime)
self.constrUB[:6] = np.dot(self.b, alpha) + self.d
self.constrLB[:6] = self.constrUB[:6]
while (True):
if (not self.initialized):
self.imode = self.qpOasesSolver.init(self.Hess, self.grad,
self.constrMat, self.lb,
self.ub, self.constrLB,
self.constrUB,
maxActiveSetIter,
maxComputationTime)
else:
self.imode = self.qpOasesSolver.hotstart(
self.grad, self.lb, self.ub, self.constrLB, self.constrUB,
maxActiveSetIter, maxComputationTime)
if (self.imode == 0):
self.initialized = True
if (self.imode == 0
or self.imode == PyReturnValue.INIT_FAILED_INFEASIBILITY or
self.imode == PyReturnValue.HOTSTART_STOPPED_INFEASIBILITY
or self.Hess[0, 0] >= MAX_HESSIAN_REGULARIZATION):
break
self.initialized = False
self.Hess *= 10.0
maxActiveSetIter = np.array([maxIter])
maxComputationTime = np.array(maxTime)
if (self.verb > -1):
print "[%s] WARNING %s. Increasing Hessian regularization to %f" % (
self.name, qpOasesSolverMsg(self.imode), self.Hess[0, 0])
self.qpTime = maxComputationTime
self.iter = 1 + maxActiveSetIter[0]
if (self.imode == 0):
self.qpOasesSolver.getPrimalSolution(self.x)
self.qpOasesSolver.getDualSolution(self.y)
if ((self.x < self.lb - self.INEQ_VIOLATION_THR).any()):
self.initialized = False
raise ValueError("[%s] ERROR lower bound violated" %
(self.name) + str(self.x) + str(self.lb))
if ((self.x > self.ub + self.INEQ_VIOLATION_THR).any()):
self.initialized = False
raise ValueError("[%s] ERROR upper bound violated" %
(self.name) + str(self.x) + str(self.ub))
if ((np.dot(self.constrMat, self.x) >
self.constrUB + self.INEQ_VIOLATION_THR).any()):
self.initialized = False
raise ValueError(
"[%s] ERROR constraint upper bound violated " %
(self.name) +
str(np.min(np.dot(self.constrMat, self.x) -
self.constrUB)))
if ((np.dot(self.constrMat, self.x) <
self.constrLB - self.INEQ_VIOLATION_THR).any()):
self.initialized = False
raise ValueError(
"[%s] ERROR constraint lower bound violated " %
(self.name) +
str(np.max(np.dot(self.constrMat, self.x) -
self.constrLB)))
(dx, alpha_min,
alpha_max) = self.compute_max_deceleration_derivative()
else:
self.initialized = False
dx = 0.0
alpha_min = 0.0
alpha_max = 0.0
if (self.verb > 0):
print "[%s] ERROR Qp oases %s" % (self.name,
qpOasesSolverMsg(self.imode))
if (self.qpTime >= maxTime):
if (self.verb > 0):
print "[%s] Max time reached %f after %d iters" % (
self.name, self.qpTime, self.iter)
self.imode = 9
self.computationTime = time.time() - start
return (self.imode, self.x[-1], dx, alpha_min, alpha_max)
def getContactForces(self):
''' Get the contact forces obtained by solving the last LP '''
cg = 4
nContacts = self.G4.shape[1] / cg
f = np.empty((3, nContacts))
for i in range(nContacts):
f[:, i] = np.dot(self.G4[:, cg * i:cg * i + cg],
self.x[cg * i:cg * i + cg])
return f
def reset(self):
self.initialized = False
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)
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 = vstack([A, A])
lb_C = h
ub_C = h
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
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])
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)
# Solve second QP.
nWSR = np.array([10])
for i in range(100000):
for j in range(1, 100):
g_new[0] = i%j
class qpSolver(object):
"""
Solver for the following QP:
minimize 0.5 x' H x + g' x
subject to b_lb <= B x <= b_ub
x_lb <= x <= x_ub
"""
NO_WARM_START = False
name = ""
# solver name
n = 0
# number of variables
m_in = 0
# number of equalities/inequalities
x = []
# last solution
Hess = []
# Hessian
grad = []
# gradient
B = []
b_ub = []
b_lb = []
x_lb = []
x_ub = []
maxIter = 0
# max number of iterations
verb = 0
# verbosity level of the solver (0=min, 2=max)
iter = 0
# current iteration number
computationTime = 0.0
# total computation time
qpTime = 0.0
# time taken to solve the QP(s) only
qpOasesSolver = []
options = []
# qp oases solver's options
def __init__(self, name, n, m_in, maxIter=1000, verb=1):
self.name = name
self.iter = 0
self.maxIter = maxIter
self.verb = verb
self.m_in = m_in
self.n = n
self.iter = 0
self.qpOasesSolver = SQProblem(self.n, self.m_in)
#, HessianType.SEMIDEF);
self.options = Options()
if (self.verb <= 1):
self.options.printLevel = PrintLevel.NONE
elif (self.verb == 2):
self.options.printLevel = PrintLevel.LOW
elif (self.verb == 3):
self.options.printLevel = PrintLevel.MEDIUM
elif (self.verb > 4):
self.options.printLevel = PrintLevel.DEBUG_ITER
print("set high print level")
self.options.enableRegularisation = True
self.qpOasesSolver.setOptions(self.options)
self.initialized = False
self.Hess = np.identity(self.n)
self.grad = np.zeros(self.n)
self.x_lb = np.array(self.n * [
-1e10,
])
self.x_ub = np.array(self.n * [
1e10,
])
self.B = np.zeros((self.m_in, self.n))
self.b_ub = np.zeros(self.m_in) + 1e10
self.b_lb = np.zeros(self.m_in) - 1e10
self.x = np.zeros(self.n)
''' Solve the specified quadratic program. If some problem data are not supplied, the last specified
values are used. By default upper bounds are set to 1e10 and lower bounds to -1e10, while B=0.
'''
def solve(self,
H,
g,
x_lb=None,
x_ub=None,
B=None,
b_lb=None,
b_ub=None,
maxIter=None,
maxTime=100.0):
start = time.time()
# copy all data to avoid problem with memory alignement with qpOases
if (type(H) == np.matrix):
self.Hess = H.A.squeeze()
else:
self.Hess = np.copy(H)
if (type(g) == np.matrix):
self.grad = g.A.squeeze()
else:
self.grad = np.copy(g)
if (x_lb is not None):
if (type(x_lb) == np.matrix):
self.x_lb = x_lb.A.squeeze()
else:
self.x_lb = np.copy(x_lb)
if (x_ub is not None):
if (type(x_ub) == np.matrix):
self.x_ub = x_ub.A.squeeze()
else:
self.x_ub = np.copy(x_ub)
if (b_lb is not None):
if (type(b_lb) == np.matrix):
self.b_lb = b_lb.A.squeeze()
else:
self.b_lb = np.copy(b_lb)
if (b_ub is not None):
if (type(b_ub) == np.matrix):
self.b_ub = b_ub.A.squeeze()
else:
self.b_ub = np.copy(b_ub)
if (B is not None):
if (type(B) == np.matrix):
self.B = B.A.squeeze()
else:
self.B = np.copy(B)
if (maxIter == None):
maxIter = self.maxIter
maxActiveSetIter = np.array([maxIter])
maxComputationTime = np.array(maxTime)
self.imode = self.qpOasesSolver.init(self.Hess, self.grad, self.B,
self.x_lb, self.x_ub, self.b_lb,
self.b_ub, maxActiveSetIter,
maxComputationTime)
self.qpTime = maxComputationTime
self.iter = 1 + maxActiveSetIter[0]
self.qpOasesSolver.getPrimalSolution(self.x)
self.print_qp_oases_error_message(self.imode, self.name)
Bx = np.dot(self.B, self.x)
for i in range(self.m_in):
if (Bx[i] > b_ub[i] + EPS):
print("Constraint %d upper bound violated, B*x = %f, b_ub=%f" %
(i, Bx[i], b_ub[i]))
if (Bx[i] < b_lb[i] - EPS):
print("Constraint %d lower bound violated, B*x = %f, b_lb=%f" %
(i, Bx[i], b_lb[i]))
# termination conditions
if (self.qpTime >= maxTime):
if (self.verb > 0):
print("[%s] Max time reached %f after %d iters" %
(self.name, self.qpTime, self.iter))
self.imode = 9
self.computationTime = time.time() - start
return self.x
def print_qp_oases_error_message(self, imode, solver_name):
if (imode != 0 and self.verb >= 0):
if (imode == PyReturnValue.HOTSTART_STOPPED_INFEASIBILITY):
print("[%s] ERROR Qp oases HOTSTART_STOPPED_INFEASIBILITY" %
solver_name)
# 60
elif (imode == PyReturnValue.MAX_NWSR_REACHED):
print("[%s] ERROR Qp oases RET_MAX_NWSR_REACHED" % solver_name)
# 63
elif (imode == PyReturnValue.STEPDIRECTION_FAILED_CHOLESKY):
print("[%s] ERROR Qp oases STEPDIRECTION_FAILED_CHOLESKY" %
solver_name)
# 68
elif (imode == PyReturnValue.HOTSTART_FAILED_AS_QP_NOT_INITIALISED
):
print(
"[%s] ERROR Qp oases HOTSTART_FAILED_AS_QP_NOT_INITIALISED"
% solver_name)
# 53
elif (imode == PyReturnValue.INIT_FAILED_INFEASIBILITY):
print("[%s] ERROR Qp oases INIT_FAILED_INFEASIBILITY" %
solver_name)
# 37
# RET_INIT_FAILED_HOTSTART = 36
elif (imode == PyReturnValue.UNKNOWN_BUG):
print("[%s] ERROR Qp oases UNKNOWN_BUG" % solver_name)
# 9
else:
print("[%s] ERROR Qp oases %d " % (solver_name, imode))
