- Generated by
1.8.17
|
Qontrol
|
The general optimisation problem formulation laying under Qontrol is the following:
\begin{equation}\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{equation}
Where \(\boldsymbol{x}\) is a control variable (joint velocity, torque, ...). This section details the meaning behind all these terms
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.
A task is defined as the following cost function:
\begin{equation} ||E\boldsymbol{x} - \boldsymbol{f}||^2_{S W}, \end{equation}
with \(S\) a selection matrix and \(W\) a weighted matrix such that each term are inferior to 1. The weighting matrix is used to defined 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 :
Let's consider a \(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{equation}\begin{array}{ccc}\boldsymbol{\dot{q}^{opt}} = & \displaystyle \arg\min_{\boldsymbol{\dot{q}}} & ||J\boldsymbol{\dot{q}} - \boldsymbol{v}^*||^2 \end{array}. \end{equation}
In this example \(E = J(\boldsymbol{q})\) and \(\boldsymbol{f} = \boldsymbol{v}^*\).
In practice a QP solver solves :
\begin{equation} \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{equation}
The previous task definition can be expressed as a Hessian and a gradient
\begin{equation} \label{eq:least-square_formulation} \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{equation}
with \(H = 2 E^T S W E^T \) and \(\boldsymbol{g} = -2 E^T S W \boldsymbol{f}\)
Multiple task 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 :
\begin{equation} \sum\limits_{i=1}^{n_{t}}\omega_i||E_i\boldsymbol{x} - \boldsymbol{f}_i||^2_{S_i W_i}.\end{equation}
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.
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 solution 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.
The robot is also subject to a set of constraints. These constraint can either be intrinsics such as joint limits (position, velocity, acceleration, ...) or Cartesian limits. They can also be extrinsic and depend on the environment (virtual walls, ...).
One constraint can be be written as an inequality. Consider a sec of \(n\) constraints they can be written this way:
\begin{equation} \boldsymbol{l_b} \le A\boldsymbol{x} \le \boldsymbol{u_b} \end{equation}
where \(\boldsymbol{l_b}\) and \(\boldsymbol{u_b}\) are respectively the constraint lower and upper bound, and \(A\) is the constraint matrix.
The previously described robot controlled at the joint velocity level is also subject to a set of constraint. 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:
\begin{equation} \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} \end{equation}
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}\)
Multiple constraints is a concatenation of all the inequalities :
\begin{equation}\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{equation}
The definition of tasks and constraints requires to know the robot model at a given instant. Qontrol offers generic structure gathering the required model data for the writting of its pr-implemented tasks and constraints. It also implements either the KDL or pinocchio model library fill this generic structure.
Over the years, several QP solvers have been released to the public. Qontrol propose a generic structure for such solver and implemtents different solver library such as qpmad or qpOASES. The user can easily choose at compile time which solver it wants to use. He can also set the solver options to optimize the resolution of the QP problem.
1.8.17