The goal

In this tutorial we will see how to write a simple controller for robocop.

We will consider a kinematic controller for serial robots.

When you’re not writing code for a framework such as robocop you tend to rely on a bunch of assumptions based on the robot your are using. These could be:

number of degrees of freedom
type of joints (e.g all revolute)
its geometry/kinematics (e.g Denavit-Hartenberg parameters)
the controller interface (e.g joint position commands)
the type of tasks to perform (e.g TCP positioning)

The first thing to do is try to remove as much assumptions as possible. For this tutorial, we won’t rely on any of the assumptions listed above as we want to make a truly generic controller.

The controller requirements are:

no loops in the kinematic tree (i.e we don’t deal with parallel robots)
a robocop::KinematicTreeModel will be provided to us
output a joint level velocity control
be able to handle multiple joint and body level tasks and constraints at the same time

For 1. it is an assumption in the model, not something we will check for.

Regarding 3., we could have chosen to output joint positions instead but it would make the controller a bit more complicated and it wouldn’t play well if the robot runs at a different frequency than the controller (which can happen, especially in multi-robot scenarios).

Point 4. is where most of the thinking as to come because you need to shift from “my robot is doing one thing” to “I don’t know what my robot will do”.

Please also keep in mind that controllers are meant to be generic enough to be shared and used by multiple people, so you don’t really know what the other users will do with it. You really have to try and keep things as open as possible, otherwise people will come and complain about not being able to do X or Y.

The math

If we were to control the end effector of a serial manipulator we would simply write this and be done with it:

$\dot{q} = J^+ \ \dot{x}^{ref}$

where $\dot{q}$ is the joint velocity command, $\dot{x}^{ref}$ the end effector velocity to follow and $J$ the end effector’s Jacobian. $J^+$ denotes the pseudo-inverse of the Jacobian. We don’t know the size of the Jacobian in advance so we can’t simply use an inverse. Note however that there are a lot of subtleties and different approaches to pseudo-inversion (direct, damped, dynamically damped) but we won’t go into this here as it’s out of scope, we’ll just assume that the pseudo-inverse exists and is valid.

If you were to track positions instead of velocities you would typically do something like this instead:

$\dot{q} = J^ \ \lambda \frac{x^{ref}-x}{T}$

where $\lambda$ is a gain factor and $T$ the control period.

But let’s focus on velocity control for now and we’ll see later on how we can implement position control on top of that easily with the tools provided by robocop.

So the next step is to turn the single task control law into a multitask one:

$\dot{q} = \sum_{i=0}^{Nj}{\dot{q}_{i}^{ref}} + \sum_{i=0}^{Nb}{J_i^+ \ \dot{x}_{i}^{ref}}$

where $Nj$ and $Nb$ are the number of active joint and body tasks respectively, $\dot{q}_{i}^{ref}$ each joint task reference velocity and $J_i$ and $\dot{x}_{i}^{ref}$ each body task Jacobian and reference velocity.

We don’t have a single Jacobian matrix because each task can target a different body. For instance there can be one on the end effector and one on the elbow to try to avoid collisions.

You could say “but there can be conflicts between all these tasks, how to we deal with that?”.

The solution is simple, we don’t.

It’s up to the user to configure the controller in a sensible way. You may think that having multiple active joints tasks will always be problematic, but since they can target different joint groups it could be totally fine. And mixing different velocity sources to reach a goal might be what the user wants so there’s not really anything we can so, or should do, to prevent that.

And keep in mind that this is just an example for the sake of the tutorial. If you need such a controller, use the controller provided by robocop-qp-controller/kinematic-tree-qp-controller where tasks can have weights and priorities to handle complex scenarios.

Back to the math. We need a way to implement some form of constraint. Here we’ll go with a simple solution:

$\dot{q}^{des} = \sum_{i=0}^{Nj}{\dot{q}_{i}^{ref}} + \sum_{i=0}^{Nb}{J_i^+ \ \dot{x}_{i}^{ref}}$ $\dot{q} = \alpha \ \dot{q}^{des}$

with $\alpha$ a scalar in the $[0,1]$ range.

This scalar acts as a velocity reduction mechanism.

At 1 there is no velocity reduction so the desired velocity will be tracked.

Between 0 and 1 the velocity is reduced but since it’s a scalar gain, the resulting motion will still be in the same direction, which wouldn’t be the case if we chose to saturate the output for instance.

At 0 the robot won’t be able to move.

That’s it for the math, now we can move to the implementation.

Implementation

The code for the controller written for this tutorial is hosted here, in case you want to browse it directly.

The package

First thing first, create a new PID package for your controller. Here our package is called robocop-tutorial-controller.

In the root CMakeLists.txt file we need to define two mandatory dependencies and two others to build our example application:

PID_Dependency(robocop-core VERSION 0.1)
PID_Dependency(yaml-cpp)

if(BUILD_EXAMPLES)
    PID_Dependency(robocop-model-pinocchio VERSION 0.1)
    PID_Dependency(robocop-example-description VERSION 0.1)
endif()

robocop-core is the package providing all the base classes and concepts we will rely upon for our implementation. yaml-cpp will allow us to parse configuration options.

robocop-model-pinocchio provides one implementation of KinematicTreeModel and robocop-example-description robot descriptions that can be used for example code like this (don’t use a “real” description package otherwise you can quickly fall into circular dependencies).

Then we have to declare two libraries in src/CMakeLists.txt:

PID_Component(
    processors
    DEPEND
        robocop/core
        yaml-cpp/yaml-cpp
    CXX_STANDARD 17
    WARNING_LEVEL ALL
)

PID_Component(
    tutorial-controller
    USAGE robocop/controller/tutorial_controller.h
    EXPORT
        robocop/core
    DEPEND
        robocop-tutorial-controller/processors
    CXX_STANDARD 17
    WARNING_LEVEL ALL
)

The processors component is a module library (no header files) that will be loaded by the code generator (i.e. ran at build time) in order to get data thet must be added to the world joints and/or bodies.

The tutorial-controller component is the actual library in which we will provide our controller. The dependency to the processors is not actually needed by the library, but it’s a common practice to simplify usage of the library (more on that later).

We will also write an example application in order to show to future users how the controller is supposed to be used. So in apps/CMakeLists.txt we add:

PID_Component(
    example
    EXAMPLE
    DEPEND
        robocop-tutorial-controller/tutorial-controller
        robocop/model-pinocchio
        robocop/example-description
    RUNTIME_RESOURCES
        robocop-tutorial-controller
    CXX_STANDARD 17
    WARNING_LEVEL ALL
)

Robocop_Generate(
    example
    robocop-tutorial-controller/example/config.yaml
)

Notice the call to Robocop_Generate, which is a CMake function provided by robocop-core that will configure the build system in order to call the code generator when needed. Its first argument is the component to generate the code for and the second the path to a configuration file to be found inside the package’s share/resources folder.

Lastly, here is the configuration file we will use for our example application:

models:
  - path: robocop-example-description/models/kuka_lwr.urdf
    namespace: lwr

