Visual Servoing Platform  version 3.5.1 under development (2023-09-22)
vpQuadProg Class Reference

#include <visp3/core/vpQuadProg.h>

Public Member Functions

Instantiated solvers <br>
bool solveQPe (const vpMatrix &Q, const vpColVector &r, vpColVector &x, const double &tol=1e-6) const
 
bool solveQPi (const vpMatrix &Q, const vpColVector &r, const vpMatrix &C, const vpColVector &d, vpColVector &x, bool use_equality=false, const double &tol=1e-6)
 
bool solveQP (const vpMatrix &Q, const vpColVector &r, vpMatrix A, vpColVector b, const vpMatrix &C, const vpColVector &d, vpColVector &x, const double &tol=1e-6)
 

Managing sequential calls to solvers <br>

std::vector< unsigned int > active
 
std::vector< unsigned int > inactive
 
vpColVector x1
 
vpMatrix Z
 
bool setEqualityConstraint (const vpMatrix &A, const vpColVector &b, const double &tol=1e-6)
 
void resetActiveSet ()
 
static void fromCanonicalCost (const vpMatrix &H, const vpColVector &c, vpMatrix &Q, vpColVector &r, const double &tol=1e-6)
 
static bool solveQPe (const vpMatrix &Q, const vpColVector &r, vpMatrix A, vpColVector b, vpColVector &x, const double &tol=1e-6)
 
static vpColVector solveSVDorQR (const vpMatrix &A, const vpColVector &b)
 
static bool solveByProjection (const vpMatrix &Q, const vpColVector &r, vpMatrix &A, vpColVector &b, vpColVector &x, const double &tol=1e-6)
 
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)
 

Detailed Description

This class provides a solver for Quadratic Programs.

The cost function is written under the form $ \min ||\mathbf{Q}\mathbf{x} - \mathbf{r}||^2$.

If a cost function is written under the canonical form $\min \frac{1}{2}\mathbf{x}^T\mathbf{H}\mathbf{x} + \mathbf{c}^T\mathbf{x}$ then fromCanonicalCost() can be used to retrieve Q and r from H and c.

Equality constraints are solved through projection into the kernel.

Inequality constraints are solved with active sets.

In order to be used sequentially, the decomposition of the equality constraint may be stored. The last active set is always stored and used to warm start the next call.

Warning
The solvers are only available if c++11 or higher is activated during build. Configure ViSP using cmake -DUSE_CXX_STANDARD=11.
Examples
quadprog.cpp, and quadprog_eq.cpp.

Definition at line 69 of file vpQuadProg.h.

Member Function Documentation

◆ checkDimensions()

static unsigned int vpQuadProg::checkDimensions ( const vpMatrix Q,
const vpColVector r,
const vpMatrix A,
const vpColVector b,
const vpMatrix C,
const vpColVector d,
const std::string  fct 
)
inlinestaticprotected

Performs a dimension check of passed QP matrices and vectors.

If any inconsistency is detected, displays a summary and throws an exception.

Parameters
Q: cost matrix (dimension c x n)
r: cost vector (dimension c)
A: pointer to the equality matrix (if any, dimension m x n)
b: pointer to the equality vector (if any, dimension m)
C: pointer to the inequality matrix (if any, dimension p x n)
d: pointer to the inequality vector (if any, dimension p)
fct: name of the solver that called this function
Returns
the dimension of the search space.

Definition at line 136 of file vpQuadProg.h.

References vpException::dimensionError, vpArray2D< Type >::getCols(), and vpArray2D< Type >::getRows().

Referenced by solveQP(), solveQPe(), and solveQPi().

◆ fromCanonicalCost()

void vpQuadProg::fromCanonicalCost ( const vpMatrix H,
const vpColVector c,
vpMatrix Q,
vpColVector r,
const double &  tol = 1e-6 
)
static

