vpQuadProg.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Quadratic Programming
 *
 * Authors:
 * Olivier Kermorgant
 *
 *****************************************************************************/

#include <algorithm>
#include <visp3/core/vpMatrixException.h>
#include <visp3/core/vpQuadProg.h>

#ifdef VISP_HAVE_CPP11_COMPATIBILITY

#ifdef VISP_HAVE_GSL /* be careful of the copy below */
void vpQuadProg::fromCanonicalCost(const vpMatrix &H, const vpColVector &c, vpMatrix &Q, vpColVector &r, const double &tol)
#else
void vpQuadProg::fromCanonicalCost(const vpMatrix &/*H*/, const vpColVector &/*c*/,
                                   vpMatrix &/*Q*/, vpColVector &/*r*/, const double &/*tol*/)
#endif

{
#ifdef VISP_HAVE_GSL
  const unsigned int n = H.getCols();
  if(H.getRows() != n || c.getRows() != n)
  {
    throw(vpException(vpMatrixException::dimensionError, "vpQuadProg::fromCanonicalCost: H is not square or not the same dimension as c"));
  }

  vpColVector d(n);
  vpMatrix    V(n,n);
  // Compute the eigenvalues and eigenvectors
  H.eigenValues(d, V);
  // find first non-null eigen value
  unsigned int k = 0;
  for(unsigned int i = 0; i < n; ++i)
  {
    if(d[i] > tol)
      d[i] = sqrt(d[i]);
    else if(d[i] < tol)
      throw(vpException(vpMatrixException::matrixError, "vpQuadProg::fromCanonicalCost: H is not positive"));
    else
      k = i+1;
  }
  // build (Q,r) such that H = Q.^T.Q and c = -Q^T.r
  vpMatrix D(n-k,n-k);
  vpMatrix P(n-k,n);
  D.diag(d.extract(k,n-k));
  for(unsigned int i = 0; i < n-k; ++i)
    P[i][k+i] = 1;

  Q = D*P*V.transpose();
  r = -Q.t().pseudoInverse()*c;
#else
  throw(vpException(vpException::functionNotImplementedError, "Symmetric matrix decomposition is not implemented. You "
                                                              "should install GSL 3rd party"));
#endif
}

bool vpQuadProg::setEqualityConstraint(const vpMatrix &A, const vpColVector &b, const double &tol)
{
  x1 = b;
  Z = A;
  if(A.getRows() == b.getRows() && vpLinProg::colReduction(Z, x1, false, tol))
    return true;

  std::cout << "vpQuadProg::setEqualityConstraint: equality constraint infeasible" << std::endl;
  return false;
}

bool vpQuadProg::solveByProjection(const vpMatrix &Q, const vpColVector &r,
                                   vpMatrix &A, vpColVector &b,
                                   vpColVector &x, const double &tol)
{
  if(A.getRows())
  {
    if(!vpLinProg::colReduction(A, b, false, tol))
      return false;

    if(A.getCols() && (Q*A).infinityNorm() > tol)
      x = b + A*(Q*A).solveBySVD(r - Q*b);
    else
      x = b;
  }
  else
    x = Q.solveBySVD(r);
  return true;
}

bool vpQuadProg::solveQPe (const vpMatrix &Q, const vpColVector &r,
                           vpColVector &x, const double &tol) const
{
  const unsigned int n = Q.getCols();
  if(Q.getRows() != r.getRows() ||
     Z.getRows() != n ||
     x1.getRows() != n)
  {
    std::cout << "vpQuadProg::solveQPe: wrong dimension\n" <<
                 "Q: " << Q.getRows() << "x" << Q.getCols() << " - r: " << r.getRows() << std::endl;
    std::cout << "Z: " << Z.getRows() << "x" << Z.getCols() << " - x1: " << x1.getRows() << std::endl;
    throw vpMatrixException::dimensionError;
  }
  if(Z.getCols())
  {
    if((Q*Z).infinityNorm() > tol)
      x = x1 + Z*(Q*Z).solveBySVD(r - Q*x1);
    else
      x = x1;
  }
  else
    x = Q.solveBySVD(r);
  return true;
}