processors:
  controller:
    type: robocop-tutorial-controller/processors/tutorial-controller
    options:
      joint_group: lwr_joints

  model:
    type: robocop-model-ktm/processors/model-ktm
    options:
      implementation: pinocchio
      input: state
      forward_kinematics: true
      forward_velocity: false

It’s a good idea to start with a config file before writing the code as it forces you to think about what configurations options are required by your controller and how you should name them.

Here the only option we define is joint_group because we need to know which joints we have to control and for which we have to declare the data that we will provide (here a JointVelocity command).

The `processors` module

Now that everything is in place and we have the format for our config file, we can write the module that will parse out controller config options.

In src/processors/register_processors.cpp we have:

#include <robocop/core/generate_content.h>

#include <fmt/format.h>

#include <yaml-cpp/yaml.h>

bool robocop::generate_content(const std::string& type,
                               const std::string& config,
                               WorldGenerator& world) noexcept {
    if (type != "tutorial-controller") {
        return false;
    }

    auto options = YAML::Load(config);

    if (not options["joint_group"]) {
        fmt::print(
            stderr,
            "Missing required 'joint_group' field for the 'qp' processor. "
            "You can use the 'all' value to target all the robot's joints\n");
        return false;
    }

    const auto joint_group = options["joint_group"].as<std::string>();
    if (not world.has_joint_group(joint_group)) {
        fmt::print(stderr,
                   "The given joint group ({}) does not match any known joint group ({})\n",
                   joint_group, fmt::join(world.joint_groups(), ", "));
        return false;
    }

    world.add_joint_group_command(joint_group, "JointVelocity");

    return true;
}

The entry point for all robocop modules is a function with the following signature:

bool robocop::generate_content(const std::string& type,
                               const std::string& config,
                               WorldGenerator& world) noexcept;

You must include the robocop/core/generate_content.h file in order to get this function called by the code generator.

The other includes are specific to what you need to implement that function.

Here the function is quite simple since we have only one option but the general pattern is usually the same:

Check if the processor type matches what you expect (you can handle multiple types in the same module). This type is the last part of the processor’s type field in the corresponding YAML configuration.
Load the given YAML configuration (here we use yaml-cpp)
If you have mandatory options, check for their existence, and if possible, check if they values are valid.
Add all the data that you produce to the world. Here it is only a JointVelocity command on all joints of the given joint group
Return false if you detect any error and true if everything is valid.

The `tutorial-controller` library

We will need both header and source files for our library so you can already create the following folders to match the typical controllers file structure:

include
└── tutorial-controller
    └── robocop
        └── controllers
            └── tutorial_controller
                ├── constraints
                │   ├── body
                │   └── joint
                └── tasks
                    ├── body
                    └── joint
src
└── tutorial-controller
    ├── constraints
    │   ├── body
    │   └── joint
    └── tasks
        ├── body
        └── joint

The headers go to a robocop/controller/my_controller_name folder, which itself has subfolders for its provided tasks and constraints.

Now we can start writing actual code.

We will start with the base classes for our tasks, then constraints and finally we will write our controller.

Tasks base classes

First we recall our task equation: $\dot{q}^{des} = \sum_{i=0}^{Nj}{\dot{q}_{i}^{ref}} + \sum_{i=0}^{Nb}{J_i^+ \ \dot{x}_{i}^{ref}}$

For the joint tasks it’s quite straightforward, we just need a way for each task to provide its $\dot{q}^{ref}$ vector.

For body tasks, we have two options:

Provide the body velocity $\dot{x}^{ref}$
Provide the projected body velocity in joint space $J^+ \ \dot{x}^{ref}$

We will go with the first option as it keeps the code for the tasks simpler and keeps the Jacobian matrix inversion in one place, inside the controller. The last point would make it easy to change how this inversion is being performed (possibly depending on user options) without having to rewrite all the tasks’ code. And remember that other people than you might write tasks.

So now that we know what each task must provide we can write our base classes. There are probably a lot of ways to achieve the same result but here we will go with a simple implementation using virtual functions.

Declaration

In include/tutorial-controller/robocop/controllers/tutorial_controller/task.h we have the following declarations:

#pragma once

#include <robocop/core/joint_group_task.h>
#include <robocop/core/body_task.h>

namespace robocop {

class TutorialController;

class TutorialJointGroupTask
    : public robocop::JointGroupTask<TutorialController> {
public:
    TutorialJointGroupTask(TutorialController* controller,
                           JointGroupBase& joint_group);

    const JointVelocity& last_velocity() const;

    const JointVelocity& compute();

protected:
    virtual void compute_joint_velocity(JointVelocity& joint_velocity) = 0;

private:
    JointVelocity joint_velocity_;
};

template <>
struct BaseJointTask<TutorialController> {
    using type = TutorialJointGroupTask;
};

class TutorialBodyTask : public robocop::BodyTask<TutorialController> {
public:
    TutorialBodyTask(TutorialController* controller, BodyRef task_body,
                     ReferenceBody body_of_reference);

    const SpatialVelocity& last_velocity_in_ref() const;
    const SpatialVelocity& last_velocity_in_world() const;

    const SpatialVelocity& compute();

protected:
    virtual void
    compute_body_velocity(SpatialVelocity& body_velocity_in_ref) = 0;

private:
    SpatialVelocity body_velocity_in_ref_;
    SpatialVelocity body_velocity_in_world_;
};

template <>
struct BaseBodyTask<TutorialController> {
    using type = TutorialBodyTask;
};

} // namespace robocop

We start by including both robocop/core/joint_group_task.h and robocop/core/body_task.h since we are declaring both base classes in the same file. You could split your base class declarations in different files if you wish.

Then, in the robocop namespace we start by forward declaring our controller class TutorialController. Since the controller will include this file we need a forward declaration to avoid circular dependencies.

Next there is our joint group base task:

class TutorialJointGroupTask
    : public robocop::JointGroupTask<TutorialController> {
public:
    TutorialJointGroupTask(TutorialController* controller,
                           JointGroupBase& joint_group);

    const JointVelocity& last_velocity() const;

    const JointVelocity& compute();

protected:
    virtual void compute_joint_velocity(JointVelocity& joint_velocity) = 0;

private:
    JointVelocity joint_velocity_;
};

The class must have a unique name to not conflict with anything else in the robocop namespace. Here we will prefix everything with Tutorial. You can also define it in a subnamespace to avoid any problem.

In order to be recognized as a joint group task, we have to inherit from robocop::JointGroupTask. This class is a template that expect the associated controller type as a template parameter.

Then we can declare our constructor, which expects a pointer to a controller and the joint group to act on. You can add more parameters if you need but these two are required by the parent class constructor so you always have them.

Finally, we have our custom API to implement our task, which consists of:

A private JointVelocity joint_velocity_ member variable to hold the last velocity generated by the task (need to be initialized to zero) and a public read-only accessor last_velocity().
A public const JointVelocity& compute() function that will be called by the controller to get the new velocity reference.
A protected void compute_joint_velocity(JointVelocity& joint_velocity) pure virtual function to be implemented by the tasks that will be called inside the compute() function.

