Visual Servoing Platform  version 3.6.1 under development (2024-12-09)
vpQuadProg.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Quadratic Programming
32  */
33 
34 #include <algorithm>
35 #include <visp3/core/vpMatrixException.h>
36 #include <visp3/core/vpQuadProg.h>
37 
38 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
39 BEGIN_VISP_NAMESPACE
96  const double &tol)
97 {
98 #if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV)
99  unsigned int n = H.getCols();
100  if (H.getRows() != n || c.getRows() != n) {
102  "vpQuadProg::fromCanonicalCost: H is not square or not the same dimension as c"));
103  }
104 
105  vpColVector d(n);
106  vpMatrix V(n, n);
107  // Compute the eigenvalues and eigenvectors
108  H.eigenValues(d, V);
109  // find first non-nullptr eigen value
110  unsigned int k = 0;
111  for (unsigned int i = 0; i < n; ++i) {
112  if (d[i] > tol)
113  d[i] = sqrt(d[i]);
114  else if (d[i] < tol)
115  throw(vpException(vpMatrixException::matrixError, "vpQuadProg::fromCanonicalCost: H is not positive"));
116  else
117  k = i + 1;
118  }
119  // build (Q,r) such that H = Q.^T.Q and c = -Q^T.r
120  vpMatrix D(n - k, n - k);
121  vpMatrix P(n - k, n);
122  D.diag(d.extract(k, n - k));
123  for (unsigned int i = 0; i < n - k; ++i)
124  P[i][k + i] = 1;
125 
126  Q = D * P * V.transpose();
127  r = -Q.t().pseudoInverse() * c;
128 #else
129  throw(vpException(vpException::functionNotImplementedError, "Symmetric matrix decomposition is not implemented. You "
130  "should install Lapack, Eigen3 or OpenCV 3rd party"));
131 #endif
132 }
133 
147 bool vpQuadProg::setEqualityConstraint(const vpMatrix &A, const vpColVector &b, const double &tol)
148 {
149  x1 = b;
150  Z = A;
151  if (A.getRows() == b.getRows() && vpLinProg::colReduction(Z, x1, false, tol))
152  return true;
153 
154  std::cout << "vpQuadProg::setEqualityConstraint: equality constraint infeasible" << std::endl;
155  return false;
156 }
157 
177  const double &tol)
178 {
179  if (A.getRows()) {
180  if (!vpLinProg::colReduction(A, b, false, tol))
181  return false;
182 
183  if (A.getCols() && (Q * A).infinityNorm() > tol)
184  x = b + A * (Q * A).solveBySVD(r - Q * b);
185  else
186  x = b;
187  }
188  else
189  x = Q.solveBySVD(r);
190  return true;
191 }
192 
248 bool vpQuadProg::solveQPe(const vpMatrix &Q, const vpColVector &r, vpColVector &x, const double &tol) const
249 {
250  unsigned int n = Q.getCols();
251  if (Q.getRows() != r.getRows() || Z.getRows() != n || x1.getRows() != n) {
252  std::cout << "vpQuadProg::solveQPe: wrong dimension\n"
253  << "Q: " << Q.getRows() << "x" << Q.getCols() << " - r: " << r.getRows() << std::endl;
254  std::cout << "Z: " << Z.getRows() << "x" << Z.getCols() << " - x1: " << x1.getRows() << std::endl;
256  }
257  if (Z.getCols()) {
258  if ((Q * Z).infinityNorm() > tol)
259  x = x1 + Z * (Q * Z).solveBySVD(r - Q * x1);
260  else
261  x = x1;
262  }
263  else
264  x = Q.solveBySVD(r);
265  return true;
266 }
267 
318  const double &tol)
319 {
320  checkDimensions(Q, r, &A, &b, nullptr, nullptr, "solveQPe");
321 
322  if (!solveByProjection(Q, r, A, b, x, tol)) {
323  std::cout << "vpQuadProg::solveQPe: equality constraint infeasible" << std::endl;
324  return false;
325  }
326  return true;
327 }
328 
384 bool vpQuadProg::solveQP(const vpMatrix &Q, const vpColVector &r, vpMatrix A, vpColVector b, const vpMatrix &C,
385  const vpColVector &d, vpColVector &x, const double &tol)
386 {
387  if (A.getRows() == 0)
388  return solveQPi(Q, r, C, d, x, false, tol);
389 
390  checkDimensions(Q, r, &A, &b, &C, &d, "solveQP");
391 
392  if (!vpLinProg::colReduction(A, b, false, tol)) {
393  std::cout << "vpQuadProg::solveQP: equality constraint infeasible" << std::endl;
394  return false;
395  }
396 
397  if (A.getCols() && solveQPi(Q * A, r - Q * b, C * A, d - C * b, x, false, tol)) {
398  x = b + A * x;
399  return true;
400  }
401  else if (vpLinProg::allLesser(C, b, d, tol)) // Ax = b has only 1 solution
402  {
403  x = b;
404  return true;
405  }
406  std::cout << "vpQuadProg::solveQP: inequality constraint infeasible" << std::endl;
407  return false;
408 }
409 
458 bool vpQuadProg::solveQPi(const vpMatrix &Q, const vpColVector &r, const vpMatrix &C, const vpColVector &d,
459  vpColVector &x, bool use_equality, const double &tol)
460 {
461  unsigned int n = checkDimensions(Q, r, nullptr, nullptr, &C, &d, "solveQPi");
462 
463  if (use_equality) {
464  if (Z.getRows() == n) {
465  if (Z.getCols() && solveQPi(Q * Z, r - Q * x1, C * Z, d - C * x1, x, false, tol)) {
466  // back to initial solution
467  x = x1 + Z * x;
468  return true;
469  }
470  else if (vpLinProg::allLesser(C, x1, d, tol)) {
471  x = x1;
472  return true;
473  }
474  std::cout << "vpQuadProg::solveQPi: inequality constraint infeasible" << std::endl;
475  return false;
476  }
477  else
478  std::cout << "vpQuadProg::solveQPi: use_equality before setEqualityConstraint" << std::endl;
479  }
480 
481  const unsigned int p = C.getRows();
482 
483  // look for trivial solution
484  // r = 0 and d > 0 -> x = 0
485  if (vpLinProg::allZero(r, tol) && (d.getRows() == 0 || vpLinProg::allGreater(d, -tol))) {
486  x.resize(n);
487  return true;
488  }
489 
490  // go for solver
491  // build feasible point
492  vpMatrix A;
493  vpColVector b;
494  // check active set - all values should be < rows of C
495  for (auto v : active) {
496  if (v >= p) {
497  active.clear();
498  std::cout << "vpQuadProg::solveQPi: some constraints have been removed since last call\n";
499  break;
500  }
501  }
502 
503  // warm start from previous active set
504  A.resize((unsigned int)active.size(), n);
505  b.resize((unsigned int)active.size());
506  for (unsigned int i = 0; i < active.size(); ++i) {
507  for (unsigned int j = 0; j < n; ++j)
508  A[i][j] = C[active[i]][j];
509  b[i] = d[active[i]];
510  }
511  if (!solveByProjection(Q, r, A, b, x, tol))
512  x.resize(n);
513 
514  // or from simplex if we really have no clue
515  if (!vpLinProg::allLesser(C, x, d, tol)) {
516  // feasible point with simplex:
517  // min r
518  // st C.(x + z1 - z2) + y - r = d
519  // st z1, z2, y, r >= 0
520  // dim r = violated constraints
521  // feasible if r can be minimized to 0
522 
523  // count how many violated constraints
524  vpColVector e = d - C * x;
525  unsigned int k = 0;
526  for (unsigned int i = 0; i < p; ++i) {
527  if (e[i] < -tol)
528  k++;
529  }
530  // cost vector
531  vpColVector c(2 * n + p + k);
532  for (unsigned int i = 0; i < k; ++i)
533  c[2 * n + p + i] = 1;
534 
535  vpColVector xc(2 * n + p + k);
536 
537  vpMatrix A_lp(p, 2 * n + p + k);
538  unsigned int l = 0;
539  for (unsigned int i = 0; i < p; ++i) {
540  // copy [C -C] part
541  for (unsigned int j = 0; j < n; ++j) {
542  A_lp[i][j] = C[i][j];
543  A_lp[i][n + j] = -C[i][j];
544  }
545  // y-part
546  A_lp[i][2 * n + i] = 1;
547  if (e[i] < -tol) {
548  // r-part
549  A_lp[i][2 * n + p + l] = -1;
550  xc[2 * n + p + l] = -e[i];
551  l++;
552  }
553  else
554  xc[2 * n + i] = e[i];
555  }
556  vpLinProg::simplex(c, A_lp, e, xc);
557 
558  // r-part should be 0
559  if (!vpLinProg::allLesser(xc.extract(2 * n + p, k), tol)) {
560  std::cout << "vpQuadProg::solveQPi: inequality constraints not feasible" << std::endl;
561  return false;
562  }
563 
564  // update x to feasible point
565  x += xc.extract(0, n) - xc.extract(n, n);
566  // init active/inactive sets from y-part of x
567  active.clear();
568  active.reserve(p);
569  inactive.clear();
570  for (unsigned int i = 0; i < p; ++i) {
571  if (C.getRow(i) * x - d[i] < -tol)
572  inactive.push_back(i);
573  else
574  active.push_back(i);
575  }
576  }
577  else // warm start feasible
578  {
579  // using previous active set, check that inactive is sync
580  if (active.size() + inactive.size() != p) {
581  inactive.clear();
582  for (unsigned int i = 0; i < p; ++i) {
583  if (std::find(active.begin(), active.end(), i) == active.end())
584  inactive.push_back(i);
585  }
586  }
587  }
588 
589  vpMatrix Ap;
590  bool update_Ap = true;
591  unsigned int last_active = C.getRows();
592 
593  vpColVector u, g = r - Q * x, mu;
594 
595  // solve at one iteration
596  while (true) {
597  A.resize((unsigned int)active.size(), n);
598  b.resize((unsigned int)active.size());
599  for (unsigned int i = 0; i < active.size(); ++i) {
600  for (unsigned int j = 0; j < n; ++j)
601  A[i][j] = C[active[i]][j];
602  }
603 
604  if (update_Ap && active.size())
605  Ap = A.pseudoInverse(); // to get Lagrange multipliers if needed
606 
607  if (!solveByProjection(Q, g, A, b, u, tol)) {
608  std::cout << "vpQuadProg::solveQPi: QP seems infeasible, too many constraints activated\n";
609  return false;
610  }
611 
612  // 0-update = optimal or useless activated constraints
613  if (vpLinProg::allZero(u, tol)) {
614  // compute multipliers if any
615  unsigned int ineqInd = (unsigned int)active.size();
616  if (active.size()) {
617  mu = -Ap.transpose() * Q.transpose() * (Q * u - g);
618  // find most negative one if any - except last activated in case of degeneracy
619  double ineqMax = -tol;
620  for (unsigned int i = 0; i < mu.getRows(); ++i) {
621  if (mu[i] < ineqMax && active[i] != last_active) {
622  ineqInd = i;
623  ineqMax = mu[i];
624  }
625  }
626  }
627 
628  if (ineqInd == active.size()) // KKT condition no useless constraint
629  return true;
630 
631  // useless inequality, deactivate
632  inactive.push_back(active[ineqInd]);
633  if (active.size() == 1)
634  active.clear();
635  else
636  active.erase(active.begin() + ineqInd);
637  update_Ap = true;
638  }
639  else // u != 0, can improve xc
640  {
641  unsigned int ineqInd = 0;
642  // step length to next constraint
643  double alpha = 1;
644  for (unsigned int i = 0; i < inactive.size(); ++i) {
645  const double Cu = C.getRow(inactive[i]) * u;
646  if (Cu > tol) {
647  const double a = (d[inactive[i]] - C.getRow(inactive[i]) * x) / Cu;
648  if (a < alpha) {
649  alpha = a;
650  ineqInd = i;
651  }
652  }
653  }
654  if (alpha < 1) {
655  last_active = inactive[ineqInd];
656  if (active.size()) {
657  auto it = active.begin();
658  while (it != active.end() && *it < inactive[ineqInd])
659  it++;
660  active.insert(it, inactive[ineqInd]);
661  }
662  else
663  active.push_back(inactive[ineqInd]);
664  inactive.erase(inactive.begin() + ineqInd);
665  update_Ap = true;
666  }
667  else
668  update_Ap = false;
669  // update x for next iteration
670  x += alpha * u;
671  g -= alpha * Q * u;
672  }
673  }
674 }
675 
687 {
688  // assume A is full rank
689  if (A.getRows() > A.getCols())
690  return A.solveBySVD(b);
691  return A.solveByQR(b);
692 }
693 END_VISP_NAMESPACE
694 #else
695 void dummy_vpQuadProg() { };
696 #endif
unsigned int getCols() const
Definition: vpArray2D.h:337
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:362
unsigned int getRows() const
Definition: vpArray2D.h:347
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
vpColVector extract(unsigned int r, unsigned int colsize) const
Definition: vpColVector.h:405
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:66
@ dimensionError
Bad dimension.
Definition: vpException.h:71
static bool allGreater(const vpColVector &x, const double &thr=1e-6)
Definition: vpLinProg.h:220
static bool allZero(const vpColVector &x, const double &tol=1e-6)
Definition: vpLinProg.h:149
static bool allLesser(const vpMatrix &C, const vpColVector &x, const vpColVector &d, const double &thr=1e-6)
Definition: vpLinProg.h:186
static bool colReduction(vpMatrix &A, vpColVector &b, bool full_rank=false, const double &tol=1e-6)
Definition: vpLinProg.cpp:97
static bool simplex(const vpColVector &c, vpMatrix A, vpColVector b, vpColVector &x, const double &tol=1e-6)
Definition: vpLinProg.cpp:553
@ matrixError
Matrix operation error.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
vpColVector eigenValues() const
Definition: vpMatrix.cpp:1192
void solveBySVD(const vpColVector &B, vpColVector &x) const
vpRowVector getRow(unsigned int i) const
Definition: vpMatrix.cpp:590
void solveByQR(const vpColVector &b, vpColVector &x) const
vpMatrix pseudoInverse(double svThreshold=1e-6) const
vpMatrix transpose() const
vpMatrix t() const
std::vector< unsigned int > inactive
Definition: vpQuadProg.h:109
std::vector< unsigned int > active
Definition: vpQuadProg.h:104
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:384
vpMatrix Z
Definition: vpQuadProg.h:119
vpColVector x1
Definition: vpQuadProg.h:114
bool solveQPe(const vpMatrix &Q, const vpColVector &r, vpColVector &x, const double &tol=1e-6) const
Definition: vpQuadProg.cpp:248
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:141
static void fromCanonicalCost(const vpMatrix &H, const vpColVector &c, vpMatrix &Q, vpColVector &r, const double &tol=1e-6)
Definition: vpQuadProg.cpp:95
bool setEqualityConstraint(const vpMatrix &A, const vpColVector &b, const double &tol=1e-6)
Definition: vpQuadProg.cpp:147
static vpColVector solveSVDorQR(const vpMatrix &A, const vpColVector &b)
Definition: vpQuadProg.cpp:686
bool solveQPi(const vpMatrix &Q, const vpColVector &r, const vpMatrix &C, const vpColVector &d, vpColVector &x, bool use_equality=false, const double &tol=1e-6)
Definition: vpQuadProg.cpp:458
static bool solveByProjection(const vpMatrix &Q, const vpColVector &r, vpMatrix &A, vpColVector &b, vpColVector &x, const double &tol=1e-6)
Definition: vpQuadProg.cpp:176