Visual Servoing Platform  version 3.5.1 under development (2022-07-01)
vpQuadProg.h
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 #ifndef vpQuadProgh
40 #define vpQuadProgh
41 
42 #include <stdlib.h>
43 #include <vector>
44 #include <visp3/core/vpConfig.h>
45 #include <visp3/core/vpLinProg.h>
46 #include <visp3/core/vpMatrix.h>
47 #include <visp3/core/vpMatrixException.h>
48 
74 class VISP_EXPORT vpQuadProg
75 {
76 public:
77 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
80  bool solveQPe(const vpMatrix &Q, const vpColVector &r, vpColVector &x, const double &tol = 1e-6) const;
81 
82  bool solveQPi(const vpMatrix &Q, const vpColVector &r, const vpMatrix &C, const vpColVector &d, vpColVector &x,
83  bool use_equality = false, const double &tol = 1e-6);
84 
85  bool solveQP(const vpMatrix &Q, const vpColVector &r, vpMatrix A, vpColVector b, const vpMatrix &C,
86  const vpColVector &d, vpColVector &x, const double &tol = 1e-6);
88 
91  bool setEqualityConstraint(const vpMatrix &A, const vpColVector &b, const double &tol = 1e-6);
95  void resetActiveSet() { active.clear(); }
97 
98  static void fromCanonicalCost(const vpMatrix &H, const vpColVector &c, vpMatrix &Q, vpColVector &r,
99  const double &tol = 1e-6);
100  static bool solveQPe(const vpMatrix &Q, const vpColVector &r, vpMatrix A, vpColVector b, vpColVector &x,
101  const double &tol = 1e-6);
102 
103 protected:
107  std::vector<unsigned int> active;
111  std::vector<unsigned int> inactive;
120 
121  static vpColVector solveSVDorQR(const vpMatrix &A, const vpColVector &b);
122 
123  static bool solveByProjection(const vpMatrix &Q, const vpColVector &r, vpMatrix &A, vpColVector &b, vpColVector &x,
124  const double &tol = 1e-6);
125 
141  static unsigned int checkDimensions(const vpMatrix &Q, const vpColVector &r, const vpMatrix *A, const vpColVector *b,
142  const vpMatrix *C, const vpColVector *d, const std::string fct)
143  {
144  // check data consistency
145  unsigned int n = Q.getCols();
146  const bool Ab = (A != nullptr && b != nullptr && A->getRows());
147  const bool Cd = (C != nullptr && d != nullptr && C->getRows());
148 
149  if ((Ab && n != A->getCols()) || (Cd && n != C->getCols()) || (Ab && A->getRows() != b->getRows()) ||
150  (Cd && C->getRows() != d->getRows()) || Q.getRows() != r.getRows()) {
151  std::cout << "vpQuadProg::" << fct << ": wrong dimension\n"
152  << "Q: " << Q.getRows() << "x" << Q.getCols() << " - r: " << r.getRows() << std::endl;
153  if (Ab)
154  std::cout << "A: " << A->getRows() << "x" << A->getCols() << " - b: " << b->getRows() << std::endl;
155  if (Cd)
156  std::cout << "C: " << C->getRows() << "x" << C->getCols() << " - d: " << d->getRows() << std::endl;
158  }
159  return n;
160  }
161 #endif
162 };
163 #endif
unsigned int getCols() const
Definition: vpArray2D.h:281
unsigned int getRows() const
Definition: vpArray2D.h:291
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
@ dimensionError
Bad dimension.
Definition: vpException.h:95
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
This class provides a solver for Quadratic Programs.
Definition: vpQuadProg.h:75
std::vector< unsigned int > inactive
Definition: vpQuadProg.h:111
std::vector< unsigned int > active
Definition: vpQuadProg.h:107
vpMatrix Z
Definition: vpQuadProg.h:119
vpColVector x1
Definition: vpQuadProg.h:115
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
void resetActiveSet()
Definition: vpQuadProg.h:95