Visual Servoing Platform  version 3.6.1 under development (2024-03-29)
vpQuadProg.h
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 #ifndef _vpQuadProg_h_
35 #define _vpQuadProg_h_
36 
37 #include <stdlib.h>
38 #include <vector>
39 #include <visp3/core/vpConfig.h>
40 #include <visp3/core/vpLinProg.h>
41 #include <visp3/core/vpMatrix.h>
42 #include <visp3/core/vpMatrixException.h>
43 
69 class VISP_EXPORT vpQuadProg
70 {
71 public:
72 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
75  bool solveQPe(const vpMatrix &Q, const vpColVector &r, vpColVector &x, const double &tol = 1e-6) const;
76 
77  bool solveQPi(const vpMatrix &Q, const vpColVector &r, const vpMatrix &C, const vpColVector &d, vpColVector &x,
78  bool use_equality = false, const double &tol = 1e-6);
79 
80  bool solveQP(const vpMatrix &Q, const vpColVector &r, vpMatrix A, vpColVector b, const vpMatrix &C,
81  const vpColVector &d, vpColVector &x, const double &tol = 1e-6);
83 
86  bool setEqualityConstraint(const vpMatrix &A, const vpColVector &b, const double &tol = 1e-6);
87 
91  void resetActiveSet() { active.clear(); }
93 
94  static void fromCanonicalCost(const vpMatrix &H, const vpColVector &c, vpMatrix &Q, vpColVector &r,
95  const double &tol = 1e-6);
96  static bool solveQPe(const vpMatrix &Q, const vpColVector &r, vpMatrix A, vpColVector b, vpColVector &x,
97  const double &tol = 1e-6);
98 
99 protected:
103  std::vector<unsigned int> active;
104 
108  std::vector<unsigned int> inactive;
109 
114 
119 
120  static vpColVector solveSVDorQR(const vpMatrix &A, const vpColVector &b);
121 
122  static bool solveByProjection(const vpMatrix &Q, const vpColVector &r, vpMatrix &A, vpColVector &b, vpColVector &x,
123  const double &tol = 1e-6);
124 
140  static unsigned int checkDimensions(const vpMatrix &Q, const vpColVector &r, const vpMatrix *A, const vpColVector *b,
141  const vpMatrix *C, const vpColVector *d, const std::string fct)
142  {
143  // check data consistency
144  unsigned int n = Q.getCols();
145  const bool Ab = (A != nullptr && b != nullptr && A->getRows());
146  const bool Cd = (C != nullptr && d != nullptr && C->getRows());
147 
148  if ((Ab && n != A->getCols()) || (Cd && n != C->getCols()) || (Ab && A->getRows() != b->getRows()) ||
149  (Cd && C->getRows() != d->getRows()) || Q.getRows() != r.getRows()) {
150  std::cout << "vpQuadProg::" << fct << ": wrong dimension\n"
151  << "Q: " << Q.getRows() << "x" << Q.getCols() << " - r: " << r.getRows() << std::endl;
152  if (Ab)
153  std::cout << "A: " << A->getRows() << "x" << A->getCols() << " - b: " << b->getRows() << std::endl;
154  if (Cd)
155  std::cout << "C: " << C->getRows() << "x" << C->getCols() << " - d: " << d->getRows() << std::endl;
157  }
158  return n;
159  }
160 #endif
161 };
162 #endif
unsigned int getCols() const
Definition: vpArray2D.h:274
unsigned int getRows() const
Definition: vpArray2D.h:284
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
@ dimensionError
Bad dimension.
Definition: vpException.h:83
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
This class provides a solver for Quadratic Programs.
Definition: vpQuadProg.h:70
std::vector< unsigned int > inactive
Definition: vpQuadProg.h:108
std::vector< unsigned int > active
Definition: vpQuadProg.h:103
vpMatrix Z
Definition: vpQuadProg.h:118
vpColVector x1
Definition: vpQuadProg.h:113
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:140
void resetActiveSet()
Definition: vpQuadProg.h:91