Once a controller has been developped for the first time it most probably implements a relatively limited set of tasks. Let’s take as example the controller developped during the tutorial on controller implementation: this controller for now implements only velocity tasks for joint groups and bodies.

Task customization

Every base task of a controller can be directly customized by using an interpolator. An interpolator is used to trasnform, at each time step, a task target input into a target output, which in turn is used to compute task effect (e.g. computed by compute_joint_velocity() in the tutorial). By default the indentity interpolator is used by any task (see claass IdentityInterpolator), it simply forward target input to output.

Robocop core library provides a set of interpolators that you can use to configure your tasks :

  • LinearInterpolator for simple linear interpolation using a weight (in ]0-1]) to generate interpediate values.
  • CubicInterpolator and QuinticInterpolator use 3rd and 5th order polynomial functions and a weight (in ]0-1]) to generate interpediate values.
  • CubicConstrainedInterpolator and QuinticConstrainedInterpolator same a previous but with respectively constrained first order and second order time derivatives, relative to a time step. In this case the weight is replaced by the time step.
  • RateLimiter generate a trajectory that allows to reach the target using limits on first derivative output.
  • LinearTimedInterpolator, CubicTimedInterpolator : and QuinticTimedInterpolator : timed version of corresponding interpolators which impose the trajectory to be generated in a fixed amount of time, regarding a time step.

These are only the basic interpolators and more complex ones are/can be provided by specific packages. For instance instance for an online trajectory generator with phase synchronization and max derivative constraints, reader can refer to robocop-reflexxes package.

To customize your tasks using interpolators you will have to configure them this way :

auto& velocity_task  = &controller_.add_task<robocop::BodyVelocityTask>(
    "velocity_task", my_robot_arm, robocop::ReferenceBody{arm_base},
    robocop::RootBody{arm_base});

velocity_task->target()
    .interpolator()
    .set<robocop::CubicConstrainedInterpolator>(
        robocop::SpatialAcceleration::constant(1.0, arm_base.frame()),
        controller.time_step());

Interpolator of a task is defined by changing the interpolator of its target, by calling the set function. This template function simply instanciates the interpolator corresponding to the type passed as template parameter (here CubicConstrainedInterpolator) and function arguments are passed to the constructor of the corresponding type (in the example the first derivative data defining the constraint and the time step). You can this way customize same task type with various interpolators depending on the general behavior you want to obtain.

The big benefit of RoboCoP task model is that interpolators are highly reusable accross any type of task and accross any implementation of tasks proposed by controller.

That being said, from time to time, you may also want to define a very specific interpolation for a specific task, that is obviously completely specific to your application. For that RoboCop provides the GenericInterpolator class that takes as input a function that simply computes target value from target input value. Here is an example:

auto var = rand() % 2; 
velocity_task->target()
    .interpolator()
    .set<robocop::GenericInterpolator>(
        [&var](const robocop::SpatialVelocity& input, robocop::SpatialVelocity& output){
            if(var){
                output = input
            }
            else{
                output = input/2;
            }
    });

The GenericInterpolator is typically intended to be used with lambda functions to provide an application specific behavior for a task interpolator. This lambda takes 2 arguments, input (const reference) and output (reference), whose types must match the task target type (in the example the body velocity task target is a SpatialVelocity).

Task, Constraint and Configuration composition mechanism

RoboCoP also allows to compose tasks and contraints:

  • tasks can have sub tasks
  • constraints can have sub constraints
  • configurations are set of sub tasks and sub constraints. This is basically a convenient way to (de)activate a bunch of tasks and constraints at the same time. By default configurations have no parameters or targets contrarily to constraints and tasks.

Sub tasks and sub constraints are normal tasks and constraints that are enabled when the container task/constraint/configuration is enabled and disabled when it is disabled, and while enabled they are evaluated AFTER their super task. Every controller implementation must ensure that enabling/evaluating/disabling any task, constraint or configuration ends in enabling/evaluating/disabling each of its sub tasks and/or sub constraints (using a recursive approach). To sum up, the evaluation sequence for a given task or constraint is:

1. (only for tasks) Evaluate interpolator, generates next **target output value**.
2. Perform task/constraints internal computations from its target output value and parameters values .
3. Evaluate subtasks/subconstraints recursively using same scheme.

By default task composition mechanism is thus really simple because there is no direct functionnal relationship between a container task/constraint/configuration and its contained sub tasks and/or sub constraints except (de)activation coupling. In other words a container is just a container, nothing more. Obviously, each controller implementation should provide at least a generic configuration that can just be used to activate a bunch of other tasks and constraints, this later is by default called EmptyConfiguration. Obviously the pratical utility of such a process is questionable because:

  • you can easily replace it with a function call that enable/disable corresponding subtasks “by hand”.
  • most of time when using a task or constraints we need to set its parameters and/or target. With the composition mechanism this would force us to have a look inside the container configuration and finally do the same as if we were directly using corresponding sub tasks. Anyway we advise to specialize EmptyConfiguration for each controller so that users have a homogeneous way to deal with tas:s/constraints composition at application development time.