Changes a canonical quadratic cost $\min \frac{1}{2}\mathbf{x}^T\mathbf{H}\mathbf{x} + \mathbf{c}^T\mathbf{x}$ to the formulation used by this class $ \min ||\mathbf{Q}\mathbf{x} - \mathbf{r}||^2$.

Computes $(\mathbf{Q}, \mathbf{r})$ such that $\mathbf{H} = \mathbf{Q}^T\mathbf{Q}$ and $\mathbf{c} = -\mathbf{Q}^T\mathbf{r}$.

Parameters
H: canonical symmetric positive cost matrix (dimension n x n)
c: canonical cost vector (dimension n)
Q: custom cost matrix (dimension rank(H) x n)
r: custom cost vector (dimension rank(H))
tol: tolerance to test ranks
Warning
This method is only available if the Gnu Scientific Library (GSL) is detected as a third party library.

Here is an example:

$\begin{array}{lll} \mathbf{x} = & \arg\min & 2x_1^2 + x_2^2 + x_1x_2 + x_1 + x_2\\ & \text{s.t.}& x_1 + x_2 = 1 \end{array} \Leftrightarrow \begin{array}{lll} \mathbf{x} = & \arg\min & \frac{1}{2}\mathbf{x}^T\left[\begin{array}{cc}4 & 1 \\ 1 & 2\end{array}\right]\mathbf{x} + [1~1]\mathbf{x}\\ & \text{s.t.}& [1~1]\mathbf{x} = 1 \end{array} $

#include <visp3/core/vpLinProg.h>
int main()
{
vpMatrix H(2,2), A(1,2);
vpColVector c(2), b(1);
H[0][0] = 4;
H[0][1] = H[1][0] = 1;
H[1][1] = 2;
c[0] = c[1] = 1;
A[0][0] = A[0][1] = 1;
b[0] = 1;
vpQuadProg::solveQPe(Q, r, A, b, x);
std::cout << x.t() << std::endl; // prints (0.25 0.75)
}
Implementation of column vector and the associated operations.
Definition: vpColVector.h:167
vpRowVector t() const
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:152
static void fromCanonicalCost(const vpMatrix &H, const vpColVector &c, vpMatrix &Q, vpColVector &r, const double &tol=1e-6)
Definition: vpQuadProg.cpp:95
bool solveQPe(const vpMatrix &Q, const vpColVector &r, vpColVector &x, const double &tol=1e-6) const
Definition: vpQuadProg.cpp:244

Definition at line 95 of file vpQuadProg.cpp.

References vpException::dimensionError, vpMatrix::eigenValues(), vpException::functionNotImplementedError, vpArray2D< Type >::getCols(), vpArray2D< Type >::getRows(), vpMatrixException::matrixError, vpMatrix::pseudoInverse(), vpMatrix::t(), and vpMatrix::transpose().

◆ resetActiveSet()

void vpQuadProg::resetActiveSet ( )
inline

Resets the active set that was found by a previous call to solveQP() or solveQPi(), if any.

Examples
quadprog.cpp.

Definition at line 90 of file vpQuadProg.h.

◆ setEqualityConstraint()

bool vpQuadProg::setEqualityConstraint ( const vpMatrix A,
const vpColVector b,
const double &  tol = 1e-6 
)

Saves internally the column reduction of the passed equality constraint:

$\mathbf{A}\mathbf{x} = \mathbf{b} \Leftrightarrow \mathbf{x} = \mathbf{x}_1 + \mathbf{Z}\mathbf{z} $

Parameters
A: equality matrix (dimension m x n)
b: equality vector (dimension m)
tol: tolerance to test the ranks
Returns
True if $\mathbf{A}\mathbf{x} = \mathbf{b}$ has a solution.
See also
solveQPi(), solveQP()
Examples
quadprog_eq.cpp.

Definition at line 147 of file vpQuadProg.cpp.

References vpLinProg::colReduction(), vpArray2D< Type >::getRows(), x1, and Z.

◆ solveByProjection()

