def get_dq(self, q, e1, J1, e2, J2, e3, J3, e4, J4):
de1 = self.lambda1 * e1
de2 = self.lambda2 * e2
de3 = self.lambda3 * e3
de4 = self.lambda4 * e4
W = self.w1 * np.dot(J1.T, J1) + self.w2 * np.dot(
J2.T, J2) + self.w3 * np.dot(J3.T, J3) + self.w4 * np.dot(
J4.T, J4)
p = -2 * (self.w1 * np.dot(J1.T, de1) + self.w2 * np.dot(J2.T, de2) +
self.w3 * np.dot(J3.T, de3) + self.w4 * np.dot(J4.T, de4))
lower_limits = np.maximum((self.qmin - q[7:]) / self.dt, self.dqmin)
upper_limits = np.minimum((self.qmax - q[7:]) / self.dt, self.dqmax)
lower_limits = np.hstack((self.lfb, lower_limits))
upper_limits = np.hstack((self.ufb, upper_limits))
# Solver
solver = QProblemB(19)
options = Options()
options.setToMPC()
options.printLevel = PrintLevel.LOW
solver.setOptions(options)
nWSR = np.array([10])
solver.init(W, p, lower_limits, upper_limits, nWSR)
dq = np.zeros(19)
solver.getPrimalSolution(dq)
return dq
def solveLeastSquare(A,
b,
lb=None,
ub=None,
A_in=None,
lb_in=None,
ub_in=None):
'''
Solve the least square problem:
minimize || A*x-b ||^2
subject to lb_in <= A_in*x <= ub_in
lb <= x <= ub
'''
n = A.shape[1]
m_in = 0
if A_in is not None:
m_in = A_in.shape[0]
if lb_in is None:
lb_in = np.array(m_in * [-1e99])
if ub_in is None:
ub_in = np.array(m_in * [1e99])
if lb is None:
lb = np.array(n * [-1e99])
if ub is 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
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
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 solveLeastSquare(A,
b,
lb=None,
ub=None,
A_in=None,
lb_in=None,
ub_in=None,
maxIterations=None,
maxComputationTime=60.0,
regularization=1e-8):
n = A.shape[1]
m_in = 0
A = np.asarray(A)
b = np.asarray(b).squeeze()
if (A_in is not None):
m_in = A_in.shape[0]
if (lb_in is None):
lb_in = np.array(m_in * [-1e99])
else:
lb_in = np.asarray(lb_in).squeeze()
if (ub_in is None):
ub_in = np.array(m_in * [1e99])
else:
ub_in = np.asarray(ub_in).squeeze()
if (lb is None):
lb = np.array(n * [-1e99])
else:
lb = np.asarray(lb).squeeze()
if (ub is None):
ub = np.array(n * [1e99])
else:
ub = np.asarray(ub).squeeze()
# 0.5||Ax-b||^2 = 0.5(x'A'Ax - 2b'Ax + b'b) = 0.5x'A'Ax - b'Ax +0.5b'b
Hess = np.dot(A.T, A) + regularization * np.identity(n)
grad = -np.dot(A.T, b)
if (maxIterations is None):
maxActiveSetIter = np.array([100 + 2 * m_in + 2 * n])
else:
maxActiveSetIter = np.array([maxIterations])
maxComputationTime = np.array([maxComputationTime])
options = Options()
options.printLevel = PrintLevel.NONE
#NONE, LOW, MEDIUM
options.enableRegularisation = False
if (m_in == 0):
qpOasesSolver = QProblemB(n)
#, HessianType.SEMIDEF);
qpOasesSolver.setOptions(options)
# beware that the Hessian matrix may be modified by this function
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)
x = np.empty(n)
qpOasesSolver.getPrimalSolution(x)
#print "QP cost:", 0.5*(np.linalg.norm(np.dot(A, x)-b)**2);
return (imode, np.asmatrix(x).T)
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.printLevel = PrintLevel.NONE
qp.setOptions(options)
# Solve first QP.
nWSR = 10
qp.init(H, g, lb, ub, nWSR)
# Solve second QP.
nWSR = 10;
qp.hotstart(g_new, lb_new, ub_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, "example1b")
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)
#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.
example = QProblemB(2)
options = Options()
options.enableFlippingBounds = BooleanType.FALSE
options.initialStatusBounds = SubjectToStatus.INACTIVE
options.numRefinementSteps = 1
example.setOptions(options)
# Solve first QP.
nWSR = 10
example.init(H, g, lb, ub, nWSR)
print("\nnWSR = %d\n\n" % nWSR)
# Solve second QP.
nWSR = 10