GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/task/task.cpp Lines: 111 150 74.0 %
Date: 2023-03-13 12:09:37 Branches: 122 280 43.6 %

Line Branch Exec Source
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 */
15
#include <dynamic-graph/all-commands.h>
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>
28
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Task, "Task");
29
30
/* --------------------------------------------------------------------- */
31
/* --- CLASS ----------------------------------------------------------- */
32
/* --------------------------------------------------------------------- */
33
34
8
Task::Task(const std::string &n)
35
    : TaskAbstract(n),
36
      featureList(),
37
      withDerivative(false),
38
16
      controlGainSIN(NULL, "sotTask(" + n + ")::input(double)::controlGain"),
39
16
      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
16
                          "sotTask(" + n + ")::input(flag)::controlSelec"),
44
      errorSOUT(boost::bind(&Task::computeError, this, _1, _2), sotNOSIGNAL,
45
16
                "sotTask(" + n + ")::output(vector)::error"),
46
      errorTimeDerivativeSOUT(
47
          boost::bind(&Task::computeErrorTimeDerivative, this, _1, _2),
48
          errorSOUT,
49








72
          "sotTask(" + n + ")::output(vector)::errorTimeDerivative") {
50

8
  taskSOUT.setFunction(
51
      boost::bind(&Task::computeTaskExponentialDecrease, this, _1, _2));
52

8
  jacobianSOUT.setFunction(boost::bind(&Task::computeJacobian, this, _1, _2));
53
54
8
  taskSOUT.addDependency(controlGainSIN);
55
8
  taskSOUT.addDependency(errorSOUT);
56
8
  taskSOUT.addDependency(errorTimeDerivativeSOUT);
57
58
8
  jacobianSOUT.addDependency(controlSelectionSIN);
59
60

8
  controlSelectionSIN = true;
61
62

16
  signalRegistration(controlGainSIN << dampingGainSINOUT << controlSelectionSIN
63

8
                                    << errorSOUT << errorTimeDerivativeSOUT);
64
65
8
  initCommands();
66
8
}
67
68
8
void Task::initCommands(void) {
69
  using namespace dynamicgraph::command;
70
  //
71
  // Commands
72
  //
73
8
  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
8
      "    \n";
82

8
  addCommand("add",
83
8
             makeCommandVoid1(*this, &Task::addFeatureFromName, docstring));
84
85

8
  addCommand("setWithDerivative",
86
8
             makeDirectSetter(*this, &withDerivative,
87

16
                              docDirectSetter("withDerivative", "bool")));
88

8
  addCommand("getWithDerivative",
89
8
             makeDirectGetter(*this, &withDerivative,
90

16
                              docDirectGetter("withDerivative", "bool")));
91
  // ClearFeatureList
92
  docstring =
93
      "    \n"
94
      "    Clear the list of features of the task\n"
95
8
      "    \n";
96
97

8
  addCommand("clear",
98
8
             makeCommandVoid0(*this, &Task::clearFeatureList, docstring));
99
  // List features
100
  docstring =
101
      "    \n"
102
      "    Returns the list of features of the task\n"
103
8
      "    \n";
104
105


8
  addCommand("list", new command::task::ListFeatures(*this, docstring));
106
8
}
107
108
9
void Task::addFeature(FeatureAbstract &s) {
109
9
  featureList.push_back(&s);
110
9
  jacobianSOUT.addDependency(s.jacobianSOUT);
111
9
  errorSOUT.addDependency(s.errorSOUT);
112
9
  errorTimeDerivativeSOUT.addDependency(s.getErrorDot());
113
9
}
114
115
void Task::addFeatureFromName(const std::string &featureName) {
116
  FeatureAbstract &feature =
117
      PoolStorage::getInstance()->getFeature(featureName);
118
  addFeature(feature);
119
}
120
121
void Task::clearFeatureList(void) {
122
  for (FeatureList_t::iterator iter = featureList.begin();
123
       iter != featureList.end(); ++iter) {
124
    FeatureAbstract &s = **iter;
125
    jacobianSOUT.removeDependency(s.jacobianSOUT);
126
    errorSOUT.removeDependency(s.errorSOUT);
127
    errorTimeDerivativeSOUT.removeDependency(s.getErrorDot());
128
  }
129
130
  featureList.clear();
131
}
132
133
void Task::setControlSelection(const Flags &act) { controlSelectionSIN = act; }
134
void Task::addControlSelection(const Flags &act) {
135
  Flags fl = controlSelectionSIN.accessCopy();
136
  fl &= act;
137
  controlSelectionSIN = fl;
138
}
139
void Task::clearControlSelection(void) { controlSelectionSIN = Flags(false); }
140
141
6
void Task::setWithDerivative(const bool &s) { withDerivative = s; }
142
bool Task::getWithDerivative(void) { return withDerivative; }
143
144
/* --- COMPUTATION ---------------------------------------------------------- */
145
/* --- COMPUTATION ---------------------------------------------------------- */
146
/* --- COMPUTATION ---------------------------------------------------------- */
147
148
606
dynamicgraph::Vector &Task::computeError(dynamicgraph::Vector &error,
149
                                         int time) {
150
  sotDEBUG(15) << "# In " << getName() << " {" << endl;
151
152
606
  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
606
    dynamicgraph::Vector::Index dimError = error.size();
172
606
    if (0 == dimError) {
173
12
      dimError = 1;
174
12
      error.resize(dimError);
175
12
      error.setZero();
176
    }
177
178
1212
    dynamicgraph::Vector vectTmp;
179
606
    int cursorError = 0;
180
181
    /* For each cell of the list, recopy value of s, s_star and error. */
182
1212
    for (FeatureList_t::iterator iter = featureList.begin();
183
1818
         iter != featureList.end(); ++iter) {
184
606
      FeatureAbstract &feature = **iter;
185
186
      /* Get s, and store it in the s vector. */
187
      sotDEBUG(45) << "Feature <" << feature.getName() << ">." << std::endl;
188
606
      const dynamicgraph::Vector &partialError = feature.errorSOUT(time);
189
190
606
      const dynamicgraph::Vector::Index dim = partialError.size();
191
642
      while (cursorError + dim > dimError)  // DEBUG It was >=
192
      {
193
36
        dimError *= 2;
194
36
        error.resize(dimError);
195
36
        error.setZero();
196
      }
197
198
4242
      for (int k = 0; k < dim; ++k) {
199

3636
        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
606
    error.conservativeResize(cursorError);
207
  } catch SOT_RETHROW;
208
209
  sotDEBUG(35) << "error_final: " << error << std::endl;
210
  sotDEBUG(15) << "# Out }" << endl;
211
606
  return error;
212
}
213
214
126
dynamicgraph::Vector &Task::computeErrorTimeDerivative(
215
    dynamicgraph::Vector &res, int time) {
216
126
  res.resize(errorSOUT(time).size());
217
126
  dynamicgraph::Vector::Index cursor = 0;
218
219
252
  for (FeatureList_t::iterator iter = featureList.begin();
220
252
       iter != featureList.end(); ++iter) {
221
126
    FeatureAbstract &feature = **iter;
222
223

126
    const dynamicgraph::Vector &partialErrorDot = feature.getErrorDot()(time);
224
126
    const dynamicgraph::Vector::Index dim = partialErrorDot.size();
225

126
    res.segment(cursor, dim) = partialErrorDot;
226
126
    cursor += dim;
227
  }
228
229
126
  return res;
230
}
231
232
126
VectorMultiBound &Task::computeTaskExponentialDecrease(
233
    VectorMultiBound &errorRef, int time) {
234
  sotDEBUG(15) << "# In {" << endl;
235
126
  const dynamicgraph::Vector &errSingleBound = errorSOUT(time);
236
126
  const double &gain = controlGainSIN(time);
237
126
  errorRef.resize(errSingleBound.size());
238
239
882
  for (unsigned int i = 0; i < errorRef.size(); ++i)
240
756
    errorRef[i] = -errSingleBound(i) * gain;
241
242
126
  if (withDerivative) {
243
126
    const dynamicgraph::Vector &de = errorTimeDerivativeSOUT(time);
244
882
    for (unsigned int i = 0; i < errorRef.size(); ++i)
245
756
      errorRef[i] = errorRef[i].getSingleBound() - de(i);
246
  }
247
248
  sotDEBUG(15) << "# Out }" << endl;
249
126
  return errorRef;
250
}
251
252
32
dynamicgraph::Matrix &Task::computeJacobian(dynamicgraph::Matrix &J, int time) {
253
  sotDEBUG(15) << "# In {" << endl;
254
255
32
  if (featureList.empty()) {
256
    throw(ExceptionTask(ExceptionTask::EMPTY_LIST, "Empty feature list"));
257
  }
258
259
  try {
260
32
    dynamicgraph::Matrix::Index dimJ = J.rows();
261
32
    dynamicgraph::Matrix::Index nbc = J.cols();
262
32
    if (0 == dimJ) {
263
8
      dimJ = 1;
264
8
      J.resize(dimJ, nbc);
265
    }
266
267
32
    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
64
    for (FeatureList_t::iterator iter = featureList.begin();
272
64
         iter != featureList.end(); ++iter) {
273
32
      FeatureAbstract &feature = **iter;
274
      sotDEBUG(25) << "Feature <" << feature.getName() << ">" << endl;
275
276
      /* Get s, and store it in the s vector. */
277
32
      const dynamicgraph::Matrix &partialJacobian = feature.jacobianSOUT(time);
278
32
      const dynamicgraph::Matrix::Index nbr = partialJacobian.rows();
279
      sotDEBUG(25) << "Jp =" << endl << partialJacobian << endl;
280
281
32
      if (0 == nbc) {
282
8
        nbc = partialJacobian.cols();
283
8
        J.resize(nbc, dimJ);
284

24
      } else if (partialJacobian.cols() != nbc)
285
        throw ExceptionTask(
286
            ExceptionTask::NON_ADEQUATE_FEATURES,
287
            "Features from the list don't have compatible-size jacobians.");
288
289
80
      while (cursorJ + nbr >= dimJ) {
290
48
        dimJ *= 2;
291
48
        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
1120
      for (int kc = 0; kc < nbc; ++kc) {
297
        // 	  if( selection(kc) )
298
7616
        for (unsigned int k = 0; k < nbr; ++k) {
299

6528
          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
32
      cursorJ += nbr;
305
    }
306
307
    /* If too much memory has been allocated, resize. */
308
32
    J.conservativeResize(cursorJ, nbc);
309
  } catch SOT_RETHROW;
310
311
  sotDEBUG(15) << "# Out }" << endl;
312
32
  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
}