We could have only a pure virtual compute() public function but this pattern allows to make common internal operations before and/or after the actual user code.

Now that our base joint group task is declared, we can register it for our controller, using the following template specialization:

template <>
struct BaseJointTask<TutorialController> {
    using type = TutorialJointGroupTask;
};

The BaseJointTask class template is declared in robocop/core/joint_group_task.h. This mechanism allows for robocop code to retrieve the base class of any element of a controller. If you don’t provide these specializations then your controller will be treated as if these elements are not supported.

We will go quickly about the base body task as it is very similar to the joint group one:

class TutorialBodyTask : public robocop::BodyTask<TutorialController> {
public:
    TutorialBodyTask(TutorialController* controller, BodyRef task_body,
                     ReferenceBody body_of_reference);

    const SpatialVelocity& last_velocity_in_ref() const;
    const SpatialVelocity& last_velocity_in_world() const;

    const SpatialVelocity& compute();

protected:
    virtual void
    compute_body_velocity(SpatialVelocity& body_velocity_in_ref) = 0;

private:
    SpatialVelocity body_velocity_in_ref_;
    SpatialVelocity body_velocity_in_world_;
};

The pattern is the same as before, the main differences are:

Different parent class (robocop::BodyTask)
Different constructor arguments (all body tasks target a body and have a body acting as a reference frame)
We have the reference velocity both in the reference frame and in the world frame

Jacobian matrices map velocities from joint space to velocities in the world so we must provide velocities in that frame. But in order to let tasks work in their reference frame, we will perform this transformation internally inside the compute() function and provide that to the controller.

It’s registration is also very similar to the joint group task:

template <>
struct BaseBodyTask<TutorialController> {
    using type = TutorialBodyTask;
};

Definition

With our two classes fully declared, we can write their implementation in src/tutorial-controller/task.cpp:

#include <robocop/controllers/tutorial_controller/task.h>

#include <robocop/controllers/tutorial_controller.h>

namespace robocop {

TutorialJointGroupTask::TutorialJointGroupTask(TutorialController* controller,
                                               JointGroupBase& joint_group)
    : JointGroupTask{controller, joint_group},
      joint_velocity_{phyq::zero, joint_group.dofs()} {
}

const JointVelocity& TutorialJointGroupTask::last_velocity() const {
    return joint_velocity_;
}

const JointVelocity& TutorialJointGroupTask::compute() {
    compute_joint_velocity(joint_velocity_);
    return joint_velocity_;
}

TutorialBodyTask::TutorialBodyTask(TutorialController* controller,
                                   BodyRef task_body,
                                   ReferenceBody body_of_reference)
    : BodyTask{controller, task_body, body_of_reference},
      body_velocity_in_ref_{phyq::zero, reference().frame().ref()},
      body_velocity_in_world_{phyq::zero, controller->world().frame()} {
}

const SpatialVelocity& TutorialBodyTask::last_velocity_in_ref() const {
    return body_velocity_in_ref_;
}

const SpatialVelocity& TutorialBodyTask::last_velocity_in_world() const {
    return body_velocity_in_world_;
}

const SpatialVelocity& TutorialBodyTask::compute() {
    compute_body_velocity(body_velocity_in_ref_);

    const auto& ref_to_world =
        controller().model().get_transformation(reference().name(), "world");

    body_velocity_in_world_ = ref_to_world * body_velocity_in_ref_;

    return body_velocity_in_world_;
}

} // namespace robocop

The first step is to include our previously written header as well as the controller’s one (not written yet).

#include <robocop/controllers/tutorial_controller/task.h>

#include <robocop/controllers/tutorial_controller.h>

The controller one is needed since we only have a forward declaration in our task.h header but we need the full declaration here.

For the joint group task, we don’t need to do much actually, just initialize our internal velocity vector to zero and the proper size, implement its trivial accessor and have the compute() function solely call the compute_joint_velocity() virtual function and return the generated velocity:

TutorialJointGroupTask::TutorialJointGroupTask(TutorialController* controller,
                                               JointGroupBase& joint_group)
    : JointGroupTask{controller, joint_group},
      joint_velocity_{phyq::zero, joint_group.dofs()} {
}

const JointVelocity& TutorialJointGroupTask::last_velocity() const {
    return joint_velocity_;
}

const JointVelocity& TutorialJointGroupTask::compute() {
    compute_joint_velocity(joint_velocity_);
    return joint_velocity_;
}

There’s a bit more going on for the body task so we will break it down a bit more. First the constructor:

TutorialBodyTask::TutorialBodyTask(TutorialController* controller,
                                   BodyRef task_body,
                                   ReferenceBody body_of_reference)
    : BodyTask{controller, task_body, body_of_reference},
      body_velocity_in_ref_{phyq::zero, reference().frame().ref()},
      body_velocity_in_world_{phyq::zero, controller->world().frame()} {
}

As with the joint group task we forward the parameters to the parent class constructor and then initialize our internal data. The important thing to note is the initialization of body_velocity_in_ref_:

body_velocity_in_ref_{phyq::zero, reference().frame().ref()}

Its reference frame is a reference to our reference body one. This means that even if the reference body gets changed after the task is created, body_velocity_in_ref_ will always have the correct reference frame. Do not use body_of_reference.frame().ref() as body_of_reference is a temporary that will get destroyed at the end of this constructor and so any reference to it, or its frame, will become invalid at that point.

We’ll skip the two accessors as they are trivial and just detail the compute() function:

const SpatialVelocity& TutorialBodyTask::compute() {
    compute_body_velocity(body_velocity_in_ref_);

    const auto& ref_to_world =
        controller().model().get_transformation(reference().name(), "world");

    body_velocity_in_world_ = ref_to_world * body_velocity_in_ref_;

    return body_velocity_in_world_;
}

We first call the compute_body_velocity() virtual function in order to get the task desired velocity in its reference frame.

Then we need to transform that velocity from the reference frame to the world frame, as explained earlier. To do so, we simply ask the controller’s associated model for that.

Once we have that transformation, we can use it to compute the desired velocity in world frame and return that result.

And that’s about it for the base classes. Now joint group and body that can be implemented by simply inheriting from these base classes and implement the associated pure virtual function to provide their reference velocity.

Velocity tasks

In this tutorial we will only implement joint group and body velocity tasks.

It is enough to understand how to implement actual tasks for this controller and if you need to define different kind of tasks, you can head to the other tutorial on this subject.

These two tasks won’t have much to do apart from forwarding the output of their target interpolator to the controller after applying their selection matricies (all tasks in robocop have selection matricies).

Joint group velocity task

Declaration

We declare our joint group velocity task in include/tutorial-controller/robocop/controllers/tutorial_controller/tasks/joint/velocity.h:

#pragma once

#include <robocop/controllers/tutorial_controller/task.h>

#include <robocop/core/tasks/joint/velocity.h>

