C++ implementation

Introduction

New entity types are defined by

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:

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

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.

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.

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:

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