def __init__(self, name, maxIter=1000, maxTime=100.0, useWarmStart=True, verb=0): solver_LP_abstract.SolverLPAbstract.__init__(self, name, maxIter, maxTime, useWarmStart, verb) self._hessian_regularization = DEFAULT_HESSIAN_REGULARIZATION 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._n = -1 self._m_con = -1 self._qpOasesSolver = None
def changeInequalityNumber(self, m_in): # print "[%s] Changing number of inequality constraints from %d to %d" % (self.name, self.m_in, m_in); if (m_in == self.m_in): return self.m_in = m_in self.iter = 0 self.qpOasesSolver = SQProblem(self.n, m_in) #, HessianType.POSDEF SEMIDEF self.options = Options() self.options.setToReliable() if (self.verb <= 0): self.options.printLevel = PrintLevel.NONE elif (self.verb == 1): self.options.printLevel = PrintLevel.LOW elif (self.verb == 2): self.options.printLevel = PrintLevel.MEDIUM elif (self.verb > 2): self.options.printLevel = PrintLevel.DEBUG_ITER print "set high print level" # self.options.enableRegularisation = False; # self.options.enableFlippingBounds = BooleanType.FALSE # self.options.initialStatusBounds = SubjectToStatus.INACTIVE # self.options.setToMPC(); # self.qpOasesSolver.printOptions(); self.qpOasesSolver.setOptions(self.options) 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 __init__ (self, n, mu, accuracy=1e-4, maxIter=30, verb=0): self.name = "StagProj"; self.n = n; self.mu = mu; self.iter = 0; self.accuracy = accuracy; self.maxIter = maxIter; self.verb = verb; self.options = Options(); if(self.verb<=0): self.options.printLevel = PrintLevel.NONE; elif(self.verb==1): self.options.printLevel = PrintLevel.LOW; elif(self.verb==2): self.options.printLevel = PrintLevel.MEDIUM; elif(self.verb>2): self.options.printLevel = PrintLevel.DEBUG_ITER; print "set high print level" self.options.enableRegularisation = True; self.changeContactsNumber(0); self.S_T = np.zeros((self.n+6,self.n)); self.S_T[6:6+self.n,:] = np.identity(self.n); self.t = 0; self.meanComputationTime = 0.0; self.maxComputationTime = 0.0; self.meanIterations = 0.0; self.maxIterations = 0.0; return;
def __init__(self, constraint_list, path, path_discretization): assert qpoases_FOUND, "toppra is unable to find any installation of qpoases!" super(qpOASESSolverWrapper, self).__init__( constraint_list, path, path_discretization ) # Currently only support Canonical Linear Constraint self.nC = 2 # First constraint is x + 2 D u <= xnext_max, second is xnext_min <= x + 2D u for i, constraint in enumerate(constraint_list): if constraint.get_constraint_type() != ConstraintType.CanonicalLinear: raise NotImplementedError a, b, c, F, v, _, _ = self.params[i] if a is not None: if constraint.identical: self.nC += F.shape[0] else: self.nC += F.shape[1] self._A = np.zeros((self.nC, self.nV)) self._lA = -np.ones(self.nC) self._hA = -np.ones(self.nC) option = Options() option.printLevel = PrintLevel.NONE self.solver = SQProblem(self.nV, self.nC) self.solver.setOptions(option)
def __initSolver(self, arg_dim, num_in): self.solver = SQProblem(arg_dim, num_in) # , HessianType.POSDEF SEMIDEF self.options = Options() self.options.setToReliable() self.solver.setOptions(self.options)
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 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 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 setup_solver(self): option = Options() if logger.getEffectiveLevel() == logging.DEBUG: option.printLevel = PrintLevel.HIGH else: option.printLevel = PrintLevel.NONE self.solver_up = SQProblem(self.nV, self.nC) self.solver_up.setOptions(option) self.solver_down = SQProblem(self.nV, self.nC) self.solver_down.setOptions(option) self.solver_up_recent_index = -2 self.solver_down_recent_index = -2
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 setup_solver(self): """Initiate two internal solvers for warm-start. """ option = Options() if logger.getEffectiveLevel() == logging.DEBUG: # option.printLevel = PrintLevel.HIGH option.printLevel = PrintLevel.NONE else: option.printLevel = PrintLevel.NONE self.solver_minimizing = SQProblem(self.nV, self.nC) self.solver_minimizing.setOptions(option) self.solver_maximizing = SQProblem(self.nV, self.nC) self.solver_maximizing.setOptions(option) self.solver_minimizing_recent_index = -2 self.solver_maximizing_recent_index = -2
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 test_m44_reliable_sparse(self): test_name = 'mm44_reliable_sparse.txt' print(test_name) # QP Options options = Options() options.setToReliable() options.printLevel = PrintLevel.NONE isSparse = True useHotstarts = False # run QP benchmarks results = run_benchmarks(benchmarks, options, isSparse, useHotstarts, self.nWSR, self.cpu_time, self.TOL) # print and write results string = results2str(results) print(string) write_results(test_name, string) assert get_nfail(results) <= 0, 'One ore more tests failed.'
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 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 solve(self, i, vref): """build the problem and solve it """ #we build the N-sized reference vectors if (i + self.N < self.ntime + 1): #take the values stored in the rows (i to N) vref_pred = vref[i:i + self.N, :] else: # If the matrix doesn't have N rows ahead, then repeat the last row to fill the void vref_pred = np.vstack( (vref[i:self.ntime, :], np.dot(np.ones((self.N - self.ntime + i, 1)), np.expand_dims(vref[self.ntime - 1, :], axis=0)))) #get condensed matrices (constant from on step to another) [Sx, Sy, Svx, Svy, St, Svt, Scx, Scy, Ux, Uu, D_diag] = self.condense() #build matrix V V, V_dash = self.get_walk_matrices(i) ##################stuff for the solver############################### #build constraint vectors (changing from one step to another) #footsteps and CoP upper bounds #ubounds CoP x position ubounds1 = np.vstack(([self.CoP_ubounds[0] for _ in range(self.N)])) ubounds1 = ubounds1 - np.dot(np.dot(Scx, D_diag), np.dot( Ux, self.x.T)) + np.dot(V, self.f_current[0][0]) #ubounds CoP y position ubounds2 = np.vstack([self.CoP_ubounds[1] for _ in range(self.N)]) ubounds2 = ubounds2 - np.dot(np.dot(Scy, D_diag), np.dot( Ux, self.x.T)) + np.dot(V, self.f_current[0][1]) #ubounds feet x position ubounds3 = np.vstack((self.feet_ubounds[0], self.feet_ubounds[2])) ubounds3 = ubounds3 + np.vstack( (np.dot(self.current_rotation[np.newaxis, 0, :], self.f_current[0, 0:2, np.newaxis]), np.zeros((self.future_steps - 1, 1)))) #ubounds feet y position ubounds4 = np.vstack((self.feet_ubounds[1], self.feet_ubounds[3])) ubounds4 = ubounds4 + np.vstack( (np.dot(self.current_rotation[np.newaxis, 1, :], self.f_current[0, 0:2, np.newaxis]), np.zeros((self.future_steps - 1, 1)))) #ubounds feet angles ubounds5 = self.feet_angle_ubound * np.ones((self.future_steps, 1)) ubounds5 = ubounds5 + np.vstack( (self.f_current[np.newaxis, np.newaxis, 0, 2], np.zeros((self.future_steps - 1, 1)))) ubA = np.vstack((ubounds1, ubounds2, ubounds3, ubounds4, ubounds5)) #footsteps and CoP lower bounds #lbounds CoP x position lbounds1 = np.vstack([self.CoP_lbounds[0] for _ in range(self.N)]) lbounds1 = lbounds1 - np.dot(np.dot(Scx, D_diag), np.dot( Ux, self.x.T)) + np.dot(V, self.f_current[0][0]) #lbounds CoP y position lbounds2 = np.vstack([self.CoP_lbounds[1] for _ in range(self.N)]) lbounds2 = lbounds2 - np.dot(np.dot(Scy, D_diag), np.dot( Ux, self.x.T)) + np.dot(V, self.f_current[0][1]) #lbounds feet x position lbounds3 = np.vstack((self.feet_lbounds[0], self.feet_lbounds[2])) lbounds3 = lbounds3 + np.vstack( (np.dot(self.current_rotation[np.newaxis, 0, :], self.f_current[0, 0:2, np.newaxis]), np.zeros((self.future_steps - 1, 1)))) #lbounds feet y position lbounds4 = np.vstack((self.feet_lbounds[1], self.feet_lbounds[3])) lbounds4 = lbounds4 + np.vstack( (np.dot(self.current_rotation[np.newaxis, 1, :], self.f_current[0, 0:2, np.newaxis]), np.zeros((self.future_steps - 1, 1)))) #lbounds feet angles lbounds5 = self.feet_angle_lbound * np.ones((self.future_steps, 1)) lbounds5 = lbounds5 + np.vstack( (self.f_current[np.newaxis, np.newaxis, 0, 2], np.zeros((self.future_steps - 1, 1)))) lbA = np.vstack((lbounds1, lbounds2, lbounds3, lbounds4, lbounds5)) #big A matrix (constraints matrix) A = np.zeros((self.n_constraints, self.n_dec)) #A elements for constraints for CoP position A[0:self.N, :] = np.hstack( (np.dot(np.dot(Scx, D_diag), Uu), -V_dash, np.zeros((self.N, 2 * self.future_steps)))) A[self.N:2 * self.N, :] = np.hstack( (np.dot(np.dot(Scy, D_diag), Uu), np.zeros((self.N, self.future_steps)), -V_dash, np.zeros((self.N, self.future_steps)))) A[2 * self.N, 3 * self.N] = self.current_rotation[0, 0] A[2 * self.N, 3 * self.N + 2] = self.current_rotation[0, 1] A[2 * self.N + 1, 3 * self.N] = -self.current_rotation[0, 0] A[2 * self.N + 1, 3 * self.N + 1] = self.current_rotation[0, 0] A[2 * self.N + 1, 3 * self.N + 2] = -self.current_rotation[0, 1] A[2 * self.N + 1, 3 * self.N + 3] = self.current_rotation[0, 1] A[2 * self.N + 2, 3 * self.N] = self.current_rotation[1, 0] A[2 * self.N + 2, 3 * self.N + 2] = self.current_rotation[1, 1] A[2 * self.N + 3, 3 * self.N] = -self.current_rotation[1, 0] A[2 * self.N + 3, 3 * self.N + 1] = self.current_rotation[1, 0] A[2 * self.N + 3, 3 * self.N + 2] = -self.current_rotation[1, 1] A[2 * self.N + 3, 3 * self.N + 3] = self.current_rotation[1, 1] #max angle between feet constraints A[2 * self.N + 4, 3 * self.N + 4] = 1 A[2 * self.N + 5, 3 * self.N + 4] = -1 A[2 * self.N + 5, 3 * self.N + 5] = 1 #QP solver takes one dim arrays lbA = lbA.reshape((lbA.shape[0], )) ubA = ubA.reshape((ubA.shape[0], )) #############################################################################HESSIAN AND GRADIENT#################### H = np.zeros((self.n_dec, self.n_dec)) H[0:3*self.N, 0:3*self.N] = self.alpha*np.ones((3*self.N, 3*self.N)) + self.beta*np.dot(np.dot(Svx, Uu).T, np.dot(Svx, Uu)) + self.beta*np.dot(np.dot(Svy, Uu).T, np.dot(Svy, Uu)) + \ + self.beta*np.dot(np.dot(Svt, Uu).T, np.dot(Svt, Uu)) + self.gamma*np.dot(np.dot(np.dot(Scx, D_diag), Uu).T, np.dot(np.dot(Scx, D_diag), Uu)) + \ + self.gamma*np.dot(np.dot(np.dot(Scy, D_diag), Uu).T, np.dot(np.dot(Scy, D_diag), Uu)) + self.gamma*np.dot(np.dot(St, Uu).T, np.dot(St, Uu)) H[0:3 * self.N, 3 * self.N:] = np.hstack( (-self.gamma * np.dot(np.dot(np.dot(Scx, D_diag), Uu).T, V_dash), -self.gamma * np.dot(np.dot(np.dot(Scy, D_diag), Uu).T, V_dash), -self.gamma * np.dot(np.dot(St, Uu).T, V_dash))) H[3 * self.N:, 0:3 * self.N] = np.vstack( (-self.gamma * np.dot(V_dash.T, np.dot(np.dot(Scx, D_diag), Uu)), -self.gamma * np.dot(V_dash.T, np.dot(np.dot(Scy, D_diag), Uu)), -self.gamma * np.dot(V_dash.T, np.dot(St, Uu)))) H[3 * self.N:, 3 * self.N:] = self.gamma * block_diag( np.dot(V_dash.T, V_dash), np.dot(V_dash.T, V_dash), np.dot(V_dash.T, V_dash)) g = np.zeros((self.n_dec, 1)) g[0:3*self.N, :] = self.beta*np.dot(np.dot(Svx, Uu).T, np.dot(np.dot(Svx, Ux), self.x.T) - vref_pred[:, 0][np.newaxis].T) + self.beta*np.dot(np.dot(Svy, Uu).T, np.dot(np.dot(Svy, Ux), self.x.T) - vref_pred[:, 1][np.newaxis].T) + \ self.beta*np.dot(np.dot(Svt, Uu).T, np.dot(np.dot(Svt, Ux), self.x.T) - vref_pred[:, 2][np.newaxis].T) + self.gamma*np.dot(np.dot(np.dot(Scx, D_diag), Uu).T, np.dot(np.dot(np.dot(Scx, D_diag), Ux), self.x.T) - np.dot(V, self.f_current[0][0])) + \ + self.gamma*np.dot(np.dot(np.dot(Scy, D_diag), Uu).T, np.dot(np.dot(np.dot(Scy, D_diag), Ux), self.x.T) - np.dot(V, self.f_current[0][1])) + self.gamma*np.dot(np.dot(St, Uu).T, np.dot(np.dot(St, Ux), self.x.T)) - \ - self.gamma*np.dot(np.dot(St, Uu).T, np.dot(V, self.f_current[0][2])) g[3 * self.N:50, :] = -self.gamma * np.dot( V_dash.T, np.dot(np.dot(np.dot(Scx, D_diag), Ux), self.x.T) - np.dot(V, self.f_current[0][0])) g[50:52, :] = -self.gamma * np.dot( V_dash.T, np.dot(np.dot(np.dot(Scy, D_diag), Ux), self.x.T) - np.dot(V, self.f_current[0][1])) g[52:, :] = -self.gamma * np.dot( V_dash.T, np.dot(np.dot(St, Ux), self.x.T) - np.dot(V, self.f_current[0][2])) g = g.reshape((g.shape[0], )) ########################################################################################################################### #solver options - MPC myOptions = Options() myOptions.setToMPC() self.QP.setOptions(myOptions) #setting lb and ub to huge numbers - otherwise solver gives errors lb = -1000000000. * np.ones((self.n_dec, )) ub = 1000000000. * np.ones((self.n_dec, )) #solve QP - QP.init() self.QP.init(H, g, A, lb, ub, lbA, ubA, nWSR=np.array([1000000])) #get the solution (getPrimalSolution()) x_opt = np.zeros(self.n_dec) self.QP.getPrimalSolution(x_opt) #update the state of the system, CoP and the footstep (take the first elements of the solution) self.controls = np.array([[x_opt[0], x_opt[1], x_opt[2]]]) self.x = (np.dot(self.model.A(self.T), self.x.T) + np.dot(self.model.B(self.T), self.controls.T)).T self.CoP = np.dot(self.model.D(self.T), self.x.T) #first future step self.f_future = np.array([[ x_opt[3 * self.N], x_opt[3 * self.N + self.future_steps], x_opt[3 * self.N + 2 * self.future_steps] ]]) #change the current (every 8 samples except the first step) foot position if np.mod(i, self.samples_per_step) == self.samples_per_step - 2: self.f_current = self.f_future #rotate the footstep bounds 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])]]) #swap footstep constraints tmp1, tmp2 = self.feet_ubounds[1], self.feet_lbounds[1] self.feet_ubounds[1], self.feet_lbounds[1] = self.feet_ubounds[ 3], self.feet_lbounds[3] self.feet_ubounds[3], self.feet_lbounds[3] = tmp1, tmp2 #return all the variables of interest return [self.x, self.f_current, self.CoP, self.controls]
def main(): rospy.init_node('controller_2dof', anonymous=True) rospy.Rate(10) # Setup data of QP. # Joint Weights. w1 = 1e-3 w2 = 1e-3 # Joint limits. q0_max = 0.05 q1_max = 0.15 # Links l1 = 0.1 l2 = 0.2 l3 = 0.3 # Initial joint values. q0_init = q0 = 0.01 q1_init = q1 = -0.02 # Joint target. q_des1 = 0.45 q_des2 = 0.78 # Slack limits. e1_max = 1000 e2_max = 1000 # Velocity limits.(+-) v0_max = 0.05 v1_max = 0.1 # Acceleration limits.(+-) a0_max = 0.05 * 0.5 a1_max = 0.1 * 0.5 # Others precision = 1e-2 p = 10 q_eef_init = q_eef = l1 + l2 + l3 + q0 + q1 error1 = p * (q_des1 - q_eef) error2 = p * (q_des2 - q_eef) vel_init = 0 nWSR = np.array([100]) # Dynamic goal weights w3 = (error2 / (error1 + error2))**2 # goal 1 weight w4 = (error1 / (error1 + error2))**2 # goal 2 weight # Acceleration a0_const = (v0_max - a0_max) / v0_max a1_const = (v1_max - a1_max) / v1_max example = SQProblem(4, 6) H = np.array([ w1, 0.0, 0.0, 0.0, 0.0, w2, 0.0, 0.0, 0.0, 0.0, w3, 0.0, 0.0, 0.0, 0.0, w4 ]).reshape((4, 4)) A = np.array([ 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 ]).reshape((6, 4)) g = np.array([0.0, 0.0, 0.0, 0.0]) lb = np.array([-v0_max, -v1_max, -e1_max, -e2_max]) ub = np.array([v0_max, v1_max, e1_max, e2_max]) lbA = np.array( [error1, error2, (-q0_max - q0), (-q1_max - q0), -a0_max, -a1_max]) ubA = np.array( [error1, error2, (q0_max - q0), (q1_max - q0), a0_max, a1_max]) # Setting up QProblem object options = Options() options.setToReliable() options.printLevel = PrintLevel.LOW example.setOptions(options) Opt = np.zeros(4) print( "Init pos = %g, goal_1 = %g, goal_2 = %g, error_1 = %g, error_2 = %g, q0 =%g, q1 = %g\n" % (q_eef, q_des1, q_des2, error1, error2, q0, q1)) print A i = 0 # Stopping conditions limit1 = abs(error1) limit2 = abs(error2) lim = min([limit1, limit2]) diff1 = abs(error1) diff2 = abs(error2) diff = min([diff1, diff2]) # Plotting t = np.array(i) pos_eef = np.array(q_eef) pos_0 = np.array(q0) pos_1 = np.array(q1) vel_0 = np.array(vel_init) vel_1 = np.array(vel_init) vel_eef = np.array(vel_init) p_error = np.array(error1) r_max = np.array(q_des1) r_min = np.array(q_des2) return_value = example.init(H, g, A, lb, ub, lbA, ubA, nWSR) if return_value != returnValue.SUCCESSFUL_RETURN: print "Init of QP-Problem returned without success! ERROR MESSAGE: " return -1 while not rospy.is_shutdown(): #while (diff1 > 0.00005 or diff2 > 0.0005) and limit1 > precision and limit2 > precision and i < 400: while diff > 0.00005 and lim > precision and i < 400: # Solve QP. i += 1 nWSR = np.array([100]) lbA[0] = error1 lbA[1] = error2 lbA[2] = -q0_max - q0 lbA[3] = -q1_max - q1 lbA[4] = a0_const * Opt[0] - a0_max lbA[5] = a1_const * Opt[1] - a1_max ubA[0] = error1 ubA[1] = error2 ubA[2] = q0_max - q0 ubA[3] = q1_max - q1 ubA[4] = a0_const * Opt[0] + a0_max ubA[5] = a1_const * Opt[1] + a1_max return_value = example.hotstart(H, g, A, lb, ub, lbA, ubA, nWSR) if return_value != returnValue.SUCCESSFUL_RETURN: print "Hotstart of QP-Problem returned without success! ERROR MESSAGE: " return -1 old_error1 = error1 old_error2 = error2 # Update limits example.getPrimalSolution(Opt) q0 += Opt[0] / 100 q1 += Opt[1] / 100 q_eef = l1 + l2 + l3 + q0 + q1 error1 = p * (q_des1 - q_eef) error2 = p * (q_des2 - q_eef) # Update weights w3 = (error2 / (error1 + error2))**2 w4 = (error1 / (error1 + error2))**2 H = np.array([ w1, 0.0, 0.0, 0.0, 0.0, w2, 0.0, 0.0, 0.0, 0.0, w3, 0.0, 0.0, 0.0, 0.0, w4 ]).reshape((4, 4)) # Stopping conditions limit1 = abs(error1) limit2 = abs(error2) lim = min([limit1, limit2]) diff1 = abs(error1 - old_error1) diff2 = abs(error2 - old_error2) diff = min([diff1, diff2]) #print "\nOpt = [ %g, %g, %g, %g ] \n posit= %g, w3= %g, w4= %g, q0= %g q1= %g \n" % ( #Opt[0], Opt[1], Opt[2], Opt[3], q_eef, w3, w4, q0, q1) print w3, w4 # Plotting arrays pos_eef = np.hstack((pos_eef, q_eef)) pos_0 = np.hstack((pos_0, q0)) pos_1 = np.hstack((pos_1, q1)) vel_0 = np.hstack((vel_0, Opt[0])) vel_1 = np.hstack((vel_1, Opt[1])) vel_eef = np.hstack((vel_eef, Opt[0] + Opt[1])) p_error = np.hstack((p_error, error1)) r_max = np.hstack((r_max, q_des1)) r_min = np.hstack((r_min, q_des2)) t = np.hstack((t, i)) # Plot t_eef = go.Scatter(y=pos_eef, x=t, marker=dict(size=4, ), mode='lines', name='pos_eef', line=dict(dash='dash')) t_p0 = go.Scatter(y=pos_0, x=t, marker=dict(size=4, ), mode='lines', name='pos_0', line=dict(dash='dash')) t_p1 = go.Scatter(y=pos_1, x=t, marker=dict(size=4, ), mode='lines', name='pos_1', line=dict(dash='dash')) t_v0 = go.Scatter(y=vel_0, x=t, marker=dict(size=4, ), mode='lines', name='vel_0') t_v1 = go.Scatter(y=vel_1, x=t, marker=dict(size=4, ), mode='lines', name='vel_1') t_veef = go.Scatter(y=vel_eef, x=t, marker=dict(size=4, ), mode='lines', name='vel_eef') t_er = go.Scatter(y=p_error, x=t, marker=dict(size=4, ), mode='lines', name='error') t_g1 = go.Scatter(y=r_max, x=t, marker=dict(size=4, ), mode='lines', name='goal_1', line=dict(dash='dot')) t_g2 = go.Scatter(y=r_min, x=t, marker=dict(size=4, ), mode='lines', name='goal_2', line=dict(dash='dot', width=4)) data = [t_eef, t_veef, t_g1, t_g2] layout = dict( title="Initial position EEF = %g. Goals: goal_1 = %g, goal_2 = %g" % (q_eef_init, q_des1, q_des2), xaxis=dict( title='Iterations', autotick=False, dtick=25, gridwidth=3, ), font=dict(size=18), yaxis=dict(title='Position / Velocity'), ) fig = dict(data=data, layout=layout) plotly.offline.plot(fig, filename='html/dynamic_weights_eef.html', image='png', image_filename='dynamic_2') data = [t_p0, t_v0] layout = dict(title="Initial position q0 =%g." % (q0_init), xaxis=dict( title='Iterations', autotick=False, dtick=25, gridwidth=3, ), yaxis=dict(title='Position / Velocity', gridwidth=3, range=[-0.15, 0.05]), font=dict(size=18)) fig = dict(data=data, layout=layout) plotly.offline.plot(fig, filename='html/dynamic_weights_q0.html', image='png', image_filename='dynamic_3') data = [t_p1, t_v1] layout = dict( title="Initial position q1 = %g." % (q1_init), xaxis=dict( title='Iterations', autotick=False, dtick=25, gridwidth=3, ), yaxis=dict(title='Position / Velocity', gridwidth=3, range=[-0.15, 0.05]), font=dict(size=18), ) fig = dict(data=data, layout=layout) plotly.offline.plot(fig, filename='html/dynamic_weights_q1.html', image='png', image_filename='dynamic_4') print "\n i = %g \n" % i return 0
# WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR # A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more # details. # # You should have received a copy of the GNU Lesser General Public License # along with qpsolvers. If not, see <http://www.gnu.org/licenses/>. from numpy import array, hstack, ones, vstack, zeros from qpoases import PyOptions as Options from qpoases import PyPrintLevel as PrintLevel from qpoases import PyQProblem as QProblem from qpoases import PyQProblemB as QProblemB from qpoases import PyReturnValue as ReturnValue __infty__ = 1e10 __options__ = Options() __options__.printLevel = PrintLevel.NONE def qpoases_solve_qp(P, q, G=None, h=None, A=None, b=None, initvals=None, verbose=False, max_wsr=1000): """ Solve a Quadratic Program defined as:
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 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_example2(self): # Example for qpOASES main function using the SQProblem 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. H_new = np.array([ 1.0, 0.5, 0.5, 0.5 ]).reshape((2,2)) A_new = np.array([ 1.0, 5.0 ]).reshape((2,1)) 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 SQProblem object and solution analyser. qp = SQProblem(2, 1) options = Options() options.printLevel = PrintLevel.NONE qp.setOptions(options) analyser = SolutionAnalysis() # get c++ solution from std cmd = os.path.join(bin_path, "example2") p = Popen(cmd, shell=True, stdout=PIPE) stdout, stderr = p.communicate() stdout = str(stdout).replace('\\n', '\n') stdout = stdout.replace("'", '') print(stdout) # Solve first QP ... nWSR = 10 qp.init(H, g, A, lb, ub, lbA, ubA, nWSR) # ... and analyse it. maxKKTviolation = np.zeros(1) analyser.getMaxKKTviolation(qp, maxKKTviolation) print("maxKKTviolation: %e\n"%maxKKTviolation) actual = np.asarray(maxKKTviolation) pattern = re.compile(r'maxKKTviolation: (?P<maxKKTviolation>[0-9+-e.]*)') match = pattern.findall(stdout) expected = np.asarray(match[0], dtype=float) assert_almost_equal(actual, expected, decimal=7) # Solve second QP ... nWSR = 10 qp.hotstart(H_new, g_new, A_new, lb_new, ub_new, lbA_new, ubA_new, nWSR) # ... and analyse it. analyser.getMaxKKTviolation(qp, maxKKTviolation) print("maxKKTviolation: %e\n"%maxKKTviolation) actual = np.asarray(maxKKTviolation) expected = np.asarray(match[1], dtype=float) assert_almost_equal(actual, expected, decimal=7) # ------------ VARIANCE-COVARIANCE EVALUATION -------------------- Var = np.zeros(5*5) Primal_Dual_Var = np.zeros(5*5) Var.reshape((5,5))[0,0] = 1. Var.reshape((5,5))[1,1] = 1. # ( 1 0 0 0 0 ) # ( 0 1 0 0 0 ) # Var = ( 0 0 0 0 0 ) # ( 0 0 0 0 0 ) # ( 0 0 0 0 0 ) analyser.getVarianceCovariance(qp, Var, Primal_Dual_Var) print('Primal_Dual_Var=\n', Primal_Dual_Var.reshape((5,5))) actual = Primal_Dual_Var.reshape((5,5)) pattern = re.compile(r'Primal_Dual_VAR = (?P<VAR>.*)', re.DOTALL) print(stdout) match = pattern.search(stdout) expected = match.group('VAR').strip().split("\n") expected = [x.strip().split() for x in expected] print(expected) expected = np.asarray(expected, dtype=float) assert_almost_equal(actual, expected, decimal=7)
def __init__(self, N=16, T=0.1, T_step=0.8, fsm_state='D', fsm_sl=1): """ Initialize pattern generator matrices through base class and allocate two QPs one for optimzation of orientation and one for position of CoM and feet. """ super(ClassicGenerator, self).__init__(N, T, T_step, fsm_state, fsm_sl) # TODO for speed up one can define members of BaseGenerator as # direct views of QP data structures according to walking report # Maybe do that later! # The pattern generator has to solve the following kind of # problem in each iteration # min_x 1/2 * x^T * H(w0) * x + x^T g(w0) # s.t. lbA(w0) <= A(w0) * x <= ubA(w0) # lb(w0) <= x <= ub(wo) # Because of varying H and A, we have to use the # SQPProblem class, which supports this kind of QPs # rename for convenience N = self.N nf = self.nf # define some qpOASES specific things self.cpu_time = 0.1 # upper bound on CPU time, 0 is no upper limit self.nwsr = 100 # number of working set recalculations self.options = Options() self.options.setToMPC() self.options.printLevel = PrintLevel.LOW # FOR ORIENTATIONS # define dimensions self.ori_nv = 2 * self.N self.ori_nc = (self.nc_fvel_eq + self.nc_fpos_ineq + self.nc_fvel_ineq) # setup problem self.ori_dofs = numpy.zeros(self.ori_nv) self.ori_qp = SQProblem(self.ori_nv, self.ori_nc) self.ori_qp.setOptions(self.options) # load NMPC options self.ori_H = numpy.zeros((self.ori_nv, self.ori_nv)) self.ori_A = numpy.zeros((self.ori_nc, self.ori_nv)) self.ori_g = numpy.zeros((self.ori_nv, )) self.ori_lb = -numpy.ones((self.ori_nv, )) * 1e+08 self.ori_ub = numpy.ones((self.ori_nv, )) * 1e+08 self.ori_lbA = -numpy.ones((self.ori_nc, )) * 1e+08 self.ori_ubA = numpy.ones((self.ori_nc, )) * 1e+08 self._ori_qp_is_initialized = False # save computation time and working set recalculations self.ori_qp_nwsr = 0.0 self.ori_qp_cputime = 0.0 # FOR POSITIONS # define dimensions self.pos_nv = 2 * (self.N + self.nf) self.pos_nc = (self.nc_cop + self.nc_foot_position + self.nc_fchange_eq) # setup problem self.pos_dofs = numpy.zeros(self.pos_nv) self.pos_qp = SQProblem(self.pos_nv, self.pos_nc) self.pos_qp.setOptions(self.options) self.pos_H = numpy.zeros((self.pos_nv, self.pos_nv)) self.pos_A = numpy.zeros((self.pos_nc, self.pos_nv)) self.pos_g = numpy.zeros((self.pos_nv, )) self.pos_lb = -numpy.ones((self.pos_nv, )) * 1e+08 self.pos_ub = numpy.ones((self.pos_nv, )) * 1e+08 self.pos_lbA = -numpy.ones((self.pos_nc, )) * 1e+08 self.pos_ubA = numpy.ones((self.pos_nc, )) * 1e+08 self._pos_qp_is_initialized = False # save computation time and working set recalculations self.pos_qp_nwsr = 0.0 self.pos_qp_cputime = 0.0 # dummy matrices self._ori_Q = numpy.zeros((2 * self.N, 2 * self.N)) self._ori_p = numpy.zeros((2 * self.N, )) self._pos_Q = numpy.zeros((self.N + self.nf, self.N + self.nf)) self._pos_p = numpy.zeros((self.N + self.nf, )) # add additional keys that should be saved self._data_keys.append('ori_qp_nwsr') self._data_keys.append('ori_qp_cputime') self._data_keys.append('pos_qp_nwsr') self._data_keys.append('pos_qp_cputime') # reinitialize plot data structure self.data = PlotData(self)
def test_qp_setup_with_toy_example_hack_from_qpoases_manual(self): gen = ClassicGenerator() options = Options() options.printLevel = PrintLevel.LOW # define matrices from qpoases manual H = numpy.array([1.0, 0.0, 0.0, 0.5]).reshape((2, 2)) A = numpy.array([1.0, 1.0]).reshape((1, 2)) g = numpy.array([1.5, 1.0]) lb = numpy.array([0.5, -2.0]) ub = numpy.array([5.0, 2.0]) lbA = numpy.array([-1.0]) ubA = numpy.array([2.0]) x = numpy.array([0.5, -1.5]) f = -6.25e-02 # hack pattern generator gen.ori_nv = 2 gen.ori_nc = 1 gen.ori_dofs = numpy.zeros((2, )) gen.ori_qp = SQProblem(gen.ori_nv, gen.ori_nc) gen.ori_qp.setOptions(options) gen.ori_H = H gen.ori_A = A gen.ori_g = g gen.ori_lb = lb gen.ori_ub = ub gen.ori_lbA = lbA gen.ori_ubA = ubA gen.pos_nv = 2 gen.pos_nc = 1 gen.pos_dofs = numpy.zeros((2, )) gen.pos_qp = SQProblem(gen.pos_nv, gen.pos_nc) gen.pos_qp.setOptions(options) gen.pos_H = H gen.pos_A = A gen.pos_g = g gen.pos_lb = lb gen.pos_ub = ub gen.pos_lbA = lbA gen.pos_ubA = ubA # test first qp solution gen._solve_qp() # get solution # NOTE post_process put entries into array that dont match anymore gen.pos_qp.getPrimalSolution(gen.pos_dofs) gen.ori_qp.getPrimalSolution(gen.ori_dofs) assert_allclose(gen.pos_dofs, x, rtol=RTOL, atol=ATOL) assert_allclose(gen.pos_qp.getObjVal(), f, rtol=RTOL, atol=ATOL) assert_allclose(gen.ori_dofs, x, rtol=RTOL, atol=ATOL) assert_allclose(gen.ori_qp.getObjVal(), f, rtol=RTOL, atol=ATOL) # define matrices for warmstart H_new = numpy.array([1.0, 0.5, 0.5, 0.5]).reshape((2, 2)) A_new = numpy.array([1.0, 5.0]).reshape((1, 2)) g_new = numpy.array([1.0, 1.5]) lb_new = numpy.array([0.0, -1.0]) ub_new = numpy.array([5.0, -0.5]) lbA_new = numpy.array([-2.0]) ubA_new = numpy.array([1.0]) x = numpy.array([0.5, -0.5]) f = -1.875e-01 # hack pattern generator gen.ori_H = H_new gen.ori_A = A_new gen.ori_g = g_new gen.ori_lb = lb_new gen.ori_ub = ub_new gen.ori_lbA = lbA_new gen.pos_H = H_new gen.pos_A = A_new gen.pos_g = g_new gen.pos_lb = lb_new gen.pos_ub = ub_new gen.pos_lbA = lbA_new # test qp warmstart gen._solve_qp() # get solution # NOTE post_process put entries into array that dont match anymore gen.pos_qp.getPrimalSolution(gen.pos_dofs) gen.ori_qp.getPrimalSolution(gen.ori_dofs) assert_allclose(gen.pos_dofs, x, rtol=RTOL, atol=ATOL) assert_allclose(gen.pos_qp.getObjVal(), f, rtol=RTOL, atol=ATOL) assert_allclose(gen.ori_dofs, x, rtol=RTOL, atol=ATOL) assert_allclose(gen.ori_qp.getObjVal(), f, rtol=RTOL, atol=ATOL)
def main(): rospy.init_node('controller_2dof', anonymous=True) rospy.Rate(10) # Setup data of QP. # Joint Weights. w1 = 1e-3 w2 = 1e-3 # Joint limits. q0_max = 0.05 q1_max = 0.15 # Links l1 = 0.1 l2 = 0.2 l3 = 0.25 # Initial joint values. q0_init = q0 = -0.03 q1_init = q1 = 0.0 # Joint target. q_des1 = 0.4 q_des2 = 0.7 q_des3 = 0.8 # Slack limits. e1_max = 1000 e2_max = 1000 e3_max = 1000 # Velocity limits.(+-) v0_max = 0.05 v1_max = 0.1 # Acceleration limits.(+-) a0_max = 0.05 * 0.5 a1_max = 0.1 * 0.5 # Others precision = 1e-3 p = 10 q_eef_init = q_eef = l1 + l2 + l3 + q0 + q1 error1 = p * (q_des1 - q_eef) error2 = p * (q_des2 - q_eef) error3 = p * (q_des3 - q_eef) vel_init = 0 nWSR = np.array([100]) # Dynamic goal weights sum_error = abs(error1) + abs(error2) + abs(error3) min_error = min(abs(error1), abs(error2), abs(error3)) max_error = max(abs(error1), abs(error2), abs(error3)) w3 = (1 - (abs(error1) - min_error) / (max_error - min_error))**3 # goal 1 weight w4 = (1 - (abs(error2) - min_error) / (max_error - min_error))**3 # goal 2 weight w5 = (1 - (abs(error3) - min_error) / (max_error - min_error))**3 # goal 3 weight # Acceleration a0_const = (v0_max - a0_max) / v0_max a1_const = (v1_max - a1_max) / v1_max example = SQProblem(5, 7) H = np.array([ w1, 0.0, 0.0, 0.0, 0.0, 0.0, w2, 0.0, 0.0, 0.0, 0.0, 0.0, w3, 0.0, 0.0, 0.0, 0.0, 0.0, w4, 0.0, 0.0, 0.0, 0.0, 0.0, w5 ]).reshape((5, 5)) A = np.array([ 1.0, 1.0, 1.0, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0, 0.0, 1.0, 1.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0 ]).reshape((7, 5)) g = np.array([0.0, 0.0, 0.0, 0.0, 0.0]) lb = np.array([-v0_max, -v1_max, -e1_max, -e2_max, -e3_max]) ub = np.array([v0_max, v1_max, e1_max, e2_max, e3_max]) lbA = np.array([ error1, error2, error3, (-q0_max - q0), (-q1_max - q0), -a0_max, -a1_max ]) ubA = np.array( [error1, error2, error3, (q0_max - q0), (q1_max - q0), a0_max, a1_max]) # Setting up QProblem object options = Options() options.setToReliable() options.printLevel = PrintLevel.LOW example.setOptions(options) Opt = np.zeros(5) print( "Init pos = %g, goal_1 = %g, goal_2 = %g, error_1 = %g, error_2 = %g, q0 =%g, q1 = %g\n" % (q_eef, q_des1, q_des2, error1, error2, q0, q1)) print 'A = {}\n'.format(A) i = 0 # Stopping conditions limit1 = abs(error1) limit2 = abs(error2) limit3 = abs(error3) lim = min([limit1, limit2, limit3]) diff1 = abs(error1) diff2 = abs(error2) diff3 = abs(error3) diff = min([diff1, diff2, diff3]) # Plotting t = np.array(i) pos_eef = np.array(q_eef) pos_0 = np.array(q0) pos_1 = np.array(q1) vel_0 = np.array(vel_init) vel_1 = np.array(vel_init) vel_eef = np.array(vel_init) p_error = np.array(error1) go_1 = np.array(q_des1) go_2 = np.array(q_des2) go_3 = np.array(q_des3) return_value = example.init(H, g, A, lb, ub, lbA, ubA, nWSR) if return_value != returnValue.SUCCESSFUL_RETURN: print "Init of QP-Problem returned without success! ERROR MESSAGE: " return -1 while not rospy.is_shutdown(): while (diff > 0.0009) and lim > precision and i < 400: # Solve QP. i += 1 nWSR = np.array([100]) lbA[0] = error1 lbA[1] = error2 lbA[2] = error3 lbA[3] = -q0_max - q0 lbA[4] = -q1_max - q1 lbA[5] = a0_const * Opt[0] - a0_max lbA[6] = a1_const * Opt[1] - a1_max ubA[0] = error1 ubA[1] = error2 ubA[2] = error3 ubA[3] = q0_max - q0 ubA[4] = q1_max - q1 ubA[5] = a0_const * Opt[0] + a0_max ubA[6] = a1_const * Opt[1] + a1_max return_value = example.hotstart(H, g, A, lb, ub, lbA, ubA, nWSR) if return_value != returnValue.SUCCESSFUL_RETURN: print "Hotstart of QP-Problem returned without success! ERROR MESSAGE: " return -1 old_error1 = error1 old_error2 = error2 old_error3 = error3 example.getPrimalSolution(Opt) # Update limits q0 += Opt[0] / 100 q1 += Opt[1] / 100 q_eef = l1 + l2 + l3 + q0 + q1 error1 = p * (q_des1 - q_eef) error2 = p * (q_des2 - q_eef) error3 = p * (q_des3 - q_eef) # Update weights min_error = min(abs(error1), abs(error2), abs(error3)) max_error = max(abs(error1), abs(error2), abs(error3)) w3 = (1 - (abs(error1) - min_error) / (max_error - min_error))**3 # goal 1 w4 = (1 - (abs(error2) - min_error) / (max_error - min_error))**3 # goal 2 w5 = (1 - (abs(error3) - min_error) / (max_error - min_error))**3 # goal 3 H = np.array([ w1, 0.0, 0.0, 0.0, 0.0, 0.0, w2, 0.0, 0.0, 0.0, 0.0, 0.0, w3, 0.0, 0.0, 0.0, 0.0, 0.0, w4, 0.0, 0.0, 0.0, 0.0, 0.0, w5 ]).reshape((5, 5)) # Stopping conditions limit1 = abs(error1) limit2 = abs(error2) limit3 = abs(error3) lim = min([limit1, limit2, limit3]) diff1 = abs(error1 - old_error1) diff2 = abs(error2 - old_error2) diff3 = abs(error3 - old_error3) diff = min([diff1, diff2, diff3]) # print 'weights= [ %g, %g, %g ]'%(w3, w4, w5) # print 'vel = [ %g, %g ], error = [ %g, %g, %g ] \n'% (Opt[0], Opt[1], error1, error2, error3) # Plotting arrays pos_eef = np.hstack((pos_eef, q_eef)) pos_0 = np.hstack((pos_0, q0)) pos_1 = np.hstack((pos_1, q1)) vel_0 = np.hstack((vel_0, Opt[0])) vel_1 = np.hstack((vel_1, Opt[1])) vel_eef = np.hstack((vel_eef, Opt[0] + Opt[1])) p_error = np.hstack((p_error, error1)) go_1 = np.hstack((go_1, q_des1)) go_2 = np.hstack((go_2, q_des2)) go_3 = np.hstack((go_3, q_des3)) t = np.hstack((t, i)) # Plot t_eef = go.Scatter(y=pos_eef, x=t, marker=dict(size=4, ), mode='lines', name='pos_eef', line=dict(dash='dash')) t_p0 = go.Scatter(y=pos_0, x=t, marker=dict(size=4, ), mode='lines', name='pos_0', line=dict(dash='dash')) t_p1 = go.Scatter(y=pos_1, x=t, marker=dict(size=4, ), mode='lines', name='pos_1', line=dict(dash='dash')) t_v0 = go.Scatter(y=vel_0, x=t, marker=dict(size=4, ), mode='lines', name='vel_0') t_v1 = go.Scatter(y=vel_1, x=t, marker=dict(size=4, ), mode='lines', name='vel_1') t_er = go.Scatter(y=p_error, x=t, marker=dict(size=4, ), mode='lines', name='error') t_veef = go.Scatter(y=vel_eef, x=t, marker=dict(size=4, ), mode='lines', name='vel_eef') t_g1 = go.Scatter(y=go_1, x=t, marker=dict(size=4, ), mode='lines', name='goal_1', line=dict(dash='dot')) t_g2 = go.Scatter(y=go_2, x=t, marker=dict(size=4, ), mode='lines', name='goal_2', line=dict(dash='dot', width=4)) t_g3 = go.Scatter(y=go_3, x=t, marker=dict(size=4, ), mode='lines', name='goal_3', line=dict(dash='dot', width=6)) data = [t_eef, t_veef, t_g1, t_g2, t_g3] layout = dict( title= "Initial position eef = %g. Goals: goal_1 = %g, goal_2 = %g, goal_3 = %g\n" % (q_eef_init, q_des1, q_des2, q_des3), xaxis=dict( title='Iterations', autotick=False, dtick=25, gridwidth=2, ), yaxis=dict(title='Position / Velocity'), font=dict(size=18), ) fig = dict(data=data, layout=layout) plotly.offline.plot(fig, filename='html/dynamic_3_weights_EEF.html', image='png', image_filename='dyn_3_eef_2') data = [t_p0, t_v0] layout = dict( title="Initial position q0 =%g.\n" % (q0_init), xaxis=dict(title='Iterations', autotick=False, dtick=25, gridwidth=2), yaxis=dict( title='Position / Velocity', range=[-0.11, .045], ), font=dict(size=18), ) fig = dict(data=data, layout=layout) plotly.offline.plot(fig, filename='html/dynamic_3_weights_q0.html') data = [t_p1, t_v1] layout = dict( title="Initial position q1 = %g.\n" % (q1_init), xaxis=dict(title='Iterations', autotick=False, dtick=25, gridwidth=2), yaxis=dict( title='Position / Velocity', range=[-0.11, .045], ), font=dict(size=18), ) fig = dict(data=data, layout=layout) plotly.offline.plot(fig, filename='html/dynamic_3_weights__q1.html') print "\n i = %g \n" % i return 0
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.enableCholeskyRefactorisation = 1 options.printLevel = PrintLevel.NONE qp.setOptions(options) # Solve first QP. nWSR = 10 qp.init(H, g, lb, ub, nWSR) 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) print 'xOpt_actual:', xOpt_actual print 'objVal_actual:', objVal_actual # Solve second QP. nWSR = 10 qp.hotstart(g_new, lb_new, ub_new, nWSR) 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) print 'xOpt_actual:', xOpt_actual print 'objVal_actual:', objVal_actual # 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("'", '') # get c++ solution from std pattern = re.compile(r'xOpt\s*=\s*\[\s+(?P<xOpt>([0-9., e+-])*)\];') match = pattern.findall(stdout) xOpt_expected = match[-1][0] 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.findall(stdout) print match objVal_expected = match[-1] 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 main(): rospy.init_node('controller_2dof', anonymous=True) rospy.Rate(10) # rospy.sleep(0.2) # Setup data of QP. # Weights. w1 = 1e-3 w2 = 1e-3 w3 = 1 w4 = 1 # joint goal # Joint limits. q0_max = 0.05 q1_max = 0.15 # Links l1 = 0.1 l2 = 0.2 l3 = 0.3 # Initial joint values. q0_init = q0 = 0.02 q1_init = q1 = -0.15 # Joint target. q_des = 0.6 q0_des = -0.025 q1_des = 0.05 q0_goal = True q1_goal = False # Slack limits. e_max = 1000 e0_max = 1000 # Velocity limits.(+-) v0_max = 0.05 v1_max = 0.1 # Acceleration limits.(+-) a0_max = 0.05 * 0.5 a1_max = 0.1 * 0.5 # Others precision = 5e-3 joint_precision = 5e-3 p = 10 pj = 5 q_eef_init = q_eef = l1 + l2 + l3 + q0 + q1 error = p * (q_des - q_eef) vel_init = 0 nWSR = np.array([100]) # Acceleration a0_const = (v0_max - a0_max) / v0_max a1_const = (v1_max - a1_max) / v1_max example = SQProblem(4, 7) H = np.array([ w1, 0.0, 0.0, 0.0, 0.0, w2, 0.0, 0.0, 0.0, 0.0, w3, 0.0, 0.0, 0.0, 0.0, w4 ]).reshape((4, 4)) A = np.array([ 1.0, 1.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]).reshape((7, 4)) g = np.array([0.0, 0.0, 0.0, 0.0]) lb = np.array([-v0_max, -v1_max, -e_max, -e0_max]) ub = np.array([v0_max, v1_max, e_max, e0_max]) lbA = np.array( [error, (-q0_max - q0), (-q1_max - q0), -a0_max, -a1_max, 0, 0]) ubA = np.array([error, (q0_max - q0), (q1_max - q0), a0_max, a1_max, 0, 0]) # Setting up QProblem object. if q0_goal is False and q1_goal is False: print("\nNo joint goal specified\n") return -1 elif (q0_goal is True and q1_goal is False) or (q0_goal is False and q1_goal is True): if q0_goal is True: lbA[5] = (q0_des - q0) ubA[5] = (q0_des - q0) # A[0, 0] = 0.0 A[5, 0] = 1.0 A[5, 3] = 1.0 else: lbA[5] = (q1_des - q1) ubA[5] = (q1_des - q1) A[5, 1] = 1.0 A[5, 3] = 1.0 else: A[0, 0] = 0.0 A[0, 1] = 0.0 A[0, 2] = 0.0 A[5, 0] = 1.0 A[5, 2] = 1.0 A[6, 1] = 1.0 A[6, 3] = 1.0 p = 0 error = 0 lbA[0] = 0 ubA[0] = 0 options = Options() options.setToReliable() options.printLevel = PrintLevel.LOW example.setOptions(options) print("Init pos = %g, goal = %g, error = %g, q0 =%g, q1 = %g\n" % (q_eef, q_des, error, q0, q1)) print A i = 0 limit = abs(error) ok = False Opt = np.zeros(4) # Plotting t = np.array(i) pos_eef = np.array(q_eef) pos_0 = np.array(q0) pos_1 = np.array(q1) vel_0 = np.array(vel_init) vel_1 = np.array(vel_init) vel_eef = np.array(vel_init) p_error = np.array(error) eef_goal = np.array(q_des) q0_set = np.array(q0_des) q1_set = np.array(q1_des) return_value = example.init(H, g, A, lb, ub, lbA, ubA, nWSR) if return_value != returnValue.SUCCESSFUL_RETURN: print "Init of QP-Problem returned without success! ERROR MESSAGE: " return -1 while not rospy.is_shutdown(): while (limit > precision or not ok) and i < 2000: # Solve QP. i += 1 nWSR = np.array([100]) lbA[0] = error lbA[1] = -q0_max - q0 lbA[2] = -q1_max - q1 lbA[3] = a0_const * Opt[0] - a0_max lbA[4] = a1_const * Opt[1] - a1_max ubA[0] = error ubA[1] = q0_max - q0 ubA[2] = q1_max - q1 ubA[3] = a0_const * Opt[0] + a0_max ubA[4] = a1_const * Opt[1] + a1_max return_value = example.hotstart(H, g, A, lb, ub, lbA, ubA, nWSR) if return_value != returnValue.SUCCESSFUL_RETURN: rospy.logerr( "Hotstart of QP-Problem returned without success!") return -1 # Get and print solution of QP. example.getPrimalSolution(Opt) q0 += Opt[0] / 100 q1 += Opt[1] / 100 q_eef = l1 + l2 + l3 + q0 + q1 error = p * (q_des - q_eef) limit = abs(error) # print "\nOpt = [ %g, %g, %g, %g ] \n posit= %g, error= %g, q0= %g q1= %g \n" % ( # Opt[0], Opt[1], Opt[2], Opt[3], q_eef, error, q0, q1) # Depending on joint goals if q0_goal is False and q1_goal is False: ok = True error = 0 limit = 0 elif q0_goal is True and q1_goal is False: lbA[5] = pj * (q0_des - q0) ubA[5] = pj * (q0_des - q0) if abs(lbA[5]) < joint_precision: ok = True # print "\n q0_error = %g, i = %g \n" % (lbA[5], i) else: ok = False elif q0_goal is False and q1_goal is True: lbA[5] = pj * (q1_des - q1) ubA[5] = pj * (q1_des - q1) if abs(lbA[5]) < joint_precision: ok = True # print "\n q0_error = %g, i = %g \n" % (lbA[5], i) else: lbA[5] = pj * (q0_des - q0) ubA[5] = pj * (q0_des - q0) lbA[6] = pj * (q1_des - q1) ubA[6] = pj * (q1_des - q1) if abs(lbA[5]) < joint_precision and abs( lbA[6]) < joint_precision: ok = True # print "\n q0_error = %g, q1_error = %g \n" % (lbA[5], lbA[6]) # Plotting arrays pos_eef = np.hstack((pos_eef, q_eef)) pos_0 = np.hstack((pos_0, q0)) pos_1 = np.hstack((pos_1, q1)) vel_0 = np.hstack((vel_0, Opt[0])) vel_1 = np.hstack((vel_1, Opt[1])) vel_eef = np.hstack((vel_eef, Opt[0] + Opt[1])) p_error = np.hstack((p_error, error)) eef_goal = np.hstack((eef_goal, q_des)) q0_set = np.hstack((q0_set, q0_des)) q1_set = np.hstack((q1_set, q1_des)) t = np.hstack((t, i)) # Plot t_eef = go.Scatter(y=pos_eef, x=t, marker=dict(size=4, ), mode='lines', name='pos_eef', line=dict(dash='dash')) t_p0 = go.Scatter(y=pos_0, x=t, marker=dict(size=4, ), mode='lines', name='pos_q0', line=dict(dash='dash')) t_p1 = go.Scatter(y=pos_1, x=t, marker=dict(size=4, ), mode='lines', name='pos_q1', line=dict(dash='dash')) t_v0 = go.Scatter(y=vel_0, x=t, marker=dict(size=4, ), mode='lines', name='vel_q0') t_v1 = go.Scatter(y=vel_1, x=t, marker=dict(size=4, ), mode='lines', name='vel_q1') t_veef = go.Scatter(y=vel_eef, x=t, marker=dict(size=4, ), mode='lines', name='vel_eef') t_er = go.Scatter(y=p_error, x=t, marker=dict(size=4, ), mode='lines', name='error') t_g1 = go.Scatter(y=q0_set, x=t, marker=dict(size=4, ), mode='lines', name='joint0_goal', line=dict(dash='dot')) t_g2 = go.Scatter(y=q1_set, x=t, marker=dict(size=4, ), mode='lines', name='joint1_goal', line=dict(dash='dot')) t_goal = go.Scatter(y=eef_goal, x=t, marker=dict(size=4, ), mode='lines', name='eef_goal', line=dict(dash='dot')) if q0_goal == True: data_q0 = [t_p0, t_v0, t_g1] data_q1 = [t_p1, t_v1] else: data_q0 = [t_p0, t_v0] data_q1 = [t_p1, t_v1, t_g2] layout = dict(title="Initial position q0 =%g.\n" % (q0_init), xaxis=dict( title='Iterations', autotick=False, dtick=25, gridwidth=3, tickcolor='#060', ), yaxis=dict(title='Position / Velocity', gridwidth=3, range=[-0.15, .1]), font=dict(size=18)) fig0 = dict(data=data_q0, layout=layout) plotly.offline.plot(fig0, filename='joint_goals_q0.html', image='png', image_filename='joint_q0') layout = dict(title="Initial position q1 = %g.\n" % (q1_init), xaxis=dict( title='Iterations', autotick=False, dtick=25, gridwidth=3, tickcolor='#060', ), yaxis=dict( title='Position / Velocity', gridwidth=3, ), font=dict(size=18)) fig1 = dict(data=data_q1, layout=layout) plotly.offline.plot(fig1, filename='joint_goals_q1.html', image='png', image_filename='joint_q1') data_eef = [t_eef, t_veef, t_goal] layout_eef = dict(title="Initial position EEF = %g. Goal = %g, \n" % (q_eef_init, q_des), xaxis=dict( title='Iterations', autotick=False, dtick=25, gridwidth=3, ), yaxis=dict(title='Position / Velocity', gridwidth=3), font=dict(size=16)) fig = dict(data=data_eef, layout=layout_eef) plotly.offline.plot(fig, filename='joint_goals_eef.html', image='png', image_filename='joint_eef') print "\n i = ", i return 0 print ok
def qpoases_calculation(self, error_posit): # Variable initialization self._feedback.sim_trajectory = [self.current_state] error_orient = np.array([0.0, 0.0, 0.0]) eef_pose_array = PoseArray() reached_orientation = False self.trajectory_length = 0.0 self.old_dist = 0.0 # Setting up QProblem object vel_calculation = SQProblem(self.problem_size[1], self.problem_size[0]) options = Options() options.setToDefault() options.printLevel = PrintLevel.LOW vel_calculation.setOptions(options) Opt = np.zeros(self.problem_size[1]) i = 0 # Config iai_naive_kinematics_sim, send commands clock = ProjectionClock() velocity_msg = JointState() velocity_msg.name = self.all_joint_names velocity_msg.header.stamp = rospy.get_rostime() velocity_array = [0.0 for x in range(len(self.all_joint_names))] effort_array = [0.0 for x in range(len(self.all_joint_names))] velocity_msg.velocity = velocity_array velocity_msg.effort = effort_array velocity_msg.position = effort_array clock.now = rospy.get_rostime() clock.period.nsecs = 1000000 self.pub_clock.publish(clock) # Plot for debugging '''t = np.array([i]) base_weight = np.array(self.jweights[0, 0]) arm_weight = np.array(self.jweights[4, 4]) triang_weight = np.array(self.jweights[3, 3]) low, pos, high, vel_p, error = [], [], [], [], [] error = np.array([0]) for x in range(8+3): low.append(np.array([self.joint_limits_lower[x]])) pos.append(np.array(self.joint_values[x])) vel_p.append(np.array(Opt[x])) high.append(np.array([self.joint_limits_upper[x]]))#''' tic = rospy.get_rostime() while not self.reach_position or not reached_orientation or not self.reach_pregrasp: i += 1 # Check if client cancelled goal if not self.active_goal_flag: self.success = False break if i > 500: self.abort_goal('time_out') break # Solve QP, redefine limit vectors of A. eef_pose_msg = Pose() nWSR = np.array([100]) [ac_lim_lower, ac_lim_upper] = self.acceleration_limits(Opt) self.joint_dist_lower_lim = self.joint_limits_lower - self.joint_values self.joint_dist_upper_lim = self.joint_limits_upper - self.joint_values self.lbA = np.hstack((error_posit, error_orient, self.joint_dist_lower_lim, ac_lim_lower)) self.ubA = np.hstack((error_posit, error_orient, self.joint_dist_upper_lim, ac_lim_upper)) # Recalculate H matrix self.calculate_weigths(error_posit) vel_calculation = SQProblem(self.problem_size[1], self.problem_size[0]) vel_calculation.setOptions(options) return_value = vel_calculation.init(self.H, self.g, self.A, self.lb, self.ub, self.lbA, self.ubA, nWSR) vel_calculation.getPrimalSolution(Opt) if return_value != returnValue.SUCCESSFUL_RETURN: rospy.logerr("QP-Problem returned without success! ") print 'joint limits: ', self.joint_limits_lower print 'joint values: ', self.joint_values print 'position error:', error_posit / self.prop_gain print 'eef ori: [%.3f %.3f %.3f] ' % ( self.eef_pose.p[0], self.eef_pose.p[1], self.eef_pose.p[2]) print 'goal ori : [%.3f %.3f %.3f] ' % ( self.goal_orient_euler[0], self.goal_orient_euler[0], self.goal_orient_euler[0], ) self.abort_goal('infeasible') break for x, vel in enumerate(Opt[4:self.nJoints]): velocity_msg.velocity[self.start_arm[x]] = vel velocity_msg.velocity[self.triang_list_pos] = Opt[3] velocity_msg.velocity[0] = Opt[0] velocity_msg.velocity[1] = Opt[1] # Recalculate Error in EEF position error_posit, limit_p = self.calc_posit_error(error_posit) # Recalculate Error in EEF orientation eef_orient = qo.rotation_to_quaternion(self.eef_pose.M) error_orient, reached_orientation = self.calc_orient_error( eef_orient, self.goal_quat, 0.35, limit_p) # Get trajectory length self.trajectory_length = self.get_trajectory_length( self.eef_pose.p, self.trajectory_length) # Print velocity for iai_naive_kinematics_sim velocity_msg.header.stamp = rospy.get_rostime() self.pub_velocity.publish(velocity_msg) clock.now = rospy.get_rostime() self.pub_clock.publish(clock) # Action feedback self.action_status.status = 1 self._feedback.sim_status = self.action_status.text = 'Calculating trajectory' self._feedback.sim_trajectory.append(self.current_state) self._feedback.trajectory_length = self.trajectory_length self.action_server.publish_feedback(self.action_status, self._feedback) self.action_server.publish_status() self.success = True # Store EEF pose for plotting eef_pose_msg.position.x = self.eef_pose.p[0] eef_pose_msg.position.y = self.eef_pose.p[1] eef_pose_msg.position.z = self.eef_pose.p[2] eef_pose_msg.orientation.x = eef_orient[0] eef_pose_msg.orientation.y = eef_orient[1] eef_pose_msg.orientation.z = eef_orient[2] eef_pose_msg.orientation.w = eef_orient[3] eef_pose_array.poses.append(eef_pose_msg) # Plot for debuging '''for x in range(8+3): low[x] = np.hstack((low[x], self.joint_limits_lower[x])) pos[x] = np.hstack((pos[x], self.joint_values[x])) vel_p[x] = np.hstack((vel_p[x], Opt[x])) high[x] = np.hstack((high[x], self.joint_limits_upper[x])) e = error_posit / self.prop_gain e_p = sqrt(pow(e[0], 2) + pow(e[1], 2) + pow(e[2], 2)) error = np.hstack((error, e_p)) base_weight = np.hstack((base_weight, self.jweights[0, 0])) arm_weight = np.hstack((arm_weight, self.jweights[4, 4])) triang_weight = np.hstack((triang_weight, self.jweights[3, 3])) t = np.hstack((t, i))#''' # print '\n iter: ', i # goal = euler_from_quaternion(self.goal_quat) # print 'joint_vel: ', Opt[:-6] # print 'error pos: ', error_posit / self.prop_gain # print 'eef ori: [%.3f %.3f %.3f] '%(self.eef_pose.p[0],self.eef_pose.p[1], self.eef_pose.p[2]) # print 'ori error: [%.3f %.3f %.3f] '%(error_orient[0], error_orient[1], error_orient[2]) # print 'goal ori : [%.3f %.3f %.3f] '%(goal[0], goal[1], goal[2]) # print 'slack : ', Opt[-6:] self.final_error = sqrt( pow(error_posit[0], 2) + pow(error_posit[1], 2) + pow(error_posit[2], 2)) eef_pose_array.header.stamp = rospy.get_rostime() eef_pose_array.header.frame_id = self.gripper self.pub_plot.publish(eef_pose_array) # Plot '''plt.style.use('ggplot') plt.plot(t, arm_weight, 'g-.', lw=3, label='arm') plt.plot(t, base_weight, 'k--', lw=3, label='base') plt.plot(t, triang_weight, 'm', lw=2, label='torso') plt.xlabel('Iterations') plt.ylabel('Weights') plt.title('Arm, base and torso weights') plt.grid(True) plt.legend() # plt.show() tikz_save('weights.tex', figureheight='5cm', figurewidth='16cm') plt.cla() plt.plot(t, error, 'k', lw=2) plt.xlabel('Iterations') plt.ylabel('Position Error [m]') plt.title('Position Error') plt.grid(True) plt.legend() # plt.show() tikz_save('error.tex', figureheight='4cm', figurewidth='16cm') plt.cla()#''' '''for x in range(8+3): plt.plot(t, low[x], lw=1) plt.plot(t, pos[x], lw=3) plt.plot(t, vel_p[x], lw=3) plt.plot(t, high[x], lw=1) plt.xlabel('Iterations') plt.ylabel('Position / Velocity') plt.grid(True) # plt.show() tikz_save('joint'+str(x)+'.tex', figureheight='5cm', figurewidth='4.5cm') plt.cla()''' '''plt.plot(t, pos[0], 'r--', lw=3, label='x') plt.plot(t, pos[1], 'b', lw=2, label='y') plt.xlabel('Iterations') plt.ylabel('Base Position [m]') plt.title('Trajectory of the base [m]') plt.grid(True) plt.legend() # plt.show() tikz_save('base_pos.tex', figureheight='5cm', figurewidth='16cm') plt.cla() step = 2 plt.plot(t[0:-1:step], pos[3][0:-1:step], 'o', markersize=0.5, lw=3, label='j0') plt.plot(t[0:-1:step], pos[4][0:-1:step], 'v', markersize=0.5, lw=3, label='j1') plt.plot(t[0:-1:step], pos[5][0:-1:step], 'P', markersize=0.5, lw=3, label='j2') plt.plot(t[0:-1:step], pos[6][0:-1:step], '*', markersize=0.5, lw=3, label='j3') plt.plot(t, pos[7], 'k-.', lw=3, label='j4') plt.plot(t, pos[8], '--', lw=3, label='j5') plt.plot(t, pos[9], lw=3, label='j6') plt.xlabel('Iterations') plt.ylabel('Position [rad]') plt.title('Trajectory of the joints') plt.grid(True) plt.legend(loc='upper left') tikz_save('joint_pos.tex', figureheight='8cm', figurewidth='16cm') plt.cla() plt.plot(t, vel_p[0], 'r--', lw=3, label='x') plt.plot(t, vel_p[1], 'b',lw=2, label='y') plt.xlabel('Iterations') plt.ylabel('Base Velocity [m/sec]') plt.title('Velocity of the base') plt.grid(True) plt.legend() tikz_save('base_vel.tex', figureheight='5cm', figurewidth='16cm') plt.cla() plt.plot(t[0:-1:step], vel_p[3][0:-1:step], 'o', markersize=0.5, lw=3, label='j0') plt.plot(t[0:-1:step], vel_p[4][0:-1:step], 'v', markersize=0.5, lw=3, label='j1') plt.plot(t[0:-1:step], vel_p[5][0:-1:step], 'P', markersize=0.5, lw=3, label='j2') plt.plot(t[0:-1:step], vel_p[6][0:-1:step], '*', markersize=0.5, lw=3, label='j3') plt.plot(t, vel_p[7], 'k-.', lw=3, label='j4') plt.plot(t, vel_p[8], '--', lw=3, label='j5') plt.plot(t, vel_p[9], lw=3, label='j6') plt.xlabel('Iterations') plt.ylabel('Arms Joints Velocity [rad/sec]') plt.title("Velocity of the arm's joints") plt.legend(loc='upper left') plt.grid(True) tikz_save('joint_vel.tex', figureheight='8cm', figurewidth='16cm') plt.cla()#''' toc = rospy.get_rostime() print(toc.secs - tic.secs), 'sec Elapsed' return 0
# WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR # A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more # details. # # You should have received a copy of the GNU Lesser General Public License # along with qpsolvers. If not, see <http://www.gnu.org/licenses/>. from numpy import array, hstack, ones, vstack, zeros from qpoases import PyOptions as Options from qpoases import PyPrintLevel as PrintLevel from qpoases import PyQProblem as QProblem from qpoases import PyQProblemB as QProblemB from qpoases import PyReturnValue as ReturnValue __infty = 1e10 options = Options() options.printLevel = PrintLevel.NONE 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
def main(): rospy.init_node('controller_2dof', anonymous=True) rospy.Rate(10) rospy.sleep(0.2) # Setup data of QP. # Weights. w1 = 1e-3 w2 = 1e-5 w3 = 1 # slack (attractor) # Joint limits. q0_max = 0.05 q1_max = 0.15 # Links l1 = 0.1 l2 = 0.2 l3 = 0.3 # Initial joint values. q0_init = q0 = -0.04 q1_init = q1 = -0.08 # Joint target. q_des = 0.65 # Slack limits. e_max = 1000 # Velocity limits.(+-) v0_max = 0.05 v1_max = 0.1 # Acceleration limits.(+-) a0_max = 0.05 * 0.5 a1_max = 0.1 * 0.5 # Others precision = 1e-3 p = 10 q_eef_init = q_eef = l1 + l2 + l3 + q0 + q1 error = p * (q_des - q_eef) vel_init = 0 nWSR = np.array([100]) # Acceleration a0_const = (v0_max - a0_max) / v0_max a1_const = (v1_max - a1_max) / v1_max example = SQProblem(3, 5) H = np.array([w1, 0.0, 0.0, 0.0, w2, 0.0, 0.0, 0.0, w3]).reshape((3, 3)) A = np.array([ 1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0 ]).reshape((5, 3)) g = np.array([0.0, 0.0, 0.0, 0.0]) lb = np.array([-v0_max, -v1_max, -e_max]) ub = np.array([v0_max, v1_max, e_max]) lbA = np.array([error, (-q0_max - q0), (-q1_max - q0), -a0_max, -a1_max]) ubA = np.array([error, (q0_max - q0), (q1_max - q0), a0_max, a1_max]) # Setting up QProblem object options = Options() options.setToReliable() options.printLevel = PrintLevel.LOW example.setOptions(options) print("Init pos = %g, goal = %g, error = %g, q0 =%g, q1 = %g\n" % (q_eef, q_des, error, q0, q1)) print A i = 0 n = 0 limit = abs(error) ok = False Opt = np.zeros(3) # Plotting t = np.array(i) pos_eef = np.array(q_eef) pos_0 = np.array(q0) pos_1 = np.array(q1) vel_0 = np.array(vel_init) vel_1 = np.array(vel_init) vel_eef = np.array(vel_init) p_error = np.array(error) goal = np.array(q_des) lim1 = np.array(q0_max) lim2 = np.array(q1_max) return_value = example.init(H, g, A, lb, ub, lbA, ubA, nWSR) if return_value != returnValue.SUCCESSFUL_RETURN: print "Init of QP-Problem returned without success! ERROR MESSAGE: " return -1 while not rospy.is_shutdown(): while limit > precision and i < 400: # Solve QP. i += 1 nWSR = np.array([100]) lbA[0] = error lbA[1] = -q0_max - q0 lbA[2] = -q1_max - q1 lbA[3] = a0_const * Opt[0] - a0_max lbA[4] = a1_const * Opt[1] - a1_max ubA[0] = error ubA[1] = q0_max - q0 ubA[2] = q1_max - q1 ubA[3] = a0_const * Opt[0] + a0_max ubA[4] = a1_const * Opt[1] + a1_max return_value = example.hotstart(H, g, A, lb, ub, lbA, ubA, nWSR) if return_value != returnValue.SUCCESSFUL_RETURN: print "Hotstart of QP-Problem returned without success! ERROR MESSAGE: " return -1 # Get and print solution of QP. example.getPrimalSolution(Opt) q0 += Opt[0] / 100 q1 += Opt[1] / 100 q_eef = l1 + l2 + l3 + q0 + q1 error = p * (q_des - q_eef) limit = abs(error) # print "\nOpt = [ %g, %g, %g, %g ] \n posit= %g, error= %g, q0= %g q1= %g \n" % ( # Opt[0], Opt[1], Opt[2], Opt[3], q_eef, error, q0, q1) # Plotting arrays pos_eef = np.hstack((pos_eef, q_eef)) pos_0 = np.hstack((pos_0, q0)) pos_1 = np.hstack((pos_1, q1)) vel_0 = np.hstack((vel_0, Opt[0])) vel_1 = np.hstack((vel_1, Opt[1])) vel_eef = np.hstack((vel_eef, Opt[0] + Opt[1])) goal = np.hstack((goal, q_des)) p_error = np.hstack((p_error, error)) t = np.hstack((t, i)) lim1 = np.hstack((lim1, q0_max)) lim2 = np.hstack((lim2, q1_max)) # Plot t_eef = go.Scatter(y=pos_eef, x=t, marker=dict(size=4, ), mode='lines', name='pos_eef', line=dict(dash='dash')) t_p0 = go.Scatter(y=pos_0, x=t, marker=dict(size=4, ), mode='lines', name='pos_q0', line=dict(dash='dash')) t_p1 = go.Scatter(y=pos_1, x=t, marker=dict(size=4, ), mode='lines', name='pos_q1', line=dict(dash='dash')) t_v0 = go.Scatter(y=vel_0, x=t, marker=dict(size=4, ), mode='lines', name='vel_q0') t_v1 = go.Scatter(y=vel_1, x=t, marker=dict(size=4, ), mode='lines', name='vel_q1') t_q0 = go.Scatter(y=lim1, x=t, marker=dict(size=4, ), mode='lines', name='limit_0') t_q1 = go.Scatter(y=lim2, x=t, marker=dict(size=4, ), mode='lines', name='limit_1') t_veef = go.Scatter(y=vel_eef, x=t, marker=dict(size=4, ), mode='lines', name='vel_eef') t_er = go.Scatter(y=p_error, x=t, marker=dict(size=4, ), mode='lines+markers', name='error') t_goal = go.Scatter(y=goal, x=t, marker=dict(size=4, ), mode='lines', name='goal', line=dict(dash='dot')) print "\n i = %g \n" % (i) data_eef = [t_eef, t_veef, t_goal] layout_eef = dict(title="Initial position EEF = %g. Goal = %g, \n" % (q_eef_init, q_des), xaxis=dict( title='Iterations', autotick=False, dtick=25, gridwidth=2, ), yaxis=dict(title='Position / Velocity'), font=dict(size=16)) fig = dict(data=data_eef, layout=layout_eef) plotly.offline.plot(fig, filename='html/basic_example_eef.html', image='png', image_filename='basic_eef') data = [t_p0, t_v0] layout = dict(title="Initial position q0 =%g, q1 = %g.\n" % (q0_init, q1_init), xaxis=dict(title='Iterations', autotick=False, dtick=25, gridwidth=2), yaxis=dict( title='Position / Velocity', range=[-0.08, .12], ), font=dict(size=18)) fig = dict(data=data, layout=layout) plotly.offline.plot(fig, filename='html/basic_example_q0.html', image='png', image_filename='basic_0') data = [t_p1, t_v1] layout = dict(title="Initial position q0 =%g, q1 = %g.\n" % (q0_init, q1_init), xaxis=dict(title='Iterations', autotick=False, dtick=25, gridwidth=2), yaxis=dict( title='Position / Velocity', range=[-0.08, .12], ), font=dict(size=18)) fig = dict(data=data, layout=layout) plotly.offline.plot(fig, filename='html/basic_example_q1.html', image='png', image_filename='basic_1') return 0