Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
vpQuadProg Class Reference

#include <visp3/core/vpQuadProg.h>

Public Member Functions

Instanciated solvers
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
bool setEqualityConstraint (const vpMatrix &A, const vpColVector &b, const double &tol=1e-6)
 
void resetActiveSet ()
 

Static Public Member Functions

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 Protected Member Functions

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)
 

Protected Attributes

std::vector< unsigned int > active
 
std::vector< unsigned int > inactive
 
vpColVector x1
 
vpMatrix Z
 

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 74 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 151 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)
}

Definition at line 95 of file vpQuadProg.cpp.

References vpMatrix::diag(), vpException::dimensionError, vpMatrix::eigenValues(), vpColVector::extract(), 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 101 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)
}
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)
}
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)
}
Examples:
quadprog_eq.cpp.

Definition at line 443 of file vpQuadProg.cpp.

References active, vpLinProg::allGreater(), vpLinProg::allLesser(), vpLinProg::allZero(), checkDimensions(), vpColVector::clear(), vpColVector::extract(), vpArray2D< Type >::getCols(), vpMatrix::getRow(), vpArray2D< Type >::getRows(), inactive, vpMatrix::pseudoInverse(), vpArray2D< Type >::resize(), vpColVector::resize(), vpLinProg::simplex(), vpArray2D< Type >::size(), 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 116 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 120 of file vpQuadProg.h.

Referenced by solveQPi().

◆ x1

vpColVector vpQuadProg::x1
protected

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

Definition at line 124 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 128 of file vpQuadProg.h.

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