namespace robocop {

class TutorialJointVelocityTask final
    : public robocop::Task<TutorialJointGroupTask, JointVelocity> {
public:
    TutorialJointVelocityTask(TutorialController* controller,
                              JointGroupBase& joint_group);

private:
    void compute_joint_velocity(JointVelocity& joint_velocity) override final;
};

template <>
struct JointVelocityTask<TutorialController> {
    using type = TutorialJointVelocityTask;
};

} // namespace robocop

The first include is here to provide our TutorialJointGroupTask and second one to provide JointVelocityTask, one of the core task types.

Our class inherit from robocop::Task<TutorialJointGroupTask, JointVelocity>.

The first template parameter is our base task type, which will be inherited by Task. This pattern allows to have the following hierarchy: TutorialJointVelocityTask -> Task -> TutorialJointGroupTask, which allows Task to provide special machinery knowing that its parent class is actually a task base class.

The second template parameter is the target type for our task. For a joint group velocity task, it is natural to use JointVelocity as a target, so we pass that for this template parameter.

There is a third, optional, template parameter that we don’t use here that allows to define a parameter type that can latter be accessed using the parameters() function to configure how the task should behave. These parameter types as usually simple structs that bundle all the required parameter for a task (e.g gains, thresholds, etc).

Next, our constructor is the same as our base class since we don’t need anything more (we don’t usually do).

Then, we override the compute_joint_velocity() function (and mark it as final since no one can ever override it afterwards).

Below the class declaration we have the same type of registration as we’ve seen before:

template <>
struct JointVelocityTask<TutorialController> {
    using type = TutorialJointVelocityTask;
};

This registration is not mandatory, but allows to later create this task with:

controller.add_task<robocop::JointVelocityTask>(joint_group);

in addition of:

controller.add_task<robocop::TutorialJointVelocityTask>(joint_group);

It’s a good practice to register tasks that have a corresponding core type like this as it allows users to write code in a more general way.

But it’s possible that a task doesn’t fit a core task type and so cannot be registered and be referred to by its full name.

Definition

In src/tutorial-controller/tasks/joint/velocity.cpp we have the following implementation:

#include <robocop/controllers/tutorial_controller/tasks/joint/velocity.h>

#include <robocop/controllers/tutorial_controller.h>

namespace robocop {

TutorialJointVelocityTask::TutorialJointVelocityTask(
    TutorialController* controller, JointGroupBase& joint_group)
    : Task{Target{JointVelocity{phyq::zero, joint_group.dofs()}}, controller,
           joint_group} {
}

void TutorialJointVelocityTask::compute_joint_velocity(
    JointVelocity& joint_velocity) {
    joint_velocity = selection_matrix() * target().output();
}

} // namespace robocop

The includes follow the same pattern as for the base tasks.

The constructor calls Task constructor by giving an initial value for its target and forwarding its controller and joint_group arguments. The direct target initialization is not mandatory and can be omitted, but in that case initialization should be done inside the constructor body.

Finally, the compute_joint_velocity() function simply pass the output of the task interpolator through the task selection matrix and save the result in the joint_velocity output parameter.

More complicated tasks could do more things but in the end you always have to look at the interpolator output to know what the user asks and make sure you make use of the selection matrix.

Body velocity task

Declaration

The body velocity task follows the same exact pattern as the joint group tasks so we won’t detail its declaration (in include/tutorial-controller/robocop/controllers/tutorial_controller/tasks/body/velocity.h), which should be self explanatory now:

#pragma once

#include <robocop/controllers/tutorial_controller/task.h>

#include <robocop/core/tasks/body/velocity.h>

namespace robocop {

class TutorialBodyVelocityTask final
    : public robocop::Task<TutorialBodyTask, SpatialVelocity> {
public:
    TutorialBodyVelocityTask(TutorialController* controller, BodyRef task_body,
                             ReferenceBody body_of_reference);

private:
    void
    compute_body_velocity(SpatialVelocity& body_velocity_in_ref) override final;
};

template <>
struct BodyVelocityTask<TutorialController> {
    using type = TutorialBodyVelocityTask;
};

} // namespace robocop

Definition

The implementation, in src/tutorial-controller/tasks/body/velocity.cpp, is also very similar to the joint group task, the main difference being how the target is initialized.

#include <robocop/controllers/tutorial_controller/tasks/body/velocity.h>

#include <robocop/controllers/tutorial_controller.h>

namespace robocop {

TutorialBodyVelocityTask::TutorialBodyVelocityTask(
    TutorialController* controller, BodyRef task_body,
    ReferenceBody body_of_reference)
    : Task{controller, task_body, body_of_reference} {

    // use a frame ref so that if the reference frame changes we don't have
    // anything to do
    target()->change_frame(reference().frame().ref());
    target()->set_zero();
}

void TutorialBodyVelocityTask::compute_body_velocity(
    SpatialVelocity& body_velocity_in_ref) {
    body_velocity_in_ref = selection_matrix() * target().output();
}

} // namespace robocop

This time, we don’t initialize the task target in Task initialization list. We instead do it in the constructor body because we need access to reference(), which doesn’t exist until Task is actually created (calling the function inside the initialization list is undefined behavior).

Constraints base classes

First let’s recall our constraint equation: $\dot{q} = \alpha \ \dot{q}^{des}$

This means that all constraints must receive the desired joint velocity $\dot{q}^{des}$ and compute a velocity scaling factor from that.

It will be then up to the controller to pick the lower $\alpha$ value from all the active constraints.

From an implementation point of view, this means that the interface for the joint group constraints and the body constraints should be roughly the same as they both have the same input and output.

To avoid code duplication, we will factor this common interface into a single class TutorialConstraint, that will be inherited by our TutorialJointGroupConstraint and TutorialBodyConstraint.

Declaration

We put the code for this in include/tutorial-controller/robocop/controllers/tutorial_controller/constraint.h:

#pragma once

#include <robocop/core/joint_group_constraint.h>
#include <robocop/core/body_constraint.h>

namespace robocop {

class TutorialController;

class TutorialConstraint {
public:
    TutorialConstraint() = default;

    TutorialConstraint(const TutorialConstraint&) = delete;
    TutorialConstraint(TutorialConstraint&&) noexcept = default;

    virtual ~TutorialConstraint() noexcept = default;

    TutorialConstraint& operator=(const TutorialConstraint&) = delete;
    TutorialConstraint& operator=(TutorialConstraint&&) noexcept = default;

    const double& scaling_factor();

    const double& compute(const JointVelocity& controlled_joints_velocity);

protected:
    virtual double
    compute_scaling_factor(const JointVelocity& controlled_joints_velocity) = 0;

private:
    double scaling_factor_{};
};

class TutorialJointGroupConstraint
    : public robocop::JointGroupConstraint<TutorialController>,
      public TutorialConstraint {
public:
    TutorialJointGroupConstraint(TutorialController* controller,
                                 JointGroupBase& joint_group);
};

template <>
struct BaseJointConstraint<TutorialController> {
    using type = TutorialJointGroupConstraint;
};

class TutorialBodyConstraint
    : public robocop::BodyConstraint<TutorialController>,
      public TutorialConstraint {
public:
    TutorialBodyConstraint(TutorialController* controller,
                           BodyRef constrained_body,
                           ReferenceBody body_of_reference);
};

template <>
struct BaseBodyConstraint<TutorialController> {
    using type = TutorialBodyConstraint;
};

} // namespace robocop

