sot-core  4.11.2
Hierarchical task solver plug-in for dynamic-graph.
sot.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2010,
3  * François Bleibel,
4  * Olivier Stasse,
5  *
6  * CNRS/AIST
7  *
8  */
9 
10 #ifndef __SOT_SOT_HH
11 #define __SOT_SOT_HH
12 
13 /* --------------------------------------------------------------------- */
14 /* --- INCLUDE --------------------------------------------------------- */
15 /* --------------------------------------------------------------------- */
16 
17 /* Matrix */
18 #include <dynamic-graph/linear-algebra.h>
19 
20 /* Classes standards. */
21 #include <list> /* Classe std::list */
22 
23 /* SOT */
24 #include <dynamic-graph/entity.h>
25 #include <sot/core/flags.hh>
27 
28 /* --------------------------------------------------------------------- */
29 /* --- API ------------------------------------------------------------- */
30 /* --------------------------------------------------------------------- */
31 
32 #ifndef SOTSOT_CORE_EXPORT
33 #if defined(WIN32)
34 #if defined(sot_EXPORTS)
35 #define SOTSOT_CORE_EXPORT __declspec(dllexport)
36 #else
37 #define SOTSOT_CORE_EXPORT __declspec(dllimport)
38 #endif
39 #else
40 #define SOTSOT_CORE_EXPORT
41 #endif
42 #endif
43 
44 /* --------------------------------------------------------------------- */
45 /* --- CLASS ----------------------------------------------------------- */
46 /* --------------------------------------------------------------------- */
47 
48 namespace dynamicgraph {
49 namespace sot {
50 
56 class SOTSOT_CORE_EXPORT Sot : public Entity {
57 public:
59  static const std::string CLASS_NAME;
60 
61 public:
63  virtual const std::string &getClassName() const { return CLASS_NAME; }
64 
66  typedef std::list<TaskAbstract *> StackType;
67 
68 protected:
71  StackType stack;
72 
75  unsigned int nbJoints;
76 
80 
88 
89 public:
91  static const double INVERSION_THRESHOLD_DEFAULT; // = 1e-4;
92 
93  /* static const double DIRECTIONAL_THRESHOLD_DEFAULT = 1e-2; */
94  /* static const bool USE_CONTI_INVERSE_DEFAULT = false; */
95 
97  static void taskVectorToMlVector(const VectorMultiBound &taskVector,
98  Vector &err);
99 
100 public:
102  Sot(const std::string &name);
103  ~Sot(void) { /* TODO!! */
104  }
105 
109  virtual const StackType &tasks() const { return stack; }
110 
115  virtual void push(TaskAbstract &task);
120  virtual TaskAbstract &pop(void);
121 
123  virtual bool exist(const TaskAbstract &task);
124 
128  virtual void remove(const TaskAbstract &task);
129 
132  virtual void removeDependency(const TaskAbstract &key);
133 
136  virtual void up(const TaskAbstract &task);
137 
140  virtual void down(const TaskAbstract &task);
141 
143  virtual void clear(void);
150  virtual void defineNbDof(const unsigned int &nbDof);
151  virtual const unsigned int &getNbDof() const { return nbJoints; }
152 
154 public: /* --- CONTROL --- */
161  virtual dynamicgraph::Vector &computeControlLaw(dynamicgraph::Vector &control,
162  const int &time);
163 
166 public: /* --- DISPLAY --- */
171  virtual void display(std::ostream &os) const;
173  SOTSOT_CORE_EXPORT friend std::ostream &operator<<(std::ostream &os,
174  const Sot &sot);
176 public: /* --- SIGNALS --- */
184  SignalPtr<dynamicgraph::Vector, int> q0SIN;
190  SignalPtr<dynamicgraph::Matrix, int> proj0SIN;
193  SignalPtr<double, int> inversionThresholdSIN;
195  SignalTimeDependent<dynamicgraph::Vector, int> controlSOUT;
200  virtual std::ostream &writeGraph(std::ostream &os) const;
201 };
202 } // namespace sot
203 } // namespace dynamicgraph
204 
205 #endif /* #ifndef __SOT_SOT_HH */
~Sot(void)
Definition: sot.hh:103
SignalTimeDependent< dynamicgraph::Vector, int > controlSOUT
Allow to get the result of the computed control law.
Definition: sot.hh:195
SOT_CORE_EXPORT std::ostream & operator<<(std::ostream &os, const VectorMultiBound &v)
std::vector< MultiBound > VectorMultiBound
Definition: multi-bound.hh:69
virtual const unsigned int & getNbDof() const
Definition: sot.hh:151
SignalPtr< double, int > inversionThresholdSIN
This signal allow to change the threshold for the damped pseudo-inverse on-line.
Definition: sot.hh:193
double maxControlIncrementSquaredNorm
Maximum allowed squared norm of control increment. A task whose control increment is above this value...
Definition: sot.hh:87
SignalPtr< dynamicgraph::Vector, int > q0SIN
Intrinsec velocity of the robot, that is used to initialized the recurence of the SOT (e...
Definition: sot.hh:184
SignalPtr< dynamicgraph::Matrix, int > proj0SIN
A matrix K whose columns are a base of the desired velocity. In other words, where is the free para...
Definition: sot.hh:190
bool enablePostureTaskAcceleration
Option to disable the computation of the SVD for the last task if this task is a Task with a single F...
Definition: sot.hh:79
std::list< TaskAbstract * > StackType
Defines a type for a list of tasks.
Definition: sot.hh:66
Definition: task-abstract.hh:48
StackType stack
This field is a list of controllers managed by the stack of tasks.
Definition: sot.hh:71
unsigned int nbJoints
Store the number of joints to be used in the command computed by the stack of tasks.
Definition: sot.hh:75
static const std::string CLASS_NAME
Specify the name of the class entity.
Definition: sot.hh:59
virtual const StackType & tasks() const
Definition: sot.hh:109
This class implements the Stack of Task. It allows to deal with the priority of the controllers throu...
Definition: sot.hh:56
static const double INVERSION_THRESHOLD_DEFAULT
Threshold to compute the dumped pseudo inverse.
Definition: sot.hh:91
Definition: abstract-sot-external-interface.hh:17
#define SOTSOT_CORE_EXPORT
Definition: sot.hh:40
virtual const std::string & getClassName() const
Returns the name of this class.
Definition: sot.hh:63