GCC Code Coverage Report

Directory: ./
File: src/task/task.cpp
Date: 2024-12-13 12:22:33
Exec Total Coverage
Lines: 117 156 75.0%
Branches: 114 268 42.5%
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 8 featureList(),
37 8 withDerivative(false),
38
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 8 controlGainSIN(NULL, "sotTask(" + n + ")::input(double)::controlGain"),
39
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 8 dampingGainSINOUT(NULL, "sotTask(" + n + ")::in/output(double)::damping")
40 // TODO As far as I understand, this is not used in this class.
41 ,
42
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 controlSelectionSIN(NULL,
43
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 "sotTask(" + n + ")::input(flag)::controlSelec"),
44
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 8 errorSOUT(boost::bind(&Task::computeError, this, _1, _2), sotNOSIGNAL,
45
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 "sotTask(" + n + ")::output(vector)::error"),
46
4/8
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
 8 errorTimeDerivativeSOUT(
47 boost::bind(&Task::computeErrorTimeDerivative, this, _1, _2),
48 errorSOUT,
49
2/4
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
 24 "sotTask(" + n + ")::output(vector)::errorTimeDerivative") {
50
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 8 taskSOUT.setFunction(
51 boost::bind(&Task::computeTaskExponentialDecrease, this, _1, _2));
52
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 8 jacobianSOUT.setFunction(boost::bind(&Task::computeJacobian, this, _1, _2));
53
54
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 taskSOUT.addDependency(controlGainSIN);
55
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 taskSOUT.addDependency(errorSOUT);
56
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 taskSOUT.addDependency(errorTimeDerivativeSOUT);
57
58
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 jacobianSOUT.addDependency(controlSelectionSIN);
59
60
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 controlSelectionSIN = true;
61
62
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 signalRegistration(controlGainSIN << dampingGainSINOUT << controlSelectionSIN
63
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 8 << errorSOUT << errorTimeDerivativeSOUT);
64
65
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 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
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 " \n";
82
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 addCommand("add",
83
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 makeCommandVoid1(*this, &Task::addFeatureFromName, docstring));
84
85
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 addCommand("setWithDerivative",
86
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 makeDirectSetter(*this, &withDerivative,
87
3/6
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 8 times.
✗ Branch 10 not taken.
 16 docDirectSetter("withDerivative", "bool")));
88
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 addCommand("getWithDerivative",
89
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 makeDirectGetter(*this, &withDerivative,
90
3/6
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 8 times.
✗ Branch 10 not taken.
 16 docDirectGetter("withDerivative", "bool")));
91 // ClearFeatureList
92 docstring =
93 " \n"
94 " Clear the list of features of the task\n"
95
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 " \n";
96
97
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 addCommand("clear",
98
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 makeCommandVoid0(*this, &Task::clearFeatureList, docstring));
99 // List features
100 docstring =
101 " \n"
102 " Returns the list of features of the task\n"
103
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 " \n";
104
105
4/8
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 8 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 8 times.
✗ Branch 12 not taken.
 8 addCommand("list", new command::task::ListFeatures(*this, docstring));
106 8 }
107
108 9 void Task::addFeature(FeatureAbstract &s) {
109
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 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
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 606 times.
 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
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 594 times.
 606 if (0 == dimError) {
173 12 dimError = 1;
174
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 error.resize(dimError);
175
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 error.setZero();
176 }
177
178
1/2
✓ Branch 1 taken 606 times.
✗ Branch 2 not taken.
 606 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 606 for (FeatureList_t::iterator iter = featureList.begin();
183
2/2
✓ Branch 3 taken 606 times.
✓ Branch 4 taken 606 times.
 1212 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
1/2
✓ Branch 1 taken 606 times.
✗ Branch 2 not taken.
 606 const dynamicgraph::Vector &partialError = feature.errorSOUT(time);
189
190 606 const dynamicgraph::Vector::Index dim = partialError.size();
191
2/2
✓ Branch 0 taken 36 times.
✓ Branch 1 taken 606 times.
 642 while (cursorError + dim > dimError) // DEBUG It was >=
192 {
193 36 dimError *= 2;
194
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 error.resize(dimError);
195
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 error.setZero();
196 }
197
198
2/2
✓ Branch 0 taken 3636 times.
✓ Branch 1 taken 606 times.
 4242 for (int k = 0; k < dim; ++k) {
199
2/4
✓ Branch 1 taken 3636 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3636 times.
✗ Branch 5 not taken.
 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
1/2
✓ Branch 1 taken 606 times.
✗ Branch 2 not taken.
 606 error.conservativeResize(cursorError);
207
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 606 } 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 126 for (FeatureList_t::iterator iter = featureList.begin();
220
2/2
✓ Branch 2 taken 126 times.
✓ Branch 3 taken 126 times.
 252 iter != featureList.end(); ++iter) {
221 126 FeatureAbstract &feature = **iter;
222
223
2/4
✓ Branch 1 taken 126 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 126 times.
✗ Branch 5 not taken.
 126 const dynamicgraph::Vector &partialErrorDot = feature.getErrorDot()(time);
224 126 const dynamicgraph::Vector::Index dim = partialErrorDot.size();
225
2/4
✓ Branch 1 taken 126 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 126 times.
✗ Branch 5 not taken.
 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
2/2
✓ Branch 1 taken 756 times.
✓ Branch 2 taken 126 times.
 882 for (unsigned int i = 0; i < errorRef.size(); ++i)
240 756 errorRef[i] = -errSingleBound(i) * gain;
241
242
1/2
✓ Branch 0 taken 126 times.
✗ Branch 1 not taken.
 126 if (withDerivative) {
243 126 const dynamicgraph::Vector &de = errorTimeDerivativeSOUT(time);
244
2/2
✓ Branch 1 taken 756 times.
✓ Branch 2 taken 126 times.
 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
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 32 times.
 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
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 24 times.
 32 if (0 == dimJ) {
263 8 dimJ = 1;
264
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 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 32 for (FeatureList_t::iterator iter = featureList.begin();
272
2/2
✓ Branch 2 taken 32 times.
✓ Branch 3 taken 32 times.
 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
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 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
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 24 times.
 32 if (0 == nbc) {
282 8 nbc = partialJacobian.cols();
283
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 J.resize(nbc, dimJ);
284
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 24 times.
 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
2/2
✓ Branch 0 taken 48 times.
✓ Branch 1 taken 32 times.
 80 while (cursorJ + nbr >= dimJ) {
290 48 dimJ *= 2;
291
1/2
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
 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
2/2
✓ Branch 0 taken 1088 times.
✓ Branch 1 taken 32 times.
 1120 for (int kc = 0; kc < nbc; ++kc) {
297 // if( selection(kc) )
298
2/2
✓ Branch 0 taken 6528 times.
✓ Branch 1 taken 1088 times.
 7616 for (unsigned int k = 0; k < nbr; ++k) {
299
2/4
✓ Branch 1 taken 6528 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6528 times.
✗ Branch 5 not taken.
 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
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 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 }
339