The general idea and pattern is the same as for the tasks so we won’t repeat the same details here and just focus on what’s new.

First, our TutorialConstraint is the root virtual class so we must define the proper constructors, destructor, and assignment operators (destructor must be virtual and copy must be disabled to ensure correct behavior).

Then, the meat of the class is simply:

public:
    const double& scaling_factor();

    const double& compute(const JointVelocity& controlled_joints_velocity);

protected:
    virtual double
    compute_scaling_factor(const JointVelocity& controlled_joints_velocity) = 0;

private:
    double scaling_factor_{};

We store the computed velocity scaling factor, provide a read only accessor for it, define compute() function to be called by the controller and a compute_scaling_factor pure virtual function to be implemented by actual constraints.

Both compute functions take a vector of desired velocity for all the controlled joints and return the scaling factor computed from it.

Then we have the declaration of the two base classes, both inheriting from TutorialConstraint as well as their respective core base classes, JointGroupConstraint and BodyConstraint, followed by their registration for our controller.

Definition

Our definition file src/tutorial-controller/constraint.cpp is pretty straightforward:

#include <robocop/controllers/tutorial_controller/constraint.h>

#include <robocop/controllers/tutorial_controller.h>

namespace robocop {

const double& TutorialConstraint::scaling_factor() {
    return scaling_factor_;
}

const double&
TutorialConstraint::compute(const JointVelocity& controlled_joints_velocity) {
    scaling_factor_ = compute_scaling_factor(controlled_joints_velocity);
    return scaling_factor_;
}

TutorialJointGroupConstraint::TutorialJointGroupConstraint(
    TutorialController* controller, JointGroupBase& joint_group)
    : JointGroupConstraint{controller, joint_group} {
}

TutorialBodyConstraint::TutorialBodyConstraint(TutorialController* controller,
                                               BodyRef constrained_body,
                                               ReferenceBody body_of_reference)
    : BodyConstraint{controller, constrained_body, body_of_reference} {
}

} // namespace robocop

The compute function just calls the user defined compute_scaling_factor virtual function, save the result into scaling_factor_ and returns it.

TutorialJointGroupConstraint and TutorialBodyConstraint constructors just forwards their argument to their parent class constructor.

With our base constraint classes defined, we can look the implementation of the joint group and body velocity constraints.

As with the tasks, these constraints are quite simple but enough to demonstrate how it works.

Joint group velocity constraint

Declaration

Our joint group velocity constraint header, located at include/tutorial-controller/robocop/controllers/tutorial_controller/constraints/joint/velocity.h, contains the following:

#pragma once

#include <robocop/controllers/tutorial_controller/constraint.h>

#include <robocop/core/constraints/joint/velocity.h>

namespace robocop {

struct TutorialJointVelocityConstraintParams {
    explicit TutorialJointVelocityConstraintParams(Eigen::Index dofs)
        : max_velocity{phyq::zero, dofs} {
    }

    JointVelocity max_velocity;
};

class TutorialJointVelocityConstraint final
    : public robocop::Constraint<TutorialJointGroupConstraint,
                                 TutorialJointVelocityConstraintParams> {
public:
    TutorialJointVelocityConstraint(TutorialController* controller,
                                    JointGroupBase& joint_group);

private:
    double compute_scaling_factor(
        const JointVelocity& controlled_joints_velocity) override final;
};

template <>
struct JointVelocityConstraint<TutorialController> {
    using type = TutorialJointVelocityConstraint;
};

} // namespace robocop

Almost everything should look familiar by now, what’s different is of course the base class and the use of parameters (which we didn’t have for the tasks).

We need a way to define the maximum joint velocity allowed by this constraint, and the idiomatic way is to put that limit inside a class and pass it as a template parameter to the robocop::Constraint base class.

Our parameter class is simply:

struct TutorialJointVelocityConstraintParams {
    explicit TutorialJointVelocityConstraintParams(Eigen::Index dofs)
        : max_velocity{phyq::zero, dofs} {
    }

    JointVelocity max_velocity;
};

The constructor is here to disallow default construction and so to make sure that the max_velocity vector has the correct size and is properly initialized. Here we choose to set an initial value of zero instead of, let’s say, infinity so that if the user forgets to set the velocity limit, it will be very obvious and won’t result in the robot moving as if the constraint wasn’t there.

There’s nothing more of interest here so we can move to the class implementation.

Definition

This constraint definition is put in src/tutorial-controller/constraints/joint/velocity.cpp:

#include <robocop/controllers/tutorial_controller/constraints/joint/velocity.h>

#include <robocop/controllers/tutorial_controller/controller.h>

namespace robocop {

TutorialJointVelocityConstraint::TutorialJointVelocityConstraint(
    TutorialController* controller, JointGroupBase& joint_group)
    : Constraint{TutorialJointVelocityConstraintParams{joint_group.dofs()},
                 controller, joint_group} {
}

double TutorialJointVelocityConstraint::compute_scaling_factor(
    const JointVelocity& controlled_joints_velocity) {

    const auto joint_mapping =
        controller().controlled_joints().selection_matrix_to(joint_group());

    const auto joint_group_vel = joint_mapping * controlled_joints_velocity;

    double scaling_factor{1.};

    for (ssize i = 0; i < joint_group_vel.size(); ++i) {
        scaling_factor =
            std::min(scaling_factor, parameters().max_velocity(i) /
                                         phyq::abs(joint_group_vel(i)));
    }

    return scaling_factor;
}

} // namespace robocop

The constructor should be familiar, the difference with the tasks ones is that we know have a parameter type and we have to pass a default value for it since it’s not default constructible.

The compute_scaling_factor() function is where the interesting things happen.

We can see that the function gets passed the desired velocity vector for all the controlled joints.

But our constraints targets a specific joint group, which might be a subset of the controlled ones, so we need to map the full velocity vector to one corresponding to out joint group.

This can be done by a JointGroupMapping, given by the JointGroup::selection_matrix_from()/JointGroup::selection_matrix_to() functions.

Here we go from the controlled joint to the joint group so we call controller().controlled_joints().selection_matrix_to(joint_group()) to get it.

From that, we compute our joint group desired velocity:

const auto joint_group_vel = joint_mapping * controlled_joints_velocity;

Now we need to compute the velocity scaling factor. To do so, we use the following equation: $\alpha = \min \frac{\dot{q}^{max}_i}{|\dot{q}^{des}_i|}$

