Inside the robocop-qp-controller package, the framework provides a generic QP controller scheme that can be used and adapted for different robots.

QP control

As a reminder, QP stands for Quadratic Programming and refers to optimization problems with a quadratic cost function and linear equalities and inequalities:

$\begin{aligned} \min_{x} \quad & \frac{1}{2}x^T Q x + c^T x\\ \textrm{s.t.} \quad & Ax \leq B\\ & C x = D \\ \end{aligned}$

QP controllers are quite flexible in terms of what you can make them do while remaining very fast and suitable for real time applications.

You can find weighted QP (WQP) controllers that allow to define multiple cost terms (i.e tasks), each with their own weight. This provides a way to give some kind of a soft priority to a task over another one.

There are also hierarchical QP (HQP) controllers. An HQP controller defines a QP for each task and solves them in succession in order to give tasks hard priorities. Solving multiple QPs will of course be slower than solving a single one as in the WQP case. But since solving QPs can be done very fast, unless your optimization problem is very complex (high number of variables, constraints and/or priorities) then it shouldn’t be too much of a problem.

This QP controller

The controller architecture presented here allows task to have both a priority and a weight at the same time, so we refer to it as a mixed QP, in opposition to the typical WQP and HQP controllers.

All tasks on the same priority level are weighted to give soft priorities between them. But all tasks with a lower priority, no matter their weight, cannot interfere with higher priority tasks.

Currently, two controllers are based on that:

robocop::qp::KinematicTreeController: for kinematic and/or dynamic control of kinematic trees (e.g serial manipulators, humanoids, etc)
robocop::qp::AUVController: for autonomous underwater vehicles with any number of fixed or moving thrusters

Both of these controllers rely on the robocop::qp::QPControllerBase class to provide them all the core functionalities. They themselves only deal with what is specific for their use cases, mainly:

Handle configuration options
Define the optimization variables
Handle specific constraints (e.g dynamic constraint between force and velocity variables)

So we’ll first start with an explanation of the math (just a bit) and their implementation in QPControllerBase and then see how it is used to write new controllers.

What it does

As said previously, it is a mixed QP controller so each task have a priority and a weight.

When we have tasks with different priorities, we need to build and solve multiple QPs.

The notion of priority is enforced by adding constraints for all levels except the top one.

Let’s use a simple example to illustrate this. We consider a kinematic controller for a robot manipulator and so have the joint velocities $\dot{q}$ as an optimization variable.

We will also consider only two tasks with different priorities and no constraints, except for the one we will add to enforce the hierarchy.

So our two QP will look like:

$\begin{aligned} \min_{\dot{q}} \quad & \frac{1}{2}\dot{q}^T Q \dot{q} + c^T \dot{q}\\ \end{aligned}$

Or, in the equivalent least squares form:

$\begin{aligned} \min_{\dot{q}} \quad & \frac{1}{2}\Vert R \dot{q} - d \Vert^2_2\\ \end{aligned}$

Our two tasks are body velocity tasks, so we have a Jacobian and a reference velocity for each body, which gives us these two problems to solve:

Now we want the second task (b) to have lower priority than the first one (a). How do we do this? Well, we need to make sure that any solution ( $\dot{q}_b$ ) found by the second problem will generate the same body velocity found for a by the first problem. This is done by rewriting the problem with the following constraint added:

And taking $\dot{q}_b$ as the solution to the whole problem.

If we were to add another, lower, priority level, we would have to repeat that constraint and add a new one:

$\begin{aligned} \dot{q}_a = \min_{\dot{q}} \quad & \frac{1}{2}\Vert J_a \dot{q} - \dot{x}_a^{ref} \Vert^2_2\\ \\ \dot{q}_b = \min_{\dot{q}} \quad & \frac{1}{2}\Vert J_b \dot{q} - \dot{x}_b^{ref} \Vert^2_2\\ \textrm{s.t.} \quad & J_a \dot{q} = J_a \dot{q}_a \\ \\ \dot{q}_c = \min_{\dot{q}} \quad & \frac{1}{2}\Vert J_c \dot{q} - \dot{x}_c^{ref} \Vert^2_2\\ \textrm{s.t.} \quad & J_a \dot{q} = J_a \dot{q}_a \\ \textrm{s.t.} \quad & J_b \dot{q} = J_b \dot{q}_b \\ \end{aligned}$

And taking $\dot{q}_c$ as the solution to the whole problem.

If we were to mix both weighted and hierarchical tasks we could have something like:

