Presentation

Robocop is a C++ framework that aims to bring uniformity in robot control code.

It does not revolve around a specific controller, or type of controller, but rather provide interfaces to conform to.

Robocop is somewhat opniated but tries to remain as general as possible to not exclude certain types of robots or control strategies.

This page will explain the main concepts found in Robocop but without going into too much details. Links to more detailed explanations are provided when necessary.

Main concepts

1. Code generator
2. Configuration files
3. World
4. Joint groups
5. Model
6. Controller
7. Control modes
8. World dynamic modifications
9. Quantities
10. Drivers

Code generator

This point is rather technical but important to understand many design aspects of other concepts so it’s better to address it first.

The framework has no apriori knowledge on the type of controllers and applications that will be written in it. For instance, even if most robots will provide some kind of state associated with their joints/actuators we don’t know what this state will be: position, velocity, force, something else?

We could try to be exhaustive and plan for every possible case but that would mean:

1. Most data is unnecessary for a given application
2. It’s not clear to the end user what data is actually used (what should I read from/write to?)
3. It’s impossible to plan for everything. One day someone will have to go and add another data type because a new application requires it, which makes points 1. and 2. worse

We could also rely on a fully dynamic approach where all the needed data is created on the fly when requested. This mostly solves the points 1. and 3. raised above but it’s still somewhat the same thing for 2. Also, if we want to make sure that all data that is requested in one place is actually provided elsewhere we would have to rely on specific API and machinery, which can be confusing for users.

So in order to try to cope with these issues the solution adopted for robocop is code generation. More specifically, we generate for each application:

1. The entire world description: bodies, joints, their relationships, etc.
2. The data associated with each joint and body
3. Some other things (more on these in the next sections)

This code generation is guided by a YAML configuration file that allows to (mainly):

1. Aggregate URDF models to build a complete description of the world
2. List and configure “processors” (e.g drivers, controllers, etc) that will be called during the code generation phase to list the data they will provide (i.e write to) at run time

It is important to note that the processors only list the data they provide, not the data they need. This means that if a part of the program tries to access a data that no processors provide then it will trigger an error (compile time or run time depending how it is accessed) since this data doesn’t exist in the program.

It also makes sure that only the data needed for an application is actually present in the program.

Configuration files

As hinted in the previous section, robocop relies on configuration files for its applications.

Each application requires a single configuration file.

The general format is as follows:

# Here we used URDF and other YAML files to build a representation of the world
models:
  - path: app-config-generator-test/fixed_robot/model.urdf
    joint:
      parent: world # optional: default is world, but can by any known body
      type: fixed # optional: default is fixed but any joint type is allowed
      name: robot_root_joint # optional: default is 'parent'_to_'child'
      position:
        linear: [0, 1, 2] # optional: parent to base translation (default = zero)
        euler: [0, 1.57, 0] # optional: parent to base rotation (default = zero)
    namespace: "" # optional: added before joint and body names. If not empty it will be followed by un underscore

  # Include another YAML description file (can itself include other YAMl or URDF files)
  - include: flying_robot/model.yaml
    joint:
      parent: world # optional: default is world, but can by any known body
      type: free # optional: default is fixed but any joint type is allowed

# Headers to include for data types used on joints and bodies but not defined in the robocop core library
# The headers will be included as #include "path"
headers:
  - ../my_custom_type.h

# Here we can predeclare joint groups even if more can be created at run time
# If a processor has a joint group option, you need to have it declared here or provided by an included YAML model
joint_groups:
  branch1: j[0-2] # regular expression to match joints
  branch2: [j3, j2_2] # list of joints
  robot: j.*
  branches: [branch1, branch2] # can reuse joint groups to create bigger joint groups
  my_group: branch1 # can copy joint groups to use them with a different name