bool vpQuadProg::solveByProjection ( const vpMatrix Q,
const vpColVector r,
vpMatrix A,
vpColVector b,
vpColVector x,
const double &  tol = 1e-6 
)
staticprotected

Solves a Quadratic Program under equality constraints.

$\begin{array}{lll} \mathbf{x} = & \arg\min & ||\mathbf{Q}\mathbf{x} - \mathbf{r}||^2 \\ & \text{s.t.}& \mathbf{A}\mathbf{x} = \mathbf{b}\end{array} $

Parameters
Q: cost matrix (dimension c x n)
r: cost vector (dimension c)
A: equality matrix (dimension m x n)
b: equality vector (dimension m)
x: solution (dimension n)
tol: tolerance to test the ranks
Returns
True if the solution was found.

This function is for internal use, no dimension check is performed and A and b may be modified.

Definition at line 176 of file vpQuadProg.cpp.

References vpLinProg::colReduction(), vpArray2D< Type >::getCols(), vpArray2D< Type >::getRows(), and vpMatrix::solveBySVD().

Referenced by solveQPe(), and solveQPi().

◆ solveQP()

bool vpQuadProg::solveQP ( const vpMatrix Q,
const vpColVector r,
vpMatrix  A,
vpColVector  b,
const vpMatrix C,
const vpColVector d,
vpColVector x,
const double &  tol = 1e-6 
)

Solves a Quadratic Program under equality and inequality constraints.

If the equality constraints $(\mathbf{A}, \mathbf{b})$ are the same between two calls, it is more efficient to use setEqualityConstraint() and solveQPi().

$\begin{array}{lll} \mathbf{x} = & \arg\min & ||\mathbf{Q}\mathbf{x} - \mathbf{r}||^2 \\ & \text{s.t.}& \mathbf{A}\mathbf{x} = \mathbf{b} \\ & \text{s.t.}& \mathbf{C}\mathbf{x} \leq \mathbf{d} \end{array} $

Parameters
Q: cost matrix (dimension c x n)
r: cost vector (dimension c)
A: equality matrix (dimension m x n)
b: equality vector (dimension m)
C: inequality matrix (dimension p x n)
d: inequality vector (dimension p)
x: solution (dimension n)
tol: tolerance to test the ranks
Returns
True if the solution was found.

Here is an example:

$\begin{array}{lll} \mathbf{x} = & \arg\min & (x_1-1)^2 + x_2^2 \\ & \text{s.t.}& x_1 + x_2 = 1 \\ & \text{s.t.} & x_2 \geq 1\end{array}$

#include <visp3/core/vpLinProg.h>
int main()
{
vpMatrix Q(2,2), A(1,2), C(1,2);
vpColVector r(2), b(1), d(1);
Q[0][0] = Q[1][1] = 1; r[0] = 1;
A[0][0] = A[0][1] = 1; b[0] = 1;
C[0][1] = -1; d[0] = -1;
qp.solveQP(Q, r, A, b, C, d, x);
std::cout << x.t() << std::endl; // prints (0 1)
}
This class provides a solver for Quadratic Programs.
Definition: vpQuadProg.h:70
bool solveQP(const vpMatrix &Q, const vpColVector &r, vpMatrix A, vpColVector b, const vpMatrix &C, const vpColVector &d, vpColVector &x, const double &tol=1e-6)
Definition: vpQuadProg.cpp:373
See also
resetActiveSet()
Examples
quadprog.cpp, and quadprog_eq.cpp.

Definition at line 373 of file vpQuadProg.cpp.

References vpLinProg::allLesser(), checkDimensions(), vpLinProg::colReduction(), vpArray2D< Type >::getCols(), vpArray2D< Type >::getRows(), and solveQPi().

◆ solveQPe() [1/2]

bool vpQuadProg::solveQPe ( const vpMatrix Q,
const vpColVector r,
vpColVector x,
const double &  tol = 1e-6 
) const

Solves a Quadratic Program under previously stored equality constraints (setEqualityConstraint())

