Adding a new task/constraint type

(1) Define an abstract interface

First, we will just define the programmatic API of our task. This is done by defining a pure abstract class with no constructor or implementation, just pure virtual methods. This class will inherit from TaskDescription in order to be recognised as a task, or from ConstraintDescription if it’s a constraint.

#ifndef ANGULARMOMENTUM_H
#define ANGULARMOMENTUM_H

#include <cartesian_interface/sdk/problem/Task.h>

namespace XBot { namespace Cartesian {

/**
 * @brief The AngularMomentum class models the abstract
 * interface for the task.
 *
 * NOTE: it is mandatory to inherit **virtually** from TaskDescription
 */
class AngularMomentum : public virtual TaskDescription
{

public:

    CARTESIO_DECLARE_SMART_PTR(AngularMomentum)

    /* Define the task API (pure virtual methods) */
    virtual Eigen::Vector3d getReference() const = 0;
    virtual void setReference(const Eigen::Vector3d& lref) = 0;

};

(2) Declare an implementation class

/**
 * @brief The AngularMomentumImpl class implements the abstract
 * interface
 */
class AngularMomentumImpl : public TaskDescriptionImpl,
                            public virtual AngularMomentum
{

public:

    CARTESIO_DECLARE_SMART_PTR(AngularMomentumImpl)

    /* The task implementation constructor signature must be
     * as follows */
    AngularMomentumImpl(YAML::Node task_node,
                        Context::ConstPtr context);

    /* Implement the task API */
    Eigen::Vector3d getReference() const override;
    void setReference(const Eigen::Vector3d& lref) override;

    /* Customize update, reset and log */
    void update(double time, double period) override;
    void reset() override;
    void log(MatLogger2::Ptr logger, bool init_logger, int buf_size) override;

private:

    static constexpr double REF_TTL = 1.0;

    Eigen::Vector3d _lref;
    double _ref_timeout;
};

} }

#endif // ANGULARMOMENTUM_H

(3) Implement all methods

The constructor signature must match the following one: every task is constructed by CartesIO in a uniform way, with the relevant YAML node from the problem description file, and a const share pointer to a context object. You must call the TaskDescriptionImpl constructor additionally specifying

• the task name (a specific name, different for all instances, e.g. “visual_servo_left_hand”)

• the task size (this will set the weight and indices size)

The constructor is also the place where you parse custom parameters from the given YAML. The base class will do the same jobs for all common Task parameters.

#include "AngularMomentum.h"

#include "fmt/format.h"

using namespace XBot::Cartesian;


AngularMomentumImpl::AngularMomentumImpl(YAML::Node task_node,
                                         Context::ConstPtr context):
    TaskDescriptionImpl(task_node,
                        context,
                        "AngularMomentum",
                        3),
    _lref(0,0,0),
    _ref_timeout(-1)
{
    /* Here you can parse custom YAML fields from task_node */

}

In this example, we implement a reference timeout as reference management logic. This way, if the user stops providing references, the desired value drops to zero automatically.

Eigen::Vector3d AngularMomentumImpl::getReference() const
{
    return _lref;
}

void AngularMomentumImpl::setReference(const Eigen::Vector3d& lref)
{
    _lref = lref;
    _ref_timeout = getTime() + REF_TTL;
}

Note

All methods which are overridden from the base class TaskDescriptionImpl must call the base class method!

void XBot::Cartesian::AngularMomentumImpl::update(double time, double period)
{
    // call base class
    TaskDescriptionImpl::update(time, period);

    // if the last reference has expired, the set it to zero
    if(time > _ref_timeout) _lref.setZero();

}

void XBot::Cartesian::AngularMomentumImpl::reset()
{
    // call base class
    TaskDescriptionImpl::reset();

    _lref.setZero();
}

void XBot::Cartesian::AngularMomentumImpl::log(MatLogger2::Ptr logger,
                                               bool init_logger,
                                               int buf_size)
{
    // call base class
    TaskDescriptionImpl::log(logger, init_logger, buf_size);

    if(init_logger)
    {
        logger->create(getName() + "_ref", 3, 1, buf_size);
        return;
    }

    logger->add(getName() + "_ref", _lref);
}

(4) Register the plugin

CARTESIO_REGISTER_TASK_PLUGIN(AngularMomentumImpl, AngularMomentum)

Note

All *REGISTER* macros take as second argument the task name that will be specified by users inside the type yaml field. In this case, the OpenSot adapter will correspond to a task with type: AngularMomentum