That way, if a joint desired velocity $|\dot{q}^{des}_i|$ is lower than its maximum velocity $\dot{q}^{max}_i$ , the scaling factor will be greater than one, resulting in no velocity reduction (the controller will clamp the value between 0 and 1). If the desired velocity becomes greater than the limit, the scaling factor will drop below one, imposing a slow down on the robot.

The equation is implemented by iterating over each velocity component, computing the above ratio, and saving the lowest value:

double scaling_factor{1.};

for (ssize i = 0; i < joint_group_vel.size(); ++i) {
    scaling_factor =
        std::min(scaling_factor, parameters().max_velocity(i) /
                                        phyq::abs(joint_group_vel(i)));
}

Body velocity constraint

Declaration

The body velocity constraint, declared in include/tutorial-controller/robocop/controllers/tutorial_controller/constraints/body/velocity.h, follows the exact same pattern as the equivalent joint group constraint:

#pragma once

#include <robocop/controllers/tutorial_controller/constraint.h>

#include <robocop/core/constraints/body/velocity.h>

namespace robocop {

struct TutorialBodyVelocityConstraintParams {
    explicit TutorialBodyVelocityConstraintParams(phyq::Frame frame)
        : max_velocity{phyq::zero, frame} {
    }

    TutorialBodyVelocityConstraintParams()
        : TutorialBodyVelocityConstraintParams{phyq::Frame::unknown()} {
    }

    void change_frame(const phyq::Frame& frame) {
        max_velocity.change_frame(frame);
    }

    SpatialVelocity max_velocity;
};

class TutorialBodyVelocityConstraint final
    : public robocop::Constraint<TutorialBodyConstraint,
                                 TutorialBodyVelocityConstraintParams> {
public:
    TutorialBodyVelocityConstraint(TutorialController* controller,
                                   BodyRef constrained_body,
                                   ReferenceBody body_of_reference);

private:
    double compute_scaling_factor(
        const JointVelocity& controlled_joints_velocity) override final;
};

template <>
struct BodyVelocityConstraint<TutorialController> {
    using type = TutorialBodyVelocityConstraint;
};
} // namespace robocop

Definition

As with the joint group velocity constraints, we will go into more details for the implementation, found in src/tutorial-controller/constraints/body/velocity.cpp:

#include <robocop/controllers/tutorial_controller/constraints/body/velocity.h>

#include <robocop/controllers/tutorial_controller/controller.h>

namespace robocop {

TutorialBodyVelocityConstraint::TutorialBodyVelocityConstraint(
    TutorialController* controller, BodyRef constrained_body,
    ReferenceBody body_of_reference)
    : Constraint{controller, std::move(constrained_body), body_of_reference} {

    // use a frame ref so that if the reference frame changes we don't have
    // anything to do
    parameters().change_frame(reference().frame().ref());
}

double TutorialBodyVelocityConstraint::compute_scaling_factor(
    const JointVelocity& controlled_joints_velocity) {

    const auto& body_jacobian =
        controller().model().get_body_jacobian(body().name());

    const auto joint_mapping =
        controller().controlled_joints().selection_matrix_to(
            body_jacobian.joints);

    const auto body_vel_world = body_jacobian.linear_transform *
                                (joint_mapping * controlled_joints_velocity);

    const auto world_to_ref =
        controller().model().get_transformation("world", reference().name());

    const auto body_vel_ref = world_to_ref * body_vel_world;

    double scaling_factor{1.};

    for (ssize i = 0; i < body_vel_ref.size(); ++i) {
        scaling_factor =
            std::min(scaling_factor,
                     parameters().max_velocity(i) / phyq::abs(body_vel_ref(i)));
    }

    return scaling_factor;
}

} // namespace robocop

The constructor is similar with what we have already seen, with the maximum velocity frame set as a reference to our reference body frame.

The computation of the scaling factor is similar to the previous one, but we first need to compute our body velocity from the desired joint velocities.

We can do that by using the body Jacobian, computed by the model using:

const auto& body_jacobian = controller().model().get_body_jacobian(body().name());

Now this Jacobian has columns only for the joints between the body and the world, which might be different than the controlled joints. To cope with that, we again compute a JointGroupMapping to map the two sets of joints:

const auto joint_mapping = controller().controlled_joints().selection_matrix_to(body_jacobian.joints);

With that, we can know compute the resulting body velocity in world frame:

const auto body_vel_world = body_jacobian.linear_transform * (joint_mapping * controlled_joints_velocity);

But our constraint is expressed in a given reference frame, not in world frame, so me must transform it:

const auto world_to_ref = controller().model().get_transformation("world", reference().name());
const auto body_vel_ref = world_to_ref * body_vel_world;

Now the scaling factor can be computed using the same method used in the joint group velocity constraint:

double scaling_factor{1.};

for (ssize i = 0; i < body_vel_ref.size(); ++i) {
    scaling_factor =
        std::min(scaling_factor,
                    parameters().max_velocity(i) / phyq::abs(body_vel_ref(i)));
}

Wrap up on tasks and constraints

We saw how we derived our tasks and constraints interfaces based on the equations we had for our controller and how we implemented these interfaces.

Of course all controllers will be different and you might need a different approach to fit your use case, but the general idea remains the same:

Take your controller equations
See how to generalize it for multiple tasks and constraints (if not already done)
Identify what should be the inputs and outputs for each type of task and constraint
Come up with interfaces for your base classes that follow these inputs/outputs
Try to do as much as possible in the base classes, and in the controller, so that the actual tasks and constraints are simple as possible to implement. Remember that they are customization points for your controller so the less the people writing them have to do and know about the controller internals, the better.

Controller

Okay, we are now ready to implement our controller.

Declaration

As with everything in robocop, there is some interfaces to conform to but also a lot of freedom on how you decide to implement your controller.

Here is the proposed controller declaration, found in include/tutorial-controller/robocop/controllers/tutorial_controller/controller.h:

#pragma once

#include <robocop/core/controller.h>
#include <robocop/core/kinematic_tree_model.h>
#include <robocop/core/world_ref.h>

#include <robocop/controllers/tutorial_controller/task.h>
#include <robocop/controllers/tutorial_controller/constraint.h>

namespace robocop {

class TutorialController : public Controller<TutorialController> {
public:
    TutorialController(WorldRef& world, KinematicTreeModel& model,
                       Period time_step, std::string_view processor_name);

    const JointVelocity& velocity_command() const;
    const JointVelocity& desired_velocity() const;
    const double& scaling_factor() const;

    // Redefine the model() functions here in order to have to more specialized
    // return type
    [[nodiscard]] const KinematicTreeModel& model() const {
        return static_cast<const KinematicTreeModel&>(Controller::model());
    }

    [[nodiscard]] KinematicTreeModel& model() {
        return static_cast<KinematicTreeModel&>(Controller::model());
    }

private:
    ControllerResult do_compute() override final;

    JointVelocity velocity_command_;
    JointVelocity desired_velocity_;
    double scaling_factor_{1.};
};

} // namespace robocop

We first start by including everything we need from core, then our previous headers containing the base classes declarations for our tasks and constraints.

These two last headers must be included here because the information they provide is needed by our parent class Controller<TutorialController>.

