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 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):
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 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 = np.array([10])
example.init(H, g, lb, ub, nWSR)
print("\nnWSR = %d\n\n"%nWSR)
# Solve second QP.
nWSR = np.array([10])
#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
