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