$\begin{array}{lll} \mathbf{x} = & \arg\min & ||\mathbf{Q}\mathbf{x} - \mathbf{r}||^2 \\ & \text{s.t.}& \mathbf{A}\mathbf{x} = \mathbf{b}\end{array} $

Parameters
Q: cost matrix (dimension c x n)
r: cost vector (dimension c)
x: solution (dimension n)
tol: Tolerance.
Returns
True if the solution was found.

Here is an example where the two following QPs are solved:

$\begin{array}{lll} \mathbf{x} = & \arg\min & x_1^2 + x_2^2 \\ & \text{s.t.}& x_1 + x_2 = 1\end{array}$

$\begin{array}{lll} \mathbf{x} = & \arg\min & (x_1-1)^2 + x_2^2 \\ & \text{s.t.}& x_1 + x_2 = 1\end{array}$

#include <visp3/core/vpLinProg.h>
int main()
{
vpMatrix Q(2,2), A(1,2);
vpColVector r(2), b(1);
Q[0][0] = Q[1][1] = 1;
A[0][0] = A[0][1] = b[0] = 1;
// solve x_1^2 + x_2^2
qp.solveQPe(Q, r, x);
std::cout << x.t() << std::endl; // prints (0.5 0.5)
// solve (x_1-1)^2 + x_2^2
r[0] = 1;
qp.solveQPe(Q, r, x);
std::cout << x.t() << std::endl; // prints (1 0)
}
bool setEqualityConstraint(const vpMatrix &A, const vpColVector &b, const double &tol=1e-6)
Definition: vpQuadProg.cpp:147
See also
setEqualityConstraint(), solveQPe()
Examples
quadprog_eq.cpp.

Definition at line 244 of file vpQuadProg.cpp.

References vpException::dimensionError, vpArray2D< Type >::getCols(), vpArray2D< Type >::getRows(), vpMatrix::solveBySVD(), x1, and Z.

◆ solveQPe() [2/2]

bool vpQuadProg::solveQPe ( const vpMatrix Q,
const vpColVector r,
vpMatrix  A,
vpColVector  b,
vpColVector x,
const double &  tol = 1e-6 
)
static

Solves a Quadratic Program under equality constraints.

$\begin{array}{lll} \mathbf{x} = & \arg\min & ||\mathbf{Q}\mathbf{x} - \mathbf{r}||^2 \\ & \text{s.t.}& \mathbf{A}\mathbf{x} = \mathbf{b}\end{array}$

If the equality constraints $(\mathbf{A}, \mathbf{b})$ are the same between two calls, it is more efficient to use setEqualityConstraint() and solveQPe().

Parameters
Q: cost matrix (dimension c x n)
r: cost vector (dimension c)
A: equality matrix (dimension m x n)
b: equality vector (dimension m)
x: solution (dimension n)
tol: tolerance to test the ranks
Returns
True if the solution was found.

Here is an example:

$\begin{array}{lll} \mathbf{x} = & \arg\min & (x_1-1)^2 + x_2^2 \\ & \text{s.t.}& x_1 + x_2 = 1\end{array}$

#include <visp3/core/vpLinProg.h>
int main()
{
vpMatrix Q(2,2), A(1,2);
vpColVector r(2), b(1);
Q[0][0] = Q[1][1] = 1;
r[0] = 1;
A[0][0] = A[0][1] = b[0] = 1;
vpQuadProg::solveQPe(Q, r, A, b, x);
std::cout << x.t() << std::endl; // prints (1 0)
}
See also
setEqualityConstraint(), solveQP(), solveQPi()

Definition at line 309 of file vpQuadProg.cpp.

References checkDimensions(), and solveByProjection().

◆ solveQPi()

bool vpQuadProg::solveQPi ( const vpMatrix Q,
const vpColVector r,
const vpMatrix C,
const vpColVector d,
vpColVector x,
bool  use_equality = false,
const double &  tol = 1e-6 
)

