1 |
|
|
/* |
2 |
|
|
* Copyright 2010, |
3 |
|
|
* François Bleibel, |
4 |
|
|
* Olivier Stasse, |
5 |
|
|
* |
6 |
|
|
* CNRS/AIST |
7 |
|
|
* |
8 |
|
|
*/ |
9 |
|
|
|
10 |
|
|
#ifndef __SOT_TASK_H__ |
11 |
|
|
#define __SOT_TASK_H__ |
12 |
|
|
|
13 |
|
|
/* --------------------------------------------------------------------- */ |
14 |
|
|
/* --- INCLUDE --------------------------------------------------------- */ |
15 |
|
|
/* --------------------------------------------------------------------- */ |
16 |
|
|
|
17 |
|
|
/* Matrix */ |
18 |
|
|
#include <dynamic-graph/linear-algebra.h> |
19 |
|
|
|
20 |
|
|
/* STD */ |
21 |
|
|
#include <string> |
22 |
|
|
|
23 |
|
|
/* SOT */ |
24 |
|
|
#include <sot/core/exception-task.hh> |
25 |
|
|
#include <sot/core/feature-abstract.hh> |
26 |
|
|
#include <sot/core/flags.hh> |
27 |
|
|
#include <sot/core/task-abstract.hh> |
28 |
|
|
|
29 |
|
|
/* --------------------------------------------------------------------- */ |
30 |
|
|
/* --- API ------------------------------------------------------------- */ |
31 |
|
|
/* --------------------------------------------------------------------- */ |
32 |
|
|
|
33 |
|
|
#if defined(WIN32) |
34 |
|
|
#if defined task_EXPORTS |
35 |
|
|
#define SOTTASK_EXPORT __declspec(dllexport) |
36 |
|
|
#else |
37 |
|
|
#define SOTTASK_EXPORT __declspec(dllimport) |
38 |
|
|
#endif |
39 |
|
|
#else |
40 |
|
|
#define SOTTASK_EXPORT |
41 |
|
|
#endif |
42 |
|
|
|
43 |
|
|
/* --------------------------------------------------------------------- */ |
44 |
|
|
/* --- CLASS ----------------------------------------------------------- */ |
45 |
|
|
/* --------------------------------------------------------------------- */ |
46 |
|
|
/*! |
47 |
|
|
@ingroup tasks |
48 |
|
|
@class dynamicgraph::sot::Task task.hh "Definition" |
49 |
|
|
@brief Class that defines the basic elements of a task. |
50 |
|
|
|
51 |
|
|
A task is defined as \f$ {\bf s} ={\bf e}({\bf q}) \f$ |
52 |
|
|
where \f${\bf s} \f$ is a set of features and \f${\bf q}\f$ the |
53 |
|
|
actuated joints of the robot. <br> |
54 |
|
|
It is assumes that \f$ \dot{\bf e} = - \lambda {\bf e} \f$. |
55 |
|
|
Moreover as it assumed that this task can provide: |
56 |
|
|
\f$ {\bf J} = \frac{\delta f}{\delta {\bf q}} \f$ |
57 |
|
|
It then possible to compute |
58 |
|
|
\f$ \dot{\bf q} = -\lambda {\bf J}^{\#} \dot{\bf e}\f$ |
59 |
|
|
with \f$ \dot{\bf e} = {\bf s}^{des} - {\bf s}^* \f$, |
60 |
|
|
and \f$ {\bf s}^{des}\f$ the desired feature and |
61 |
|
|
\f$ {\bf s}^* \f$ the one currently measured. |
62 |
|
|
|
63 |
|
|
It is possible to add features or clear the list of features. |
64 |
|
|
This class makes also possible to select some of the |
65 |
|
|
listed of features to compute the control law through setControlSelection, |
66 |
|
|
addControlSelection, clearControlSelection. |
67 |
|
|
*/ |
68 |
|
|
|
69 |
|
|
namespace dynamicgraph { |
70 |
|
|
namespace sot { |
71 |
|
|
|
72 |
|
|
class SOTTASK_EXPORT Task : public TaskAbstract { |
73 |
|
|
public: |
74 |
|
|
typedef std::list<FeatureAbstract *> FeatureList_t; |
75 |
|
|
|
76 |
|
|
protected: |
77 |
|
|
FeatureList_t featureList; |
78 |
|
|
bool withDerivative; |
79 |
|
|
|
80 |
|
|
DYNAMIC_GRAPH_ENTITY_DECL(); |
81 |
|
|
|
82 |
|
|
public: |
83 |
|
|
Task(const std::string &n); |
84 |
|
|
void initCommands(void); |
85 |
|
|
|
86 |
|
|
void addFeature(FeatureAbstract &s); |
87 |
|
|
void addFeatureFromName(const std::string &name); |
88 |
|
|
void clearFeatureList(void); |
89 |
|
|
FeatureList_t &getFeatureList(void) { return featureList; } |
90 |
|
|
|
91 |
|
|
void setControlSelection(const Flags &act); |
92 |
|
|
void addControlSelection(const Flags &act); |
93 |
|
|
void clearControlSelection(void); |
94 |
|
|
|
95 |
|
|
void setWithDerivative(const bool &s); |
96 |
|
|
bool getWithDerivative(void); |
97 |
|
|
|
98 |
|
|
/* --- COMPUTATION --- */ |
99 |
|
|
dynamicgraph::Vector &computeError(dynamicgraph::Vector &error, int time); |
100 |
|
|
VectorMultiBound &computeTaskExponentialDecrease(VectorMultiBound &errorRef, |
101 |
|
|
int time); |
102 |
|
|
dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &J, int time); |
103 |
|
|
dynamicgraph::Vector &computeErrorTimeDerivative(dynamicgraph::Vector &res, |
104 |
|
|
int time); |
105 |
|
|
|
106 |
|
|
/* --- SIGNALS ------------------------------------------------------------ */ |
107 |
|
|
public: |
108 |
|
|
dynamicgraph::SignalPtr<double, int> controlGainSIN; |
109 |
|
|
dynamicgraph::SignalPtr<double, int> dampingGainSINOUT; |
110 |
|
|
dynamicgraph::SignalPtr<Flags, int> controlSelectionSIN; |
111 |
|
|
dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> errorSOUT; |
112 |
|
|
dynamicgraph::SignalTimeDependent<dynamicgraph::Vector, int> |
113 |
|
|
errorTimeDerivativeSOUT; |
114 |
|
|
|
115 |
|
|
/* --- DISPLAY ------------------------------------------------------------ */ |
116 |
|
|
void display(std::ostream &os) const; |
117 |
|
|
|
118 |
|
|
/* --- Writing graph --- */ |
119 |
|
|
virtual std::ostream &writeGraph(std::ostream &os) const; |
120 |
|
|
}; |
121 |
|
|
|
122 |
|
|
} /* namespace sot */ |
123 |
|
|
} /* namespace dynamicgraph */ |
124 |
|
|
|
125 |
|
|
#endif /* #ifndef __SOT_TASK_H__ */ |