Philosophy

Optimisation based control

Problem formulation

The general optimisation problem formulation laying under Qontrol is the following:

\[\begin{split}\begin{array}{ccc}\boldsymbol{x^{opt}} = & \displaystyle \arg\min_{\boldsymbol{x}} & \sum\limits_{i=1}^{n_{t}}\omega_i||E_i\boldsymbol{x} - \boldsymbol{f}_i||^2_{S_i W_i} \\& \textrm{s.t.} & \boldsymbol{l_b} \le A\boldsymbol{x} \le \boldsymbol{u_b}. \end{array}\end{split}\]

Where \(\boldsymbol{x}\) is a control variable (joint velocity, torque, …). This section details the meaning behind all these terms.

Tasks

Constrained convex optimization methods allow the simultaneous realisation of several tasks, defined as cost functions. In order to avoid interferences between different tasks, task prioritization strategies are used.

Single tasks

A task is defined as the following cost function:

\[||E\boldsymbol{x} - \boldsymbol{f}||^2_{S W},\]

with \(S\) a selection matrix and \(W\) a weighted matrix such that each term is inferior to 1. The weighting matrix is used to define the priority of each element in the norm. The selection matrix can be used to extract only a part of interest of the cost function. Let’s see an example:

Example

Let’s consider an \(n\) degrees of freedom robot realising a task in the operational space and controlled at the joint velocity level. Its desired operational twist, expressed at some point of interest (usually the end-effector), \(\boldsymbol{v}^*\), is linked to its joint velocity through the Jacobian \(J(\boldsymbol{q})\). The task can thus be expressed as:

\[\begin{array}{ccc}\boldsymbol{\dot{q}^{opt}} = & \displaystyle \arg\min_{\boldsymbol{\dot{q}}} & ||J\boldsymbol{\dot{q}} - \boldsymbol{v}^*||^2 \end{array}.\]

In this example \(E = J(\boldsymbol{q})\) and \(\boldsymbol{f} = \boldsymbol{v}^*\).

Note

The definition of the \(E\) and \(\boldsymbol{f}\) term changes depending on the control output and the desired task. Qontrol offers pre-implemented tasks to simplify the definition of the control problem. The user defines the control output of the QP problem and the task input (desired Cartesian velocity, acceleration, …). Qontrol handles the definition of the tasks matrices. It is also possible to create a generic task and fill yourself the \(E\) and \(\boldsymbol{f}\) terms.

From task to Hessian and gradient

In practice a QP solver solves:

\[\begin{split}\begin{array}{ccc} \boldsymbol{x}^{opt} = & \displaystyle \arg\min_{ \boldsymbol{x}} & \frac{1}{2} \boldsymbol{x}^T H \boldsymbol{x} + \boldsymbol{x}^T\boldsymbol{g} \\ & \textrm{s.t.} & \boldsymbol{l_b} \le A \boldsymbol{x}\le \boldsymbol{u_b} \end{array}.\end{split}\]

The previous task definition can be expressed as a Hessian and a gradient:

\[\begin{split}\begin{split} || E\boldsymbol{x} - \boldsymbol{f} ||^2_{SW} & = \left(E\boldsymbol{x} -\boldsymbol{f}\right)^T SW\left(E\boldsymbol{x} -\boldsymbol{f}\right) \\ & = \boldsymbol{x}^TE^TSWE\boldsymbol{x} - 2 \boldsymbol{x}^TE^TSW\boldsymbol{f}+\boldsymbol{f}^T\boldsymbol{f} \\ & = \frac{1}{2} \boldsymbol{x}^TH\boldsymbol{x} + \boldsymbol{g}^T \boldsymbol{x} + r \end{split}.\end{split}\]

with \(H = 2 E^T S W E^T\) and \(\boldsymbol{g} = -2 E^T S W \boldsymbol{f}\).

Note

In practice, when using Qontrol the user specifies \(E\) and \(f\). Qontrol handles in the background the conversion to the Hessian and the gradient.

Multiple tasks

Multiple tasks can be added to the optimization problem. If no proper care is taken, tasks can conflict with one another. Qontrol defines tasks priorities using a weighting method. In the QP formulation this consists in summing the weighted cost function:

\[\sum\limits_{i=1}^{n_{t}}\omega_i||E_i\boldsymbol{x} - \boldsymbol{f}_i||^2_{S_i W_i}.\]