Solves a Quadratic Program under inequality constraints

$\begin{array}{lll} \mathbf{x} = & \arg\min & ||\mathbf{Q}\mathbf{x} - \mathbf{r}||^2 \\ & \text{s.t.}& \mathbf{C}\mathbf{x} \leq \mathbf{d} \end{array} $

Parameters
Q: cost matrix (dimension c x n)
r: cost vector (dimension c)
C: inequality matrix (dimension p x n)
d: inequality vector (dimension p)
x: solution (dimension n)
use_equality: if a previously saved equality constraint (setEqualityConstraint()) should be considered
tol: tolerance to test the ranks
Returns
True if the solution was found.

Here is an example:

$\begin{array}{lll} \mathbf{x} = & \arg\min & (x_1-1)^2 + x_2^2 \\ & \text{s.t.}& x_1 + x_2 \leq 1 \\ & \text{s.t.} & x_1, x_2 \geq 0\end{array}$

#include <visp3/core/vpLinProg.h>
int main()
{
vpMatrix Q(2,2), C(3,2);
vpColVector r(2), d(1);
Q[0][0] = Q[1][1] = 1; r[0] = 1;
C[0][0] = C[0][1] = 1; d[0] = 1;
C[1][0] = C[2][1] = -1;
qp.solveQPi(Q, r, C, d, x);
std::cout << x.t() << std::endl; // prints (1 0)
}
bool solveQPi(const vpMatrix &Q, const vpColVector &r, const vpMatrix &C, const vpColVector &d, vpColVector &x, bool use_equality=false, const double &tol=1e-6)
Definition: vpQuadProg.cpp:443
Examples
quadprog_eq.cpp.

Definition at line 443 of file vpQuadProg.cpp.

References active, vpLinProg::allGreater(), vpLinProg::allLesser(), vpLinProg::allZero(), checkDimensions(), vpColVector::extract(), vpArray2D< Type >::getCols(), vpMatrix::getRow(), vpArray2D< Type >::getRows(), inactive, vpMatrix::pseudoInverse(), vpColVector::resize(), vpArray2D< Type >::resize(), vpLinProg::simplex(), solveByProjection(), vpMatrix::transpose(), x1, and Z.

Referenced by solveQP().

◆ solveSVDorQR()

vpColVector vpQuadProg::solveSVDorQR ( const vpMatrix A,
const vpColVector b 
)
staticprotected

Pick either SVD (over-constrained) or QR (square or under-constrained)

Assumes A is full rank, hence uses SVD iif A has more row than columns.

Parameters
A: matrix (dimension m x n)
b: vector (dimension m)
Returns
least-norm solution to $\arg\min||\mathbf{A}\mathbf{x} - \mathbf{b}||$

Definition at line 664 of file vpQuadProg.cpp.

References vpArray2D< Type >::getCols(), vpArray2D< Type >::getRows(), vpMatrix::solveByQR(), and vpMatrix::solveBySVD().

Member Data Documentation

◆ active

std::vector<unsigned int> vpQuadProg::active
protected

Active set from the last call to solveQP() or solveQPi(). Used for warm starting the next call.

Definition at line 102 of file vpQuadProg.h.

Referenced by solveQPi().

◆ inactive

std::vector<unsigned int> vpQuadProg::inactive
protected

Inactive set from the last call to solveQP() or solveQPi(). Used for warm starting the next call.

Definition at line 106 of file vpQuadProg.h.

Referenced by solveQPi().

◆ x1

vpColVector vpQuadProg::x1
protected

Stored particular solution from the last call to setEqualityConstraint().

Definition at line 110 of file vpQuadProg.h.

Referenced by setEqualityConstraint(), solveQPe(), and solveQPi().

◆ Z

vpMatrix vpQuadProg::Z
protected

Stored projection to the kernel from the last call to setEqualityConstraint().

Definition at line 114 of file vpQuadProg.h.

Referenced by setEqualityConstraint(), solveQPe(), and solveQPi().