$\begin{aligned} \dot{q}_a = \min_{\dot{q}} \quad & \frac{1}{2}w_a\Vert J_a \dot{q} - \dot{x}_a^{ref} \Vert^2_2 + \frac{1}{2} w_b \Vert J_b \dot{q} - \dot{x}_b^{ref} \Vert^2_2\\ \\ \dot{q}_c = \min_{\dot{q}} \quad & \frac{1}{2}\Vert J_c \dot{q} - \dot{x}_c^{ref} \Vert^2_2\\ \textrm{s.t.} \quad & J_a \dot{q} = J_a \dot{q}_a \\ \textrm{s.t.} \quad & J_b \dot{q} = J_b \dot{q}_a \\ \end{aligned}$

You can probably see how to generalize the concept from this.

The implementation

All the QP construction and solving is handled by the coco library so that we don’t have to redo all of that here.

It is best to read coco documentation first in order to make sense of what will come next.

The class providing all the core functionalities is robocop::qp::QPControllerBase.

Its API is quite simple, here is the main part:

struct PriorityLevel {
    coco::Problem& problem();
    coco::Solver& solver();
    coco::Variable var(std::string_view name);

    //...
};

PriorityLevel priority_level(const Priority& priority);

bool define_variable(std::string_view name, ssize size);

void predeclare_variable(std::string_view name, ssize size, std::function<void()> on_creation = {});

bool is_variable_defined(std::string_view name) const;

const Eigen::VectorXd& last_variable_value(std::string_view name) const;

template <typename... Args>
coco::Value par(Args&&... args);

template <typename... Args>
coco::Value dyn_par(Args&&... args);

template <typename... Args>
coco::Value fn_par(Args&&... args);

For each priority level there is a coco::Problem, a coco::Solver and a function to access its variables. Priority levels are created on the fly, when you request one that hasn’t been asked for yet. Their priority is represented by a positive integer and are ordered in increasing values, so that the level 0 always have the highest priority. It is ok to have gaps in priorities, e.g 0,1,10,50 are valid priorities for a controller.

Then there are functions to manage the optimization variables and functions to create optimization parameters.

Note also that each controller has its own coco::Workspace as it allows for multiple controllers to run in parallel without conflicts (at least coming from coco).

Defining optimization variables

Optimization variables can be defined up front (define_variable()) or predeclared (predeclare_variable()) and so be created only when users ask for them.

Predeclared variables have an associated callaback function that is called the first time they are requested:

auto& level_0 = controller.priority_level(Priority{0});

// Add a new variable called "joint_acceleration" with a size of 7 to all existing and future priority levels
controller.define_variable("joint_acceleration", 7);

// Predeclare a variable called "joint_force" with a size of 7 and automatically add a dynamics constraint when it gets created
controller.predeclare_variable("joint_force", 7,
    [&]{
        level_0.problem().add_constraint(
            level_0.var("joint_force") ==
            controller.dyn_par(inertia_matrix) * level_0.var("joint_acceleration")
        );
    }
);

Defining cost terms and constraints

Cost terms and constraints are added by accessing a priority level and then using the typical coco API. Following the previous example:

// Ok the variable was already defined, nothing else is done
level_0.problem().minimize(level_0.var("joint_acceleration").squared_norm());

// The above callback gets executed now
level_0.problem().minimize(level_0.var("joint_force").squared_norm());

level_0.problem().add_constraint(level_0.var("joint_force") > 0);

It is best to only add constraints at the highest priority level, even though there is nothing preventing you from doing otherwise. This is because all constraints defined at the highest priority level will be copied to the other priority levels. If you add a constraint at an intermediate priority level it will only exist on that level, meaning that it won’t be enforced by lower priority levels. Only do this if you really know what you are doing.

Solving a hierarchy of tasks

With coco it is possible to minimize:

linear terms: $c^T x$
quadratic terms: $\frac{1}{2}x^T Q x + c^T x$
least squares terms: $\frac{1}{2}\Vert A x - b \Vert^2_2$

As long as all tasks belong to the same priority level, users are free to use any kind of minimization term.

However, when we end up with a hierarchy of tasks, we need to retrieve (except for the lowest priority level) each task Jacobian and target in order to add the proper constraints.

But what we will have is, for each priority level, a coco::Problem with all the terms in it with no notion of task Jacobian and reference.

The only way we can retrieve a Jacobian and a target for each term to minimize in a generic way is if they are least squares terms.

Hopefully, people tend to write their tasks as least squares terms as it is usually the simpler and more intuitive way to do. With coco, with boils down to using only expression.squared_norm() and not trying to use quadratic or linear terms directly.

The controller must guard itself again this case, so a check is implemented in the QPControllerBase::pImpl::validate_task(), and that function is called very early in QPControllerBase::pImpl::compute() (just after sorting priority levels).

The constraints needed to enforce the task hierarchy are added in the QPControllerBase::pImpl::add_constraints_for_strict_hierarchy_to() function. This function is simply implementing in a straightforward way the math we saw earlier. The only additional operation is storing the identifier of the added constraint so that we can remove them on the next controller run (since everything can change between two runs, it’s simpler to just rebuild all the internal constraints from scratch each time).

