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 |
#include <dynamic-graph/all-commands.h> |
||
11 |
|||
12 |
#include <iostream> |
||
13 |
#include <sot/core/feature-abstract.hh> |
||
14 |
#include <sot/core/pool.hh> |
||
15 |
|||
16 |
#include "sot/core/debug.hh" |
||
17 |
#include "sot/core/exception-feature.hh" |
||
18 |
|||
19 |
using namespace dynamicgraph::sot; |
||
20 |
using dynamicgraph::sot::ExceptionFeature; |
||
21 |
|||
22 |
const std::string FeatureAbstract::CLASS_NAME = "FeatureAbstract"; |
||
23 |
|||
24 |
12 |
FeatureAbstract::FeatureAbstract(const std::string &name) |
|
25 |
: Entity(name), |
||
26 |
selectionSIN(NULL, |
||
27 |
✓✗ | 24 |
"sotFeatureAbstract(" + name + ")::input(flag)::selec"), |
28 |
errordotSIN( |
||
29 |
✓✗ | 24 |
NULL, "sotFeatureAbstract(" + name + ")::input(vector)::errordotIN"), |
30 |
errorSOUT(boost::bind(&FeatureAbstract::computeError, this, _1, _2), |
||
31 |
selectionSIN, |
||
32 |
✓✗ | 24 |
"sotFeatureAbstract(" + name + ")::output(vector)::error"), |
33 |
errordotSOUT(boost::bind(&FeatureAbstract::computeErrorDot, this, _1, _2), |
||
34 |
✓✗ | 12 |
selectionSIN << errordotSIN, |
35 |
✓✗ | 24 |
"sotFeatureAbstract(" + name + ")::output(vector)::errordot") |
36 |
|||
37 |
, |
||
38 |
jacobianSOUT( |
||
39 |
boost::bind(&FeatureAbstract::computeJacobian, this, _1, _2), |
||
40 |
selectionSIN, |
||
41 |
✓✗ | 24 |
"sotFeatureAbstract(" + name + ")::output(matrix)::jacobian"), |
42 |
dimensionSOUT(boost::bind(&FeatureAbstract::getDimension, this, _1, _2), |
||
43 |
selectionSIN, |
||
44 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
132 |
"sotFeatureAbstract(" + name + ")::output(uint)::dim") { |
45 |
✓✗✓✗ |
12 |
selectionSIN = true; |
46 |
✓✗✓✗ |
24 |
signalRegistration(selectionSIN << errorSOUT << jacobianSOUT |
47 |
✓✗✓✗ |
12 |
<< dimensionSOUT); |
48 |
✓✗ | 12 |
featureRegistration(); |
49 |
✓✗ | 12 |
initCommands(); |
50 |
12 |
} |
|
51 |
|||
52 |
12 |
void FeatureAbstract::initCommands(void) { |
|
53 |
using namespace command; |
||
54 |
✓✗✓✗ ✓✗ |
24 |
addCommand("setReference", |
55 |
new dynamicgraph::command::Setter<FeatureAbstract, std::string>( |
||
56 |
*this, &FeatureAbstract::setReferenceByName, |
||
57 |
"Give the name of the reference feature.\nInput: a string " |
||
58 |
✓✗✓✗ |
12 |
"(feature name).")); |
59 |
✓✗✓✗ ✓✗ |
24 |
addCommand("getReference", |
60 |
new dynamicgraph::command::Getter<FeatureAbstract, std::string>( |
||
61 |
*this, &FeatureAbstract::getReferenceByName, |
||
62 |
"Get the name of the reference feature.\nOutput: a string " |
||
63 |
✓✗✓✗ |
12 |
"(feature name).")); |
64 |
12 |
} |
|
65 |
|||
66 |
12 |
void FeatureAbstract::featureRegistration(void) { |
|
67 |
12 |
PoolStorage::getInstance()->registerFeature(name, this); |
|
68 |
12 |
} |
|
69 |
|||
70 |
std::ostream &FeatureAbstract::writeGraph(std::ostream &os) const { |
||
71 |
Entity::writeGraph(os); |
||
72 |
|||
73 |
if (isReferenceSet()) { |
||
74 |
const FeatureAbstract *asotFA = getReferenceAbstract(); |
||
75 |
os << "\t\"" << asotFA->getName() << "\" -> \"" << getName() << "\"" |
||
76 |
<< "[ color=darkseagreen4 ]" << std::endl; |
||
77 |
} else |
||
78 |
std::cout << "asotFAT : 0" << std::endl; |
||
79 |
|||
80 |
return os; |
||
81 |
} |
||
82 |
|||
83 |
void FeatureAbstract::setReferenceByName(const std::string &name) { |
||
84 |
setReference( |
||
85 |
&dynamicgraph::sot::PoolStorage::getInstance()->getFeature(name)); |
||
86 |
} |
||
87 |
|||
88 |
std::string FeatureAbstract::getReferenceByName() const { |
||
89 |
if (isReferenceSet()) |
||
90 |
return getReferenceAbstract()->getName(); |
||
91 |
else |
||
92 |
return "none"; |
||
93 |
} |
||
94 |
|||
95 |
10 |
dynamicgraph::Vector &FeatureAbstract::computeErrorDot( |
|
96 |
dynamicgraph::Vector &res, int time) { |
||
97 |
10 |
const Flags &fl = selectionSIN.access(time); |
|
98 |
10 |
const int &dim = dimensionSOUT(time); |
|
99 |
|||
100 |
10 |
unsigned int curr = 0; |
|
101 |
10 |
res.resize(dim); |
|
102 |
|||
103 |
sotDEBUG(25) << "Dim = " << dim << std::endl; |
||
104 |
|||
105 |
✓✗✓✗ ✓✗ |
10 |
if (isReferenceSet() && getReferenceAbstract()->errordotSIN.isPlugged()) { |
106 |
const dynamicgraph::Vector &errdotDes = |
||
107 |
10 |
getReferenceAbstract()->errordotSIN(time); |
|
108 |
sotDEBUG(15) << "Err* = " << errdotDes; |
||
109 |
✗✓ | 10 |
if (errdotDes.size() < dim) { |
110 |
SOT_THROW ExceptionFeature( |
||
111 |
ExceptionFeature::UNCOMPATIBLE_SIZE, |
||
112 |
"Error: dimension uncompatible with des->errorIN size." |
||
113 |
" (while considering feature <%s>).", |
||
114 |
getName().c_str()); |
||
115 |
} |
||
116 |
|||
117 |
✓✗✓✓ |
70 |
for (int i = 0; i < errdotDes.size(); ++i) |
118 |
✓✗✓✗ ✓✗✓✗ |
60 |
if (fl(i)) res(curr++) = -errdotDes(i); |
119 |
} else |
||
120 |
res.setZero(); |
||
121 |
|||
122 |
10 |
return res; |
|
123 |
} |
Generated by: GCOVR (Version 4.2) |