# Custom data added to joints, bodies or the the world directly
# Most of the application data is provided by processors but you might need to add some yourself for application specific stuff
data:
  # Data added to all joints inside the joint groups
  joint_groups:
    - name: robot
      state:
        - JointAcceleration
      command:
        - phyq::Period<>

  # Data added to joints directly
  joints:
    - name: j3
      state:
        - JointForce
    - name: j0
      state:
        - JointPosition

  # Data added to bodies directly
  bodies:
    - name: b0
      state: [my::CustomType] # from my_custom_type.h

  # Data added to the world itself and not specific joints or bodies
  world:
    - std::string
    - int

# Now we list all the processors that we use so that they can be called to know what data they provide
processors:
  # This name is user defined and can be used at run time to recover the options below
  test-processor:
    # The type is used to actually find the processor in the dependencies. It starts with the package it belongs to, then the component that defines it and then its actual type, as defined by the component. A component can provide several types of processors
    type: robocop-core/test-processor/test-processor # package/component/processor
    # optional: the content of this file will be used to initialize the options entry below with default values
    default_options: app-config-generator-test/test_processor_default_options.yaml # sets the 'joint' option
    options:
      bodies: [world, b0, b1, b2, b3, b4, b5]

  fixedbot_driver:
    type: fixed_robot/driver # assume processor = component
    options:
      joints: [joint1, joint2]
      command_mode: position # will add joints/[joint1, joint2, ...]/command/JointPosition
      end-effector: tcp # will add bodies/tcp/SpatialExternalForce

  flyingbot_driver:
    type: flying_robot/driver
    options:
      joints: [rotor1, rotor2]

YAML files included in the models section follow the same pattern but can only provide models and joint_groups . Any other node will be discarded.

If you need to generate multiple worlds for a single application, head over to this page.

World

In robocop, the world refers to the data structure generated by the code generator using the configuration files described above.

For the above configuration the generated file looks something like this:

/*** General includes omitted ***/

#include "../my_custom_type.h"

namespace robocop {

class World {
public:
    /*** Implementation stuff omitted  ***/

    template <typename StateElem, typename CommandElem,
              typename UpperLimitsElem, typename LowerLimitsElem,
              JointType Type>
    struct Joint {
        struct Limits {
            [[nodiscard]] UpperLimitsElem& upper() {
                return upper_;
            }
            [[nodiscard]] const UpperLimitsElem& upper() const {
                return upper_;
            }
            [[nodiscard]] LowerLimitsElem& lower() {
                return lower_;
            }
            [[nodiscard]] const LowerLimitsElem& lower() const {
                return lower_;
            }

        private:
            UpperLimitsElem upper_;
            LowerLimitsElem lower_;
        };

        Joint();

        [[nodiscard]] constexpr StateElem& state();
        [[nodiscard]] constexpr const StateElem& state() const;

        [[nodiscard]] constexpr CommandElem& command();
        [[nodiscard]] constexpr const CommandElem& command();

        [[nodiscard]] constexpr Limits& limits();
        [[nodiscard]] constexpr const Limits& limits();

        [[nodiscard]] static constexpr JointType type();

        [[nodiscard]] static constexpr ssize dofs();

        // How the joint is actually actuated
        [[nodiscard]] ControlMode& control_mode();
        [[nodiscard]] const ControlMode& control_mode();

        // What the controller produced to move the joint
        [[nodiscard]] ControlMode& controller_outputs();
        [[nodiscard]] const ControlMode& controller_outputs();

    private:
        StateElem state_;
        CommandElem command_;
        Limits limits_;
        ControlMode control_mode_;
        ControlMode controller_outputs_;
    };


    template <typename BodyT, typename StateElem, typename CommandElem>
    struct Body {
        Body();

        static constexpr phyq::Frame frame();

        [[nodiscard]] constexpr StateElem& state();
        [[nodiscard]] constexpr const StateElem& state() const;

        [[nodiscard]] constexpr CommandElem& command();
        [[nodiscard]] constexpr const CommandElem& command() const;

    private:
        StateElem state_;
        CommandElem command_;
    };

