GCC Code Coverage Report

Directory: ./
File: src/feature/feature-generic.cpp
Date: 2025-05-13 12:28:21
Exec Total Coverage
Lines: 35 65 53.8%
Branches: 36 118 30.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 <sot/core/debug.hh>
16 #include <sot/core/exception-feature.hh>
17 #include <sot/core/factory.hh>
18 #include <sot/core/feature-generic.hh>
19
20 using namespace std;
21 using namespace dynamicgraph::sot;
22 using namespace dynamicgraph;
23 ✗ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureGeneric, "FeatureGeneric");
24
25 /* --------------------------------------------------------------------- */
26 /* --- CLASS ----------------------------------------------------------- */
27 /* --------------------------------------------------------------------- */
28
29 2 FeatureGeneric::FeatureGeneric(const string &pointName)
30 : FeatureAbstract(pointName),
31 2 dimensionDefault(0),
32
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 errorSIN(NULL, "sotFeatureGeneric(" + name + ")::input(vector)::errorIN"),
33
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 jacobianSIN(NULL,
34
3/6
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
 6 "sotFeatureGeneric(" + name + ")::input(matrix)::jacobianIN")
35
36 {
37
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 jacobianSOUT.addDependency(jacobianSIN);
38
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 errorSOUT.addDependency(errorSIN);
39
40
4/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
 2 signalRegistration(errorSIN << jacobianSIN << errordotSIN << errordotSOUT);
41 2 }
42
43 /* --------------------------------------------------------------------- */
44 /* --------------------------------------------------------------------- */
45 /* --------------------------------------------------------------------- */
46
47 1 void FeatureGeneric::addDependenciesFromReference(void) {
48
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 assert(SP::isReferenceSet());
49 1 errorSOUT.addDependency(getReference()->errorSIN);
50 1 errordotSOUT.addDependency(getReference()->errordotSIN);
51 1 }
52
53 ✗ void FeatureGeneric::removeDependenciesFromReference(void) {
54 ✗ assert(SP::isReferenceSet());
55 ✗ errorSOUT.removeDependency(getReference()->errorSIN);
56 ✗ errordotSOUT.removeDependency(getReference()->errordotSIN);
57 }
58
59 /* --------------------------------------------------------------------- */
60 /* --------------------------------------------------------------------- */
61 /* --------------------------------------------------------------------- */
62
63 1 unsigned int &FeatureGeneric::getDimension(unsigned int &dim, int time) {
64 sotDEBUG(25) << "# In {" << endl;
65
66 1 const Flags &fl = selectionSIN.access(time);
67
68
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 if (dimensionDefault == 0) dimensionDefault = errorSIN.access(time).size();
69
70 1 dim = 0;
71
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
 7 for (unsigned int i = 0; i < dimensionDefault; ++i)
72
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 6 if (fl(i)) dim++;
73
74 sotDEBUG(25) << "# Out }" << endl;
75 1 return dim;
76 }
77
78 10 Vector &FeatureGeneric::computeError(Vector &res, int time) {
79
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 const Vector &err = errorSIN.access(time);
80
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 const Flags &fl = selectionSIN.access(time);
81
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 const int &dim = dimensionSOUT(time);
82
83 10 unsigned int curr = 0;
84
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 res.resize(dim);
85
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 10 times.
 10 if (err.size() < dim) {
86 ✗ SOT_THROW ExceptionFeature(
87 ✗ ExceptionFeature::UNCOMPATIBLE_SIZE,
88 "Error: dimension uncompatible with des->errorIN size."
89 " (while considering feature <%s>).",
90 ✗ getName().c_str());
91 }
92
93 sotDEBUG(15) << "Err = " << err;
94 sotDEBUG(25) << "Dim = " << dim << endl;
95
96
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
 10 if (isReferenceSet()) {
97
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 const Vector &errDes = getReference()->errorSIN(time);
98 sotDEBUG(15) << "Err* = " << errDes;
99
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 10 times.
 10 if (errDes.size() < dim) {
100 ✗ SOT_THROW ExceptionFeature(
101 ✗ ExceptionFeature::UNCOMPATIBLE_SIZE,
102 "Error: dimension uncompatible with des->errorIN size."
103 " (while considering feature <%s>).",
104 ✗ getName().c_str());
105 }
106
107
2/2
✓ Branch 1 taken 60 times.
✓ Branch 2 taken 10 times.
 70 for (int i = 0; i < err.size(); ++i)
108
2/4
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 60 times.
✗ Branch 4 not taken.
 60 if (fl(i))
109
5/10
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 60 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 60 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 60 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 60 times.
✗ Branch 13 not taken.
 60 if (fl(i)) res(curr++) = err(i) - errDes(i);
110 } else {
111 ✗ for (int i = 0; i < err.size(); ++i)
112 ✗ if (fl(i)) res(curr++) = err(i);
113 }
114
115 10 return res;
116 }
117
118 ✗ Matrix &FeatureGeneric::computeJacobian(Matrix &res, int time) {
119 sotDEBUGIN(15);
120
121 ✗ const Matrix &Jac = jacobianSIN.access(time);
122 ✗ const Flags &fl = selectionSIN.access(time);
123 ✗ const unsigned int &dim = dimensionSOUT(time);
124
125 ✗ unsigned int curr = 0;
126 ✗ res.resize(dim, Jac.cols());
127
128 ✗ for (unsigned int i = 0; curr < dim; ++i)
129 ✗ if (fl(i)) {
130 ✗ for (int j = 0; j < Jac.cols(); ++j) res(curr, j) = Jac(i, j);
131 ✗ curr++;
132 }
133
134 sotDEBUGOUT(15);
135 ✗ return res;
136 }
137
138 /* --------------------------------------------------------------------- */
139 /* --------------------------------------------------------------------- */
140 /* --------------------------------------------------------------------- */
141
142 ✗ void FeatureGeneric::display(std::ostream &os) const {
143 ✗ os << "Generic <" << name << ">: " << std::endl;
144
145 try {
146 ✗ os << " error= " << errorSIN.accessCopy() << endl
147 ✗ << " J = " << jacobianSIN.accessCopy() << endl;
148 ✗ } catch (ExceptionSignal &e) {
149 ✗ os << e.what();
150 }
151 }
152