C++ implementation

Introduction

New entity types are defined by

• deriving dynamicgraph::Entity class,
• adding signals, and
• adding commands. As an example, we review below class dynamicgraph::tutorial::InvertedPendulum.

Interface

The interface is defined in file include/dynamic-graph/tutorial/inverted-pendulum.hh.

First, we include

• the header defining Entity class and
• the header defining SignalPtr template class,
• the header defining matrix and vector types:

#include <dynamic-graph/entity.h>
#include <dynamic-graph/linear-algebra.h>
#include <dynamic-graph/signal-ptr.h>

Then in namespace dynamicgraph::tutorial we define class InvertedPendulum

namespace dynamicgraph {
  namespace tutorial {
    class InvertedPendulum : public Entity
    {

with a constructor taking a name as an input

InvertedPendulum(const std::string& inName);

For the internal machinery, each entity can provide the name of the class it belongs to:

virtual const std::string& getClassName (void) const {
  return CLASS_NAME;
}

Class InvertedPendulum represents a dynamical system. The following method integrates the equation of motion over a time step

void incr(double inTimeStep);

Setters and getters will enable us later to control parameters through commands.

void setCartMass (const double& inMass) {
  cartMass_ = inMass;
}

double getCartMass () const {
  return cartMass_;
}

void setPendulumMass (const double& inMass) {
  pendulumMass_ = inMass;
}

double getPendulumMass () const {
 return pendulumMass_;
}

void setPendulumLength (const double& inLength) {
  pendulumLength_ = inLength;
}

double getPendulumLength () const {
  return pendulumLength_;
}

The name of the class is stored as a static member

static const std::string CLASS_NAME;

In the private part of the class, we store signals

SignalPtr< double, int > forceSIN;
Signal< Vector, int> stateSOUT;

and parameters

double cartMass_;
double pendulumMass_;
double pendulumLength_;
double viscosity_;

Implementation

The implementation is written in file src/inverted-pendulum.cc.

First, we include headers defining

• class FactoryStorage,
• general setter and getter commands
• the previously defined header,
• local Increment command class, and
• gravity constant:

Headers

#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/factory.h>

#include "command-increment.hh"
#include "dynamic-graph/tutorial/inverted-pendulum.hh"

dg_tutorial_inverted_pendulum_cxx_implementation_entity_registration

Entity registration

The second step consists in

• registering our new class into the entity factory and
• instantiating the static variable CLASS_NAME

using a macro defined in dynamic-graph/factory.h:

DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(InvertedPendulum, "InvertedPendulum");

Note

The two parameters of the macros are respectively

• the C++ type of the new Entity,
• the name of the type of the corresponding python class. It is highly recommended to use the same name for both.

dg_tutorial_inverted_pendulum_cxx_implementation_constructor

Constructor

Then we define the class constructor

• passing the instance name to Entity class constructor,
• initializing signals with a string following the specified format and
• initializing parameters with default values:

InvertedPendulum::InvertedPendulum(const std::string& inName) :
  Entity(inName),
  forceSIN(NULL, "InvertedPendulum("+inName+")::input(vector)::force"),
  stateSOUT("InvertedPendulum("+inName+")::output(vector)::state"),
  cartMass_(1.0), pendulumMass_(1.0), pendulumLength_(1.0), viscosity_(0.1)

We register signals into an associative array stored into Entity class

signalRegistration (forceSIN);
signalRegistration (stateSOUT);

We set input and output signal as constant with a given value

Vector state = boost::numeric::ublas::zero_vector<double>(4);
double input = 0.;
stateSOUT.setConstant(state);
forceSIN.setConstant(input);

The following lines of code define and register commands into the entity. A command is created by calling a constructor with

• a string: the name of the command,
• a pointer to a newly created command and
• a string documenting the command:

std::string docstring;

// Incr
docstring =
  "\n"
  "    Integrate dynamics for time step provided as input\n"
  "\n"
  "      take one floating point number as input\n"
  "\n";
addCommand(std::string("incr"),
           new command::Increment(*this, docstring));

In this example, command::Increment is a command specific to our class InvertedPendulum. This new command is explained in page Creating a new command.

Setter and getter commands are available through classes templated by the type of entity using the command and the type of the parameter. Be aware that only a prespecified set of types are supported for commands, see class dynamicgraph::command::Value.

docstring =
   "\n"
   "    Set cart mass\n"
   "\n";
 addCommand(std::string("setCartMass"),
            new ::dynamicgraph::command::Setter<InvertedPendulum, double>
            (*this, &InvertedPendulum::setCartMass, docstring));

 docstring =
   "\n"
   "    Get cart mass\n"
   "\n";
 addCommand(std::string("getCartMass"),
            new ::dynamicgraph::command::Getter<InvertedPendulum, double>
            (*this, &InvertedPendulum::getCartMass, docstring));

Note

It is important to notice that

• commands passed to method Entity::addCommand will be destroyed automatically by Entity class destructor. The user should therefore not destroy them,
• commands should be defined and registered in the class constructor. Commands defined later on will not be reachable by python bindings.

dg_tutorial_inverted_pendulum_cxx_implementation_newtypes

Registering new types: advanced feature

Signals are templated by the type of data they convey. In this example, we hae defined our own class of vectors InvertedPendulum::Vector. In order to be able to create signals with this type, we need to register the new type:

dynamicgraph::DefaultCastRegisterer<InvertedPendulum::Vector> IPVectorCast;

Note

The new type should implement operator<< and operator>> in order to store variables in streams.