bool vpQuadProg::solveQPe (const vpMatrix &Q, const vpColVector &r, vpMatrix A, vpColVector b,
                           vpColVector &x, const double &tol)
{
  checkDimensions(Q, r, &A, &b, NULL, NULL, "solveQPe");

  if(!solveByProjection(Q, r, A, b, x, tol))
  {
    std::cout << "vpQuadProg::solveQPe: equality constraint infeasible" << std::endl;
    return false;
  }
  return true;
}

bool vpQuadProg::solveQP(const vpMatrix &Q, const vpColVector &r,
                         vpMatrix A, vpColVector b,
                         const vpMatrix &C, const vpColVector &d,
                         vpColVector &x,
                         const double &tol)
{
  if(A.getRows() == 0)
    return solveQPi(Q, r, C, d, x, false, tol);

  checkDimensions(Q, r, &A, &b, &C, &d, "solveQP");

  if(!vpLinProg::colReduction(A, b, false, tol))
  {
    std::cout << "vpQuadProg::solveQP: equality constraint infeasible" << std::endl;
    return false;
  }

  if(A.getCols() && solveQPi(Q*A, r - Q*b, C*A, d - C*b, x, false, tol))
  {
    x = b + A*x;
    return true;
  }
  else if(vpLinProg::allLesser(C, b, d, tol))  // Ax = b has only 1 solution
  {
    x = b;
    return true;
  }
  std::cout << "vpQuadProg::solveQP: inequality constraint infeasible" << std::endl;
  return false;
}

