Directory: | ./ |
---|---|
File: | src/feature/feature-point6d.cpp |
Date: | 2024-08-13 12:13:25 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 127 | 287 | 44.3% |
Branches: | 123 | 510 | 24.1% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | /* | ||
2 | * Copyright 2010, 2011, 2012 | ||
3 | * François Bleibel, | ||
4 | * Olivier Stasse, | ||
5 | * Florent Lamiraux | ||
6 | * Nicolas Mansard | ||
7 | * | ||
8 | * CNRS/AIST | ||
9 | * | ||
10 | */ | ||
11 | |||
12 | /* --------------------------------------------------------------------- */ | ||
13 | /* --- INCLUDE --------------------------------------------------------- */ | ||
14 | /* --------------------------------------------------------------------- */ | ||
15 | |||
16 | /* --- SOT --- */ | ||
17 | //#define VP_DEBUG | ||
18 | //#define VP_DEBUG_MODE 45 | ||
19 | #include <dynamic-graph/command-bind.h> | ||
20 | #include <dynamic-graph/command-getter.h> | ||
21 | #include <dynamic-graph/command-setter.h> | ||
22 | #include <dynamic-graph/command.h> | ||
23 | |||
24 | #include <Eigen/LU> | ||
25 | #include <sot/core/debug.hh> | ||
26 | #include <sot/core/exception-feature.hh> | ||
27 | #include <sot/core/feature-point6d.hh> | ||
28 | #include <sot/core/macros.hh> | ||
29 | |||
30 | using namespace std; | ||
31 | using namespace dynamicgraph; | ||
32 | using namespace dynamicgraph::sot; | ||
33 | |||
34 | #include <sot/core/factory.hh> | ||
35 | SOT_CORE_DISABLE_WARNING_PUSH | ||
36 | SOT_CORE_DISABLE_WARNING_DEPRECATED | ||
37 | ✗ | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePoint6d, "FeaturePoint6d"); | |
38 | SOT_CORE_DISABLE_WARNING_POP | ||
39 | |||
40 | /* --------------------------------------------------------------------- */ | ||
41 | /* --- CLASS ----------------------------------------------------------- */ | ||
42 | /* --------------------------------------------------------------------- */ | ||
43 | |||
44 | const FeaturePoint6d::ComputationFrameType | ||
45 | FeaturePoint6d::COMPUTATION_FRAME_DEFAULT = FRAME_DESIRED; | ||
46 | |||
47 | 2 | FeaturePoint6d::FeaturePoint6d(const string &pointName) | |
48 | : FeatureAbstract(pointName), | ||
49 | 2 | computationFrame_(COMPUTATION_FRAME_DEFAULT), | |
50 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | positionSIN( |
51 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | NULL, "sotFeaturePoint6d(" + name + ")::input(matrixHomo)::position"), |
52 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | velocitySIN(NULL, |
53 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
4 | "sotFeaturePoint6d(" + name + ")::input(vector)::velocity"), |
54 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | articularJacobianSIN( |
55 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
4 | NULL, "sotFeaturePoint6d(" + name + ")::input(matrix)::Jq"), |
56 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | error_th_(), |
57 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | R_(), |
58 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | Rref_(), |
59 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | Rt_(), |
60 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | Rreft_(), |
61 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | P_(3, 3), |
62 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | Pinv_(3, 3), |
63 |
8/16✓ 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.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 2 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 2 times.
✗ Branch 25 not taken.
|
4 | accuracy_(1e-8) { |
64 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | jacobianSOUT.addDependency(positionSIN); |
65 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | jacobianSOUT.addDependency(articularJacobianSIN); |
66 | |||
67 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | errorSOUT.addDependency(positionSIN); |
68 | |||
69 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | signalRegistration(positionSIN << articularJacobianSIN); |
70 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | signalRegistration(errordotSOUT << velocitySIN); |
71 |
3/6✓ 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.
|
2 | errordotSOUT.setFunction( |
72 | boost::bind(&FeaturePoint6d::computeErrordot, this, _1, _2)); | ||
73 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | errordotSOUT.addDependency(velocitySIN); |
74 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | errordotSOUT.addDependency(positionSIN); |
75 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | errordotSOUT.addDependency(errorSOUT); |
76 | |||
77 | // Commands | ||
78 | // | ||
79 | { | ||
80 | using namespace dynamicgraph::command; | ||
81 | 2 | std::string docstring; | |
82 | // Set computation frame | ||
83 | docstring = | ||
84 | "Set computation frame\n" | ||
85 | "\n" | ||
86 | " Input:\n" | ||
87 | " a string: 'current' or 'desired'.\n" | ||
88 | " If 'current', the error is defined as the rotation " | ||
89 | "vector (VectorUTheta)\n" | ||
90 | " corresponding to the position of the reference in the " | ||
91 | "current frame:\n" | ||
92 | " -1 *\n" | ||
93 | " error = utheta (M M )\n" | ||
94 | " If 'desired', *-1\n" | ||
95 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | " error = utheta (M M)\n"; |
96 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
|
2 | addCommand("frame", |
97 | new dynamicgraph::command::Setter<FeaturePoint6d, std::string>( | ||
98 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | *this, &FeaturePoint6d::computationFrame, docstring)); |
99 | docstring = | ||
100 | "Get frame of computation of the error\n" | ||
101 | "\n" | ||
102 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | " See command 'frame' for definition.\n"; |
103 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
|
2 | addCommand("getFrame", |
104 | new dynamicgraph::command::Getter<FeaturePoint6d, std::string>( | ||
105 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | *this, &FeaturePoint6d::computationFrame, docstring)); |
106 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | addCommand( |
107 | "keep", | ||
108 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | makeCommandVoid0( |
109 | *this, &FeaturePoint6d::servoCurrentPosition, | ||
110 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
|
4 | docCommandVoid0( |
111 | "modify the desired position to servo at current pos."))); | ||
112 | 2 | } | |
113 | 2 | } | |
114 | |||
115 | 1 | void FeaturePoint6d::addDependenciesFromReference(void) { | |
116 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
|
1 | assert(isReferenceSet()); |
117 | 1 | errorSOUT.addDependency(getReference()->positionSIN); | |
118 | 1 | jacobianSOUT.addDependency(getReference()->positionSIN); | |
119 | 1 | } | |
120 | |||
121 | ✗ | void FeaturePoint6d::removeDependenciesFromReference(void) { | |
122 | ✗ | assert(isReferenceSet()); | |
123 | ✗ | errorSOUT.removeDependency(getReference()->positionSIN); | |
124 | ✗ | jacobianSOUT.removeDependency(getReference()->positionSIN); | |
125 | } | ||
126 | |||
127 | /* --------------------------------------------------------------------- */ | ||
128 | /* --------------------------------------------------------------------- */ | ||
129 | /* --------------------------------------------------------------------- */ | ||
130 | 1 | void FeaturePoint6d::computationFrame(const std::string &inFrame) { | |
131 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
|
1 | if (inFrame == "current") |
132 | ✗ | computationFrame_ = FRAME_CURRENT; | |
133 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | else if (inFrame == "desired") |
134 | 1 | computationFrame_ = FRAME_DESIRED; | |
135 | else { | ||
136 | ✗ | std::string msg("FeaturePoint6d::computationFrame: " + inFrame + | |
137 | ": invalid argument,\n" | ||
138 | ✗ | "expecting 'current' or 'desired'"); | |
139 | ✗ | throw ExceptionFeature(ExceptionFeature::GENERIC, msg); | |
140 | } | ||
141 | 1 | } | |
142 | |||
143 | /// \brief Get computation frame | ||
144 | ✗ | std::string FeaturePoint6d::computationFrame() const { | |
145 | ✗ | switch (computationFrame_) { | |
146 | ✗ | case FRAME_CURRENT: | |
147 | ✗ | return "current"; | |
148 | ✗ | case FRAME_DESIRED: | |
149 | ✗ | return "desired"; | |
150 | } | ||
151 | ✗ | assert(false && "Case not handled"); | |
152 | return "Case not handled"; | ||
153 | } | ||
154 | /* --------------------------------------------------------------------- */ | ||
155 | /* --------------------------------------------------------------------- */ | ||
156 | /* --------------------------------------------------------------------- */ | ||
157 | |||
158 | 1 | unsigned int &FeaturePoint6d::getDimension(unsigned int &dim, int time) { | |
159 | sotDEBUG(25) << "# In {" << endl; | ||
160 | |||
161 | 1 | const Flags &fl = selectionSIN.access(time); | |
162 | |||
163 | 1 | dim = 0; | |
164 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
|
7 | for (int i = 0; i < 6; ++i) |
165 |
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++; |
166 | |||
167 | sotDEBUG(25) << "# Out }" << endl; | ||
168 | 1 | return dim; | |
169 | } | ||
170 | |||
171 | /** Compute the interaction matrix from a subset of | ||
172 | * the possible features. | ||
173 | */ | ||
174 | ✗ | Matrix &FeaturePoint6d::computeJacobian(Matrix &J, int time) { | |
175 | sotDEBUG(15) << "# In {" << endl; | ||
176 | |||
177 | ✗ | const Matrix &Jq = articularJacobianSIN(time); | |
178 | ✗ | const int &dim = dimensionSOUT(time); | |
179 | ✗ | const Flags &fl = selectionSIN(time); | |
180 | |||
181 | sotDEBUG(25) << "dim = " << dimensionSOUT(time) << " time:" << time << " " | ||
182 | << dimensionSOUT.getTime() << " " << dimensionSOUT.getReady() | ||
183 | << endl; | ||
184 | sotDEBUG(25) << "selec = " << selectionSIN(time) << " time:" << time << " " | ||
185 | << selectionSIN.getTime() << " " << selectionSIN.getReady() | ||
186 | << endl; | ||
187 | |||
188 | sotDEBUG(15) << "Dimension=" << dim << std::endl; | ||
189 | |||
190 | ✗ | const Matrix::Index cJ = Jq.cols(); | |
191 | ✗ | J.resize(dim, cJ); | |
192 | ✗ | Matrix LJq(6, cJ); | |
193 | |||
194 | ✗ | if (FRAME_CURRENT == computationFrame_) { | |
195 | /* The Jacobian on rotation is equal to Jr = - hdRh Jr6d. | ||
196 | * The Jacobian in translation is equalt to Jt = [hRw(wthd-wth)]x Jr - Jt. | ||
197 | */ | ||
198 | ✗ | const MatrixHomogeneous &wMh = positionSIN(time); | |
199 | ✗ | MatrixRotation wRh; | |
200 | ✗ | wRh = wMh.linear(); | |
201 | ✗ | MatrixRotation wRhd; | |
202 | ✗ | Vector hdth(3), Rhdth(3); | |
203 | |||
204 | ✗ | if (isReferenceSet()) { | |
205 | ✗ | const MatrixHomogeneous &wMhd = getReference()->positionSIN(time); | |
206 | ✗ | wRhd = wMhd.linear(); | |
207 | ✗ | for (unsigned int i = 0; i < 3; ++i) hdth(i) = wMhd(i, 3) - wMh(i, 3); | |
208 | } else { | ||
209 | ✗ | wRhd.setIdentity(); | |
210 | ✗ | for (unsigned int i = 0; i < 3; ++i) hdth(i) = -wMh(i, 3); | |
211 | } | ||
212 | ✗ | Rhdth = (wRh.inverse()) * hdth; | |
213 | ✗ | MatrixRotation hdRh; | |
214 | ✗ | hdRh = (wRhd.inverse()) * wRh; | |
215 | |||
216 | ✗ | Matrix Lx(6, 6); | |
217 | ✗ | for (unsigned int i = 0; i < 3; i++) { | |
218 | ✗ | for (unsigned int j = 0; j < 3; j++) { | |
219 | ✗ | if (i == j) { | |
220 | ✗ | Lx(i, j) = -1; | |
221 | } else { | ||
222 | ✗ | Lx(i, j) = 0; | |
223 | } | ||
224 | ✗ | Lx(i + 3, j) = 0; | |
225 | ✗ | Lx(i + 3, j + 3) = -hdRh(i, j); | |
226 | } | ||
227 | } | ||
228 | ✗ | const double &X = Rhdth(0), &Y = Rhdth(1), &Z = Rhdth(2); | |
229 | ✗ | Lx(0, 4) = -Z; | |
230 | ✗ | Lx(0, 5) = Y; | |
231 | ✗ | Lx(1, 3) = Z; | |
232 | ✗ | Lx(1, 5) = -X; | |
233 | ✗ | Lx(2, 3) = -Y; | |
234 | ✗ | Lx(2, 4) = X; | |
235 | ✗ | Lx(0, 3) = 0; | |
236 | ✗ | Lx(1, 4) = 0; | |
237 | ✗ | Lx(2, 5) = 0; | |
238 | sotDEBUG(15) << "Lx= " << Lx << endl; | ||
239 | |||
240 | ✗ | LJq = Lx * Jq; | |
241 | ✗ | } else { | |
242 | /* The Jacobian in rotation is equal to Jr = hdJ = hdRh Jr. | ||
243 | * The Jacobian in translation is equal to Jr = hdJ = hdRh Jr. */ | ||
244 | ✗ | const MatrixHomogeneous &wMh = positionSIN(time); | |
245 | ✗ | MatrixRotation wRh; | |
246 | ✗ | wRh = wMh.linear(); | |
247 | ✗ | MatrixRotation hdRh; | |
248 | |||
249 | ✗ | if (isReferenceSet()) { | |
250 | ✗ | const MatrixHomogeneous &wMhd = getReference()->positionSIN(time); | |
251 | ✗ | MatrixRotation wRhd; | |
252 | ✗ | wRhd = wMhd.linear(); | |
253 | ✗ | hdRh = (wRhd.inverse()) * wRh; | |
254 | } else { | ||
255 | ✗ | hdRh = wRh; | |
256 | } | ||
257 | |||
258 | ✗ | LJq.fill(0); | |
259 | ✗ | for (unsigned int i = 0; i < 3; i++) | |
260 | ✗ | for (unsigned int j = 0; j < cJ; j++) { | |
261 | ✗ | for (unsigned int k = 0; k < 3; k++) { | |
262 | ✗ | LJq(i, j) += hdRh(i, k) * Jq(k, j); | |
263 | ✗ | LJq(i + 3, j) += hdRh(i, k) * Jq(k + 3, j); | |
264 | } | ||
265 | } | ||
266 | } | ||
267 | |||
268 | /* Select the active line of Jq. */ | ||
269 | ✗ | unsigned int rJ = 0; | |
270 | ✗ | for (unsigned int r = 0; r < 6; ++r) | |
271 | ✗ | if (fl(r)) { | |
272 | ✗ | for (unsigned int c = 0; c < cJ; ++c) J(rJ, c) = LJq(r, c); | |
273 | ✗ | rJ++; | |
274 | } | ||
275 | |||
276 | sotDEBUG(15) << "# Out }" << endl; | ||
277 | ✗ | return J; | |
278 | } | ||
279 | |||
280 | #define SOT_COMPUTE_H1MH2(wMh, wMhd, hMhd) \ | ||
281 | { \ | ||
282 | MatrixHomogeneous hMw; \ | ||
283 | hMw = wMh.inverse(Eigen::Affine); \ | ||
284 | sotDEBUG(15) << "hMw = " << hMw << endl; \ | ||
285 | hMhd = hMw * wMhd; \ | ||
286 | sotDEBUG(15) << "hMhd = " << hMhd << endl; \ | ||
287 | } | ||
288 | |||
289 | /** Compute the error between two visual features from a subset | ||
290 | * a the possible features. | ||
291 | */ | ||
292 | 4 | Vector &FeaturePoint6d::computeError(Vector &error, int time) { | |
293 | sotDEBUGIN(15); | ||
294 | |||
295 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | const Flags &fl = selectionSIN(time); |
296 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | const MatrixHomogeneous &wMh = positionSIN(time); |
297 | sotDEBUG(15) << "wMh = " << wMh << endl; | ||
298 | |||
299 | /* Computing only translation: * | ||
300 | * trans( hMw wMhd ) = htw + hRw wthd * | ||
301 | * = -hRw wth + hrW wthd * | ||
302 | * = hRw ( wthd - wth ) * | ||
303 | * The second line is obtained by writting hMw as the inverse of wMh. */ | ||
304 | |||
305 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | MatrixHomogeneous hMhd; |
306 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
4 | if (isReferenceSet()) { |
307 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | const MatrixHomogeneous &wMhd = getReference()->positionSIN(time); |
308 | sotDEBUG(15) << "wMhd = " << wMhd << endl; | ||
309 |
1/3✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | switch (computationFrame_) { |
310 | ✗ | case FRAME_CURRENT: | |
311 | ✗ | SOT_COMPUTE_H1MH2(wMh, wMhd, hMhd); | |
312 | ✗ | break; | |
313 | 4 | case FRAME_DESIRED: | |
314 |
4/8✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
|
4 | SOT_COMPUTE_H1MH2(wMhd, wMh, hMhd); |
315 | 4 | break; // Compute hdMh indeed. | |
316 | }; | ||
317 | } else { | ||
318 | ✗ | switch (computationFrame_) { | |
319 | ✗ | case FRAME_CURRENT: | |
320 | ✗ | hMhd = wMh.inverse(); | |
321 | ✗ | break; | |
322 | ✗ | case FRAME_DESIRED: | |
323 | ✗ | hMhd = wMh; | |
324 | ✗ | break; // Compute hdMh indeed. | |
325 | }; | ||
326 | } | ||
327 | |||
328 | sotDEBUG(25) << "dim = " << dimensionSOUT(time) << " time:" << time << " " | ||
329 | << dimensionSOUT.getTime() << " " << dimensionSOUT.getReady() | ||
330 | << endl; | ||
331 | sotDEBUG(25) << "selec = " << selectionSIN(time) << " time:" << time << " " | ||
332 | << selectionSIN.getTime() << " " << selectionSIN.getReady() | ||
333 | << endl; | ||
334 | |||
335 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | error.resize(dimensionSOUT(time)); |
336 | 4 | unsigned int cursor = 0; | |
337 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 4 times.
|
16 | for (unsigned int i = 0; i < 3; ++i) { |
338 |
4/8✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 12 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 12 times.
✗ Branch 10 not taken.
|
12 | if (fl(i)) error(cursor++) = hMhd(i, 3); |
339 | } | ||
340 | |||
341 |
3/14✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✓ Branch 15 taken 4 times.
✗ Branch 16 not taken.
|
4 | if (fl(3) || fl(4) || fl(5)) { |
342 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | MatrixRotation hRhd; |
343 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | hRhd = hMhd.linear(); |
344 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | error_th_.fromRotationMatrix(hRhd); |
345 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 4 times.
|
16 | for (unsigned int i = 0; i < 3; ++i) { |
346 |
4/8✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 12 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 12 times.
✗ Branch 12 not taken.
|
12 | if (fl(i + 3)) error(cursor++) = error_th_.angle() * error_th_.axis()(i); |
347 | } | ||
348 | } | ||
349 | |||
350 | sotDEBUGOUT(15); | ||
351 | 4 | return error; | |
352 | } | ||
353 | |||
354 | 4 | void FeaturePoint6d::inverseJacobianRodrigues() { | |
355 |
1/2✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
4 | const double &r1 = error_th_.angle() * error_th_.axis()(0); |
356 |
1/2✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
4 | const double &r2 = error_th_.angle() * error_th_.axis()(1); |
357 |
1/2✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
4 | const double &r3 = error_th_.angle() * error_th_.axis()(2); |
358 | 4 | double r1_2 = r1 * r1; | |
359 | 4 | double r2_2 = r2 * r2; | |
360 | 4 | double r3_2 = r3 * r3; | |
361 | 4 | double r1_3 = r1 * r1_2; | |
362 | 4 | double r2_3 = r2 * r2_2; | |
363 | 4 | double r3_3 = r3 * r3_2; | |
364 | 4 | double r1_4 = r1_2 * r1_2; | |
365 | 4 | double r2_4 = r2_2 * r2_2; | |
366 | 4 | double r3_4 = r3_2 * r3_2; | |
367 | 4 | double norm_2 = r3_2 + r2_2 + r1_2; | |
368 | |||
369 |
1/2✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
|
4 | if (norm_2 < accuracy_) { |
370 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | P_.setIdentity(); |
371 | } else { | ||
372 | // This code has been generated by maxima software | ||
373 | ✗ | P_(0, 0) = | |
374 | ✗ | ((r3_2 + r2_2) * sqrt(norm_2) * sin(sqrt(norm_2)) + r1_2 * r3_2 + | |
375 | ✗ | r1_2 * r2_2 + r1_4) / | |
376 | ✗ | (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4); | |
377 | ✗ | P_(0, 1) = | |
378 | ✗ | -(r1 * r2 * sqrt(norm_2) * sin(sqrt(norm_2)) + | |
379 | ✗ | (r3_3 + (r2_2 + r1_2) * r3) * cos(sqrt(norm_2)) - r3_3 - | |
380 | ✗ | r1 * r2 * r3_2 + (-r2_2 - r1_2) * r3 - r1 * r2_3 - r1_3 * r2) / | |
381 | ✗ | (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4); | |
382 | ✗ | P_(0, 2) = | |
383 | ✗ | -(r1 * r3 * sqrt(norm_2) * sin(sqrt(norm_2)) + | |
384 | ✗ | (-r2 * r3_2 - r2_3 - r1_2 * r2) * cos(sqrt(norm_2)) - r1 * r3_3 + | |
385 | ✗ | r2 * r3_2 + (-r1 * r2_2 - r1_3) * r3 + r2_3 + r1_2 * r2) / | |
386 | ✗ | (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4); | |
387 | ✗ | P_(1, 0) = | |
388 | ✗ | -(r1 * r2 * sqrt(norm_2) * sin(sqrt(norm_2)) + | |
389 | ✗ | ((-r2_2 - r1_2) * r3 - r3_3) * cos(sqrt(norm_2)) + r3_3 - | |
390 | ✗ | r1 * r2 * r3_2 + (r2_2 + r1_2) * r3 - r1 * r2_3 - r1_3 * r2) / | |
391 | ✗ | (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4); | |
392 | ✗ | P_(1, 1) = | |
393 | ✗ | ((r3_2 + r1_2) * sqrt(norm_2) * sin(sqrt(norm_2)) + r2_2 * r3_2 + r2_4 + | |
394 | ✗ | r1_2 * r2_2) / | |
395 | ✗ | (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4); | |
396 | ✗ | P_(1, 2) = | |
397 | ✗ | -(r2 * r3 * sqrt(norm_2) * sin(sqrt(norm_2)) + | |
398 | ✗ | (r1 * r3_2 + r1 * r2_2 + r1_3) * cos(sqrt(norm_2)) - r2 * r3_3 - | |
399 | ✗ | r1 * r3_2 + (-r2_3 - r1_2 * r2) * r3 - r1 * r2_2 - r1_3) / | |
400 | ✗ | (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4); | |
401 | ✗ | P_(2, 0) = | |
402 | ✗ | -(r1 * r3 * sqrt(norm_2) * sin(sqrt(norm_2)) + | |
403 | ✗ | (r2 * r3_2 + r2_3 + r1_2 * r2) * cos(sqrt(norm_2)) - r1 * r3_3 - | |
404 | ✗ | r2 * r3_2 + (-r1 * r2_2 - r1_3) * r3 - r2_3 - r1_2 * r2) / | |
405 | ✗ | (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4); | |
406 | ✗ | P_(2, 1) = | |
407 | ✗ | -(r2 * r3 * sqrt(norm_2) * sin(sqrt(norm_2)) + | |
408 | ✗ | (-r1 * r3_2 - r1 * r2_2 - r1_3) * cos(sqrt(norm_2)) - r2 * r3_3 + | |
409 | ✗ | r1 * r3_2 + (-r2_3 - r1_2 * r2) * r3 + r1 * r2_2 + r1_3) / | |
410 | ✗ | (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4); | |
411 | ✗ | P_(2, 2) = | |
412 | ✗ | ((r2_2 + r1_2) * sqrt(norm_2) * sin(sqrt(norm_2)) + r3_4 + | |
413 | ✗ | (r2_2 + r1_2) * r3_2) / | |
414 | ✗ | (r3_4 + (2 * r2_2 + 2 * r1_2) * r3_2 + r2_4 + 2 * r1_2 * r2_2 + r1_4); | |
415 | } | ||
416 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | Pinv_ = P_.inverse(); |
417 | 4 | } | |
418 | |||
419 | 4 | Vector &FeaturePoint6d::computeErrordot(Vector &errordot, int time) { | |
420 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | if (isReferenceSet()) { |
421 | 4 | const Vector &velocity = getReference()->velocitySIN(time); | |
422 | 4 | const MatrixHomogeneous &M = positionSIN(time); | |
423 | 4 | const MatrixHomogeneous &Mref = getReference()->positionSIN(time); | |
424 | // Linear velocity if the reference frame | ||
425 | 4 | v_(0) = velocity(0); | |
426 | 4 | v_(1) = velocity(1); | |
427 | 4 | v_(2) = velocity(2); | |
428 | // Angular velocity if the reference frame | ||
429 | 4 | omega_(0) = velocity(3); | |
430 | 4 | omega_(1) = velocity(4); | |
431 | 4 | omega_(2) = velocity(5); | |
432 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | R_ = M.linear(); |
433 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | t_ = M.translation(); |
434 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | Rt_ = R_.transpose(); |
435 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | Rref_ = Mref.linear(); |
436 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | tref_ = Mref.translation(); |
437 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | Rreft_ = Rref_.transpose(); |
438 | 4 | errorSOUT.recompute(time); | |
439 | 4 | inverseJacobianRodrigues(); | |
440 |
1/3✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | switch (computationFrame_) { |
441 | ✗ | case FRAME_CURRENT: | |
442 | // \dot{e}_{t} = R^{T} v | ||
443 | ✗ | errordot_t_ = Rt_ * v_; | |
444 | // \dot{e}_{\theta} = P^{-1}(e_{theta})R^{*T}\omega | ||
445 | ✗ | Rreftomega_ = Rreft_ * omega_; | |
446 | ✗ | errordot_th_ = Pinv_ * Rreftomega_; | |
447 | ✗ | break; | |
448 | 4 | case FRAME_DESIRED: | |
449 |
4/8✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
|
4 | errordot_t_ = Rreft_ * (omega_.cross(tref_ - t_) - v_); |
450 |
3/6✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
|
4 | errordot_th_ = -Pinv_ * (Rt_ * omega_); |
451 | 4 | break; | |
452 | } | ||
453 | } else { | ||
454 | ✗ | errordot_t_.setZero(); | |
455 | ✗ | errordot_th_.setZero(); | |
456 | } | ||
457 | |||
458 | 4 | const Flags &fl = selectionSIN(time); | |
459 | 4 | errordot.resize(dimensionSOUT(time)); | |
460 | 4 | unsigned int cursor = 0; | |
461 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 4 times.
|
16 | for (unsigned int i = 0; i < 3; ++i) { |
462 |
2/4✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
|
12 | if (fl(i)) { |
463 | 12 | errordot(cursor++) = errordot_t_(i); | |
464 | } | ||
465 | } | ||
466 | |||
467 |
3/14✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✓ Branch 15 taken 4 times.
✗ Branch 16 not taken.
|
4 | if (fl(3) || fl(4) || fl(5)) { |
468 |
2/2✓ Branch 0 taken 12 times.
✓ Branch 1 taken 4 times.
|
16 | for (unsigned int i = 0; i < 3; ++i) { |
469 |
2/4✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
|
12 | if (fl(i + 3)) { |
470 | 12 | errordot(cursor++) = errordot_th_(i); | |
471 | } | ||
472 | } | ||
473 | } | ||
474 | |||
475 | 4 | return errordot; | |
476 | } | ||
477 | |||
478 | /* Modify the value of the reference (sdes) so that it corresponds | ||
479 | * to the current position. The effect on the servo is to maintain the | ||
480 | * current position and correct any drift. */ | ||
481 | ✗ | void FeaturePoint6d::servoCurrentPosition(void) { | |
482 | sotDEBUGIN(15); | ||
483 | |||
484 | ✗ | if (!isReferenceSet()) { | |
485 | ✗ | sotERROR << "The reference is not set, this function should not be called" | |
486 | ✗ | << std::endl; | |
487 | ✗ | throw ExceptionFeature( | |
488 | ✗ | ExceptionFeature::GENERIC, | |
489 | ✗ | "The reference is not set, this function should not be called"); | |
490 | } | ||
491 | ✗ | getReference()->positionSIN = positionSIN.accessCopy(); | |
492 | |||
493 | sotDEBUGOUT(15); | ||
494 | } | ||
495 | |||
496 | static const char *featureNames[] = {"X ", "Y ", "Z ", "RX", "RY", "RZ"}; | ||
497 | ✗ | void FeaturePoint6d::display(std::ostream &os) const { | |
498 | ✗ | os << "Point6d <" << name << ">: ("; | |
499 | |||
500 | try { | ||
501 | ✗ | const Flags &fl = selectionSIN.accessCopy(); | |
502 | ✗ | bool first = true; | |
503 | ✗ | for (int i = 0; i < 6; ++i) | |
504 | ✗ | if (fl(i)) { | |
505 | ✗ | if (first) { | |
506 | ✗ | first = false; | |
507 | } else { | ||
508 | ✗ | os << ","; | |
509 | } | ||
510 | ✗ | os << featureNames[i]; | |
511 | } | ||
512 | ✗ | os << ") "; | |
513 | ✗ | } catch (ExceptionAbstract e) { | |
514 | ✗ | os << " selectSIN not set."; | |
515 | } | ||
516 | } | ||
517 |