    // Contains all declared joints
    struct Joints {
        struct J0
            : Joint<JointState<JointAcceleration, JointForce, JointPosition,
                               JointVelocity>,
                    JointCommand<JointVelocity, phyq::Period<>>,
                    JointUpperLimits<JointForce, JointPosition, JointVelocity>,
                    JointLowerLimits<JointPosition>, JointType::Planar> {
            J0();

            static constexpr std::string_view name() {
                return "j0";
            }

            static constexpr std::string_view parent() {
                return "b0";
            }

            static constexpr std::string_view child() {
                return "b1";
            }

            static Eigen::Vector3d axis();

            static phyq::Spatial<phyq::Position> origin();

        } j0;

        /*** Other joints ***/

    private:
        friend class robocop::World;
        std::tuple<J0*, J1*, J2*, J2_2*, J3*, World_to_b0*> all_{
            &j0, &j1, &j2, &j2_2, &j3, &world_to_b0};
    };

    // Contains all declared bodies
    struct Bodies {
        struct B0
            : Body<B0, BodyState<SpatialForce, SpatialPosition, my::CustomType>,
                   BodyCommand<SpatialVelocity>> {

            B0();

            static constexpr std::string_view name() {
                return "b0";
            }

            static phyq::Spatial<phyq::Position> center_of_mass();

            static phyq::Angular<phyq::Mass> inertia();

            static phyq::Mass<> mass();

            static const BodyVisuals& visuals();

        } b0;

        /*** Other bodies ***/

    private:
        friend class robocop::World;
        std::tuple<B0*, B1*, B2*, B3*, B4*, B5*, World*> all_{
            &b0, &b1, &b2, &b3, &b4, &b5, &world};
    };

    // Contains the global world data
    struct Data {
        std::tuple<int, std::string> data;

        template <typename T>
        T& get();
    };

    World();

    World(const World& other);

    World(World&& other) noexcept;

    ~World() = default;

    World& operator=(const World& other);

    World& operator=(World&& other) noexcept = delete;

    [[nodiscard]] constexpr Joints& joints();

    [[nodiscard]] constexpr const Joints& joints() const;

    [[nodiscard]] constexpr Bodies& bodies();
    [[nodiscard]] constexpr const Bodies& bodies() const;

    [[nodiscard]] JointGroups& joint_groups();
    [[nodiscard]] const JointGroups& joint_groups() const;

    [[nodiscard]] JointGroup& joint_group(std::string_view name);
    [[nodiscard]] const JointGroup& joint_group(std::string_view name) const;

    [[nodiscard]] JointGroup& all_joints() noexcept;
    [[nodiscard]] const JointGroup& all_joints() const noexcept;

    [[nodiscard]] JointRef& joint(std::string_view name);
    [[nodiscard]] const JointRef& joint(std::string_view name) const;

    [[nodiscard]] BodyRef& body(std::string_view name);
    [[nodiscard]] const BodyRef& body(std::string_view name) const;

    [[nodiscard]] BodyRef& world();
    [[nodiscard]] const BodyRef& world() const;

    [[nodiscard]] static phyq::Frame frame();

    [[nodiscard]] static constexpr ssize dofs();

    [[nodiscard]] static constexpr ssize joint_count();

    [[nodiscard]] static constexpr ssize body_count();

    [[nodiscard]] static constexpr std::array<std::string_view, 6> joint_names();

    [[nodiscard]] static constexpr std::array<std::string_view, 7> body_names();

    [[nodiscard]] Data& data();
    [[nodiscard]] const Data& data() const;

    [[nodiscard]] WorldRef& ref();
    [[nodiscard]] const WorldRef& ref() const;

    [[nodiscard]] operator WorldRef&();
    [[nodiscard]] operator const WorldRef&() const;

private:
    WorldRef make_world_ref();

    Joints joints_;
    Bodies bodies_;
    WorldRef world_ref_;
    JointGroups joint_groups_;
    Data world_data_;
};

} // namespace robocop

As you can see, the world is a big data structure divided in three main parts, each defined in its owwn sub-structure:

