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])