Quadratic Programming solver using active set and null-space projections. More...

#include <EigenOpt/quadratic_programming.hpp>

Public Member Functions
	EigenOptTypedefs (Scalar)

	Solver (int xdim, int rdim, const Scalar &tolerance)
	Give dimensions for x, Q and r explicitly.

template<class D1 , class D2 >
	Solver (const Eigen::EigenBase< D1 > &Q, const Eigen::EigenBase< D2 > &r, const Scalar &tolerance)
	Deduce dimensions from the input matrices.

template<class D1 , class D2 >
void	updateObjective (const Eigen::EigenBase< D1 > &Q, const Eigen::EigenBase< D2 > &r)
	Updates the objective matrix.

void	resetActiveSet ()
	Clear the current active set, preventing warm starts.

void	clearConstraints ()
	Removes constraints and clear the active set.

template<class D1 , class D2 >
bool	setConstraints (const Eigen::EigenBase< D1 > &C, const Eigen::EigenBase< D2 > &d)
	Add inequality constraints to the problem.

template<class D1 , class D2 , class D3 , class D4 >
bool	setConstraints (const Eigen::MatrixBase< D1 > &A, const Eigen::MatrixBase< D2 > &b, const Eigen::EigenBase< D3 > &C, const Eigen::EigenBase< D4 > &d)
	Add equality and inequality constraints to the problem.

template<class D1 , class D2 >
bool	updateInequalities (const Eigen::EigenBase< D1 > &C, const Eigen::EigenBase< D2 > &d)
	Update inequality constraints to the problem.

template<class D >
bool	solve (Eigen::MatrixBase< D > &x)
	Solve the optimization problem.

Private Member Functions
template<class D >
bool	solveY (Eigen::MatrixBase< D > &y)
	Solve the problem in the y variable.

template<class D >
bool	guess (Eigen::MatrixBase< D > &y)
	Find an initial solution to start the active-set algorithm.

bool	initActiveSet ()
	Initialize the active-set from the current solution.

Private Attributes
const Scalar	tol
	Small tolerance used in calculations.

const int	nx
	Number of decision variables.

const int	nr
	Number of rows in the objective.

int	ny
	Number of variables after removing the equality constraints.

int	mi
	Number of inequality constraints.

int	me
	number of equality constraints.

MatrixXs	Q
	Matrix of coefficients for the objective function.

VectorXs	r
	Vector of coefficients for the objective function.

MatrixXs	Z
	Matrix that projects into the kernel of the equality constraints matrix.

VectorXs	xeq
	A particular solution to the equality constraints.

MatrixXs	Qy
	Modified matrix of coefficients for the objective function.

VectorXs	ry
	Modified vector of coefficients for the objective function.

MatrixXs	Cy
	Modified inequality constraints matrix.

VectorXs	dy
	Modified inequality constraints vector.

VectorXs	yu
	Unconstrained minimum of the objective.

VectorXs	yk
	Current guess of the decision variables.

MatrixXs	Ca
	Subset of Cy, corresponding to active constraints.

VectorXs	da
	Subset of dy, corresponding to active constraints.

std::vector< int >	active
	List of constraints in the active set.

std::vector< int >	inactive
	List of constraints not in the active set.

Detailed Description

template<class Scalar>
class EigenOpt::quadratic_programming::Solver< Scalar >

Quadratic Programming solver using active set and null-space projections.

This class implements a quadratic programming solver to minimize a given cost function as in:

\begin{equation} \min_{\bm{x}} \left\| \bm{Q} \bm{x} - \bm{r} \right\|^2 \quad\text{subject to:}\quad \begin{cases} \bm{A} \bm{x} = \bm{b} \\ \bm{C} \bm{x} \leq \bm{d} \end{cases} \end{equation}

The reason for using such a specific form for the objective is that it is very well suited for robotics applications, where mathematical models often are written as \( \dot{\bm{s}} = \bm{J} \dot{\bm{q}} \), where \( \bm{s} \) is a vector of features while \( \bm{q} \) are the generalized coordinates of the system - whose derivatives often serve as inputs for the system. Advanced control laws can thus be formulated using quadratic programming.

To determine a solution for the problem, the first step is to remove the equalities via a null-space projection strategy. The system of equalities is solved to find a particular solution \(\bm{x}_{eq}\) such that \( \bm{A} \bm{x}_{eq} = \bm{b} \). Then, an orthonormal basis of the kernel of \( \bm{A} \) - denoted as \( \bm{Z} \) - is computed. By definition, its property is that \( \bm{A}\bm{Z} = \bm{0} \). The original decision vector is thus substituted in the original problem using the parameterization

