Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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 vpQuadProgh
35#define vpQuadProgh
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
69class VISP_EXPORT vpQuadProg
70{
71public:
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);
90 void resetActiveSet() { active.clear(); }
92
93 static void fromCanonicalCost(const vpMatrix &H, const vpColVector &c, vpMatrix &Q, vpColVector &r,
94 const double &tol = 1e-6);
95 static bool solveQPe(const vpMatrix &Q, const vpColVector &r, vpMatrix A, vpColVector b, vpColVector &x,
96 const double &tol = 1e-6);
97
98protected:
102 std::vector<unsigned int> active;
106 std::vector<unsigned int> inactive;
115
116 static vpColVector solveSVDorQR(const vpMatrix &A, const vpColVector &b);
117
118 static bool solveByProjection(const vpMatrix &Q, const vpColVector &r, vpMatrix &A, vpColVector &b, vpColVector &x,
119 const double &tol = 1e-6);
120
136 static unsigned int checkDimensions(const vpMatrix &Q, const vpColVector &r, const vpMatrix *A, const vpColVector *b,
137 const vpMatrix *C, const vpColVector *d, const std::string fct)
138 {
139 // check data consistency
140 unsigned int n = Q.getCols();
141 const bool Ab = (A != nullptr && b != nullptr && A->getRows());
142 const bool Cd = (C != nullptr && d != nullptr && C->getRows());
143
144 if ((Ab && n != A->getCols()) || (Cd && n != C->getCols()) || (Ab && A->getRows() != b->getRows()) ||
145 (Cd && C->getRows() != d->getRows()) || Q.getRows() != r.getRows()) {
146 std::cout << "vpQuadProg::" << fct << ": wrong dimension\n"
147 << "Q: " << Q.getRows() << "x" << Q.getCols() << " - r: " << r.getRows() << std::endl;
148 if (Ab)
149 std::cout << "A: " << A->getRows() << "x" << A->getCols() << " - b: " << b->getRows() << std::endl;
150 if (Cd)
151 std::cout << "C: " << C->getRows() << "x" << C->getCols() << " - d: " << d->getRows() << std::endl;
153 }
154 return n;
155 }
156#endif
157};
158#endif
unsigned int getCols() const
Definition vpArray2D.h:280
unsigned int getRows() const
Definition vpArray2D.h:290
Implementation of column vector and the associated operations.
@ dimensionError
Bad dimension.
Definition vpException.h:83
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
This class provides a solver for Quadratic Programs.
Definition vpQuadProg.h:70
std::vector< unsigned int > inactive
Definition vpQuadProg.h:106
std::vector< unsigned int > active
Definition vpQuadProg.h:102
vpMatrix Z
Definition vpQuadProg.h:114
vpColVector x1
Definition vpQuadProg.h:110
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:136
void resetActiveSet()
Definition vpQuadProg.h:90