1. Joints: every joint in the model has a corresponding Joint defined and instanciated here. Each joint has a type (here j0 is a planar joint) and separated data for its state, command and lower and upper limits. The Joint base class handles the access to these while the specific class (here J0) specifies the joint specific elements (name, parent, child, etc). Note that only the elements specified in the URDF models are reported here. For instance, if a joint doesn’t have a origin tag in the URDF, there won’t be an origin() accessor in the corresponding class.
2. Bodies: the same as for joints but for the bodies in the model (referred to links in URDF). The difference is that a body can only have state and command (no limits) and of course has no type.
3. Data: data attached to the world directly and not to a specific joint or body.

Static access

Once you have instanciated such a world, you can directly access any of the elements described above.

For instance:

auto world = robocop::World{};
auto& j0 = world.joints().j0;

auto& j0_pos = j0.state().get<robocop::JointPosition>();
j0_pos.set_random(); // read/write access

fmt::print("{}\n", j0.parent());

But if you try to access undeclared data or attributes it will trigger a compilation error:

// static_assert failed ... "The requested type doesn't exist on this element"
auto& j0_temp = j0.state().get<robocop::JointTemperature>();

// error: no member named 'mimic' in 'robocop::World::Joints::J0'
fmt::print("{}\n", j0.mimic());

Dynamic access

As a user you can access the world statically as shown above and benefic from compile time checks.

But since since world is a type specific to an application it cannot be known in advance, and so refered to, by any library.

To cope with this issue, a dynamic view of the world (a robocop::WorldRef) can be created and passed around, and is what most, if not all, robocop API will ask.

You will find a similar API to the actual World class but here any access is dynamic, meaning that any incorrect access will lead to run time problems.

There are two ways to obtain a WorldRef from a World:

1. implicit convertion: probably the most common case, i.e: ```cpp void func(robocop::WorldRef& world);

int main() { auto world = robocop::World{}; func(world); }

 2. calling the `World::ref()` function, i.e:
 ```cpp
  void func(robocop::WorldRef& world);

 int main() {
    auto world = robocop::World{};
    func(world.ref());
 }

You can see that the WorldRef returned by World is a reference and not a value. This is because World creates an internal WorldRef on construction and then merely gives access to it.

This allows two things:

1. Don’t waste time recreating a WorldRef each time, which is an expensive operation
2. Provide reference/pointer stability to the WorldRef, which allows code to safely hold on references to it

The second point is made possible by the fact that World is not a moveable type. If for some reason you need to move Worlds around you have to put them in a moveable container (e.g a std::unique_ptr)

Pointer stability

Since the World class is not moveable all of the data it contains will remain at fixed memory location for the duration of the whole program.

This implies that you can safely hold references and pointers to any of its subobject.

Joint groups

We mentioned joint groups previously without really explaining what they are.

Well, as the name implies, they are a bunch of joints grouped together. This groupping allows to treat the joints as a single unit and access any of their data as a single concatenated vector.

The size of these vectors will be the sum of all the degrees of freedom of the joints contained in the group.

The drawing below illustrate this principle for a joint group made of 4 joints, the first 3 have a single degree of freedom while the last one has 3.

The black boxes represent a vector data type (e.g JointPosition), the blue boxes the individual elements in that vector and the arrows show where each element is read from or written to in the joint group vector.

Joint groups can be created freely and on the fly and there are no restriction to the joints you can put inside them. A joint can appear in multiple joint groups for instance.

World update

There is an important point to remember when working with joints groups: their internal vectors are only updated when requested.

So if, for example, you don’t ask a joint group to update its JointPosition state from the world then that state will eventually become different from the actual state of the joints. This can be desirable or not depending on the use case so it’s up to the user to choose what to do.

To understand how to control this, let’s look at the JointGroupBase::Cache API provided by state(), command(), limits().lower() and limits().upper():

template <typename T>
[[nodiscard]] const typename T::template to_vector<>&
get(bool read_from_world = true) const;

template <typename T>
const typename T::template to_vector<>&
set(const T& vec, bool write_to_world = true);

template <typename T, /* ... */>
void update(const Callable& callable, bool update_world = true);

template </* ... */>
void update(const Callable& callable, bool update_world = true);

template <typename T>
void read_from_world();

template <typename T>
void write_to_world() const;

