sot.hh

Go to the documentation of this file.

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

#ifndef __SOT_SOT_HH
#define __SOT_SOT_HH

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

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

/* Classes standards. */
#include <list> /* Classe std::list   */

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

#include <sot/core/flags.hh>
#include <sot/core/task-abstract.hh>

/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#ifndef SOTSOT_CORE_EXPORT
#if defined(WIN32)
#if defined(sot_EXPORTS)
#define SOTSOT_CORE_EXPORT __declspec(dllexport)
#else
#define SOTSOT_CORE_EXPORT __declspec(dllimport)
#endif
#else
#define SOTSOT_CORE_EXPORT
#endif
#endif

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

namespace dynamicgraph {
namespace sot {

class SOTSOT_CORE_EXPORT Sot : public Entity {
 public:
  static const std::string CLASS_NAME;

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

  typedef std::list<TaskAbstract *> StackType;

 protected:
  StackType stack;

  unsigned int nbJoints;

  bool enablePostureTaskAcceleration;

  double maxControlIncrementSquaredNorm;

 public:
  static const double INVERSION_THRESHOLD_DEFAULT;  // = 1e-4;

  /*   static const double DIRECTIONAL_THRESHOLD_DEFAULT = 1e-2; */
  /*   static const bool USE_CONTI_INVERSE_DEFAULT = false; */

  static void taskVectorToMlVector(const VectorMultiBound &taskVector,
                                   Vector &err);

 public:
  Sot(const std::string &name);
  ~Sot(void) { /* TODO!! */
  }

  virtual const StackType &tasks() const { return stack; }

  virtual void push(TaskAbstract &task);
  virtual TaskAbstract &pop(void);

  virtual bool exist(const TaskAbstract &task);

  virtual void remove(const TaskAbstract &task);

  virtual void removeDependency(const TaskAbstract &key);

  virtual void up(const TaskAbstract &task);

  virtual void down(const TaskAbstract &task);

  virtual void clear(void);
  virtual void defineNbDof(const unsigned int &nbDof);
  virtual const unsigned int &getNbDof() const { return nbJoints; }

 public: /* --- CONTROL --- */
  virtual dynamicgraph::Vector &computeControlLaw(dynamicgraph::Vector &control,
                                                  const int &time);

 public: /* --- DISPLAY --- */
  virtual void display(std::ostream &os) const;
  SOTSOT_CORE_EXPORT friend std::ostream &operator<<(std::ostream &os,
                                                     const Sot &sot);
 public: /* --- SIGNALS --- */
  SignalPtr<dynamicgraph::Vector, int> q0SIN;
  SignalPtr<dynamicgraph::Matrix, int> proj0SIN;
  SignalPtr<double, int> inversionThresholdSIN;
  SignalTimeDependent<dynamicgraph::Vector, int> controlSOUT;
  virtual std::ostream &writeGraph(std::ostream &os) const;
};
}  // namespace sot
}  // namespace dynamicgraph

#endif /* #ifndef __SOT_SOT_HH */