GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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 |
} |
Generated by: GCOVR (Version 4.2) |