First thing to notice is that the get<T>() function returns a reference to a const vector, so you cannot do this:

auto& pos = joint_group.state().get<JointPosition>();
pos.set_random();

This is no make sure that users don’t expect their modifications to be propagated to the world automatically. For a read/modify/write operation you have to do:

// read the values from the joints, update the internal vector and save a copy of it
auto pos = joint_group.state().get<JointPosition>();

// modify it
pos.set_random();

// save it in the joint group internal vector and apply it to the joints
joint_group.state().set(pos);

The reference returned by get<T>() can be safely saved and reused (as long as the joint group lives) as it will always refer to the joint group internal vector current state, i.e:

const auto& pos = joint_group.state().get<JointPosition>();

auto new_pos = pos;
new_pos *= 2.;

joint_group.state().set(new_pos);

assert(new_pos == pos); // true

For a read/modify/write operations it’s usually simpler to use the update() function, which works with a callback function:

joint_group.state().update([](JointPosition& pos) { pos *= 2.; });

The data will be copied from the joints to the internal vector before calling the callback (with that vector passed as an argument) and vice-versa after the function returns.

All the get(), set() and update() functions take an additional optional boolean parameter to indicate if the internal vector must be read from and/or written to the world. It is true by default so that the default behavior is the expected one. But if for some reason you wish to work with the internal vector for some time (e.g doing multiple operations in a sequence) with reflecting its state with the world, you can set this boolean parameter to false.

You can use the read_from_world<T>() function to update the internal vector of a specific type (and thus all the possibly held references to that vector) with the data from the joints. write_to_world<T>() does the opposite and take the current internal vector values and dispatch them to the joints.

read_from_world<T>() and write_to_world<T>() are the functions that are called, or not, depending on the boolean parameter, so these are equivalent:

const auto& pos = joint_group.state().get<JointPosition>();
// Is the same as
joint_group.state().read_from_world<JointPosition>();
const auto& pos = joint_group.state().get<JointPosition>(false);

const auto& pos = joint_group.state().set(new_pos);
// Is the same as
joint_group.state().set(new_pos, false);
joint_group.state().write_to_world<JointPosition>();

joint_group.state().update([](JointPosition& pos) { pos *= 2.; });
// Is the same as
joint_group.state().read_from_world<JointPosition>();
joint_group.state().update([](JointPosition& pos) { pos *= 2.; }, false);
joint_group.state().write_to_world<JointPosition>();

Model

It is most likely that you will need some kind of model of your robot/world in order to use it.

By model we mean the set of algorithms that allow to compute quantities and data of interest for a controller. This can include things like body spatial position, reference frame transformations, body Jacobians, etc.

But since there is no one size fits all solution for robot modeling, there are multiple types of model provided that suit different use cases.

Currently, there are two specialization of the base Model class provided:

• KinematicTreeModel: for all tree-like structures of rigid bodies
• AUVModel: for autonomous underwater vehicles

Each controller will expect one of these model types in order to work.

As with everything in robocop, the core library doesn’t provide implementations for most of these models’ functions, just the interfaces. Separate packages are in charge of linking third-party libraries with these model interfaces (see the modeling category of the framework).

There are still some general things with regards to models that are worth mentioning here.

First, all functions (i.e algorithm) that depend on a certain state take a Model::Input parameter as the last argument, with a default value of Model::Input::State. This parameters allows to ask the algorithms to run either on the state or on the command data. Most of the times we only care about the state and so the default value is fine. But in some cases using the command data as an input can be useful, for instance to compute a body velocity that will result of a computed joint velocity command.

Then, all functions returning references (which is most of them) make use of memoization to avoid recomputing the same thing multiple times. For instance, the following code will compute the body jacobian only once:

robocop::KinematicTreeModel model = ...;

model.forward_kinematics();
const auto& j1 = model.get_body_jacobian("tcp"); // compute the jacobian and returns it
const auto& j2 = model.get_body_jacobian("tcp"); // returns previous result

model.forward_kinematics(); // will mark all previous results are obsolete
const auto& j3 = model.get_body_jacobian("tcp"); // recompute the jacobian and returns it