This Controller class will provide all the API the users will interact with and the necessary machinery to make this work.

For the constructor, we will need to pass to our parent constructor a WorldRef, a Model, a JointGroup (controlled joints) and a Period (execution period). So we ask users to give us everything except for the joint group, which we will retrieve from the controller configuration. That’s why we ask what is the name of the processor associated with this instance of the controller in the configuration file.

The only function that must be implemented is do_compute(). This is where you have to run your computations and update the controlled joints commands. It returns a ControllerResult, which is an enumeration with three possible values:

SolutionFound: the controller found a solution and updated the joint commands
PartialSolutionFound: the controller found a partial solution and updated the joint commands (not all tasks were considered, e.g in a hierarchical controller)
NoSolution: the controller couldn’t find a solution and did not update the joint commands

In our case, it is impossible to find partial solutions so we will never return PartialSolutionFound.

There are also a set of callback functions that can be overridden in order to know when tasks, constraints and configurations are added or removed from the controller. These function names follow the {generic,joint,body}_{task,constraint,configuration}_{added,removed} pattern and take a reference to the object in question as a parameter.

For the specific of our controller, we decided to store the actual joint velocity command, the desired one (sum of tasks output) and the velocity scaling factor coming from the constraints.

As previously, we provide read only accessor for them.

Since the Controller core class doesn’t know what type of model we will be using, it only provides model() functions returning a Model. But we know by design that we will use a KinematicTreeModel so we redefine the model() functions to perform the cast and so provide a more refined type. This is optional of course, but handy both for implementing the controller and for its users.

Definition

The implementation of the controller is found in src/tutorial-controller/controller.cpp:

#include <robocop/controllers/tutorial_controller.h>

#include <robocop/core/processors_config.h>

namespace robocop {

namespace {

JointVelocity
compute_total_joint_task_velocity(TutorialJointGroupTask& task,
                                  TutorialController& controller) {
    auto total_vel =
        JointVelocity{phyq::zero, controller.controlled_joints().dofs()};

    if (task.is_enabled()) {
        // Compute the matrix mapping joints in the task joint group to the
        // controlled joints
        const auto joint_mapping =
            controller.controlled_joints().selection_matrix_from(
                task.joint_group());

        total_vel += joint_mapping * task.compute();

        for (auto& [name, subtask] : task.subtasks()) {
            total_vel += compute_total_joint_task_velocity(
                static_cast<TutorialJointGroupTask&>(*subtask), controller);
        }
    }

    return total_vel;
}

JointVelocity compute_total_body_task_velocity(TutorialBodyTask& task,
                                               TutorialController& controller) {
    auto total_vel =
        JointVelocity{phyq::zero, controller.controlled_joints().dofs()};

    if (task.is_enabled()) {
        // Compute the Jacobian associated with the task's body
        const auto& body_jacobian =
            controller.model().get_body_jacobian(task.body().name());

        // (pseudo-)invert it
        const auto body_jacobian_inverse =
            body_jacobian.inverse().linear_transform;

        // Compute the matrix mapping joints in the jacobian to the
        // controlled joints
        const auto joint_mapping =
            controller.controlled_joints().selection_matrix_from(
                body_jacobian.joints);

        // Compute the joint level velocity from the task level one
        total_vel += joint_mapping * (body_jacobian_inverse * task.compute());

        for (auto& [name, subtask] : task.subtasks()) {
            total_vel += compute_total_body_task_velocity(
                static_cast<TutorialBodyTask&>(*subtask), controller);
        }
    }

    return total_vel;
}

template <typename ConstraintType>
double compute_total_scaling_factor(ConstraintType& constraint,
                                    const JointVelocity& desired_velocity) {
    double global_scaling_factor{1.};

    if (constraint.is_enabled()) {
        // Clamp the return value between 0 and 1 here to avoid doing it in
        // each constraint
        const auto scaling_factor =
            std::clamp(constraint.compute(desired_velocity), 0., 1.);

        // Save the lowest scaling factor
        global_scaling_factor = std::min(global_scaling_factor, scaling_factor);

        for (auto& [name, subconstraint] : constraint.subconstraints()) {
            const auto sub_scaling_factor = compute_total_scaling_factor(
                static_cast<ConstraintType&>(*subconstraint), desired_velocity);

            global_scaling_factor =
                std::min(global_scaling_factor, sub_scaling_factor);
        }
    }

    return global_scaling_factor;
}

} // namespace

TutorialController::TutorialController(WorldRef& world,
                                       KinematicTreeModel& model,
                                       Period time_step,
                                       std::string_view processor_name)
    : Controller{world, model,
                 world.joint_group(ProcessorsConfig::option_for<std::string>(
                     processor_name, "joint_group")),
                 time_step},
      velocity_command_{phyq::zero, controlled_joints().dofs()},
      desired_velocity_{phyq::zero, controlled_joints().dofs()} {
}

const JointVelocity& TutorialController::velocity_command() const {
    return velocity_command_;
}

const JointVelocity& TutorialController::desired_velocity() const {
    return desired_velocity_;
}

const double& TutorialController::scaling_factor() const {
    return scaling_factor_;
}

ControllerResult TutorialController::do_compute() {
    desired_velocity_.set_zero();

    for (auto& task : joint_tasks()) {
        desired_velocity_ += compute_total_joint_task_velocity(task, *this);
    }

    for (auto& task : body_tasks()) {
        desired_velocity_ += compute_total_body_task_velocity(task, *this);
    }

    scaling_factor_ = 1.;

    for (auto& constraint : joint_constraints()) {
        scaling_factor_ = std::min(
            scaling_factor_,
            compute_total_scaling_factor(constraint, desired_velocity_));
    }

    for (auto& constraint : body_constraints()) {
        scaling_factor_ = std::min(
            scaling_factor_,
            compute_total_scaling_factor(constraint, desired_velocity_));
    }

    velocity_command_ = scaling_factor_ * desired_velocity_;

    controlled_joints().command().set(velocity_command_);

    return ControllerResult::SolutionFound;
}

} // namespace robocop

We will ignore the anonymous namespace providing utility functions for now and come back to it later.

So we have our constructor:

TutorialController::TutorialController(WorldRef& world,
                                       KinematicTreeModel& model,
                                       Period time_step,
                                       std::string_view processor_name)
    : Controller{world, model,
                 world.joint_group(ProcessorsConfig::option_for<std::string>(
                     processor_name, "joint_group")),
                 time_step},
      velocity_command_{phyq::zero, controlled_joints().dofs()},
      desired_velocity_{phyq::zero, controlled_joints().dofs()} {

    controlled_joints().controller_outputs() = control_modes::velocity;
}

As explained above, we have to extract the joint group from the configuration file. This is done by this line:

world.joint_group(ProcessorsConfig::option_for<std::string>(processor_name, "joint_group"))

Which asks to retrieve the joint_group option for the processor named processor_name in the configuration file, convert it into an std::string and pass it to WorldRef::joint_group() to get the actual JointGroup to give to our parent class constructor.

