GCC Code Coverage Report

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