The Optimization Problem

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

(1) $\underset{\optvar}{\argmin} &\quad \ft(\optvar) \\ \text{s.t.} &\quad G\optvar \leq \bs{h} \\ &\quad A\optvar = \bs{b} \tc$

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 = $n$ , i.e. $\optvar \in \R^{n}$ . Next we impose that $\ft(\optvar)$ be quadratic in $\optvar$ , leaving us with an unconstrained Quadratic Program, or QP:

(2) $\underset{\optvar}{\argmin} \quad f(\optvar) &= \frac{1}{2} \optvar^{\top}H\optvar + \bs{g}^{\top}\optvar + r \\ &= \optvar^{\top}(E^{\top}E)\optvar - 2(E^{\top}\fvec)^{\top}\optvar + \fvec^{\top}\fvec \\ &= \left\| E\optvar - \fvec \right\|^{2}_{2} \tc$

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

$\optvar$ the optimization vector
$H$ the hessian matrix ( $n \times n$ )
$\bs{g}$ the gradient vector ( $n \times 1$ )
$E$ the linear matrix of the affine function ( $n \times n$ )
$f$ the origin vector ( $n \times 1$ )

The last line of (2), $\left\| E\optvar - \fvec \right\|^{2}_{2}$ , is the least-squares formulation. We will continue using the least squares version, which admits an analytical minimum-norm solution, $\optvar^{*}$ , in the unconstrained case.

(3) $\optvar^{*} = \underset{\optvar}{\argmin} \left\| E\optvar - \fvec \right\|^{2}_{2} = E^{\dagger}\fvec \tc$

where $E^{\dagger}$ is the Moore-Penrose pseudoinverse of the $E$ 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) $\underset{\optvar}{\argmin} &\quad \left\| E\optvar - \fvec \right\|^{2}_{2} \\ \text{s.t.} &\quad A\optvar = \bs{b} \tc$

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

(5) $\underbrace{\bmat{E^{\top}E & A^{\top} \\ A & \0}}_{\text{KKT Matrix}} \bmat{\optvar \\ \bs{z}} &= \bmat{E^{\top}\fvec \\ \bs{b}} \\ \Leftrightarrow \bmat{\optvar \\ \bs{z}} &= \bmat{E^{\top}E & A^{\top} \\ A & \0}^{-1} \bmat{E^{\top}\fvec \\ \bs{b}} \tc$

where $\bs{z}$ 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) $\underset{\optvar}{\argmin} &\quad \left\| E\optvar - \fvec \right\|^{2}_{2} \\ \text{s.t.} &\quad A\optvar = \bs{b} \\ &\quad G\optvar \leq \bs{h} \tp$

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 $\optvar^{*}$ 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) $\left\| E\optvar - \fvec \right\|^{2}_{W} \Leftrightarrow \left\| \sqrt{W} \left( E\optvar - \fvec \right) \right\|^{2}$

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

In ORCA, $W$ is actually composed of two components, the norm weighting $W'$ and the selection matrix, $S$ ,

(8) $W = SW'$

$S$ 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 $S$ to 0 allows us to ignore orientation errors.

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

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

$\frac{1}{2} \optvar^{\top}H\optvar + \bs{g}^{\top}\optvar \\ \Leftrightarrow \optvar^{\top}(E^{\top}WE)\optvar - 2 (WE^{\top}\fvec)^{\top}\optvar$

Note

$r = \fvec^{\top}\fvec$ 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 ;
}

void WeightedEuclidianNormFunction::QuadraticCost::computeGradient(const Eigen::VectorXd& SelectionVector
                                                , 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,

$\bs{lb} \leq C\optvar \leq \bs{ub} \tp$

$C$ the constraint matrix ( $n \times n$ )
$\bs{lb}$ and $\bs{ub}$ the lower and upper bounds of $C\optvar$ ( $n \times 1$ )

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

$A\optvar = \bs{b} \Leftrightarrow \bs{b} \leq A\optvar \leq \bs{b}$

$G\optvar \leq \bs{h} \Leftrightarrow \bmat{G\optvar \\ -G\optvar} \leq \bmat{\bs{ub_{h}} \\ -\bs{lb_{h}}} \Leftrightarrow \bs{lb_{h}} \leq G\optvar \leq \bs{ub_{h}}$

ORCA QP

In ORCA the full QP is expressed as,

$\underset{\optvar}{\argmin} &\quad \frac{1}{2} \optvar^{\top}H\optvar + \bs{g}^{\top}\optvar \\ \text{s.t.} &\quad \bs{lb} \leq \optvar \leq \bs{ub} \\ &\quad \bs{lb} \leq C\optvar \leq \bs{ub}\tc$

Note

For convenience an explicit constraint on the optimization variable $\optvar$ is included in the problem because it is so common. This constraint is identical to the second line: $\bs{lb} \leq C\optvar \leq \bs{ub}$ when $C$ 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.

$\bmat{\bs{lb}_{1} \\ \bs{lb}_{2} \\ \vdots \\ \bs{lb}_{n_{C}} } \leq \bmat{C_{1} \\ C_{2} \\ \vdots \\ C_{n_{C}} } \optvar \leq \bmat{\bs{ub}_{1} \\ \bs{ub}_{2} \\ \vdots \\ \bs{ub}_{n_{C}} }$