feature-abstract.hh

Go to the documentation of this file.

/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */

#ifndef __SOT_FEATURE_ABSTRACT_H__
#define __SOT_FEATURE_ABSTRACT_H__

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* Matrix */
#include <dynamic-graph/linear-algebra.h>

/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>

#include <sot/core/flags.hh>
#include <sot/core/pool.hh>

#include "sot/core/api.hh"

/* STD */
#include <string>

namespace dynamicgraph {
namespace sot {

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

class SOT_CORE_EXPORT FeatureAbstract : public Entity {
 public:
  static const std::string CLASS_NAME;

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

  void featureRegistration(void);

  void initCommands(void);

 public:
  FeatureAbstract(const std::string &name);
  virtual ~FeatureAbstract(void){};

  virtual unsigned int &getDimension(unsigned int &res, int time) = 0;

  inline unsigned int getDimension(int time) {
    unsigned int res;
    getDimension(res, time);
    return res;
  }

  inline unsigned int getDimension(void) const {
    return dimensionSOUT.accessCopy();
  }
  virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res,
                                             int time) = 0;

  virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res,
                                                int time) = 0;

  virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res,
                                                int time);

  /* --- SIGNALS ------------------------------------------------------------ */
 public:
  SignalPtr<Flags, int> selectionSIN;

  SignalPtr<dynamicgraph::Vector, int> errordotSIN;

  SignalTimeDependent<dynamicgraph::Vector, int> errorSOUT;

  SignalTimeDependent<dynamicgraph::Vector, int> errordotSOUT;

  SignalTimeDependent<dynamicgraph::Matrix, int> jacobianSOUT;

  SignalTimeDependent<unsigned int, int> dimensionSOUT;

  virtual std::ostream &writeGraph(std::ostream &os) const;

  virtual SignalTimeDependent<dynamicgraph::Vector, int> &getErrorDot() {
    return errordotSOUT;
  }

  /* --- REFERENCE VALUE S* ------------------------------------------------- */
 public:
  virtual void setReference(FeatureAbstract *sdes) = 0;
  virtual void unsetReference(void) { setReference(NULL); }
  virtual const FeatureAbstract *getReferenceAbstract(void) const = 0;
  virtual FeatureAbstract *getReferenceAbstract(void) = 0;
  virtual bool isReferenceSet(void) const { return false; }

  virtual void addDependenciesFromReference(void) = 0;
  virtual void removeDependenciesFromReference(void) = 0;

  /* Commands for bindings. */
  void setReferenceByName(const std::string &name);
  std::string getReferenceByName(void) const;
};

template <class FeatureSpecialized>
class FeatureReferenceHelper {
  FeatureSpecialized *ptr;
  FeatureAbstract *ptrA;

 public:
  FeatureReferenceHelper(void) : ptr(NULL) {}

  void setReference(FeatureAbstract *sdes);
  // void setReferenceByName( const std::string & name );
  void unsetReference(void) { setReference(NULL); }
  bool isReferenceSet(void) const { return ptr != NULL; }
  FeatureSpecialized *getReference(void) { return ptr; }
  const FeatureSpecialized *getReference(void) const { return ptr; }
};

template <class FeatureSpecialized>
void FeatureReferenceHelper<FeatureSpecialized>::setReference(
    FeatureAbstract *sdes) {
  ptr = dynamic_cast<FeatureSpecialized *>(sdes);
  ptrA = ptr;
}

#define DECLARE_REFERENCE_FUNCTIONS(FeatureSpecialized)             \
  typedef FeatureReferenceHelper<FeatureSpecialized> SP;            \
  virtual void setReference(FeatureAbstract *sdes) {                \
    if (sdes == NULL) {                                             \
      /* UNSET */                                                   \
      if (SP::isReferenceSet()) removeDependenciesFromReference();  \
      SP::unsetReference();                                         \
    } else {                                                        \
      /* SET */                                                     \
      SP::setReference(sdes);                                       \
      if (SP::isReferenceSet()) addDependenciesFromReference();     \
    }                                                               \
  }                                                                 \
  virtual const FeatureAbstract *getReferenceAbstract(void) const { \
    return SP::getReference();                                      \
  }                                                                 \
  virtual FeatureAbstract *getReferenceAbstract(void) {             \
    return (FeatureAbstract *)SP::getReference();                   \
  }                                                                 \
  bool isReferenceSet(void) const { return SP::isReferenceSet(); }  \
  virtual void addDependenciesFromReference(void);                  \
  virtual void removeDependenciesFromReference(void)
/* END OF define DECLARE_REFERENCE_FUNCTIONS */

#define DECLARE_NO_REFERENCE                                           \
  virtual void setReference(FeatureAbstract *) {}                      \
  virtual const FeatureAbstract *getReferenceAbstract(void) const {    \
    return NULL;                                                       \
  }                                                                    \
  virtual FeatureAbstract *getReferenceAbstract(void) { return NULL; } \
  virtual void addDependenciesFromReference(void) {}                   \
  virtual void removeDependenciesFromReference(void) {}                \
  /* To force a ; */ bool NO_REFERENCE
/* END OF define DECLARE_REFERENCE_FUNCTIONS */

}  // namespace sot
}  // namespace dynamicgraph

#endif  // #ifndef __SOT_FEATURE_ABSTRACT_H__