bool vpQuadProg::solveQPi(const vpMatrix &Q, const vpColVector &r,
                          const vpMatrix &C, const vpColVector &d,
                          vpColVector &x, const bool use_equality,
                          const double &tol)
{
  const unsigned int n = checkDimensions(Q, r, NULL, NULL, &C, &d, "solveQPi");

  if(use_equality)
  {
    if(Z.getRows() == n)
    {
      if(Z.getCols() && solveQPi(Q*Z, r - Q*x1, C*Z, d - C*x1, x, false, tol))
      {
        // back to initial solution
        x = x1 + Z*x;
        return true;
      }
      else if(vpLinProg::allLesser(C, x1, d, tol))
      {
        x = x1;
        return true;
      }
      std::cout << "vpQuadProg::solveQPi: inequality constraint infeasible" << std::endl;
      return false;
    }
    else
      std::cout << "vpQuadProg::solveQPi: use_equality before setEqualityConstraint" << std::endl;
  }

  const unsigned int p = C.getRows();

  // look for trivial solution
  // r = 0 and d > 0 -> x = 0
  if(vpLinProg::allZero(r, tol) &&
     (d.getRows() == 0 || vpLinProg::allGreater(d, -tol)))
  {
    x.resize(n);
    return true;
  }

  // go for solver
  // build feasible point
  vpMatrix A;
  vpColVector b;
  // check active set - all values should be < rows of C
  for(auto v: active)
  {
    if(v >= p)
    {
      active.clear();
      std::cout << "vpQuadProg::solveQPi: some constraints have been removed since last call\n";
      break;
    }
  }

  // warm start from previous active set
  A.resize((unsigned int)active.size(), n);
  b.resize((unsigned int)active.size());
  for(unsigned int i = 0; i < active.size(); ++i)
  {
    for(unsigned int j = 0; j < n; ++j)
      A[i][j] = C[active[i]][j];
    b[i] = d[active[i]];
  }
  if(!solveByProjection(Q, r, A, b, x, tol))
    x.resize(n);

  // or from simplex if we really have no clue
  if(!vpLinProg::allLesser(C, x, d, tol))
  {
    // feasible point with simplex:
    //  min r
    //   st C.(x + z1 - z2) + y - r = d
    //   st z1, z2, y, r >= 0
    // dim r = violated constraints
    // feasible if r can be minimized to 0

    // count how many violated constraints
    vpColVector e = d - C*x;
    unsigned int k = 0;
    for(unsigned int i = 0; i < p; ++i)
    {
      if(e[i] < -tol)
        k++;
    }
    // cost vector
    vpColVector c(2*n+p+k);
    for(unsigned int i = 0; i < k; ++i)
      c[2*n+p+i] = 1;

    vpColVector xc(2*n+p+k);

    vpMatrix A_lp(p, 2*n+p+k);
    unsigned int l = 0;
    for(unsigned int i = 0; i < p; ++i)
    {
      // copy [C -C] part
      for(unsigned int j = 0; j < n; ++j)
      {
        A_lp[i][j] = C[i][j];
        A_lp[i][n+j] = -C[i][j];
      }
      // y-part
      A_lp[i][2*n+i] = 1;
      if(e[i] < -tol)
      {
        // r-part
        A_lp[i][2*n+p+l] = -1;
        xc[2*n+p+l] = -e[i];
        l++;
      }
      else
        xc[2*n+i] = e[i];
    }
    vpLinProg::simplex(c, A_lp, e, xc);

    // r-part should be 0
    if(!vpLinProg::allLesser(xc.extract(2*n+p, k), tol))
    {
      std::cout << "vpQuadProg::solveQPi: inequality constraints not feasible" << std::endl;
      return false;
    }

    // update x to feasible point
    x += xc.extract(0,n) - xc.extract(n,n);
    // init active/inactive sets from y-part of x
    active.clear();
    active.reserve(p);
    inactive.clear();
    for(unsigned int i = 0; i < p; ++i)
    {
      if(C.getRow(i)*x - d[i] < -tol)
        inactive.push_back(i);
      else
        active.push_back(i);
    }
  }
  else  // warm start feasible
  {
    // using previous active set, check that inactive is sync
    if(active.size() + inactive.size() != p)
    {
      inactive.clear();
      for(unsigned int i = 0; i < p; ++i)
      {
        if(std::find(active.begin(), active.end(), i) == active.end())
          inactive.push_back(i);
      }
    }
  }

  vpMatrix Ap;
  bool update_Ap = true;
  unsigned int last_active = C.getRows();

  vpColVector u, g = r - Q*x, mu;

  // solve at one iteration
  while (true)
  {
    A.resize((unsigned int)active.size(), n);
    b.resize((unsigned int)active.size());
    for(unsigned int i = 0; i < active.size(); ++i)
    {
      for(unsigned int j = 0; j < n; ++j)
        A[i][j] = C[active[i]][j];
    }

    if(update_Ap && active.size())
      Ap = A.pseudoInverse(); // to get Lagrange multipliers if needed

    if(!solveByProjection(Q, g, A, b, u, tol))
    {
      std::cout << "vpQuadProg::solveQPi: QP seems infeasible, too many constraints activated\n";
      return false;
    }

    // 0-update = optimal or useless activated constraints
    if(vpLinProg::allZero(u, tol))
    {
      // compute multipliers if any
      unsigned int ineqInd = (unsigned int)active.size();
      if(active.size())
      {
        mu = -Ap.transpose() * Q.transpose() * (Q*u - g);
        // find most negative one if any - except last activated in case of degeneracy
        double ineqMax = -tol;
        for(unsigned int i = 0; i < mu.getRows(); ++i)
        {
          if(mu[i] < ineqMax && active[i] != last_active)
          {
            ineqInd = i;
            ineqMax = mu[i];
          }
        }
      }

      if(ineqInd == active.size())   // KKT condition no useless constraint
        return true;

      // useless inequality, deactivate
      inactive.push_back(active[ineqInd]);
      if(active.size() == 1)
        active.clear();
      else
        active.erase(active.begin()+ineqInd);
      update_Ap = true;
    }
    else    // u != 0, can improve xc
    {
      unsigned int ineqInd = 0;
      // step length to next constraint
      double alpha = 1;
      for(unsigned int i = 0; i < inactive.size(); ++i)
      {
        const double Cu = C.getRow(inactive[i])*u;
        if(Cu > tol)
        {
          const double a = (d[inactive[i]] - C.getRow(inactive[i])*x)/Cu;
          if(a < alpha)
          {
            alpha = a;
            ineqInd = i;
          }
        }
      }
      if(alpha < 1)
      {
        last_active = inactive[ineqInd];
        if(active.size())
        {
          auto it = active.begin();
          while(it != active.end() && *it < inactive[ineqInd])
            it++;
          active.insert(it, inactive[ineqInd]);
        }
        else
          active.push_back(inactive[ineqInd]);
        inactive.erase(inactive.begin()+ineqInd);
        update_Ap = true;
      }
      else
        update_Ap = false;
      // update x for next iteration
      x += alpha * u;
      g -= alpha*Q*u;
    }
  }
}
#else
void dummy_vpQuadProg(){};
#endif