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;
 
  size_type 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 size_type &nbDof);
  virtual const size_type &getNbDof() const { return nbJoints; }
 
 public: /* --- CONTROL --- */
  virtual dynamicgraph::Vector &computeControlLaw(dynamicgraph::Vector &control,
                                                  const sigtime_t &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, sigtime_t> q0SIN;
  SignalPtr<dynamicgraph::Matrix, sigtime_t> proj0SIN;
  SignalPtr<double, sigtime_t> inversionThresholdSIN;
  SignalTimeDependent<dynamicgraph::Vector, sigtime_t> controlSOUT;
  virtual std::ostream &writeGraph(std::ostream &os) const;
};
}  // namespace sot
}  // namespace dynamicgraph
 
#endif /* #ifndef __SOT_SOT_HH */