Regularization term

As we saw previously, for each priority level we end with a QP problem like this to solve:

$\begin{aligned} \min_{x} \quad & \frac{1}{2}x^T Q x + c^T x\\ \textrm{s.t.} \quad & Ax \leq B\\ & C x = D \\ \end{aligned}$

It is possible to have zeros on the diagonal of $Q$ if there are variables with no cost term defined for them. Not all solvers handle with case well. To circumvent the issue, we try to regularize the optimization problem by adding a small value, the solver tolerance, on each diagonal element of $Q$ lower than that tolerance.

That way we make sure that all variables have a cost defined for them.

We do this directly on $Q$ (level.solver_data.quadratic_cost) after coco built the problem and just before we ask coco to solve it. That way we don’t have to add any term to the problems, which would create problems if we were to do it this way (they would be treated as “tasks” and hierarchy constraints would be added for them) and would be just less efficient.

Overall algorithm

The overall algorithm for the controller is:

sort the priority levels
validate tasks for strict hierarchy
call pre_solve_callback() (overridable by implementations)
for each priority level
1. remove any internal constraint (possibly added in the last execution)
2. add internal constraints for strict hierarchy with the higher levels
3. build the problem
4. add regularization term
5. copy constraints from the highest priority level
6. solve the problem
7. did we find a solution?
  - yes -> mark the current level as the last one solved
  - no -> exit the loop
at least one level has been solved?
- yes ->
  1. save the last solved level optimization variables values
  2. call post_solve_callback() (overridable by implementations)
  3. return ControllerResult::SolutionFound or ControllerResult::PartialSolutionFound depending if we managed to solve all priority levels or not
- no -> return ControllerResult::NoSolution

Support classes

Aside from the qp::QPControllerBase class, a few classes are provided to help writing specialized QP controllers.

These are meant to implement some concepts (e.g weight, priority, etc) and factor some code that will be shared between all specializations.

qp::JointGroupVariables

Since tasks and constraint can target only a subset of the controlled joints, we need to apply a mapping matrix to go from all controlled joints to the task/constraint joints. Joint group tasks also have joint selection matrices that need to be applied as well.

So calling qp::JointGroupVariables::joint_group_variable(var_name) will return a coco::LinearTerm and not a coco::Variable so that these operations are always applied automatically and thus avoid users to forget them when implementing tasks and constraints.

There are two constructors:

One for body tasks/constraints

JointGroupVariables(qp::QPControllerBase* controller, Priority* priority,
                    const JointGroupBase& joint_group,
                    const JointGroupBase& all_joints);

And one for joint tasks/constraints:

JointGroupVariables(qp::QPControllerBase* controller, Priority* priority,
                    const JointGroupBase& joint_group,
                    const JointGroupBase& all_joints,
                    SelectionMatrix<Eigen::Dynamic>* selection_matrix);

You can see that they both take a pointer to their associated priority so that they can later provide the variables for the proper priority level.

Body tasks also have a joint group to specify since they can have a root body specified in order to target only the joints between the task body and the root body.

This class is meant to be inherited by task and constraint base classes so that they can properly access the optimization variables.

qp::ProblemElement

qp::ProblemElement is here to represent all elements (cost terms and constraints) added by a single entity (i.e task, constraint or configuration).

Grouping them like this allows to manage them as one thing which simplifies activation, deactivation and removal of any controller element.

qp::Priority

qp::Priority represents a task priority.

Tasks can have subtasks but we don’t want these subtasks to:

take a higher priority than their parent tasks
have to deal with the fact that there are subtasks or top level tasks

To do so, qp::Priority defines two priority levels: a minimum priority and a local priority. The real priority is the sum of these two.

The minimum priority is set to the parent task real priority.

As an example, if we consider a task A of local priority 2 and two subtasks B and C of local priority 0 and 1 the real priorities will be:

A: 2
B: 2
C: 3

If the priority of task A is changed to 0, the minimum priority of B and C will be updated, but the local ones will remain the same, leading to:

A: 0
B: 0
C: 1

qp::TaskWeight

qp::TaskWeight implements a similar behavior to qp::Priority.

It stores the weight of the parent task (set to 1 if top level) and the local weight, to provide the real weight by multiplying the two.

qp::Task

qp::Task is meant to be inherited by tasks base classes, along with qp::JointGroupVariables, to provide the API needed to implement QP tasks.

Its public API allows to set the priority and weight of the task.

Its protected API allows to call coco like functions:

par(), dyn_par() and fn_par() to create parameters (inside the controller’s workspace)
minimize(), maximize() to add cost terms
remove() to remove cost terms

qp::Constraint