\begin{equation} \bm{x} = \bm{x}_{eq} + \bm{Z} \bm{y} \end{equation}

where \( \bm{y} \) is a new, lower-dimensional, decision vector. Note that no matter its value, equality constraints will be satisfied. The problem now becomes:

\begin{equation} \min_{\bm{y}} \left\| \bm{Q}_y \bm{y} - \bm{r}_y \right\|^2 \quad\text{subject to:}\quad \bm{C}_y \bm{y} \leq \bm{d}_y \end{equation}

where:

\begin{align} \bm{Q}_y &\doteq \bm{Q}\bm{Z} \\ \bm{r}_y &\doteq \bm{r} - \bm{Q}\bm{x}_{eq} \\ \bm{C}_y &\doteq \bm{C}\bm{Z} \\ \bm{d}_y &\doteq \bm{d} - \bm{C}\bm{x}_{eq} \end{align}

The problem can be solved using an active-set strategy, whose main steps are:

Determine an initial feasible solution that satisfies all inequalities.
Parameterize the decision vector as \( \bm{y} = \bm{y}_k + \bm{p} \), where \( \bm{y}_k \) is the current solution and \( \bm{p} \) is a "step".
Given a set of "active constraints", compute \( \bm{p} \) so that it minimizes the objective while enforcing the active constraints.
Rather than jumping to the new iterate immediately, now consider the line parameterization \( \bm{y} = \bm{y}_k + \alpha \bm{p} \) where \( 0 \leq \alpha \leq 1 \). The value of this coefficient is evaluated as the largest possible value that does not cause any new constraint to be violated. If the final value is less than 1, it means that a new constraint has just activated andi t is thus added to the active set. If \( \alpha = 1 \), then the full step can be performed without adding new constraints to the active set.
At the new point, it is possible to compute the Lagrange multipliers associated to the active constraints. If any multiplier is negative, it means that the constraint does not need to be enforced, and it can be removed from the active set.
Given the new solution and active set, a new iteration is performed from step 2. The algorithm stops when \( \alpha = 1 \) and no constraints deactivate.

Definition at line 86 of file quadratic_programming.hpp.

Constructor & Destructor Documentation

◆ Solver() [1/2]

template<class Scalar >

EigenOpt::quadratic_programming::Solver< Scalar >::Solver	(	int	xdim,
		int	rdim,
		const Scalar &	tolerance
	)

Give dimensions for x, Q and r explicitly.

Definition at line 14 of file quadratic_programming.hxx.

◆ Solver() [2/2]

template<class Scalar >

template<class D1 , class D2 >

EigenOpt::quadratic_programming::Solver< Scalar >::Solver	(	const Eigen::EigenBase< D1 > &	Q,
		const Eigen::EigenBase< D2 > &	r,
		const Scalar &	tolerance
	)

Deduce dimensions from the input matrices.

Definition at line 37 of file quadratic_programming.hxx.

Member Function Documentation

◆ clearConstraints()

template<class Scalar >

void EigenOpt::quadratic_programming::Solver< Scalar >::clearConstraints ( )

Removes constraints and clear the active set.

Definition at line 112 of file quadratic_programming.hxx.

◆ EigenOptTypedefs()

template<class Scalar >

EigenOpt::quadratic_programming::Solver< Scalar >::EigenOptTypedefs ( Scalar )

◆ guess()

template<class Scalar >

template<class D >

bool EigenOpt::quadratic_programming::Solver< Scalar >::guess ( Eigen::MatrixBase< D > & y )

private

Find an initial solution to start the active-set algorithm.

The following options are considered, in order:

A solution that satisfies all constraints in the active set;
Whatever the last solution was (from a previous call to solveY());
The input value passed by the user, if it has adequate dimension;
A feasible point obtained using the Simplex method.
Parameters

y A user-supplied guess.

Returns
true if a feasible point was found, false if the problem has no solution.

Definition at line 297 of file quadratic_programming.hxx.

◆ initActiveSet()

template<class Scalar >

bool EigenOpt::quadratic_programming::Solver< Scalar >::initActiveSet ( )

private

Initialize the active-set from the current solution.

Once an initial solution has been determined, it is necessary to ensure that the active set contains the constraints "touched" by such solution.

Returns: false if for some reason the current solution violates any constraint. A constraint is considered violated if \(\bm{c}_i\cdot\bm{y} - d_i \) exceeds the tolerance.

Definition at line 351 of file quadratic_programming.hxx.

◆ resetActiveSet()

template<class Scalar >

void EigenOpt::quadratic_programming::Solver< Scalar >::resetActiveSet ( )

Clear the current active set, preventing warm starts.

