HCB with MPC
Dependencies: mbed Eigen FastPWM
Diff: quadprog.cpp
- Revision:
- 25:f83396e3d25c
diff -r ef6e1092e9e6 -r f83396e3d25c quadprog.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/quadprog.cpp Mon Oct 14 10:08:13 2019 +0000 @@ -0,0 +1,738 @@ +/* +File $Id: QuadProg++.cc 232 2007-06-21 12:29:00Z digasper $ + + Author: Luca Di Gaspero + DIEGM - University of Udine, Italy + luca.digaspero@uniud.it + http://www.diegm.uniud.it/digaspero/ + + This software may be modified and distributed under the terms + of the MIT license. See the LICENSE file for details. + + */ + +#include <iostream> +#include <algorithm> +#include <cmath> +#include <limits> +#include <sstream> +#include <stdexcept> +#include "quadprog.h" +#include "math.h" + +//#define TRACE_SOLVER + +namespace quadprogpp +{ + +// Utility functions for updating some data needed by the solution method +void compute_d(Vector<double>& d, const Matrix<double>& J, const Vector<double>& np); +void update_z(Vector<double>& z, const Matrix<double>& J, const Vector<double>& d, int iq); +void update_r(const Matrix<double>& R, Vector<double>& r, const Vector<double>& d, int iq); +bool add_constraint(Matrix<double>& R, Matrix<double>& J, Vector<double>& d, int& iq, double& rnorm); +void delete_constraint(Matrix<double>& R, Matrix<double>& J, Vector<int>& A, Vector<double>& u, int n, int p, int& iq, int l); + +// Utility functions for computing the Cholesky decomposition and solving +// linear systems +void cholesky_decomposition(Matrix<double>& A); +void cholesky_solve(const Matrix<double>& L, Vector<double>& x, const Vector<double>& b); +void forward_elimination(const Matrix<double>& L, Vector<double>& y, const Vector<double>& b); +void backward_elimination(const Matrix<double>& U, Vector<double>& x, const Vector<double>& y); + +// Utility functions for computing the scalar product and the euclidean +// distance between two numbers +double scalar_product(const Vector<double>& x, const Vector<double>& y); +double distance(double a, double b); + +// Utility functions for printing vectors and matrices +void print_matrix(char* name, const Matrix<double>& A, int n = -1, int m = -1); + +template<typename T> +void print_vector(char* name, const Vector<T>& v, int n = -1); + +// The Solving function, implementing the Goldfarb-Idnani method + +double solve_quadprog(Matrix<double>& G, Vector<double>& g0, + const Matrix<double>& CE, const Vector<double>& ce0, + const Matrix<double>& CI, const Vector<double>& ci0, + Vector<double>& x) +{ + std::ostringstream msg; + int n = G.ncols(), p = CE.ncols(), m = CI.ncols(); + if (G.nrows() != n) { + msg << "The matrix G is not a squared matrix (" << G.nrows() << " x " << G.ncols() << ")"; +// throw std::logic_error(msg.str()); + } + if (CE.nrows() != n) { + msg << "The matrix CE is incompatible (incorrect number of rows " << CE.nrows() << " , expecting " << n << ")"; +// throw std::logic_error(msg.str()); + } + if (ce0.size() != p) { + msg << "The vector ce0 is incompatible (incorrect dimension " << ce0.size() << ", expecting " << p << ")"; +// throw std::logic_error(msg.str()); + } + if (CI.nrows() != n) { + msg << "The matrix CI is incompatible (incorrect number of rows " << CI.nrows() << " , expecting " << n << ")"; +// throw std::logic_error(msg.str()); + } + if (ci0.size() != m) { + msg << "The vector ci0 is incompatible (incorrect dimension " << ci0.size() << ", expecting " << m << ")"; +// throw std::logic_error(msg.str()); + } + x.resize(n); + register int i, j, k, l; /* indices */ + int ip; // this is the index of the constraint to be added to the active set + Matrix<double> R(n, n), J(n, n); + Vector<double> s(m + p), z(n), r(m + p), d(n), np(n), u(m + p), x_old(n), u_old(m + p); + double f_value, psi, c1, c2, sum, ss, R_norm; + double inf; + if (std::numeric_limits<double>::has_infinity) + inf = std::numeric_limits<double>::infinity(); + else + inf = 1.0E300; + double t, t1, t2; /* t is the step lenght, which is the minimum of the partial step length t1 + * and the full step length t2 */ + Vector<int> A(m + p), A_old(m + p), iai(m + p); + int q, iq, iter = 0; + Vector<bool> iaexcl(m + p); + + /* p is the number of equality constraints */ + /* m is the number of inequality constraints */ + q = 0; /* size of the active set A (containing the indices of the active constraints) */ +#ifdef TRACE_SOLVER + std::cout << std::endl << "Starting solve_quadprog" << std::endl; + print_matrix("G", G); + print_vector("g0", g0); + print_matrix("CE", CE); + print_vector("ce0", ce0); + print_matrix("CI", CI); + print_vector("ci0", ci0); +#endif + + /* + * Preprocessing phase + */ + + /* compute the trace of the original matrix G */ + c1 = 0.0; + for (i = 0; i < n; i++) { + c1 += G[i][i]; + } + /* decompose the matrix G in the form L^T L */ + cholesky_decomposition(G); +#ifdef TRACE_SOLVER + print_matrix("G", G); +#endif + /* initialize the matrix R */ + for (i = 0; i < n; i++) { + d[i] = 0.0; + for (j = 0; j < n; j++) + R[i][j] = 0.0; + } + R_norm = 1.0; /* this variable will hold the norm of the matrix R */ + + /* compute the inverse of the factorized matrix G^-1, this is the initial value for H */ + c2 = 0.0; + for (i = 0; i < n; i++) { + d[i] = 1.0; + forward_elimination(G, z, d); + for (j = 0; j < n; j++) + J[i][j] = z[j]; + c2 += z[i]; + d[i] = 0.0; + } +#ifdef TRACE_SOLVER + print_matrix("J", J); +#endif + + /* c1 * c2 is an estimate for cond(G) */ + + /* + * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x + * this is a feasible point in the dual space + * x = G^-1 * g0 + */ + cholesky_solve(G, x, g0); + for (i = 0; i < n; i++) + x[i] = -x[i]; + /* and compute the current solution value */ + f_value = 0.5 * scalar_product(g0, x); +#ifdef TRACE_SOLVER + std::cout << "Unconstrained solution: " << f_value << std::endl; + print_vector("x", x); +#endif + + /* Add equality constraints to the working set A */ + iq = 0; + for (i = 0; i < p; i++) { + for (j = 0; j < n; j++) + np[j] = CE[j][i]; + compute_d(d, J, np); + update_z(z, J, d, iq); + update_r(R, r, d, iq); +#ifdef TRACE_SOLVER + print_matrix("R", R, n, iq); + print_vector("z", z); + print_vector("r", r, iq); + print_vector("d", d); +#endif + + /* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint + becomes feasible */ + t2 = 0.0; + if (fabs(scalar_product(z, z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0 + t2 = (-scalar_product(np, x) - ce0[i]) / scalar_product(z, np); + + /* set x = x + t2 * z */ + for (k = 0; k < n; k++) + x[k] += t2 * z[k]; + + /* set u = u+ */ + u[iq] = t2; + for (k = 0; k < iq; k++) + u[k] -= t2 * r[k]; + + /* compute the new solution value */ + f_value += 0.5 * (t2 * t2) * scalar_product(z, np); + A[i] = -i - 1; + + if (!add_constraint(R, J, d, iq, R_norm)) { + // Equality constraints are linearly dependent +// throw std::runtime_error("Constraints are linearly dependent"); + return f_value; + } + } + + /* set iai = K \ A */ + for (i = 0; i < m; i++) + iai[i] = i; + +l1: + iter++; +#ifdef TRACE_SOLVER + print_vector("x", x); +#endif + /* step 1: choose a violated constraint */ + for (i = p; i < iq; i++) { + ip = A[i]; + iai[ip] = -1; + } + + /* compute s[x] = ci^T * x + ci0 for all elements of K \ A */ + ss = 0.0; + psi = 0.0; /* this value will contain the sum of all infeasibilities */ + ip = 0; /* ip will be the index of the chosen violated constraint */ + for (i = 0; i < m; i++) { + iaexcl[i] = true; + sum = 0.0; + for (j = 0; j < n; j++) + sum += CI[j][i] * x[j]; + sum += ci0[i]; + s[i] = sum; + psi += std::min(0.0, sum); + } +#ifdef TRACE_SOLVER + print_vector("s", s, m); +#endif + + + if (fabs(psi) <= m * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0) { + /* numerically there are not infeasibilities anymore */ + q = iq; + + return f_value; + } + + /* save old values for u and A */ + for (i = 0; i < iq; i++) { + u_old[i] = u[i]; + A_old[i] = A[i]; + } + /* and for x */ + for (i = 0; i < n; i++) + x_old[i] = x[i]; + +l2: /* Step 2: check for feasibility and determine a new S-pair */ + for (i = 0; i < m; i++) { + if (s[i] < ss && iai[i] != -1 && iaexcl[i]) { + ss = s[i]; + ip = i; + } + } + if (ss >= 0.0) { + q = iq; + + return f_value; + } + + /* set np = n[ip] */ + for (i = 0; i < n; i++) + np[i] = CI[i][ip]; + /* set u = [u 0]^T */ + u[iq] = 0.0; + /* add ip to the active set A */ + A[iq] = ip; + +#ifdef TRACE_SOLVER + std::cout << "Trying with constraint " << ip << std::endl; + print_vector("np", np); +#endif + +l2a:/* Step 2a: determine step direction */ + /* compute z = H np: the step direction in the primal space (through J, see the paper) */ + compute_d(d, J, np); + update_z(z, J, d, iq); + /* compute N* np (if q > 0): the negative of the step direction in the dual space */ + update_r(R, r, d, iq); +#ifdef TRACE_SOLVER + std::cout << "Step direction z" << std::endl; + print_vector("z", z); + print_vector("r", r, iq + 1); + print_vector("u", u, iq + 1); + print_vector("d", d); + print_vector("A", A, iq + 1); +#endif + + /* Step 2b: compute step length */ + l = 0; + /* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */ + t1 = inf; /* +inf */ + /* find the index l s.t. it reaches the minimum of u+[x] / r */ + for (k = p; k < iq; k++) { + if (r[k] > 0.0) { + if (u[k] / r[k] < t1) { + t1 = u[k] / r[k]; + l = A[k]; + } + } + } + /* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */ + if (fabs(scalar_product(z, z)) > std::numeric_limits<double>::epsilon()) { // i.e. z != 0 + t2 = -s[ip] / scalar_product(z, np); + if (t2 < 0) // patch suggested by Takano Akio for handling numerical inconsistencies + t2 = inf; + } else + t2 = inf; /* +inf */ + + /* the step is chosen as the minimum of t1 and t2 */ + t = std::min(t1, t2); +#ifdef TRACE_SOLVER + std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") "; +#endif + + /* Step 2c: determine new S-pair and take step: */ + + /* case (i): no step in primal or dual space */ + if (t >= inf) { + /* QPP is infeasible */ + // FIXME: unbounded to raise + q = iq; + return inf; + } + /* case (ii): step in dual space */ + if (t2 >= inf) { + /* set u = u + t * [-r 1] and drop constraint l from the active set A */ + for (k = 0; k < iq; k++) + u[k] -= t * r[k]; + u[iq] += t; + iai[l] = l; + delete_constraint(R, J, A, u, n, p, iq, l); +#ifdef TRACE_SOLVER + std::cout << " in dual space: " + << f_value << std::endl; + print_vector("x", x); + print_vector("z", z); + print_vector("A", A, iq + 1); +#endif + goto l2a; + } + + /* case (iii): step in primal and dual space */ + + /* set x = x + t * z */ + for (k = 0; k < n; k++) + x[k] += t * z[k]; + /* update the solution value */ + f_value += t * scalar_product(z, np) * (0.5 * t + u[iq]); + /* u = u + t * [-r 1] */ + for (k = 0; k < iq; k++) + u[k] -= t * r[k]; + u[iq] += t; +#ifdef TRACE_SOLVER + std::cout << " in both spaces: " + << f_value << std::endl; + print_vector("x", x); + print_vector("u", u, iq + 1); + print_vector("r", r, iq + 1); + print_vector("A", A, iq + 1); +#endif + + if (fabs(t - t2) < std::numeric_limits<double>::epsilon()) { +#ifdef TRACE_SOLVER + std::cout << "Full step has taken " << t << std::endl; + print_vector("x", x); +#endif + /* full step has taken */ + /* add constraint ip to the active set*/ + if (!add_constraint(R, J, d, iq, R_norm)) { + iaexcl[ip] = false; + delete_constraint(R, J, A, u, n, p, iq, ip); +#ifdef TRACE_SOLVER + print_matrix("R", R); + print_vector("A", A, iq); + print_vector("iai", iai); +#endif + for (i = 0; i < m; i++) + iai[i] = i; + for (i = p; i < iq; i++) { + A[i] = A_old[i]; + u[i] = u_old[i]; + iai[A[i]] = -1; + } + for (i = 0; i < n; i++) + x[i] = x_old[i]; + goto l2; /* go to step 2 */ + } else + iai[ip] = -1; +#ifdef TRACE_SOLVER + print_matrix("R", R); + print_vector("A", A, iq); + print_vector("iai", iai); +#endif + goto l1; + } + + /* a patial step has taken */ +#ifdef TRACE_SOLVER + std::cout << "Partial step has taken " << t << std::endl; + print_vector("x", x); +#endif + /* drop constraint l */ + iai[l] = l; + delete_constraint(R, J, A, u, n, p, iq, l); +#ifdef TRACE_SOLVER + print_matrix("R", R); + print_vector("A", A, iq); +#endif + + /* update s[ip] = CI * x + ci0 */ + sum = 0.0; + for (k = 0; k < n; k++) + sum += CI[k][ip] * x[k]; + s[ip] = sum + ci0[ip]; + +#ifdef TRACE_SOLVER + print_vector("s", s, m); +#endif + goto l2a; +} + +inline void compute_d(Vector<double>& d, const Matrix<double>& J, const Vector<double>& np) +{ + register int i, j, n = d.size(); + register double sum; + + /* compute d = H^T * np */ + for (i = 0; i < n; i++) { + sum = 0.0; + for (j = 0; j < n; j++) + sum += J[j][i] * np[j]; + d[i] = sum; + } +} + +inline void update_z(Vector<double>& z, const Matrix<double>& J, const Vector<double>& d, int iq) +{ + register int i, j, n = z.size(); + + /* setting of z = H * d */ + for (i = 0; i < n; i++) { + z[i] = 0.0; + for (j = iq; j < n; j++) + z[i] += J[i][j] * d[j]; + } +} + +inline void update_r(const Matrix<double>& R, Vector<double>& r, const Vector<double>& d, int iq) +{ + register int i, j, n = d.size(); + register double sum; + + /* setting of r = R^-1 d */ + for (i = iq - 1; i >= 0; i--) { + sum = 0.0; + for (j = i + 1; j < iq; j++) + sum += R[i][j] * r[j]; + r[i] = (d[i] - sum) / R[i][i]; + } +} + +bool add_constraint(Matrix<double>& R, Matrix<double>& J, Vector<double>& d, int& iq, double& R_norm) +{ + int n = d.size(); +#ifdef TRACE_SOLVER + std::cout << "Add constraint " << iq << '/'; +#endif + register int i, j, k; + double cc, ss, h, t1, t2, xny; + + /* we have to find the Givens rotation which will reduce the element + d[j] to zero. + if it is already zero we don't have to do anything, except of + decreasing j */ + for (j = n - 1; j >= iq + 1; j--) { + /* The Givens rotation is done with the matrix (cc cs, cs -cc). + If cc is one, then element (j) of d is zero compared with element + (j - 1). Hence we don't have to do anything. + If cc is zero, then we just have to switch column (j) and column (j - 1) + of J. Since we only switch columns in J, we have to be careful how we + update d depending on the sign of gs. + Otherwise we have to apply the Givens rotation to these columns. + The i - 1 element of d has to be updated to h. */ + cc = d[j - 1]; + ss = d[j]; + h = distance(cc, ss); + if (fabs(h) < std::numeric_limits<double>::epsilon()) // h == 0 + continue; + d[j] = 0.0; + ss = ss / h; + cc = cc / h; + if (cc < 0.0) { + cc = -cc; + ss = -ss; + d[j - 1] = -h; + } else + d[j - 1] = h; + xny = ss / (1.0 + cc); + for (k = 0; k < n; k++) { + t1 = J[k][j - 1]; + t2 = J[k][j]; + J[k][j - 1] = t1 * cc + t2 * ss; + J[k][j] = xny * (t1 + J[k][j - 1]) - t2; + } + } + /* update the number of constraints added*/ + iq++; + /* To update R we have to put the iq components of the d vector + into column iq - 1 of R + */ + for (i = 0; i < iq; i++) + R[i][iq - 1] = d[i]; +#ifdef TRACE_SOLVER + std::cout << iq << std::endl; + print_matrix("R", R, iq, iq); + print_matrix("J", J); + print_vector("d", d, iq); +#endif + + if (fabs(d[iq - 1]) <= std::numeric_limits<double>::epsilon() * R_norm) { + // problem degenerate + return false; + } + R_norm = std::max<double>(R_norm, fabs(d[iq - 1])); + return true; +} + +void delete_constraint(Matrix<double>& R, Matrix<double>& J, Vector<int>& A, Vector<double>& u, int n, int p, int& iq, int l) +{ +#ifdef TRACE_SOLVER + std::cout << "Delete constraint " << l << ' ' << iq; +#endif + register int i, j, k, qq = -1; // just to prevent warnings from smart compilers + double cc, ss, h, xny, t1, t2; + + /* Find the index qq for active constraint l to be removed */ + for (i = p; i < iq; i++) + if (A[i] == l) { + qq = i; + break; + } + + /* remove the constraint from the active set and the duals */ + for (i = qq; i < iq - 1; i++) { + A[i] = A[i + 1]; + u[i] = u[i + 1]; + for (j = 0; j < n; j++) + R[j][i] = R[j][i + 1]; + } + + A[iq - 1] = A[iq]; + u[iq - 1] = u[iq]; + A[iq] = 0; + u[iq] = 0.0; + for (j = 0; j < iq; j++) + R[j][iq - 1] = 0.0; + /* constraint has been fully removed */ + iq--; +#ifdef TRACE_SOLVER + std::cout << '/' << iq << std::endl; +#endif + + if (iq == 0) + return; + + for (j = qq; j < iq; j++) { + cc = R[j][j]; + ss = R[j + 1][j]; + h = distance(cc, ss); + if (fabs(h) < std::numeric_limits<double>::epsilon()) // h == 0 + continue; + cc = cc / h; + ss = ss / h; + R[j + 1][j] = 0.0; + if (cc < 0.0) { + R[j][j] = -h; + cc = -cc; + ss = -ss; + } else + R[j][j] = h; + + xny = ss / (1.0 + cc); + for (k = j + 1; k < iq; k++) { + t1 = R[j][k]; + t2 = R[j + 1][k]; + R[j][k] = t1 * cc + t2 * ss; + R[j + 1][k] = xny * (t1 + R[j][k]) - t2; + } + for (k = 0; k < n; k++) { + t1 = J[k][j]; + t2 = J[k][j + 1]; + J[k][j] = t1 * cc + t2 * ss; + J[k][j + 1] = xny * (J[k][j] + t1) - t2; + } + } +} + +inline double distance(double a, double b) +{ + register double a1, b1, t; + a1 = fabs(a); + b1 = fabs(b); + if (a1 > b1) { + t = (b1 / a1); + return a1 * sqrt(1.0 + t * t); + } else if (b1 > a1) { + t = (a1 / b1); + return b1 * sqrt(1.0 + t * t); + } + return a1 * sqrt(2.0); +} + + +inline double scalar_product(const Vector<double>& x, const Vector<double>& y) +{ + register int i, n = x.size(); + register double sum; + + sum = 0.0; + for (i = 0; i < n; i++) + sum += x[i] * y[i]; + return sum; +} + +void cholesky_decomposition(Matrix<double>& A) +{ + register int i, j, k, n = A.nrows(); + register double sum; + + for (i = 0; i < n; i++) { + for (j = i; j < n; j++) { + sum = A[i][j]; + for (k = i - 1; k >= 0; k--) + sum -= A[i][k]*A[j][k]; + if (i == j) { + if (sum <= 0.0) { + std::ostringstream os; + // raise error + print_matrix("A", A); + os << "Error in cholesky decomposition, sum: " << sum; +// throw std::logic_error(os.str()); +// exit(-1); + } + A[i][i] = sqrt(sum); + } else + A[j][i] = sum / A[i][i]; + } + for (k = i + 1; k < n; k++) + A[i][k] = A[k][i]; + } +} + +void cholesky_solve(const Matrix<double>& L, Vector<double>& x, const Vector<double>& b) +{ + int n = L.nrows(); + Vector<double> y(n); + + /* Solve L * y = b */ + forward_elimination(L, y, b); + /* Solve L^T * x = y */ + backward_elimination(L, x, y); +} + +inline void forward_elimination(const Matrix<double>& L, Vector<double>& y, const Vector<double>& b) +{ + register int i, j, n = L.nrows(); + + y[0] = b[0] / L[0][0]; + for (i = 1; i < n; i++) { + y[i] = b[i]; + for (j = 0; j < i; j++) + y[i] -= L[i][j] * y[j]; + y[i] = y[i] / L[i][i]; + } +} + +inline void backward_elimination(const Matrix<double>& U, Vector<double>& x, const Vector<double>& y) +{ + register int i, j, n = U.nrows(); + + x[n - 1] = y[n - 1] / U[n - 1][n - 1]; + for (i = n - 2; i >= 0; i--) { + x[i] = y[i]; + for (j = i + 1; j < n; j++) + x[i] -= U[i][j] * x[j]; + x[i] = x[i] / U[i][i]; + } +} + +void print_matrix(char* name, const Matrix<double>& A, int n, int m) +{ + std::ostringstream s; + std::string t; + if (n == -1) + n = A.nrows(); + if (m == -1) + m = A.ncols(); + + s << name << ": " << std::endl; + for (int i = 0; i < n; i++) { + s << " "; + for (int j = 0; j < m; j++) + s << A[i][j] << ", "; + s << std::endl; + } + t = s.str(); + t = t.substr(0, t.size() - 3); // To remove the trailing space, comma and newline + + std::cout << t << std::endl; +} + +template<typename T> +void print_vector(char* name, const Vector<T>& v, int n) +{ + std::ostringstream s; + std::string t; + if (n == -1) + n = v.size(); + + s << name << ": " << std::endl << " "; + for (int i = 0; i < n; i++) { + s << v[i] << ", "; + } + t = s.str(); + t = t.substr(0, t.size() - 2); // To remove the trailing space and comma + + std::cout << t << std::endl; +} + +} // namespace quadprogpp