// The same object is returned when given the same arguments so all references point to the same thing
assert(&j1 == &j2);
assert(&j1 == &j3);

This mechanism is very useful to ensure that things are not computed multiple times for nothing, as it would quickly occur in more complex scenarios.

Lastly, there are the data update functions, all called update_xxx() (e.g update_body_poses()). These functions will recompute all related quantities that have been queried before, for instance:

robocop::KinematicTreeModel model = ...;

model.forward_kinematics();
const auto& j1 = model.get_body_jacobian("tcp1"); // compute the jacobian and returns it
const auto& j2 = model.get_body_jacobian("tcp2"); // compute the jacobian and returns it

// Save some copies
auto prev_j1 = j1;
auto prev_j2 = j2;

// Change the joint state
model.world().all_joints().state().update([](JointPosition& pos) { pos.set_random(); });

model.forward_kinematics();
model.update_body_jacobians();

// j1 and j2 now refer to the updated jacobians
assert(j1 != prev_j1);
assert(j2 != prev_j2);

This can be useful to simplify the code in some cases but prefer, especially in library code, the explicit version of always calling the get_xxx functions in order to make sure that the data you need is always up-to-date.

Controller

A controller is an algorithm that relies on a model of a robot to make it reach a certain desired state.

As with models, you will find different controller types that are suited for certain types of robots.

It is hard to define an interface for a controller that doesn’t impose limitations on the type of controller you can implement.

In robocop the choice was made to only impose flexibility in the controllers and for them to rely on of a few concepts, mainly tasks and constraints.

This is to make sure that controllers in robocop are reusable (i.e not written for a single use case) and extendable (e.g you can write a new task for controller without having to touch the controller’s code).

A task allows to configure the controller in order to try reaching a certain desired state (e.g moving a body at a given velocity).

A constraint limits what commands the controller can generate in order to ensure certain criteria (e.g make sure that the commands are admissible by the actuators).

Tasks and constraints exist in three flavors:

• joint: act on a specific joint group
• body: act on a specific body
• generic: act on something different from a single joint group or a single body (e.g system total kinetic energy, all controlled bodies for collision avoidance)

Along tasks and constraints there are configurations, which are simply a bunch of tasks and constraints bundled together for easier management.

If you want an example on how to take a very basic control law and turn it into a robocop controller, please read this page.

Tasks and constraints

At the high level, all tasks have a target and can have parameters while constraints can only have parameters.

Task target interpolation

All tasks targets have an interpolator to generate, from the user input, the actual reference used by the controller:

This interpolator mechanism is fully customizable and allows to decouple the actual target (e.g a point to reach in the world) from its reaching strategy (e.g use a specific trajectory generator).

By default, the IdentityInterpolator is used, which simply provides the user input as a reference for the controller (i.e does nothing). The core library provides several interpolators (e.g LinearInterpolator, RateLimiter, CubicConstrainedInterpolator, etc.) and other can be found in separate packages.

Tasks with a feedback loop

You will see that some tasks are based on the TaskWithFeedback class template. We won’t go in the details here but basically it allows to reuse an existing task and add a feedback loop to it to create a new task:

For a kinematic controller working at the velocity level, a typical use case is the implementation of a position task on top of a velocity task.

Implementing a task using TaskWithFeedback only requires simple and straightforward code since everything is done by that class and the base task.

Doing the same thing without it means writing a new task that interacts closely with the controller, which is more involved and requires some knowledge on how the controller works.

For such tasks, the feedback law can be set to anything deriving from FeedbackLoopAlgorithm. The core package provides common feedback loops (e.g proportional, PID, etc) but as most things in robocop, you can provide your own.

This customization point allows user to chose what feedback algorithm is best suited for their use case without the need to write and maintain multiple tasks.

Task selection matrix

All joint group and body tasks have a selection matrix to control which degree of freedom should be considered.

They both rely on the SelectionMatrix class, but the one for body tasks provide additional member functions (x(), y(), z(), rx(), ry(), rz()) to access the dofs in a clear way.