But a controller may also provide some composite tasks/constraints/configurations types, like for instance the CollisionAvoidanceConfiguration defined in the robocop-qp-controller package. This configuration agregates a CollisionConstraint (which ensures that the controller solution will not contain collisions) and a CollisionRepulsionTask that moves, as far as possible, the joint links away from collisions. This time, to ensure consistency of both sub elements, the parameters of the configuration (those of the processor computing possible collisions and collision distances) must be transmitted to both constraint and task. This transmission is performed in the on_update() function of the configuration.

On can also propose to define new meta tasks, based on the composition mechanism. The mechanism of tasks with feedback, explained in next section, is a typical example.

Tasks with feedback

Now let’s suppose we want to implement new tasks, for example joint position tasks. Basically we have two options: implement them directly in the controller or using the task composition mechanism. The first option is not preferred because it would force us to add more specific stuff directly in the controler to deal with position tasks: basically a way to compute velocities from positions. Robocop offers a better way to achieve that using tasks with feedback.

Tasks with feedback (TWB for short) is a specific meta type of tasks that use the composition mechanism presented before. A TWB is defined from an existing task type and a target type. For instance to implement the position task, we define a task that JointPosition as target type and use the already defined velocity task:

#pragma once

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

#include <robocop/core/tasks/joint/position.h>
#include <robocop/core/task_with_feedback.h>

namespace robocop {

class TutorialJointPositionTask final
    : public robocop::TaskWithFeedback<JointPosition,
                                       TutorialJointVelocityTask> {
public:
    TutorialJointPositionTask(TutorialController* controller,
                              JointGroupBase& joint_group): 
       TaskWithFeedback{Target{JointPosition{phyq::zero, joint_group.dofs()}},
                       controller, joint_group}{}
private:
    void compute_joint_velocity(JointVelocity& joint_velocity) override final{
        joint_velocity.set_zero();
    }
};

template <>
struct JointPositionTask<TutorialController> {
    using type = TutorialJointPositionTask;
};

} // namespace robocop

Now the new task type inherits from TaskWithFeedback. What is important here are the template parameters:

  • JointPosition is the type of the new task target.
  • TutorialJointVelocityTask is the type of the subtask used to implement the new task.

The function compute_joint_velocity() simply generate a null velocity because it will not directly contribute to generate the command. Only its subtask will generate an adequate command.

The basic idea is that a TWB is configured with a feedback loop that implements a specific feedback controller. The TWB :

  • automatically create a subtask with corresponding type (here TutorialJointVelocityTask).
  • ensures that all its properties (selection matrix, reference in case of body tasks) are correctly forwarded to its subtask at each time step (when ots update() function is called)
  • replace the sub task interpolator with a specific interpolator, that calls the feedback loop of the TWB. The feedback loop takes the TWB’s target output (output of TWB interpolator) and compute the sub task’s target input based on the chose algorithm.

One may notice that no feedback algorithm appears here and it is perfectly normal. Like interpolators feedback algorithms are chose at the very last moment, when instancianting TWB, for instance:

auto& position_task  = controller_.add_task<robocop::JointPositionTask>(
    "position_task", my_robot_arm_joints);

position_task.set_feedback<robocop::ProportionalFeedback>()
        .gain()
        .set_contant(10);

The set_feedback is used to configrue the feedback algorithm used, here a proportional controller, by specifying the Feedback algorithm type (here ProportionalFeedback). Then this function returns a reference to the feedback loop that you can use to tune some parameters (in the example proportional gain is set to a constant value of 10).

Same as for interpolators, what is important here is that the feedback algorithms because highly reusable accross mostly anykind of task and controllers. Core library of robocop provides basic feedback algorithms (proportional, integral, PID, and their saturated version) but user can define new ones as needed.

New composite meta tasks

For now the TWB is the only composite meta task provided in robocop-core but new ones may be designed. We can for instance imagine a meta task that accept a set of subtasks and that progressively ponderates weight of its subtasks to switch from one sub task to the other along time. This could be a way to implement smooth transition between a given set of subtasks in charge of controling the same body or the same joint group but using different modalities (for instance using different sensors). The meta task could configure each subtask with a specific interpolator that gives the ponderation (in [0-1]) to apply to the given sub task’s weight, the sum of all ponderations being 1. This time the meta task could be configure not with a controller but with a weight distributon strategy.

Another composite meta task could be a visual servoing task whose target can be any kind of feature vector and which call a function that implements interaction matrix computation (whose dimensions are related to the target) and that finally computes the next body position, velocity, acceleration or force, depending on the subtask target type. The pattern is quite close to the TWB but this time can probably be imagined only for body tasks, even if from a pure technical point of view dealing with joint group tasks would not be very diffult to implement.

The main challenge in designing new meta-task is to abstract away from a specific controller or a specific application, in which case a basic composite task would be sufficient. Remember that the main intent of composite meta task is to provide a common pattern for various types of tasks that can be used in various situation wit variosu data types for targets. This genericity is more complicated to invent than to implement.