In the constructor body we have this line:

controlled_joints().controller_outputs() = control_modes::velocity;

This is to instruct other components of the program that this controller is outputting velocities. This is to distinguish, in the joints command data, what comes directly from the controller and what is/should be computed based on that.

We also skip the trivial accessors as there is nothing special to say about them.

This leads us to the do_compute() function:

ControllerResult TutorialController::do_compute() {
    desired_velocity_.set_zero();

    for (auto& task : joint_tasks()) {
        desired_velocity_ += compute_total_joint_task_velocity(task, *this);
    }

    for (auto& task : body_tasks()) {
        desired_velocity_ += compute_total_body_task_velocity(task, *this);
    }

    scaling_factor_ = 1.;

    for (auto& constraint : joint_constraints()) {
        scaling_factor_ = std::min(
            scaling_factor_,
            compute_total_scaling_factor(constraint, desired_velocity_));
    }

    for (auto& constraint : body_constraints()) {
        scaling_factor_ = std::min(
            scaling_factor_,
            compute_total_scaling_factor(constraint, desired_velocity_));
    }

    velocity_command_ = scaling_factor_ * desired_velocity_;

    controlled_joints().command().set(velocity_command_);

    return ControllerResult::SolutionFound;
}

The algorithm implemented here is the following:

Compute the desired joint velocity by summing the output of all joint and body tasks
Find the minimum scaling factor among all joint and body constraints
Compute the joint velocity command by scaling the desired one
Set that velocity as a command on the controlled joints
Return with success

Which is a very close implementation of the math we started with.

Now we can look at these utility functions inside the anonymous namespace. The reason they’re not private member functions is to avoid having implementation details, which can change over time, in our class declaration, and it this case it doesn’t change much having them as free functions.

Let’s start with the first one:

JointVelocity
compute_total_joint_task_velocity(TutorialJointGroupTask& task,
                                  TutorialController& controller) {
    auto total_vel =
        JointVelocity{phyq::zero, controller.controlled_joints().dofs()};

    if (task.is_enabled()) {
        // Compute the matrix mapping joints in the task joint group to the
        // controlled joints
        const auto joint_mapping =
            controller.controlled_joints().selection_matrix_from(
                task.joint_group());

        total_vel += joint_mapping * task.compute();

        for (auto& [name, subtask] : task.subtasks()) {
            total_vel += compute_total_joint_task_velocity(
                static_cast<TutorialJointGroupTask&>(*subtask), controller);
        }
    }

    return total_vel;
}

As the name suggests, the goal of this function is to compute the total velocity generated by this task. But why do we need such a function you may ask. Why can’t we just call task.compute()?

There are two reasons for this:

First, there are common operations we have to do each time (checking if the task is enabled and map the output of its joint group to the controlled joints) so better to do it in one place.
Then, each task in robocop can have subtasks, which is simply a collection of tasks, that can themselves have subtasks as well. So we need to iterate recursively on all the subtasks in order to get the total velocity output for each single top level task. Finally it’s better, and simpler, to implement this into a recursive function.

In order to simplify the code, we assume here that all subtasks are of the same type as the top level one (i.e all joint tasks). That’s why we only do a static_cast here. But if you want to support heterogeneous subtasks for a controller that shares a similar architecture to this one, you would have to use some mechanism to distinguish between all types of tasks.

For the body velocity tasks the process is quite similar:

JointVelocity compute_total_body_task_velocity(TutorialBodyTask& task,
                                               TutorialController& controller) {
    auto total_vel =
        JointVelocity{phyq::zero, controller.controlled_joints().dofs()};

    if (task.is_enabled()) {
        // Compute the Jacobian associated with the task's body
        const auto& body_jacobian =
            controller.model().get_body_jacobian(task.body().name());

        // (pseudo-)invert it
        const auto body_jacobian_inverse =
            body_jacobian.inverse().linear_transform;

        // Compute the matrix mapping joints in the jacobian to the
        // controlled joints
        const auto joint_mapping =
            controller.controlled_joints().selection_matrix_from(
                body_jacobian.joints);

        // Compute the joint level velocity from the task level one
        total_vel += joint_mapping * (body_jacobian_inverse * task.compute());

        for (auto& [name, subtask] : task.subtasks()) {
            total_vel += compute_total_body_task_velocity(
                static_cast<TutorialBodyTask&>(*subtask), controller);
        }
    }

    return total_vel;
}

The difference is that in this case we need to get the task’s body jacobian, invert it and use a joint group mapping to compute the equivalent joint level velocity.

Lastly, we have a single template function to deal with both kind of constraints since their handling here is the same:

template <typename ConstraintType>
double compute_total_scaling_factor(ConstraintType& constraint,
                                    const JointVelocity& desired_velocity) {
    double global_scaling_factor{1.};

    if (constraint.is_enabled()) {
        // Clamp the return value between 0 and 1 here to avoid doing it in
        // each constraint
        const auto scaling_factor =
            std::clamp(constraint.compute(desired_velocity), 0., 1.);

        // Save the lowest scaling factor
        global_scaling_factor = std::min(global_scaling_factor, scaling_factor);

        for (auto& [name, subconstraint] : constraint.subconstraints()) {
            const auto sub_scaling_factor = compute_total_scaling_factor(
                static_cast<ConstraintType&>(*subconstraint), desired_velocity);

            global_scaling_factor =
                std::min(global_scaling_factor, sub_scaling_factor);
        }
    }

    return global_scaling_factor;
}

We follow the same approach as for the tasks but here we compute a global scaling factor for this constraints and all of its subconstraints.

We can see that we clamp the output of constraint.compute() between 0 and 1. We do it here so that we don’t have to repeat it in each constraint, which simplifies their implementation a bit and avoid potential errors.

Conclusion

This was quite long but we went into all the essential parts of what makes a robocop control so that you can write your own.

You saw that a big part is tied to what your controller is actually doing and the math behind it, and for this you are quite free to do it how you want. In the end robocop only forces you to have a modular and extensible controller and make use of a few core mechanisms (e.g selection matrices, interpolators).

Before you go write your own controller, take a look at the ones already provided here. You might actually find what you need or at least take some inspiration. At the time of writing, robocop-qp-controller is the most complete package, providing two different controllers, based on the same mixed QP control scheme (tasks can have both a priority and a weight).

If you want to go further on you own, you can take the controller we wrote for this tutorial and implement new tasks and constraints as well as provide support for configurations and generic tasks and constraints. To know how to this please follow this tutorial.

The goal

The math

Implementation

The package

The processors module

The tutorial-controller library

Tasks base classes

Declaration

Definition

Velocity tasks

Joint group velocity task

Declaration

Definition

Body velocity task

Declaration

Definition

Constraints base classes

Declaration

Definition

Joint group velocity constraint

Declaration

Definition

Body velocity constraint

Declaration

Definition

Wrap up on tasks and constraints

Controller

Declaration

Definition

Conclusion

The `processors` module

The `tutorial-controller` library