Note that it is up to the writer of a task to take into account the selection matrix as there is no way to do this automatically. Keep that in mind if you ever need to write a task yourself (that doesn’t just wrap another task, like with TaskWithFeedback).

Body task/constraint reference frame

All body tasks and constraints have a reference frame, represented by another body using ReferenceBody.

This allows users to describe their tasks/constraints references and parameters in the frame of their choice without having to perform transformations themselves.

As with selection matricies, it is up to the task, constraint and/or controller implementation to deal with this aspect in some way or another.

Subtasks and subconstraints

We will use task and subtask in this section but the same applies to constraints.

It is possible to define subtasks for tasks.

A subtask is simply any other task that will get enabled and disabled at the same time as the top level task.

This can be used when implementing a task (this is what TaskWithFeedback uses internally) or by end users to aggregate multiple tasks into a bigger one and treat it as a single task.

The creation a subtask is very similar to the creation of a task:

auto& task = controller.add_task<robocop::BodyPositionTask>("feedback pos", body, ref_body);
// configure task...

auto& subtask = task.subtasks().add<robocop::BodyVelocityTask>("feedforward vel", &controller, body, ref_body);
// configure subtask...

// Disable both task and subtask
task.disable();

And since subtasks are just tasks, they can themselves also have subtasks:

auto& subsubtask = subtask.subtasks().add<robocop::BodyAccelerationTask>("feedforward acc", &controller, body, ref_body);
// configure subsubtask...

// Disable task, subtask and subsubtask
task.enable();

// Disable subtask and subsubtask but keep task enabled
subtask.disable();

Controller configurations

We saw that tasks can have subtasks and constraints subconstraints, but what if you want to bundle both tasks and constraints together in order to manage them as a single unit?

This is what controller configurations are for.

They basically have subtasks, subconstraints and, optionally, parameters.

Some controller packages might provide configurations for certain behaviors (e.g collision avoidance with a distance constraint + a repulsive term) but you can also use an empty configuration and add things to it inside your application code:

auto& my_force_config = controller.add_configuration<robocop::EmptyConfiguration>("force");
auto& force_task = my_force_config.subtasks().add<robocop::BodyForceTask>("force feedback", body, ref_body);
// configure force_task with high gains to have a good tracking

auto& vel_limit = my_force_config.subconstraints().add<robocop::BodyVelocityConstraint>("vel limit", body, ref_body);
// configure vel_limit to not go too fast if the error is large

auto& acc_limit = my_force_config.subconstraints().add<robocop::BodyAccelerationConstraint>("acc limit", body, ref_body);
// configure acc_limit to have reasonable acceleration when the error is large

// disable the task and both constraints
my_force_config.disable();

// enable the task and both constraints
my_force_config.enable();

Control modes

In robocop, a control mode (ControlMode) is a set of control inputs (ControlInput).

Control modes are used for two things:

1. To let controllers indicate what their outputs are
2. To let users indicate in what mode should a robot operate

The first point allows for things like command adapters (used in some drivers) to operate automatically.

The second point is for when a robot has multiple operation modes (e.g joint position, joint velocity, joint torque). In this cases it is up to the user to tell which mode to select depending on their knowledge of the robot and the application.

These control modes instances can be accessed on each joint using control_mode() and controller_outputs():

joint.controller_output() = robocop::control_modes::velocity;

joint.control_mode() = robocop::control_modes::position;

They can also be found on joint groups in the form of JointGroupControlMode:

joint_group.controller_output().are_all(robocop::control_modes::velocity);
joint_group.controller_output().set_all(robocop::control_modes::velocity);

// same as joint_group.control_mode().set_all(robocop::control_modes::force);
joint_group.control_mode() = robocop::control_modes::force;
joint_group.control_mode()[2] = robocop::control_modes::position;
joint_group.control_mode().are_all_equal(); // false

Some control inputs and control modes are predefined in the core library but it is possible to define new ones. Example from robocop-kuka-lwr-driver:

namespace robocop::control_modes {

inline const ControlMode kuka_lwr_impedance =
    ControlMode{control_inputs::force, control_inputs::position,
                control_inputs::stiffness, control_inputs::damping_ratio};

}