Here \(\omega_i\) is a scalar value specifying the relative weight of a task relatively to the others. Usually \(\omega_i \in [0,1]\), and a weight of 1 relates to a task of maximal priority.

Note

In Qontrol this sum is called the Task set. Users can add tasks to the task set and specify all the previously described parameters. Qontrol handles the conversion of all these tasks to a final Hessian and gradient that will be sent to the QP solver.

Special case of regularisation tasks

When the robot is redundant relatively to its tasks, not all the robot degrees of freedom are constrained. These cases must be avoided in a QP formulation otherwise there exists an infinite set of solutions and the QP solver might not know which one to choose and crash. A solution to solve this issue is to add a regularisation task. This task uses all the degrees of freedom of the robot with a very small relative weight (usually \(\omega \approx 10^{-5}\)). The weighting term ensures that the regularization task doesn’t interfere with the main task.

Note

For a redundant robot, if the main task is in Cartesian space, the regularisation task can be used to realise a secondary task in the nullspace of the robot main task.

Constraints

The robot is also subject to a set of constraints. These constraints can either be intrinsic such as joint limits (position, velocity, acceleration, …) or Cartesian limits. They can also be extrinsic and depend on the environment (virtual walls, …).

Single constraint

One constraint can be written as an inequality. Consider a set of \(n\) constraints, they can be written this way:

\[\boldsymbol{l_b} \le A\boldsymbol{x} \le \boldsymbol{u_b}\]

where \(\boldsymbol{l_b}\) and \(\boldsymbol{u_b}\) are respectively the constraint lower and upper bound, and \(A\) is the constraint matrix.

Example

The previously described robot controlled at the joint velocity level is also subject to a set of constraints. For instance, it must not violate its joint position limits, i.e., \(\boldsymbol{q}^{min} \leq \boldsymbol{q_{k+1}}(\boldsymbol{\dot{q}_{k+1}}) \leq \boldsymbol{q}^{max}\).

Considering that \(\boldsymbol{q_{k+1}} = \boldsymbol{q_{k}} + \Delta t \boldsymbol{\dot{q}_{k+1}}\). The previous constraint can be reformulated as:

\[\frac{\boldsymbol{\dot{q}}^{min} - \boldsymbol{\dot{q}}}{\Delta t} \leq \boldsymbol{\dot{q}_{k+1}} \le \boldsymbol{ub} = \frac{\boldsymbol{\dot{q}}^{max} - \boldsymbol{\dot{q}}}{\Delta t}\]

So in this case, \(A = I_{n\times n}\), \(\boldsymbol{lb} = \frac{\boldsymbol{\dot{q}}^{min} - \boldsymbol{\dot{q}}}{\Delta t}\) and \(\boldsymbol{ub} = \frac{\boldsymbol{\dot{q}}^{max} - \boldsymbol{\dot{q}}}{\Delta t}\).

Note

The definition of the different terms changes depending on the control output and the type of constraint. Qontrol offers pre-implemented constraints to simplify the definition of the control problem. The user defines the control output of the QP problem and the type of constraint (joint limit, …). Qontrol handles the definition of the final constraint matrix. It is also possible to create a generic constraint and fill yourself the \(\boldsymbol{l_b}\), \(\boldsymbol{u_b}\) and \(A\) terms.

Multiple constraints

Multiple constraints is a concatenation of all the inequalities:

\[\begin{split}\begin{pmatrix}\boldsymbol{l_{b_0}} \\ \vdots \\ \boldsymbol{l_{b_n}} \end{pmatrix} \le \begin{pmatrix}\boldsymbol{A_0} \\ \vdots \\ \boldsymbol{A_n} \end{pmatrix} \boldsymbol{x} \le \begin{pmatrix}\boldsymbol{u_{b_0}} \\ \vdots \\ \boldsymbol{u_{b_n}} \end{pmatrix}\end{split}\]

Note

In Qontrol this concatenation is called the Constraint set. Users can add constraints to the constraints set and Qontrol handles the concatenation.

Robot model

The definition of tasks and constraints requires knowing the robot model at a given instant. Qontrol offers a generic structure gathering the required model data for the writing of its pre-implemented tasks and constraints. It also implements either the KDL or Pinocchio model library to fill this generic structure.

QP solver

Over the years, several QP solvers have been released to the public. Qontrol proposes a generic structure for such solvers and implements different solver libraries such as qpmad or qpOASES. The user can easily choose at compile time which solver they want to use. They can also set the solver options to optimize the resolution of the QP problem.