Preliminaries

Given a task \( f \circ rhs \) where \( f \) is a function, \( \circ \) is one of the following operators: \( \leq, = , \geq \) and \( rhs \) is vector, we define the task error function \( e = f - rhs \).
The goal of a task dynamics is to specify the desired value of one time derivative of \( e \), such that the value of \( f \) stays in a region that fulfills the task or converge to it.

In this example we will see how to implement the task dynamics

\( \dot{e}^* = -k_p(e) e \qquad (1) \)

that is a proportional-like dynamics with an adaptive gain, where

\( k_p(e) = a \exp(-b \left\|e\right\|) + c \qquad (2) \)

To keep it simple \( a \), \( b \), \( c \) and thus \( k_p \) will be scalars.

This task dynamics computes a desired first-order time derivative of the error function \( e \). We say its order is one.

Implementation outline

From the user point of view, a task dynamics can be just specified by its type and parameters (e.g. adaptive proportional with gain a, b and c in this example). This can be done independently of the task \( f \circ rhs \) with which it will be used. From the computational point of view, the task dynamics needs to be instantiated as a node of the computation graph taking the value and possibly the derivatives of \(f \) as an input. This step can only be done if the task is known.

At the code level, this duality of viewpoints translates into the implementation of two classes:

A user-dedicated class, deriving from tvm::task_dynamics::abstract::TaskDynamics that acts as a lightweight description of the task and a factory for
A computation-related class, deriving from tvm::task_dynamics::abstract::TaskDynamicsImpl that implements a node of the computation graph.

We will call AdaptiveProportional the user-dedicated class. It simply needs to store a, b and c, and is required to override the two following methods

Order order_() const

std::unique_ptr<TaskDynamicsImpl> impl_(FunctionPtr f, constraint::Type t, const Eigen::VectorXd& rhs) const

The first one has to return the task dynamics order (one, in our case), the second creates an instance of the computation-related class.

The second class could be implemented separately of the first but the convention taken in TVM is to make it a subclass of the first, with the name Impl. A few other classes rely on this convention such as tvm::task_dynamics::Clamped, that would not be compatible with our example if we didn't do so. This second class overrides the method

void updateValue()

which implements the computation of the desired error derivative, i.e. the formula (1) and (2) for our example. For that it will need to store store a, b and c as well.

Implementation details

As a direct transcription of the outline above, our AdaptiveProportional has the following declaration (assuming the proper namespaces):

class AdaptiveProportional : public abstract::TaskDynamics
{
  public:
    class Impl : public task_dynamics::abstract::TaskDynamicsImpl { ... };
 
    AdaptiveProportional(double a, double b, double c);
 
  protected:
    task_dynamics::Order order_() const override;
    std::unique_ptr<task_dynamics::abstract::TaskDynamicsImpl> impl_(FunctionPtr f, constraint::Type t, const Eigen::VectorXd& rhs) const override;
 
     TASK_DYNAMICS_DERIVED_FACTORY(a_, b_, c_)
 
  private:
    double a_, b_, c_;
};

(The impl class is described below)

The implementation is straightforward:

the contructor trivially assigns a, b and c to the relevant fields

AdaptiveProportional::AdaptiveProportional(double a, double b, double c)
 : a_(a), b_(b), c_(c)
{
}

order_() simply returns tvm::task_dynamics::Order::One.

impl_ is given the description of the task \( f \circ rhs \) through parameters f, t and rhs. From these and fields a_, b_, c_, it constructs a std::unique_ptr on AdaptiveProportional::Impl:

std::unique_ptr<task_dynamics::abstract::TaskDynamicsImpl> AdaptiveProportional::impl_(FunctionPtr f, constraint::Type t, const Eigen::VectorXd& rhs) const
{
  return std::make_unique<Impl>(f, t, rhs, a_, b_, c_);
}

TASK_DYNAMICS_DERIVED_FACTORY(a_, b_, c_) is required to enable the use of our new task dynamic in a composable task dynamic (for example, to create a Clamped<AdaptiveProportional> dynamic). The arguments provided to the macro are the arguments that are required by AdaptiveProportional::Impl

The Impl class is declared as

class Impl : public task_dynamics::abstract::TaskDynamicsImpl
{
public:
  Impl(FunctionPtr f, constraint::Type t, const Eigen::VectorXd & rhs, double a, double b, double c);
  void updateValue() override;
 
private:
  double a_, b_, c_;
};

Once again, the constructor is straightforward. The main attention point is that it needs to call the constructor of TaskDynamicsImpl to pass it not only the parameters f, t and rhs, but also the order of the task dynamics.

AdaptiveProportional::Impl::Impl(FunctionPtr f, constraint::Type t, const Eigen::VectorXd& rhs, double a, double b, double c)
  : TaskDynamicsImpl(task_dynamics::Order::One, f,t,rhs), a_(a), b_(b), c_(c)
{
}

The updateValue() method is the heart of the implementation. It will be called by the computation graph to update the value of the TaskDynamicsImpl instance (accessible through the value() method). Within methods of Impl, we have access to the task parameters through the function(), type() and rhs() methods. Furthermore, we have access to the value_ member, which is where to put the output of our computations.
The implementation is as follows

void AdaptiveProportional::Impl::updateValue()
{
  value_ = function().value() - rhs();
  double kp = a_ * exp(-b_ * value_.norm()) + c_;
  value_ *= -kp;
}

The first line stores (temporarily) the value of the error function \( e \) in value_. The second line computes the adaptive gain according to (2). The last line computes the desired error velocity as in (1) and store it in value_, as required.

To go further

On this example

The above implementation is a minimal working example, and could be improved or extended in several ways:

Checking the validity of the input parameters a_, b_, c_ in the constructor of Impl. These parameters should be non-negative. It would be even be better to check the validity also in the constructor of AdaptiveProportional, to report errors as early as possible, and at a place that will be more user-friendly.
Adding getter and setters for the parameters (with the required checks).
Having a and c be possibly diagonal matrices (represented by a vector) or full matrices. This would require additionnal checks on the vector/matrix sizes with respect to the size of \( f \).

General notes

Here are some points to keep in minds when implementing new task dynamics: