Visual Servoing Platform
version 3.3.0
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);
101
void
resetActiveSet
()
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
;
124
vpColVector
x1
;
128
vpMatrix
Z
;
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;
173
throw
vpMatrixException::dimensionError
;
174
}
175
return
n;
176
}
177
#endif
178
};
179
#endif
vpQuadProg::Z
vpMatrix Z
Definition:
vpQuadProg.h:128
vpQuadProg::inactive
std::vector< unsigned int > inactive
Definition:
vpQuadProg.h:120
vpQuadProg::active
std::vector< unsigned int > active
Definition:
vpQuadProg.h:116
vpException::dimensionError
@ dimensionError
Bad dimension.
Definition:
vpException.h:95
vpColVector
Implementation of column vector and the associated operations.
Definition:
vpColVector.h:131
vpMatrix
Implementation of a matrix and operations on matrices.
Definition:
vpMatrix.h:165
vpArray2D::getCols
unsigned int getCols() const
Definition:
vpArray2D.h:279
vpQuadProg::x1
vpColVector x1
Definition:
vpQuadProg.h:124
vpQuadProg::resetActiveSet
void resetActiveSet()
Definition:
vpQuadProg.h:101
vpQuadProg
This class provides a solver for Quadratic Programs.
Definition:
vpQuadProg.h:75
vpQuadProg::checkDimensions
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
vpArray2D::getRows
unsigned int getRows() const
Definition:
vpArray2D.h:289
modules
core
include
visp3
core
vpQuadProg.h
Generated by
1.8.18