# The Optimization Problem

Returning to our generic representation of a whole-body controller presented in Overview,

(1)

we make some important assumptions about the structure of the problem. Firstly, we make the assumtion that our control problem is continous and has size = , i.e. . Next we impose that be quadratic in , leaving us with an unconstrained Quadratic Program, or QP:

(2)

In (2), the first line is the classical formulation of a QP:

• the optimization vector
• the hessian matrix ()
• the linear matrix of the affine function ()
• the origin vector ()

The last line of (2), , is the least-squares formulation. We will continue using the least squares version, which admits an analytical minimum-norm solution, , in the unconstrained case.

(3)

where is the Moore-Penrose pseudoinverse of the matrix. This solution will be found assuming the rank of the linear system is consistent.

Adding an affine equality constraint produces a constrained least squares problem,

(4)

which can be solved analytically, assuming a solution exists, using the Karush Kuhn Tucker (KKT) equations,

(5)

where is the solution to the dual problem and contains the Lagrange multipliers.

Adding an affine inequality constraint to the problem produces the following QP,

(6)

Equation (6) can no longer be solved analytically and one must use numerical methods such as interior point, or active set methods.

Note

For more details on convex optimization, check out Boyd and Vandenberghe’s book: http://web.stanford.edu/~boyd/cvxbook/

Resolution of (6) with a numerical solver, such as qpOASES, will provide a globally optimal solution for provided that the constraint equations are consistent, i.e. the set of possible solutions is not empty.

## Objective Function Implementation

Within ORCA the QP objective function is formulated as a weighted Euclidean norm of an affine function,

(7)

In (7), is the weight of the euclidean norm () and must be a positive symmetric definite matrix.

In ORCA, is actually composed of two components, the norm weighting and the selection matrix, ,

(8)

is a matrix with either 1’s or 0’s on the diagonal which allows us to ignore all or parts of the affine function we are computing. Concretely this means we can ignore components of the task error. More information on tasks is provided in the Control Objectives (Tasks) section.

For example…

For a Cartesian position task, setting the low 3 entries on the diagonal of to 0 allows us to ignore orientation errors.

For practicality’s sake we set from a vector with the function setSelectionVector(const Eigen::VectorXd& s), which creates a diagonal matrix from s.

Given from (8), the hessian and gradient are calculated as,

Note

is dropped from the objective function because it does not change the optimal solution of the QP.

In the code, these calculations can be found in WeightedEuclidianNormFunction:

void WeightedEuclidianNormFunction::QuadraticCost::computeHessian(const Eigen::VectorXd& SelectionVector
, const Eigen::MatrixXd& Weight
, const Eigen::MatrixXd& A)
{
Hessian_.noalias() = SelectionVector.asDiagonal() * Weight * A.transpose() * A ;
}

, const Eigen::MatrixXd& Weight
, const Eigen::MatrixXd& A
, const Eigen::VectorXd& b)
{
Gradient_.noalias() =  2.0 * SelectionVector.asDiagonal() * Weight * A.transpose() * b ;
}


## Constraint Implementation

Constraints are written as double bounded linear functions,

• the constraint matrix ()
• and the lower and upper bounds of ()

Thus to convert our standard affine constraint forms we have the following relationships:

## ORCA QP

In ORCA the full QP is expressed as,

Note

For convenience an explicit constraint on the optimization variable is included in the problem because it is so common. This constraint is identical to the second line: when is the identity matrix.

In the next sections we show how to formulate the different task and constraint types one might need to control a robot. In section Multi-Objective Optimization, we show how to combine multiple objective functions (tasks) in one controller allowing us to exploit the redundancy of the system.

Note

Multiple constraints can be combined through vertical concatenation of their matrices and vectors. I.e.