World dynamic modifications

It is possible to dynamically add new “robots”, a URDF description of anything in fact, to the world and remove them later on. These operations are available on a WorldRef.

When adding a new robot to the world, you must first build its URDF representation using the urdf-tools library. It can be done programmatically or by loading .urdf files:

urdftools::Robot dyn_rob;

// Parse a URDF description or load a file
dyn_rob = urdftools::parse_file(PID_PATH("my_objects/tacked_object.urdf"));

// Or build the model programmatically
urdftools::Joint world_to_tracked_object;
world_to_tracked_object.name = "world_to_tracked_object";
world_to_tracked_object.type = urdftools::Joint::Type::Fixed;
world_to_tracked_object.parent = "world";
world_to_tracked_object.child = "tracked_object";
dyn_rob.joints.push_back(world_to_tracked_object);

urdftools::Link tracked_object;
tracked_object.name = "tracked_object";
tracked_object.inertial = urdftools::Link::Inertial{};
tracked_object.inertial->mass = 2_kg;
dyn_rob.links.push_back(tracked_object);

auto added_rob = world.add_robot(dyn_rob);

// You can add data on the added joints and bodies if needed
added_rob.body("tracked_object").add_state<robocop::SpatialPosition>();

// After addition, you can access the new bodies and joints as usual
world.body("tracked_object").state().update(
  [](robocop::SpatialPosition& pos){
    pos.set_random();
  }
);

// When you don't need it anymore, you can remove it from the world
world.remove_robot(added_rob);

Each time you add or remove a robot from the world, signals are emitted to inform other parts of the code that the world structure has changed. Typical subscribers of these signals are things like models, simulators and visualization tools. If you need to catch these modifications, just register callbacks like this:

world.on_robot_added([](const DynamicRobotResult& added_robot) {
  fmt::print("{} bodies and {} joints added\n", added_rob.bodies().size(), added_rob.joints().size());
});

world.on_robot_removed([](const RemovedRobot& removed_rob) {
  fmt::print("{} bodies and {} joints added\n", removed_rob.bodies().size(), removed_rob.joints().size());
});

Quantities

robocop reuses quantities from the physical-quantities library by providing type aliases and some refinements when needed, e.g (part of robocop/core/quantities.h):

namespace robocop {

// We use physical-quantities default template parameters, so using double and storing values

using Acceleration = phyq::Acceleration<>;
using Current = phyq::Current<>;
//...

// Vector quantities have dynamic sizes
using JointForce = phyq::Vector<phyq::Force>;

// Acts the same as a vector of force but allow to distinguish between the two on the type level
class JointBiasForce : public phyq::Vector<phyq::Force> {
public:
    using phyq::Vector<phyq::Force>::Vector;
    using phyq::Vector<phyq::Force>::operator=;

    template <int OtherSize = -1, typename T = double,
              phyq::Storage OtherS = phyq::Storage::Value>
    using to_vector = JointBiasForce;

    JointBiasForce(const phyq::Vector<phyq::Force>& other) : Vector{other} {
    }
};
//...

using SpatialPosition = phyq::Spatial<phyq::Position>;
//...

using LinearPosition = phyq::Linear<phyq::Position>;
//...

using AngularPosition = phyq::Angular<phyq::Position>;
//...

} // namespace robocop

Since most types are simply aliases, you can use the aliases or the phyq type interchangeably. But for consistency when using the robocop specific types (e.g JointBiasForce in the example above) prefer to use the robocop aliases when writing code inside the robocop framework.

Drivers

robocop doesn’t provide a concept for drivers and so they could be written in any way.

However, it is advised to follow the RPC methodology for writing any kind of driver as it makes sure that all drivers follow the same pattern and share the same behavior.

Also, if a new driver must be written for a robot, start by writing an RPC driver outside of robocop (tutorial here) and then wrap in for use inside robocop (tutorial here). That way you won’t have to reimplement the driver if you later need to use that robot in another framework, or even outside of any framework.