Visual Servoing Platform  version 3.2.1 under development (2019-05-20)
vpQuadProg.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Quadratic Programming
33  *
34  * Authors:
35  * Olivier Kermorgant
36  *
37  *****************************************************************************/
38 
39 #include <algorithm>
40 #include <visp3/core/vpMatrixException.h>
41 #include <visp3/core/vpQuadProg.h>
42 
43 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
44 
97 #ifdef VISP_HAVE_GSL /* be careful of the copy below */
98 void vpQuadProg::fromCanonicalCost(const vpMatrix &H, const vpColVector &c, vpMatrix &Q, vpColVector &r, const double &tol)
99 #else
100 void vpQuadProg::fromCanonicalCost(const vpMatrix &/*H*/, const vpColVector &/*c*/,
101  vpMatrix &/*Q*/, vpColVector &/*r*/, const double &/*tol*/)
102 #endif
103 
104 {
105 #ifdef VISP_HAVE_GSL
106  const unsigned int n = H.getCols();
107  if(H.getRows() != n || c.getRows() != n)
108  {
109  throw(vpException(vpMatrixException::dimensionError, "vpQuadProg::fromCanonicalCost: H is not square or not the same dimension as c"));
110  }
111 
112  vpColVector d(n);
113  vpMatrix V(n,n);
114  // Compute the eigenvalues and eigenvectors
115  H.eigenValues(d, V);
116  // find first non-null eigen value
117  unsigned int k = 0;
118  for(unsigned int i = 0; i < n; ++i)
119  {
120  if(d[i] > tol)
121  d[i] = sqrt(d[i]);
122  else if(d[i] < tol)
123  throw(vpException(vpMatrixException::matrixError, "vpQuadProg::fromCanonicalCost: H is not positive"));
124  else
125  k = i+1;
126  }
127  // build (Q,r) such that H = Q.^T.Q and c = -Q^T.r
128  vpMatrix D(n-k,n-k);
129  vpMatrix P(n-k,n);
130  D.diag(d.extract(k,n-k));
131  for(unsigned int i = 0; i < n-k; ++i)
132  P[i][k+i] = 1;
133 
134  Q = D*P*V.transpose();
135  r = -Q.t().pseudoInverse()*c;
136 #else
137  throw(vpException(vpException::functionNotImplementedError, "Symmetric matrix decomposition is not implemented. You "
138  "should install GSL 3rd party"));
139 #endif
140 }
141 
155 bool vpQuadProg::setEqualityConstraint(const vpMatrix &A, const vpColVector &b, const double &tol)
156 {
157  x1 = b;
158  Z = A;
159  if(A.getRows() == b.getRows() && vpLinProg::colReduction(Z, x1, false, tol))
160  return true;
161 
162  std::cout << "vpQuadProg::setEqualityConstraint: equality constraint infeasible" << std::endl;
163  return false;
164 }
165 
185  vpMatrix &A, vpColVector &b,
186  vpColVector &x, const double &tol)
187 {
188  if(A.getRows())
189  {
190  if(!vpLinProg::colReduction(A, b, false, tol))
191  return false;
192 
193  if(A.getCols() && (Q*A).infinityNorm() > tol)
194  x = b + A*(Q*A).solveBySVD(r - Q*b);
195  else
196  x = b;
197  }
198  else
199  x = Q.solveBySVD(r);
200  return true;
201 }
202 
255 bool vpQuadProg::solveQPe (const vpMatrix &Q, const vpColVector &r,
256  vpColVector &x, const double &tol) const
257 {
258  const unsigned int n = Q.getCols();
259  if(Q.getRows() != r.getRows() ||
260  Z.getRows() != n ||
261  x1.getRows() != n)
262  {
263  std::cout << "vpQuadProg::solveQPe: wrong dimension\n" <<
264  "Q: " << Q.getRows() << "x" << Q.getCols() << " - r: " << r.getRows() << std::endl;
265  std::cout << "Z: " << Z.getRows() << "x" << Z.getCols() << " - x1: " << x1.getRows() << std::endl;
267  }
268  if(Z.getCols())
269  {
270  if((Q*Z).infinityNorm() > tol)
271  x = x1 + Z*(Q*Z).solveBySVD(r - Q*x1);
272  else
273  x = x1;
274  }
275  else
276  x = Q.solveBySVD(r);
277  return true;
278 }
279 
326  vpColVector &x, const double &tol)
327 {
328  checkDimensions(Q, r, &A, &b, NULL, NULL, "solveQPe");
329 
330  if(!solveByProjection(Q, r, A, b, x, tol))
331  {
332  std::cout << "vpQuadProg::solveQPe: equality constraint infeasible" << std::endl;
333  return false;
334  }
335  return true;
336 }
337 
389 bool vpQuadProg::solveQP(const vpMatrix &Q, const vpColVector &r,
390  vpMatrix A, vpColVector b,
391  const vpMatrix &C, const vpColVector &d,
392  vpColVector &x,
393  const double &tol)
394 {
395  if(A.getRows() == 0)
396  return solveQPi(Q, r, C, d, x, false, tol);
397 
398  checkDimensions(Q, r, &A, &b, &C, &d, "solveQP");
399 
400  if(!vpLinProg::colReduction(A, b, false, tol))
401  {
402  std::cout << "vpQuadProg::solveQP: equality constraint infeasible" << std::endl;
403  return false;
404  }
405 
406  if(A.getCols() && solveQPi(Q*A, r - Q*b, C*A, d - C*b, x, false, tol))
407  {
408  x = b + A*x;
409  return true;
410  }
411  else if(vpLinProg::allLesser(C, b, d, tol)) // Ax = b has only 1 solution
412  {
413  x = b;
414  return true;
415  }
416  std::cout << "vpQuadProg::solveQP: inequality constraint infeasible" << std::endl;
417  return false;
418 }
419 
465 bool vpQuadProg::solveQPi(const vpMatrix &Q, const vpColVector &r,
466  const vpMatrix &C, const vpColVector &d,
467  vpColVector &x, const bool use_equality,
468  const double &tol)
469 {
470  const unsigned int n = checkDimensions(Q, r, NULL, NULL, &C, &d, "solveQPi");
471 
472  if(use_equality)
473  {
474  if(Z.getRows() == n)
475  {
476  if(Z.getCols() && solveQPi(Q*Z, r - Q*x1, C*Z, d - C*x1, x, false, tol))
477  {
478  // back to initial solution
479  x = x1 + Z*x;
480  return true;
481  }
482  else if(vpLinProg::allLesser(C, x1, d, tol))
483  {
484  x = x1;
485  return true;
486  }
487  std::cout << "vpQuadProg::solveQPi: inequality constraint infeasible" << std::endl;
488  return false;
489  }
490  else
491  std::cout << "vpQuadProg::solveQPi: use_equality before setEqualityConstraint" << std::endl;
492  }
493 
494  const unsigned int p = C.getRows();
495 
496  // look for trivial solution
497  // r = 0 and d > 0 -> x = 0
498  if(vpLinProg::allZero(r, tol) &&
499  (d.getRows() == 0 || vpLinProg::allGreater(d, -tol)))
500  {
501  x.resize(n);
502  return true;
503  }
504 
505  // go for solver
506  // build feasible point
507  vpMatrix A;
508  vpColVector b;
509  // check active set - all values should be < rows of C
510  for(auto v: active)
511  {
512  if(v >= p)
513  {
514  active.clear();
515  std::cout << "vpQuadProg::solveQPi: some constraints have been removed since last call\n";
516  break;
517  }
518  }
519 
520  // warm start from previous active set
521  A.resize((unsigned int)active.size(), n);
522  b.resize((unsigned int)active.size());
523  for(unsigned int i = 0; i < active.size(); ++i)
524  {
525  for(unsigned int j = 0; j < n; ++j)
526  A[i][j] = C[active[i]][j];
527  b[i] = d[active[i]];
528  }
529  if(!solveByProjection(Q, r, A, b, x, tol))
530  x.resize(n);
531 
532  // or from simplex if we really have no clue
533  if(!vpLinProg::allLesser(C, x, d, tol))
534  {
535  // feasible point with simplex:
536  // min r
537  // st C.(x + z1 - z2) + y - r = d
538  // st z1, z2, y, r >= 0
539  // dim r = violated constraints
540  // feasible if r can be minimized to 0
541 
542  // count how many violated constraints
543  vpColVector e = d - C*x;
544  unsigned int k = 0;
545  for(unsigned int i = 0; i < p; ++i)
546  {
547  if(e[i] < -tol)
548  k++;
549  }
550  // cost vector
551  vpColVector c(2*n+p+k);
552  for(unsigned int i = 0; i < k; ++i)
553  c[2*n+p+i] = 1;
554 
555  vpColVector xc(2*n+p+k);
556 
557  vpMatrix A_lp(p, 2*n+p+k);
558  unsigned int l = 0;
559  for(unsigned int i = 0; i < p; ++i)
560  {
561  // copy [C -C] part
562  for(unsigned int j = 0; j < n; ++j)
563  {
564  A_lp[i][j] = C[i][j];
565  A_lp[i][n+j] = -C[i][j];
566  }
567  // y-part
568  A_lp[i][2*n+i] = 1;
569  if(e[i] < -tol)
570  {
571  // r-part
572  A_lp[i][2*n+p+l] = -1;
573  xc[2*n+p+l] = -e[i];
574  l++;
575  }
576  else
577  xc[2*n+i] = e[i];
578  }
579  vpLinProg::simplex(c, A_lp, e, xc);
580 
581  // r-part should be 0
582  if(!vpLinProg::allLesser(xc.extract(2*n+p, k), tol))
583  {
584  std::cout << "vpQuadProg::solveQPi: inequality constraints not feasible" << std::endl;
585  return false;
586  }
587 
588  // update x to feasible point
589  x += xc.extract(0,n) - xc.extract(n,n);
590  // init active/inactive sets from y-part of x
591  active.clear();
592  active.reserve(p);
593  inactive.clear();
594  for(unsigned int i = 0; i < p; ++i)
595  {
596  if(C.getRow(i)*x - d[i] < -tol)
597  inactive.push_back(i);
598  else
599  active.push_back(i);
600  }
601  }
602  else // warm start feasible
603  {
604  // using previous active set, check that inactive is sync
605  if(active.size() + inactive.size() != p)
606  {
607  inactive.clear();
608  for(unsigned int i = 0; i < p; ++i)
609  {
610  if(std::find(active.begin(), active.end(), i) == active.end())
611  inactive.push_back(i);
612  }
613  }
614  }
615 
616  vpMatrix Ap;
617  bool update_Ap = true;
618  unsigned int last_active = C.getRows();
619 
620  vpColVector u, g = r - Q*x, mu;
621 
622  // solve at one iteration
623  while (true)
624  {
625  A.resize((unsigned int)active.size(), n);
626  b.resize((unsigned int)active.size());
627  for(unsigned int i = 0; i < active.size(); ++i)
628  {
629  for(unsigned int j = 0; j < n; ++j)
630  A[i][j] = C[active[i]][j];
631  }
632 
633  if(update_Ap && active.size())
634  Ap = A.pseudoInverse(); // to get Lagrange multipliers if needed
635 
636  if(!solveByProjection(Q, g, A, b, u, tol))
637  {
638  std::cout << "vpQuadProg::solveQPi: QP seems infeasible, too many constraints activated\n";
639  return false;
640  }
641 
642  // 0-update = optimal or useless activated constraints
643  if(vpLinProg::allZero(u, tol))
644  {
645  // compute multipliers if any
646  unsigned int ineqInd = (unsigned int)active.size();
647  if(active.size())
648  {
649  mu = -Ap.transpose() * Q.transpose() * (Q*u - g);
650  // find most negative one if any - except last activated in case of degeneracy
651  double ineqMax = -tol;
652  for(unsigned int i = 0; i < mu.getRows(); ++i)
653  {
654  if(mu[i] < ineqMax && active[i] != last_active)
655  {
656  ineqInd = i;
657  ineqMax = mu[i];
658  }
659  }
660  }
661 
662  if(ineqInd == active.size()) // KKT condition no useless constraint
663  return true;
664 
665  // useless inequality, deactivate
666  inactive.push_back(active[ineqInd]);
667  if(active.size() == 1)
668  active.clear();
669  else
670  active.erase(active.begin()+ineqInd);
671  update_Ap = true;
672  }
673  else // u != 0, can improve xc
674  {
675  unsigned int ineqInd = 0;
676  // step length to next constraint
677  double alpha = 1;
678  for(unsigned int i = 0; i < inactive.size(); ++i)
679  {
680  const double Cu = C.getRow(inactive[i])*u;
681  if(Cu > tol)
682  {
683  const double a = (d[inactive[i]] - C.getRow(inactive[i])*x)/Cu;
684  if(a < alpha)
685  {
686  alpha = a;
687  ineqInd = i;
688  }
689  }
690  }
691  if(alpha < 1)
692  {
693  last_active = inactive[ineqInd];
694  if(active.size())
695  {
696  auto it = active.begin();
697  while(it != active.end() && *it < inactive[ineqInd])
698  it++;
699  active.insert(it, inactive[ineqInd]);
700  }
701  else
702  active.push_back(inactive[ineqInd]);
703  inactive.erase(inactive.begin()+ineqInd);
704  update_Ap = true;
705  }
706  else
707  update_Ap = false;
708  // update x for next iteration
709  x += alpha * u;
710  g -= alpha*Q*u;
711  }
712  }
713 }
714 #else
715 void dummy_vpQuadProg(){};
716 #endif
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpColVector eigenValues() const
Definition: vpMatrix.cpp:4714
vpColVector extract(unsigned int r, unsigned int colsize) const
Definition: vpColVector.h:220
static bool colReduction(vpMatrix &A, vpColVector &b, bool full_rank=false, const double &tol=1e-6)
Definition: vpLinProg.cpp:97
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
vpColVector x1
Definition: vpQuadProg.h:124
static bool solveByProjection(const vpMatrix &Q, const vpColVector &r, vpMatrix &A, vpColVector &b, vpColVector &x, const double &tol=1e-6)
Definition: vpQuadProg.cpp:184
error that can be emited by ViSP classes.
Definition: vpException.h:71
static bool allGreater(const vpColVector &x, const double &thr=1e-6)
Definition: vpLinProg.h:230
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
unsigned int getCols() const
Definition: vpArray2D.h:279
static void fromCanonicalCost(const vpMatrix &H, const vpColVector &c, vpMatrix &Q, vpColVector &r, const double &tol=1e-6)
Definition: vpQuadProg.cpp:98
vpRowVector getRow(const unsigned int i) const
Definition: vpMatrix.cpp:3933
static bool simplex(const vpColVector &c, vpMatrix A, vpColVector b, vpColVector &x, const double &tol=1e-6)
Definition: vpLinProg.cpp:564
std::vector< unsigned int > inactive
Definition: vpQuadProg.h:120
void solveBySVD(const vpColVector &B, vpColVector &x) const
Definition: vpMatrix.cpp:1758
void diag(const double &val=1.0)
Definition: vpMatrix.cpp:782
void clear()
Definition: vpColVector.h:175
vpMatrix transpose() const
Definition: vpMatrix.cpp:394
unsigned int getRows() const
Definition: vpArray2D.h:289
bool setEqualityConstraint(const vpMatrix &A, const vpColVector &b, const double &tol=1e-6)
Definition: vpQuadProg.cpp:155
std::vector< unsigned int > active
Definition: vpQuadProg.h:116
vpMatrix t() const
Definition: vpMatrix.cpp:375
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
bool solveQPe(const vpMatrix &Q, const vpColVector &r, vpColVector &x, const double &tol=1e-6) const
Definition: vpQuadProg.cpp:255
static bool allLesser(const vpMatrix &C, const vpColVector &x, const vpColVector &d, const double &thr=1e-6)
Definition: vpLinProg.h:194
vpMatrix Z
Definition: vpQuadProg.h:128
bool solveQPi(const vpMatrix &Q, const vpColVector &r, const vpMatrix &C, const vpColVector &d, vpColVector &x, const bool use_equality=false, const double &tol=1e-6)
Definition: vpQuadProg.cpp:465
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2021
Function not implemented.
Definition: vpException.h:90
static bool allZero(const vpColVector &x, const double &tol=1e-6)
Definition: vpLinProg.h:155
static unsigned int checkDimensions(const vpMatrix &Q, const vpColVector &r, const vpMatrix *A, const vpColVector *b, const vpMatrix *C, const vpColVector *d, const std::string fct)
Definition: vpQuadProg.h:149
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310
bool solveQP(const vpMatrix &Q, const vpColVector &r, vpMatrix A, vpColVector b, const vpMatrix &C, const vpColVector &d, vpColVector &x, const double &tol=1e-6)
Definition: vpQuadProg.cpp:389