task.cpp
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 /* --------------------------------------------------------------------- */
11 /* --- INCLUDE --------------------------------------------------------- */
12 /* --------------------------------------------------------------------- */
13 
14 /* SOT */
16 
17 #include <sot/core/debug.hh>
18 #include <sot/core/pool.hh>
19 #include <sot/core/task.hh>
20 
21 #include "../src/task/task-command.h"
22 
23 using namespace std;
24 using namespace dynamicgraph::sot;
25 using namespace dynamicgraph;
26 
27 #include <sot/core/factory.hh>
29 
30 /* --------------------------------------------------------------------- */
31 /* --- CLASS ----------------------------------------------------------- */
32 /* --------------------------------------------------------------------- */
33 
34 Task::Task(const std::string &n)
35  : TaskAbstract(n),
36  featureList(),
37  withDerivative(false),
38  controlGainSIN(NULL, "sotTask(" + n + ")::input(double)::controlGain"),
39  dampingGainSINOUT(NULL, "sotTask(" + n + ")::in/output(double)::damping")
40  // TODO As far as I understand, this is not used in this class.
41  ,
42  controlSelectionSIN(NULL,
43  "sotTask(" + n + ")::input(flag)::controlSelec"),
44  errorSOUT(boost::bind(&Task::computeError, this, _1, _2), sotNOSIGNAL,
45  "sotTask(" + n + ")::output(vector)::error"),
46  errorTimeDerivativeSOUT(
47  boost::bind(&Task::computeErrorTimeDerivative, this, _1, _2),
48  errorSOUT,
49  "sotTask(" + n + ")::output(vector)::errorTimeDerivative") {
51  boost::bind(&Task::computeTaskExponentialDecrease, this, _1, _2));
52  jacobianSOUT.setFunction(boost::bind(&Task::computeJacobian, this, _1, _2));
53 
57 
59 
60  controlSelectionSIN = true;
61 
64 
65  initCommands();
66 }
67 
68 void Task::initCommands(void) {
69  using namespace dynamicgraph::command;
70  //
71  // Commands
72  //
73  std::string docstring;
74  // AddFeature
75  docstring =
76  " \n"
77  " Add a feature to the task\n"
78  " \n"
79  " Input:\n"
80  " - name of the feature\n"
81  " \n";
82  addCommand("add",
83  makeCommandVoid1(*this, &Task::addFeatureFromName, docstring));
84 
85  addCommand("setWithDerivative",
87  docDirectSetter("withDerivative", "bool")));
88  addCommand("getWithDerivative",
90  docDirectGetter("withDerivative", "bool")));
91  // ClearFeatureList
92  docstring =
93  " \n"
94  " Clear the list of features of the task\n"
95  " \n";
96 
97  addCommand("clear",
98  makeCommandVoid0(*this, &Task::clearFeatureList, docstring));
99  // List features
100  docstring =
101  " \n"
102  " Returns the list of features of the task\n"
103  " \n";
104 
105  addCommand("list", new command::task::ListFeatures(*this, docstring));
106 }
107 
109  featureList.push_back(&s);
113 }
114 
115 void Task::addFeatureFromName(const std::string &featureName) {
116  FeatureAbstract &feature =
117  PoolStorage::getInstance()->getFeature(featureName);
118  addFeature(feature);
119 }
120 
122  for (FeatureList_t::iterator iter = featureList.begin();
123  iter != featureList.end(); ++iter) {
124  FeatureAbstract &s = **iter;
128  }
129 
130  featureList.clear();
131 }
132 
136  fl &= act;
137  controlSelectionSIN = fl;
138 }
140 
141 void Task::setWithDerivative(const bool &s) { withDerivative = s; }
143 
144 /* --- COMPUTATION ---------------------------------------------------------- */
145 /* --- COMPUTATION ---------------------------------------------------------- */
146 /* --- COMPUTATION ---------------------------------------------------------- */
147 
149  int time) {
150  sotDEBUG(15) << "# In " << getName() << " {" << endl;
151 
152  if (featureList.empty()) {
153  throw(ExceptionTask(ExceptionTask::EMPTY_LIST, "Empty feature list"));
154  }
155 
156  try {
157  /* The vector dimensions are not known before the affectation loop.
158  * They thus should be allocated on the flight, in the loop.
159  * The first assumption is that the size has not changed. A double
160  * reallocation (realloc(dim*2)) is done if necessary. In particulary,
161  * [log_2(dim)+1] reallocations are done for the first error computation.
162  * If the allocated size is too large, a correction is done after the loop.
163  * The algotithmic cost is linear in affectation, logarthmic in allocation
164  * numbers and linear in allocation size.
165  * No assumptions are made concerning size of each vector: they are
166  * not said equal, and could be different.
167  */
168 
169  /* First assumption: vector dimensions have not changed. If 0, they are
170  * initialized to dim 1.*/
171  dynamicgraph::Vector::Index dimError = error.size();
172  if (0 == dimError) {
173  dimError = 1;
174  error.resize(dimError);
175  error.setZero();
176  }
177 
178  dynamicgraph::Vector vectTmp;
179  int cursorError = 0;
180 
181  /* For each cell of the list, recopy value of s, s_star and error. */
182  for (FeatureList_t::iterator iter = featureList.begin();
183  iter != featureList.end(); ++iter) {
184  FeatureAbstract &feature = **iter;
185 
186  /* Get s, and store it in the s vector. */
187  sotDEBUG(45) << "Feature <" << feature.getName() << ">." << std::endl;
188  const dynamicgraph::Vector &partialError = feature.errorSOUT(time);
189 
190  const dynamicgraph::Vector::Index dim = partialError.size();
191  while (cursorError + dim > dimError) // DEBUG It was >=
192  {
193  dimError *= 2;
194  error.resize(dimError);
195  error.setZero();
196  }
197 
198  for (int k = 0; k < dim; ++k) {
199  error(cursorError++) = partialError(k);
200  }
201  sotDEBUG(35) << "feature: " << partialError << std::endl;
202  sotDEBUG(35) << "error: " << error << std::endl;
203  }
204 
205  /* If too much memory has been allocated, resize. */
206  error.conservativeResize(cursorError);
207  } catch SOT_RETHROW;
208 
209  sotDEBUG(35) << "error_final: " << error << std::endl;
210  sotDEBUG(15) << "# Out }" << endl;
211  return error;
212 }
213 
215  dynamicgraph::Vector &res, int time) {
216  res.resize(errorSOUT(time).size());
217  dynamicgraph::Vector::Index cursor = 0;
218 
219  for (FeatureList_t::iterator iter = featureList.begin();
220  iter != featureList.end(); ++iter) {
221  FeatureAbstract &feature = **iter;
222 
223  const dynamicgraph::Vector &partialErrorDot = feature.getErrorDot()(time);
224  const dynamicgraph::Vector::Index dim = partialErrorDot.size();
225  res.segment(cursor, dim) = partialErrorDot;
226  cursor += dim;
227  }
228 
229  return res;
230 }
231 
233  VectorMultiBound &errorRef, int time) {
234  sotDEBUG(15) << "# In {" << endl;
235  const dynamicgraph::Vector &errSingleBound = errorSOUT(time);
236  const double &gain = controlGainSIN(time);
237  errorRef.resize(errSingleBound.size());
238 
239  for (unsigned int i = 0; i < errorRef.size(); ++i)
240  errorRef[i] = -errSingleBound(i) * gain;
241 
242  if (withDerivative) {
244  for (unsigned int i = 0; i < errorRef.size(); ++i)
245  errorRef[i] = errorRef[i].getSingleBound() - de(i);
246  }
247 
248  sotDEBUG(15) << "# Out }" << endl;
249  return errorRef;
250 }
251 
253  sotDEBUG(15) << "# In {" << endl;
254 
255  if (featureList.empty()) {
256  throw(ExceptionTask(ExceptionTask::EMPTY_LIST, "Empty feature list"));
257  }
258 
259  try {
260  dynamicgraph::Matrix::Index dimJ = J.rows();
261  dynamicgraph::Matrix::Index nbc = J.cols();
262  if (0 == dimJ) {
263  dimJ = 1;
264  J.resize(dimJ, nbc);
265  }
266 
267  dynamicgraph::Matrix::Index cursorJ = 0;
268  // const Flags& selection = controlSelectionSIN(time);
269 
270  /* For each cell of the list, recopy value of s, s_star and error. */
271  for (FeatureList_t::iterator iter = featureList.begin();
272  iter != featureList.end(); ++iter) {
273  FeatureAbstract &feature = **iter;
274  sotDEBUG(25) << "Feature <" << feature.getName() << ">" << endl;
275 
276  /* Get s, and store it in the s vector. */
277  const dynamicgraph::Matrix &partialJacobian = feature.jacobianSOUT(time);
278  const dynamicgraph::Matrix::Index nbr = partialJacobian.rows();
279  sotDEBUG(25) << "Jp =" << endl << partialJacobian << endl;
280 
281  if (0 == nbc) {
282  nbc = partialJacobian.cols();
283  J.resize(nbc, dimJ);
284  } else if (partialJacobian.cols() != nbc)
285  throw ExceptionTask(
287  "Features from the list don't have compatible-size jacobians.");
288 
289  while (cursorJ + nbr >= dimJ) {
290  dimJ *= 2;
291  J.conservativeResize(dimJ, nbc);
292  }
293  // TODO If controlSelectionSIN is really to be removed,
294  // then the following loop is equivalent to:
295  // J.middleRows (cursorJ, nbr) = partialJacobian;
296  for (int kc = 0; kc < nbc; ++kc) {
297  // if( selection(kc) )
298  for (unsigned int k = 0; k < nbr; ++k) {
299  J(cursorJ + k, kc) = partialJacobian(k, kc);
300  }
301  // else
302  // for( unsigned int k=0;k<nbr;++k ) J(cursorJ+k,kc) = 0.;
303  }
304  cursorJ += nbr;
305  }
306 
307  /* If too much memory has been allocated, resize. */
308  J.conservativeResize(cursorJ, nbc);
309  } catch SOT_RETHROW;
310 
311  sotDEBUG(15) << "# Out }" << endl;
312  return J;
313 }
314 
315 /* --- DISPLAY ------------------------------------------------------------ */
316 /* --- DISPLAY ------------------------------------------------------------ */
317 /* --- DISPLAY ------------------------------------------------------------ */
318 
319 void Task::display(std::ostream &os) const {
320  os << "Task " << name << ": " << endl;
321  os << "--- LIST --- " << std::endl;
322 
323  for (FeatureList_t::const_iterator iter = featureList.begin();
324  iter != featureList.end(); ++iter) {
325  os << "-> " << (*iter)->getName() << endl;
326  }
327 }
328 
329 std::ostream &Task::writeGraph(std::ostream &os) const {
330  FeatureList_t::const_iterator itFeatureAbstract;
331  itFeatureAbstract = featureList.begin();
332  while (itFeatureAbstract != featureList.end()) {
333  os << "\t\"" << (*itFeatureAbstract)->getName() << "\" -> \"" << getName()
334  << "\"" << endl;
335  itFeatureAbstract++;
336  }
337  return os;
338 }
std::string docDirectGetter(const std::string &name, const std::string &type)
CommandVoid0< E > * makeCommandVoid0(E &entity, boost::function< void(void)> function, const std::string &docString)
void addControlSelection(const Flags &act)
Definition: task.cpp:134
void clearFeatureList(void)
Definition: task.cpp:121
Eigen::VectorXd Vector
s
Vec3f n
DirectGetter< E, T > * makeDirectGetter(E &entity, T *ptr, const std::string &docString)
int i
void signalRegistration(const SignalArray< int > &signals)
dynamicgraph::SignalTimeDependent< VectorMultiBound, int > taskSOUT
FeatureList_t featureList
Definition: task.hh:77
dynamicgraph::SignalPtr< double, int > controlGainSIN
Definition: task.hh:108
void addFeature(FeatureAbstract &s)
Definition: task.cpp:108
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Task, "Task")
std::vector< MultiBound > VectorMultiBound
Definition: multi-bound.hh:71
dynamicgraph::SignalPtr< Flags, int > controlSelectionSIN
Definition: task.hh:110
void setControlSelection(const Flags &act)
Definition: task.cpp:133
CommandVoid1< E, T > * makeCommandVoid1(E &entity, boost::function< void(const T &)> function, const std::string &docString)
void setWithDerivative(const bool &s)
Definition: task.cpp:141
void addFeatureFromName(const std::string &name)
Definition: task.cpp:115
int dim
dynamicgraph::Vector & computeErrorTimeDerivative(dynamicgraph::Vector &res, int time)
Definition: task.cpp:214
dynamicgraph::SignalTimeDependent< dynamicgraph::Vector, int > errorSOUT
Definition: task.hh:111
virtual std::ostream & writeGraph(std::ostream &os) const
Definition: task.cpp:329
SignalTimeDependent< dynamicgraph::Matrix, int > jacobianSOUT
Jacobian of the error wrt the robot state: .
bool getWithDerivative(void)
Definition: task.cpp:142
void display(std::ostream &os) const
Definition: task.cpp:319
VectorMultiBound & computeTaskExponentialDecrease(VectorMultiBound &errorRef, int time)
Definition: task.cpp:232
virtual SignalTimeDependent< dynamicgraph::Vector, int > & getErrorDot()
virtual void removeDependency(const SignalBase< Time > &signal)
dynamicgraph::Matrix & computeJacobian(dynamicgraph::Matrix &J, int time)
Definition: task.cpp:252
virtual void addDependency(const SignalBase< Time > &signal)
static PoolStorage * getInstance()
Definition: pool.cpp:147
#define SOT_RETHROW
virtual const T & accessCopy() const
dynamicgraph::SignalPtr< double, int > dampingGainSINOUT
Definition: task.hh:109
void clearControlSelection(void)
Definition: task.cpp:139
dynamicgraph::SignalTimeDependent< dynamicgraph::Matrix, int > jacobianSOUT
#define sotDEBUG(level)
Definition: debug.hh:165
FCL_REAL size() const
This class gives the abstract definition of a feature.
const std::string & getName() const
Eigen::MatrixXd Matrix
DYNAMIC_GRAPH_DLLAPI SignalArray< int > sotNOSIGNAL
std::size_t Index
dynamicgraph::SignalTimeDependent< dynamicgraph::Vector, int > errorTimeDerivativeSOUT
Definition: task.hh:113
Class that defines the basic elements of a task.
Definition: task.hh:72
dynamicgraph::Vector & computeError(dynamicgraph::Vector &error, int time)
Definition: task.cpp:148
void addCommand(const std::string &name, command::Command *command)
SignalTimeDependent< dynamicgraph::Vector, int > errorSOUT
This signal returns the error between the desired value and the current value : . ...
std::string docDirectSetter(const std::string &name, const std::string &type)
void initCommands(void)
Definition: task.cpp:68
virtual void setFunction(boost::function2< T &, T &, Time > t, Mutex *mutexref=NULL)
DirectSetter< E, T > * makeDirectSetter(E &entity, T *ptr, const std::string &docString)


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Wed Jun 21 2023 02:51:26