| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| 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 |
computationFrame_(COMPUTATION_FRAME_DEFAULT), |
||
50 |
positionSIN( |
||
51 |
✓✗ | 4 |
NULL, "sotFeaturePoint6d(" + name + ")::input(matrixHomo)::position"), |
52 |
velocitySIN(NULL, |
||
53 |
✓✗ | 4 |
"sotFeaturePoint6d(" + name + ")::input(vector)::velocity"), |
54 |
articularJacobianSIN( |
||
55 |
✓✗ | 4 |
NULL, "sotFeaturePoint6d(" + name + ")::input(matrix)::Jq"), |
56 |
error_th_(), |
||
57 |
R_(), |
||
58 |
Rref_(), |
||
59 |
Rt_(), |
||
60 |
Rreft_(), |
||
61 |
P_(3, 3), |
||
62 |
Pinv_(3, 3), |
||
63 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
14 |
accuracy_(1e-8) { |
64 |
✓✗ | 2 |
jacobianSOUT.addDependency(positionSIN); |
65 |
✓✗ | 2 |
jacobianSOUT.addDependency(articularJacobianSIN); |
66 |
|||
67 |
✓✗ | 2 |
errorSOUT.addDependency(positionSIN); |
68 |
|||
69 |
✓✗✓✗ |
2 |
signalRegistration(positionSIN << articularJacobianSIN); |
70 |
✓✗✓✗ |
2 |
signalRegistration(errordotSOUT << velocitySIN); |
71 |
✓✗✓✗ ✓✗ |
2 |
errordotSOUT.setFunction( |
72 |
boost::bind(&FeaturePoint6d::computeErrordot, this, _1, _2)); |
||
73 |
✓✗ | 2 |
errordotSOUT.addDependency(velocitySIN); |
74 |
✓✗ | 2 |
errordotSOUT.addDependency(positionSIN); |
75 |
✓✗ | 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 |
✓✗ | 2 |
" error = utheta (M M)\n"; |
96 |
✓✗✓✗ |
2 |
addCommand("frame", |
97 |
new dynamicgraph::command::Setter<FeaturePoint6d, std::string>( |
||
98 |
✓✗✓✗ |
2 |
*this, &FeaturePoint6d::computationFrame, docstring)); |
99 |
docstring = |
||
100 |
"Get frame of computation of the error\n" |
||
101 |
"\n" |
||
102 |
✓✗ | 2 |
" See command 'frame' for definition.\n"; |
103 |
✓✗✓✗ |
2 |
addCommand("getFrame", |
104 |
new dynamicgraph::command::Getter<FeaturePoint6d, std::string>( |
||
105 |
✓✗✓✗ |
2 |
*this, &FeaturePoint6d::computationFrame, docstring)); |
106 |
✓✗✓✗ |
2 |
addCommand( |
107 |
"keep", |
||
108 |
✓✗ | 2 |
makeCommandVoid0( |
109 |
*this, &FeaturePoint6d::servoCurrentPosition, |
||
110 |
✓✗✓✗ |
4 |
docCommandVoid0( |
111 |
"modify the desired position to servo at current pos."))); |
||
112 |
} |
||
113 |
2 |
} |
|
114 |
|||
115 |
1 |
void FeaturePoint6d::addDependenciesFromReference(void) { |
|
116 |
✗✓ | 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 |
if (inFrame == "current") |
132 |
computationFrame_ = FRAME_CURRENT; |
||
133 |
✓✗ | 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 |
✓✓ | 7 |
for (int i = 0; i < 6; ++i) |
165 |
✓✗✓✗ |
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 |
✓✗ | 4 |
const Flags &fl = selectionSIN(time); |
296 |
✓✗ | 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 |
✓✗ | 4 |
MatrixHomogeneous hMhd; |
306 |
✓✗✓✗ |
4 |
if (isReferenceSet()) { |
307 |
✓✗ | 4 |
const MatrixHomogeneous &wMhd = getReference()->positionSIN(time); |
308 |
sotDEBUG(15) << "wMhd = " << wMhd << endl; |
||
309 |
✗✓✗ | 4 |
switch (computationFrame_) { |
310 |
case FRAME_CURRENT: |
||
311 |
SOT_COMPUTE_H1MH2(wMh, wMhd, hMhd); |
||
312 |
break; |
||
313 |
4 |
case FRAME_DESIRED: |
|
314 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
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 |
✓✗✓✗ |
4 |
error.resize(dimensionSOUT(time)); |
336 |
4 |
unsigned int cursor = 0; |
|
337 |
✓✓ | 16 |
for (unsigned int i = 0; i < 3; ++i) { |
338 |
✓✗✓✗ ✓✗✓✗ |
12 |
if (fl(i)) error(cursor++) = hMhd(i, 3); |
339 |
} |
||
340 |
|||
341 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✓✗ |
4 |
if (fl(3) || fl(4) || fl(5)) { |
342 |
✓✗ | 4 |
MatrixRotation hRhd; |
343 |
✓✗✓✗ |
4 |
hRhd = hMhd.linear(); |
344 |
✓✗ | 4 |
error_th_.fromRotationMatrix(hRhd); |
345 |
✓✓ | 16 |
for (unsigned int i = 0; i < 3; ++i) { |
346 |
✓✗✓✗ ✓✗✓✗ |
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 |
4 |
const double &r1 = error_th_.angle() * error_th_.axis()(0); |
|
356 |
4 |
const double &r2 = error_th_.angle() * error_th_.axis()(1); |
|
357 |
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 |
✓✗ | 4 |
if (norm_2 < accuracy_) { |
370 |
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 |
✓✗ | 4 |
Pinv_ = P_.inverse(); |
417 |
4 |
} |
|
418 |
|||
419 |
4 |
Vector &FeaturePoint6d::computeErrordot(Vector &errordot, int time) { |
|
420 |
✓✗ | 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 |
✓✗ | 4 |
R_ = M.linear(); |
433 |
✓✗ | 4 |
t_ = M.translation(); |
434 |
✓✗ | 4 |
Rt_ = R_.transpose(); |
435 |
✓✗ | 4 |
Rref_ = Mref.linear(); |
436 |
✓✗ | 4 |
tref_ = Mref.translation(); |
437 |
✓✗ | 4 |
Rreft_ = Rref_.transpose(); |
438 |
4 |
errorSOUT.recompute(time); |
|
439 |
4 |
inverseJacobianRodrigues(); |
|
440 |
✗✓✗ | 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 |
errordot_t_ = Rreft_ * (omega_.cross(tref_ - t_) - v_); |
450 |
✓✗✓✗ ✓✗ |
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 |
✓✓ | 16 |
for (unsigned int i = 0; i < 3; ++i) { |
462 |
✓✗✓✗ |
12 |
if (fl(i)) { |
463 |
12 |
errordot(cursor++) = errordot_t_(i); |
|
464 |
} |
||
465 |
} |
||
466 |
|||
467 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✓✗ |
4 |
if (fl(3) || fl(4) || fl(5)) { |
468 |
✓✓ | 16 |
for (unsigned int i = 0; i < 3; ++i) { |
469 |
✓✗✓✗ |
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 |
} |
| Generated by: GCOVR (Version 4.2) |