qp::Task is meant to be inherited by tasks base classes, along with qp::JointGroupVariables, to provide the API needed to implement QP constraints.

It has no public API (no weight or priority here).

Its protected API allows to call coco like functions:

par(), dyn_par() and fn_par() to create parameters (inside the controller’s workspace)
add_constraint() to add constraints
remove_constraint() to remove cost constraints

KinematicTreeController

The KinematicTreeController specializes QPControllerBase for robots with a tree-like structure.

This controller pre-declare two optimization variables:

joint_velocity
joint_force

and disallow creating additional variables. This limitation is somewhat artificial and could be lifted but its implications must be properly evaluated first.

The controller provides these configuration options:

velocity_output (bool): whether the controller should update the controlled joints velocity command
force_output (bool): whether the controller should update the controlled joints force command
include_bias_force_in_command (bool): when force_output is enabled, tell if the bias forces (gravity + coriolis + centrifugal) should be included in the force command or not. The value for this option depends if the robot performs its own bias forces compensation or not. For instance, a Franka Panda robot always performs its own compensation and so this option must be set to false. Refer to your robot operating modes to know how to set this option.

When the have to deal with both joint velocity and joint force at the same time, we add an equality constraint for consistency with the robot’s dynamic model:

$\tau = M \frac{\dot{q} - \dot{q}_{state}}{T}$

where $\tau$ is the joint_force variable, $\dot{q}$ the joint_velocity variable, $M$ the inertia matrix of the controlled joints, $\dot{q}_{state}$ the current joint velocity and $T$ the controller time step.

When the joint force variable or output is activated, we also add two inequality constraints to satisfy the robot’s joint force limits while considering the bias forces that will need to be added later (by the controller or the robot itself).

$\begin{align*} \tau &\leq \tau_{max} - \tau_{bias} \\ \tau &\geq -\tau_{max} - \tau_{bias} \end{align*}$

Lastly, the controller overrides the two callbacks from the base class:

pre_solve_callback(): update the data refered to by coco dynamic parameters inside the above constraints
post_solve_callback(): extract and/or compute the velocity and force outputs and update the joint command data. When an output is activated but the associated variable is not defined, we compute its value using the other one and the dynamic model.

Body tasks

Body tasks rely on body Jacobians to express their objectives based on joint level variables.

The KinematicTreeBodyTask base class provides three functions for that:

body_jacobian_in_root(): Jacobian of the controlled body with the root body as reference frame
body_jacobian_in_ref(): Jacobian of the controlled body with the reference body as reference frame
body_jacobian_in_ref_with_selection(): Jacobian of the controlled body with the reference body as reference frame and with the task selection matrix applied.

In most cases body_jacobian_in_ref_with_selection() is what needs to be used directly in tasks. For instance in BodyVelocityTask:

auto jacobian = dyn_par(body_jacobian_in_ref_with_selection());
auto velocity_target = dyn_par(target());

minimize(
    (jacobian * joint_group_variable("joint_velocity") - velocity_target)
        .squared_norm());

The other Jacobian variants are intermediary computational steps and are provided since they might be needed in more complex task formulations.

Body constraints

As with body tasks, a few functions are provided to help implementing body constraints:

ref_to_world_transformation(): a phyq::Transformation to transform quantities from the reference frame to the world frame
from_ref_to_world_frame(quantity): return a coco::Value that represents the transformation of a quantity from the reference frame to the world frame
world_to_body_jacobian(): Jacobian of the constrained body with the world as its root

For example, from BodyVelocityConstraint:

auto max_vel = from_ref_to_world_frame(parameters().max_velocity);
auto generated_vel = dyn_par(world_to_body_jacobian()) *
                      joint_group_variable("joint_velocity");

add_constraint(generated_vel <= max_vel);
add_constraint(generated_vel >= -max_vel);

Other tasks and constraints

Joint and generic tasks and constraints simply inherit from the utility classes explained earlier without providing any new functionnality.

AUVController

At the time of writing, this controller is very simple as it only defines one joint_force variable which is the force generated by the thrusters.

Body tasks and constraints

In order to map body forces to joint level (thrusters) forces we need to use an actuation matrix.

Both AUVBodyTask and AUVBodyConstraint provide the following functions (through detail::AUVControllerBodyElement):

actuation_matrix_in_body(): actuation matrix in the body own frame
actuation_matrix_in_ref(): actuation matrix in the reference frame

In addition to that, AUVBodyTask provides a actuation_matrix_in_ref_with_selection() function that can be used to retrieve the body actuation matrix with the transformation to the reference frame and the selection matrix applied. Which can be used like in BodyForcetask:

auto generated_force = dyn_par(actuation_matrix_in_ref_with_selection()) *
                        joint_group_variable("joint_force");
auto force_target = dyn_par(target());

minimize((force_target - generated_force).squared_norm());