GCC Code Coverage Report

Directory: ./
File: src/feature/feature-joint-limits.cpp
Date: 2025-05-13 12:28:21
Exec Total Coverage
Lines: 0 73 0.0%
Branches: 0 156 0.0%
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/feature-joint-limits.hh>
18 using namespace std;
19
20 #include <../src/feature/feature-joint-limits-command.h>
21
22 #include <sot/core/factory.hh>
23
24 /* --------------------------------------------------------------------- */
25 /* --- CLASS ----------------------------------------------------------- */
26 /* --------------------------------------------------------------------- */
27
28 using namespace dynamicgraph::sot;
29 using namespace dynamicgraph;
30 ✗ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeatureJointLimits, "FeatureJointLimits");
31
32 const double FeatureJointLimits::THRESHOLD_DEFAULT = .9;
33
34 ✗ FeatureJointLimits::FeatureJointLimits(const string &fName)
35 : FeatureAbstract(fName),
36 ✗ threshold(THRESHOLD_DEFAULT)
37
38 ,
39 ✗ jointSIN(NULL,
40 ✗ "sotFeatureJointLimits(" + name + ")::input(vector)::joint"),
41 ✗ upperJlSIN(NULL,
42 ✗ "sotFeatureJointLimits(" + name + ")::input(vector)::upperJl"),
43 ✗ lowerJlSIN(NULL,
44 ✗ "sotFeatureJointLimits(" + name + ")::input(vector)::lowerJl"),
45 ✗ widthJlSINTERN(
46 boost::bind(&FeatureJointLimits::computeWidthJl, this, _1, _2),
47 ✗ upperJlSIN << lowerJlSIN,
48 ✗ "sotFeatureJointLimits(" + name + ")::input(vector)::widthJl") {
49 ✗ errorSOUT.addDependency(jointSIN);
50 ✗ errorSOUT.addDependency(upperJlSIN);
51 ✗ errorSOUT.addDependency(lowerJlSIN);
52
53 ✗ signalRegistration(jointSIN << upperJlSIN << lowerJlSIN << widthJlSINTERN);
54
55 // Commands
56 //
57 ✗ std::string docstring;
58 // Actuate
59 docstring =
60 " \n"
61 " Actuate\n"
62 ✗ " \n";
63 ✗ addCommand("actuate",
64 ✗ new command::featureJointLimits::Actuate(*this, docstring));
65 }
66
67 /* --------------------------------------------------------------------- */
68
69 ✗ void FeatureJointLimits::addDependenciesFromReference(void) {}
70 ✗ void FeatureJointLimits::removeDependenciesFromReference(void) {}
71
72 /* --------------------------------------------------------------------- */
73 /* --------------------------------------------------------------------- */
74 /* --------------------------------------------------------------------- */
75
76 ✗ unsigned int &FeatureJointLimits::getDimension(unsigned int &dim, int time) {
77 sotDEBUG(25) << "# In {" << endl;
78
79 ✗ const Flags &fl = selectionSIN.access(time);
80 ✗ const Matrix::Index NBJL = upperJlSIN.access(time).size();
81
82 ✗ dim = 0;
83 ✗ for (Matrix::Index i = 0; i < NBJL; ++i)
84 ✗ if (fl(static_cast<int>(i))) dim++;
85
86 sotDEBUG(25) << "# Out }" << endl;
87 ✗ return dim;
88 }
89
90 ✗ Vector &FeatureJointLimits::computeWidthJl(Vector &res, const int &time) {
91 sotDEBUGIN(15);
92
93 ✗ const Vector UJL = upperJlSIN.access(time);
94 ✗ const Vector LJL = lowerJlSIN.access(time);
95 ✗ const Vector::Index SIZE = UJL.size();
96 ✗ res.resize(SIZE);
97
98 ✗ for (Vector::Index i = 0; i < SIZE; ++i) {
99 ✗ res(i) = UJL(i) - LJL(i);
100 }
101
102 sotDEBUGOUT(15);
103 ✗ return res;
104 }
105
106 /** Compute the interaction matrix from a subset of
107 * the possible features.
108 */
109 ✗ Matrix &FeatureJointLimits::computeJacobian(Matrix &J, int time) {
110 sotDEBUG(15) << "# In {" << endl;
111
112 ✗ const unsigned int SIZE = dimensionSOUT.access(time);
113 ✗ const Vector q = jointSIN.access(time);
114 ✗ const Flags &fl = selectionSIN(time);
115 // const unsigned int SIZE_FF=SIZE+freeFloatingSize;
116 ✗ const Vector::Index SIZE_TOTAL = q.size();
117 ✗ const Vector WJL = widthJlSINTERN.access(time);
118 ✗ J.resize(SIZE, SIZE_TOTAL);
119 ✗ J.setZero();
120
121 ✗ unsigned int idx = 0;
122 ✗ for (unsigned int i = 0; i < SIZE_TOTAL; ++i) {
123 ✗ if (fl(i)) {
124 ✗ if (fabs(WJL(i)) > 1e-3)
125 ✗ J(idx, i) = 1 / WJL(i);
126 else
127 ✗ J(idx, i) = 1.;
128 ✗ idx++;
129 }
130 }
131 // if( 0!=freeFloatingIndex )
132 // for( unsigned int i=0;i<freeFloatingIndex;++i )
133 // {
134 // if( fabs(WJL(i))>1e-3 ) J(i,i)=1/WJL(i); else J(i,i)=1.;
135 // }
136
137 // if( SIZE!=freeFloatingIndex )
138 // for( unsigned int i=freeFloatingIndex;i<SIZE;++i )
139 // {
140 // if( fabs(WJL(i))>1e-3 ) J(i,i+freeFloatingSIZE)=1/WJL(i);
141 // else J(i,i)=1.;
142 // }
143
144 sotDEBUG(15) << "# Out }" << endl;
145 ✗ return J;
146 }
147
148 /** Compute the error between two visual features from a subset
149 * a the possible features.
150 */
151 ✗ Vector &FeatureJointLimits::computeError(Vector &error, int time) {
152 sotDEBUGIN(15);
153
154 ✗ const Flags &fl = selectionSIN(time);
155 ✗ const Vector q = jointSIN.access(time);
156 ✗ const Vector UJL = upperJlSIN.access(time);
157 ✗ const Vector LJL = lowerJlSIN.access(time);
158 ✗ const Vector WJL = widthJlSINTERN.access(time);
159 ✗ const int SIZE = dimensionSOUT.access(time);
160 ✗ const Vector::Index SIZE_TOTAL = q.size();
161
162 sotDEBUG(25) << "q = " << q << endl;
163 sotDEBUG(25) << "ljl = " << LJL << endl;
164 sotDEBUG(25) << "Wjl = " << WJL << endl;
165 sotDEBUG(25) << "dim = " << SIZE << endl;
166
167 ✗ assert(UJL.size() == SIZE_TOTAL);
168 ✗ assert(WJL.size() == SIZE_TOTAL);
169 ✗ assert(LJL.size() == SIZE_TOTAL);
170 ✗ assert(SIZE <= SIZE_TOTAL);
171
172 ✗ error.resize(SIZE);
173
174 ✗ unsigned int parcerr = 0;
175 ✗ for (int i = 0; i < SIZE_TOTAL; ++i) {
176 ✗ if (fl(i)) {
177 ✗ error(parcerr++) = (q(i) - LJL(i)) / WJL(i) * 2 - 1;
178 }
179 }
180
181 sotDEBUGOUT(15);
182 ✗ return error;
183 }
184
185 ✗ void FeatureJointLimits::display(std::ostream &os) const {
186 ✗ os << "JointLimits <" << name << "> ... TODO";
187 }
188