Definition at line 98 of file quadratic_programming.hxx.

◆ setConstraints() [1/2]

template<class Scalar >

template<class D1 , class D2 >

bool EigenOpt::quadratic_programming::Solver< Scalar >::setConstraints	(	const Eigen::EigenBase< D1 > &	C,
		const Eigen::EigenBase< D2 > &	d
	)

Add inequality constraints to the problem.

This resets all previous constraints and resets the active set.

Warning: If a call to this method fails due to infeasible constraints, the problem is left unconstrained. Calling solve() will result in solving \( \bm{Q} \bm{x} = \bm{r} \) in the least-squares sense.

Parameters

C	Inequalities constraints matrix.
d	Inequalities constraints vector.

Returns: true if constraints are feasible, false otherwise.

Definition at line 128 of file quadratic_programming.hxx.

◆ setConstraints() [2/2]

template<class Scalar >

template<class D1 , class D2 , class D3 , class D4 >

bool EigenOpt::quadratic_programming::Solver< Scalar >::setConstraints	(	const Eigen::MatrixBase< D1 > &	A,
		const Eigen::MatrixBase< D2 > &	b,
		const Eigen::EigenBase< D3 > &	C,
		const Eigen::EigenBase< D4 > &	d
	)

Add equality and inequality constraints to the problem.

This resets all previous constraints and resets the active set.

Warning: If a call to this method fails due to infeasible constraints, the problem is left unconstrained. Calling solve() will result in solving \( \bm{Q} \bm{x} = \bm{r} \) in the least-squares sense.

Parameters

A	Equalities constraints matrix.
b	Equalities constraints vector.
C	Inequalities constraints matrix.
d	Inequalities constraints vector.

Returns: true if constraints are feasible, false otherwise.

Definition at line 139 of file quadratic_programming.hxx.

◆ solve()

template<class Scalar >

template<class D >

bool EigenOpt::quadratic_programming::Solver< Scalar >::solve ( Eigen::MatrixBase< D > & x )

Solve the optimization problem.

Parameters

[out] x Solution of the problem, if one was found.

Returns: true if the optimizaion was successful, false if the problem is not feasible.

Definition at line 273 of file quadratic_programming.hxx.

◆ solveY()

template<class Scalar >

template<class D >

bool EigenOpt::quadratic_programming::Solver< Scalar >::solveY ( Eigen::MatrixBase< D > & y )

private

Solve the problem in the y variable.

Parameters

[out] y Solution in the y variable, if one was found.

Returns: true if the optimizaion was successful, false if the problem is not feasible.

Definition at line 389 of file quadratic_programming.hxx.

◆ updateInequalities()

template<class Scalar >

template<class D1 , class D2 >

bool EigenOpt::quadratic_programming::Solver< Scalar >::updateInequalities	(	const Eigen::EigenBase< D1 > &	C,
		const Eigen::EigenBase< D2 > &	d
	)

Update inequality constraints to the problem.

Existing equality constraints will not be removed. If the constraint dimensions have not changed, the active set will not be reset and feasibility is not tested either. The reason for this method to exists is to help solving multiple similar (but not identical) problems sequentially, warm-starting each one with the information obtained in the previous problem.

Parameters

C	Inequalities constraints matrix.
d	Inequalities constraints vector.

Returns: true if the constraints did not change dimension, or if they did and the new ones are feasible.

Definition at line 197 of file quadratic_programming.hxx.

◆ updateObjective()

template<class Scalar >

template<class D1 , class D2 >

void EigenOpt::quadratic_programming::Solver< Scalar >::updateObjective	(	const Eigen::EigenBase< D1 > &	Q,
		const Eigen::EigenBase< D2 > &	r
	)

private

Matrix that projects into the kernel of the equality constraints matrix.

Definition at line 219 of file quadratic_programming.hpp.

The documentation for this class was generated from the following files:

include/EigenOpt/quadratic_programming.hpp
include/EigenOpt/quadratic_programming.hxx

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ Solver() [1/2]

◆ Solver() [2/2]

Member Function Documentation

◆ clearConstraints()

◆ EigenOptTypedefs()

◆ guess()

◆ initActiveSet()

◆ resetActiveSet()

◆ setConstraints() [1/2]

◆ setConstraints() [2/2]

◆ solve()

◆ solveY()

◆ updateInequalities()

◆ updateObjective()

Member Data Documentation

◆ active

◆ Ca

◆ Cy

◆ da

◆ dy

◆ inactive

◆ me

◆ mi

◆ nr

◆ nx

◆ ny

◆ Q

◆ Qy

◆ r

◆ ry

◆ tol

◆ xeq

◆ yk

◆ yu

◆ Z