Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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 <vector>
43 #include <stdlib.h>
44 #include <visp3/core/vpConfig.h>
45 #include <visp3/core/vpMatrix.h>
46 #include <visp3/core/vpMatrixException.h>
47 #include <visp3/core/vpLinProg.h>
48 
74 class VISP_EXPORT vpQuadProg
75 {
76 public:
77 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
78 
80  bool solveQPe(const vpMatrix &Q, const vpColVector &r,
81  vpColVector &x, const double &tol = 1e-6) const;
82 
83  bool solveQPi(const vpMatrix &Q, const vpColVector &r,
84  const vpMatrix &C, const vpColVector &d,
85  vpColVector &x,
86  bool use_equality = false,
87  const double &tol = 1e-6);
88 
89  bool solveQP(const vpMatrix &Q, const vpColVector &r,
90  vpMatrix A, vpColVector b,
91  const vpMatrix &C, const vpColVector &d,
92  vpColVector &x, const double &tol = 1e-6);
94 
97  bool setEqualityConstraint(const vpMatrix &A, const vpColVector &b, const double &tol = 1e-6);
102  {
103  active.clear();
104  }
106 
107  static void fromCanonicalCost(const vpMatrix &H, const vpColVector &c, vpMatrix &Q, vpColVector &r, const double &tol = 1e-6);
108  static bool solveQPe(const vpMatrix &Q, const vpColVector &r,
109  vpMatrix A, vpColVector b,
110  vpColVector &x, const double &tol = 1e-6);
111 
112 protected:
116  std::vector<unsigned int> active;
120  std::vector<unsigned int> inactive;
129 
130  static vpColVector solveSVDorQR(const vpMatrix &A, const vpColVector &b);
131 
132  static bool solveByProjection(const vpMatrix &Q, const vpColVector &r,
133  vpMatrix &A, vpColVector &b,
134  vpColVector &x, const double &tol = 1e-6);
135 
151  static unsigned int checkDimensions(const vpMatrix &Q, const vpColVector &r,
152  const vpMatrix* A, const vpColVector* b,
153  const vpMatrix* C, const vpColVector* d,
154  const std::string fct)
155  {
156  // check data consistency
157  unsigned int n = Q.getCols();
158  const bool Ab = (A != nullptr && b != nullptr && A->getRows());
159  const bool Cd = (C != nullptr && d != nullptr && C->getRows());
160 
161  if ( (Ab && n != A->getCols()) ||
162  (Cd && n != C->getCols()) ||
163  (Ab && A->getRows() != b->getRows()) ||
164  (Cd && C->getRows() != d->getRows()) ||
165  Q.getRows() != r.getRows())
166  {
167  std::cout << "vpQuadProg::" << fct << ": wrong dimension\n" <<
168  "Q: " << Q.getRows() << "x" << Q.getCols() << " - r: " << r.getRows() << std::endl;
169  if(Ab)
170  std::cout << "A: " << A->getRows() << "x" << A->getCols() << " - b: " << b->getRows() << std::endl;
171  if(Cd)
172  std::cout << "C: " << C->getRows() << "x" << C->getCols() << " - d: " << d->getRows() << std::endl;
174  }
175  return n;
176  }
177 #endif
178 };
179 #endif
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
vpColVector x1
Definition: vpQuadProg.h:124
unsigned int getRows() const
Definition: vpArray2D.h:289
unsigned int getCols() const
Definition: vpArray2D.h:279
std::vector< unsigned int > inactive
Definition: vpQuadProg.h:120
This class provides a solver for Quadratic Programs.
Definition: vpQuadProg.h:74
std::vector< unsigned int > active
Definition: vpQuadProg.h:116
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
void resetActiveSet()
Definition: vpQuadProg.h:101
vpMatrix Z
Definition: vpQuadProg.h:128
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:151