YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
IFrameTransformTest.h
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
6#ifndef IFRAMETRANSFORMTEST_H
7#define IFRAMETRANSFORMTEST_H
8
9#define _USE_MATH_DEFINES
10#include <math.h>
11
12#include <yarp/sig/Matrix.h>
13#include <yarp/sig/Vector.h>
14#include <yarp/math/Quaternion.h>
15#include <yarp/dev/IFrameTransform.h>
16#include <yarp/os/Network.h>
17#include <yarp/os/Time.h>
18#include <yarp/os/LogStream.h>
19
20#include <yarp/math/Math.h>
21#include <yarp/math/FrameTransform.h>
22
23#include <cmath>
24#include <vector>
25#include <chrono>
26#include <thread>
27
28#include <catch2/catch_amalgamated.hpp>
29
30#include "IFrameTransformTest.h"
31
32using namespace yarp::os;
33using namespace yarp::dev;
34using namespace yarp::math;
35
36namespace yarp::dev::tests
37{
38 inline bool isEqual(const yarp::sig::Vector& v1, const yarp::sig::Vector& v2, double precision)
39 {
40 if (v1.size() != v2.size())
41 {
42 return false;
43 }
44
45 for (size_t i = 0; i < v1.size(); i++)
46 {
47 double check = fabs(v1[i] - v2[i]);
48 if (check > precision)
49 {
50 return false;
51 }
52 }
53 return true;
54 }
55
56 inline bool isEqual(const yarp::math::Quaternion& q1, const yarp::math::Quaternion& q2, double precision)
57 {
58 yarp::sig::Vector v1 = q1.toVector();
59 yarp::sig::Vector v2 = q2.toVector();
60
61 for (size_t i = 0; i < v1.size(); i++)
62 {
63 double check = fabs(v1[i] - v2[i]);
64 if (check > precision)
65 {
66 return false;
67 }
68 }
69 return true;
70 }
71
72 inline bool isEqual(const yarp::sig::Matrix& m1, const yarp::sig::Matrix& m2, double precision)
73 {
74 if (m1.cols() != m2.cols() || m1.rows() != m2.rows())
75 {
76 return false;
77 }
78
79 for (size_t i = 0; i < m1.rows(); i++)
80 {
81 if (!isEqual(m1.getRow(i), m2.getRow(i), precision))
82 {
83 return false;
84 }
85 }
86 return true;
87 }
88
89 inline void exec_frameTransform_test_1(IFrameTransform* itf)
90 {
91 bool precision_verbose= false;
92
93 yarp::sig::Matrix m1(4, 4);
94 m1[0][0] = cos(M_PI / 4); m1[0][1] = -sin(M_PI / 4); m1[0][2] = 0; m1[0][3] = 3;
95 m1[1][0] = sin(M_PI / 4); m1[1][1] = cos(M_PI /4); m1[1][2] = 0; m1[1][3] = 1;
96 m1[2][0] = 0; m1[2][1] = 0; m1[2][2] = 1; m1[2][3] = 2;
97 m1[3][0] = 0; m1[3][1] = 0; m1[3][2] = 0; m1[3][3] = 1;
98 yarp::sig::Matrix m2(4, 4);
99 m2[0][0] = cos(M_PI / 4); m2[0][1] = 0; m2[0][2] = sin(M_PI / 4); m2[0][3] = 0.1;
100 m2[1][0] = 0; m2[1][1] = 1; m2[1][2] = 0; m2[1][3] = 0.2;
101 m2[2][0] = -sin(M_PI / 4); m2[2][1] = 0; m2[2][2] = cos(M_PI / 4); m2[2][3] = 0.3;
102 m2[3][0] = 0; m2[3][1] = 0; m2[3][2] = 0; m2[3][3] = 1;
103 yarp::sig::Matrix sibiling(4, 4);
104 sibiling[0][0] = 1; sibiling[0][1] = 0; sibiling[0][2] = 0; sibiling[0][3] = 10;
105 sibiling[1][0] = 0; sibiling[1][1] = cos(M_PI / 3); sibiling[1][2] = -sin(M_PI / 3); sibiling[1][3] = 15;
106 sibiling[2][0] = 0; sibiling[2][1] = sin(M_PI / 3); sibiling[2][2] = cos(M_PI / 3); sibiling[2][3] = 5;
107 sibiling[3][0] = 0; sibiling[3][1] = 0; sibiling[3][2] = 0; sibiling[3][3] = 1;
108 yarp::sig::Matrix m3(4, 4);
109 m3 = m1*m2;
110
111 double precision;
112 precision = 0.00000001;
113
114 itf->setTransformStatic("frame2", "frame1", m1);
115 itf->setTransformStatic("frame3", "frame2", m2);
116 itf->setTransformStatic("frame4", "frame3", m3);
117 itf->setTransformStatic("frame11", "frame10", m1);
118 itf->setTransformStatic("frame3b", "frame2", m2);
119 itf->setTransformStatic("sibiling_test_frame", "frame1", sibiling);
120
121 yarp::sig::Matrix m4(4, 4);
122 m4[0][0] = +0.9585267399; m4[0][1] = -0.2305627908; m4[0][2] = +0.1675329472; m4[0][3] = 0.1;
123 m4[1][0] = +0.2433237939; m4[1][1] = +0.9680974922; m4[1][2] = -0.0598395928; m4[1][3] = 0.2;
124 m4[2][0] = -0.1483914426; m4[2][1] = +0.0981226021; m4[2][2] = +0.9840487461; m4[2][3] = 0.3;
125 m4[3][0] = 0; m4[3][1] = 0; m4[3][2] = 0; m4[3][3] = 1;
126
127 yarp::os::Time::delay(1);
128
129 //test 0
130 {
131 yInfo("Running Sub-test0");
132 std::vector<std::string> ids;
133 itf->getAllFrameIds(ids);
134 char buff[1024]; buff[0] = 0;
135 for (size_t i = 0; i < ids.size(); i++)
136 {
137 sprintf(buff +strlen(buff), "%s ", ids[i].c_str());
138 }
139 INFO("Found frames: " << buff);
140 bool b_ids = (ids.size() == 8);
141 CHECK(b_ids); // getAllFrameIds ok
142 yInfo() << "Sub-test complete";
143 }
144
145 //test 1
146 {
147 yInfo() << "Running Sub-test1";
148 std::string parent;
149 itf->getParent("frame3", parent);
150 CHECK(parent == "frame2"); // getParent ok
151 yInfo() << "Sub-test complete";
152 }
153
154 //test 2
155 {
156 yInfo() << "Running Sub-test2";
157 yarp::sig::Matrix mt(4, 4);
158 bool b_gt = itf->getTransform("frame3", "frame1", mt);
159 isEqual(mt, m3, precision);
160 CHECK(b_gt); // getTransform ok
161 if (precision_verbose || b_gt==false)
162 {
163 INFO("Precision error:\n" + (mt - m3).toString());
164 }
165 yInfo() << "Sub-test complete";
166 }
167
168 //test3
169 {
170 bool exists = false;
171 yInfo() << "Running Sub-test3";
172 CHECK(itf->frameExists("frame3", exists));
173 CHECK(exists);
174 CHECK(itf->frameExists("frame3_err",exists)); // frameExists ok
175 CHECK(!exists);
176 yInfo() << "Sub-test complete";
177 }
178
179 //test4
180 {
181 bool canTransform = false;
182 yInfo() << "Running Sub-test4";
183 CHECK(itf->canTransform("frame2", "frame1", canTransform));
184 CHECK(canTransform);
185 CHECK(itf->canTransform("frame11", "frame1", canTransform)); // canTransform ok
186 CHECK(!canTransform);
187 yInfo() << "Sub-test complete";
188 }
189
190 //test4bis
191 {
192 yInfo() << "Running Sub-test4b";
193 bool b_canb1;
194 bool canTransform = false;
195 b_canb1 = itf->canTransform("frame3b", "frame1", canTransform);
196 CHECK(b_canb1); // canTransform Bis ok
197 CHECK(canTransform);
198 yInfo() << "Sub-test complete";
199 }
200
201 //test4tris (transform between siblings)
202 {
203 yInfo() << "Running Sub-test4c";
204 yarp::sig::Matrix sib;
205 bool canTransform = false;
206 CHECK(itf->canTransform("sibiling_test_frame", "frame3",canTransform)); // canTransform between siblings ok
207 CHECK(canTransform);
208 CHECK(itf->getTransform("sibiling_test_frame", "frame3", sib)); // getTransform between siblings ok
209 CHECK(isEqual(sib, SE3inv(m2) * SE3inv(m1) * sibiling, precision)); // transform between siblings ok
210 yInfo() << "Sub-test complete";
211 }
212
213 //test 5
214 {
215 yInfo() << "Running Sub-test5";
216 yarp::sig::Matrix mti(4, 4);
217 itf->getTransform("frame1", "frame3b", mti);
218 bool b_gt_inv = isEqual(mti, yarp::math::SE3inv(m3), precision);
219 CHECK(b_gt_inv); // inverted getTransform ok
220 if (precision_verbose || b_gt_inv==false) {
221 INFO("Precision error: " + (mti - yarp::math::SE3inv(m3)).toString());
222 }
223 yInfo() << "Sub-test complete";
224 }
225
226 //test 6
227 {
228 yInfo() << "Running Sub-test6";
229 yarp::sig::Vector in_point1(3), out_point1(3), verPoint1(4);
230 yarp::sig::Vector in_pose1(6), out_pose1(6), verPose(6);
231 yarp::math::Quaternion in_quat1, out_quat1, verQuat;
232
233 in_quat1.fromRotationMatrix(m4);
234
235 in_pose1[0] = 1; in_pose1[1] = 2; in_pose1[2] = 3;
236 in_pose1[3] = 30; in_pose1[4] = 60; in_pose1[5] = 90;
237
238 in_point1[0] = 10; in_point1[1] = 15; in_point1[2] = 5;
239
240 in_point1.push_back(1);
241 verPoint1 = m1*m2*in_point1;
242 verPoint1.pop_back();
243 in_point1.pop_back();
244
245 yarp::sig::Matrix mat(4, 4);
246 yarp::sig::Vector temp(3);
247
248 double rot[3] = { in_pose1[3], in_pose1[4], in_pose1[5] };
249 mat = yarp::math::rpy2dcm(yarp::sig::Vector(3, rot));
250 mat[0][3] = in_pose1[0]; mat[1][3] = in_pose1[1]; mat[2][3] = in_pose1[2];
251 mat = m3 * mat;
252 verPose[0] = mat[0][3]; verPose[1] = mat[1][3]; verPose[2] = mat[2][3];
253 temp = yarp::math::dcm2rpy(mat);
254 verPose[3] = temp[0]; verPose[4] = temp[1]; verPose[5] = temp[2];
255
256 verQuat.fromRotationMatrix(m1 * m2 * m4);
257
258 itf->transformPoint("frame3", "frame1", in_point1, out_point1);
259 itf->transformPose("frame3", "frame1", in_pose1, out_pose1);
260 itf->transformQuaternion("frame3", "frame1", in_quat1, out_quat1);
261
262 bool b_tpoint = isEqual(verPoint1, out_point1, precision);
263 CHECK(b_tpoint); // transformPoint ok
264 if (precision_verbose || b_tpoint == false) {
265 INFO("Precision error:" << (verPoint1 - out_point1).toString());
266 }
267
268 bool b_tpose = isEqual(verPose, out_pose1, precision);
269 CHECK(b_tpose); // transformPose ok
270 if (precision_verbose || b_tpose == false) {
271 INFO("Precision error:" << (verPose - out_pose1).toString());
272 }
273
274 bool b_tquat = isEqual(verQuat, out_quat1, precision);
275 CHECK(b_tquat); // transformQuaternion ok
276 if (precision_verbose || b_tquat == false) {
277 INFO("Precision error:" << (verQuat.toVector() - out_quat1.toVector()).toString());
278 }
279 yInfo() << "Sub-test complete";
280 }
281
282 //test 7
283 {
284 yInfo() << "Running Sub-test7";
285 std::string all_frames;
286 CHECK(itf->allFramesAsString(all_frames));
287 CHECK(all_frames.find("frame1") != std::string::npos);
288 CHECK(all_frames.find("frame2") != std::string::npos);
289 CHECK(all_frames.find("frame3") != std::string::npos);
290 CHECK(all_frames.find("frame4") != std::string::npos);
291 CHECK(all_frames.find("frame10") != std::string::npos);
292 CHECK(all_frames.find("frame11") != std::string::npos);
293 CHECK(all_frames.find("frame3b") != std::string::npos);
294 // allFramesAsString ok
295 yInfo() << "Sub-test complete";
296 }
297
298 //test 8
299 {
300 yInfo() << "Running Sub-test8";
301 itf->setTransformStatic("frame_test", "frame1", m1);
302 yarp::os::Time::delay(1);
303 bool exists = false;
304 bool del_bool = itf->frameExists("frame_test", exists);
305 CHECK(del_bool); // frame exists
306 CHECK(exists);
307 del_bool &= itf->deleteTransform("frame_test", "frame1");
308 CHECK(del_bool); // deleteTransform ok
309 del_bool &= (itf->frameExists("frame_test", exists));
310 CHECK(del_bool); // check if frame has been successfully removed
311 CHECK(!exists);
312
313 //let's wait some time and check gain to be sure that
314 //frame does not reappears....
315 yarp::os::Time::delay(0.5);
316 del_bool = itf->frameExists("frame_test", exists);
317 CHECK(del_bool);
318 CHECK(!exists);
319
320 //just print
321 std::string strs;
322 del_bool &= itf->allFramesAsString(strs);
323 INFO("Existing frames:" << strs);
324 CHECK(del_bool); // confirmed: frame does not exist anymore
325 yInfo() << "Sub-test complete";
326 }
327
328 //test 8b
329 {
330 yInfo() << "Running Sub-test8b";
331 //trying to delete non existing frame. It should return false.
332 //Nothing bad should happen...
333 bool del_bool = itf->deleteTransform("not-exist1", "not-exist2");
334 CHECK_FALSE(del_bool);
335 yInfo() << "Sub-test complete";
336 }
337
338 //test 9
339 {
340 yInfo() << "Running Sub-test9";
341 itf->clear();
342 std::vector<std::string> cids;
343 itf->getAllFrameIds(cids);
344 CHECK(cids.size() == 0); // clear ok
345 yInfo() << "Sub-test complete";
346 }
347
348 //test 10
349 {
350 yInfo() << "Running Sub-test10";
351 itf->setTransform("frame2", "frame10", m1);
352 yarp::os::Time::delay(0.050);
353 bool b_can;
354 bool canTransform = false;
355 b_can = itf->canTransform("frame2", "frame10",canTransform);
356 CHECK(b_can); // itf->setTransform ok
357 CHECK(canTransform);
358 yarp::os::Time::delay(0.6);
359 b_can = itf->canTransform("frame2", "frame10",canTransform);
360 CHECK(b_can);
361 CHECK(!canTransform); // itf->setTransform successfully expired after 0.6s
362 yInfo() << "Sub-test complete";
363 }
364
365 //test 11
366 {
367 yInfo() << "Running Sub-test11";
368 itf->clear();
369 bool set_b1 = itf->setTransform("frame2", "frame10", m1);
370 yarp::os::Time::delay(0.050);
371 yarp::sig::Matrix mt1;
372 itf->getTransform("frame2", "frame10", mt1);
373 bool set_b2 = itf->setTransform("frame2", "frame10", m2);
374 yarp::os::Time::delay(0.050);
375 yarp::sig::Matrix mt2;
376 itf->getTransform("frame2", "frame10", mt2);
377 bool a, b;
378 a = isEqual(m1, mt1, precision);
379 b = isEqual(m2, mt2, precision);
380 CHECK(set_b1);
381 CHECK(set_b2);
382 CHECK(a);
383 CHECK(b); // itf->setTransform successfully updated
384 yInfo() << "Sub-test complete";
385 }
386
387 //test 11b
388 {
389 yInfo() << "Running Sub-test11b";
390 itf->clear();
391 bool set_b1 = itf->setTransformStatic("frame2", "frame10", m1);
392 yarp::os::Time::delay(0.050);
393 yarp::sig::Matrix mt1;
394 itf->getTransform("frame2", "frame10", mt1);
395 bool set_b2 = itf->setTransformStatic("frame2", "frame10", m2);
396 yarp::os::Time::delay(0.050);
397 yarp::sig::Matrix mt2;
398 itf->getTransform("frame2", "frame10", mt2);
399 CHECK(set_b1);
400 CHECK_FALSE(set_b2);
401 CHECK(isEqual(m1, mt1, precision));
402 CHECK_FALSE(isEqual(m2, mt2, precision)); // itf->setTransformStatic successfully not-updated
403 yInfo() << "Sub-test complete";
404 }
405
406 //test 12
407 {
408 yInfo() << "Running Sub-test12";
409 itf->clear();
410 yDebug() << "Creating Transform frame2 -> frame1 at time" << std::to_string(yarp::os::Time::now());
411 CHECK(itf->setTransform("frame2", "frame1", m1));
412 yDebug() << "Created at time" << std::to_string(yarp::os::Time::now());
413 yarp::os::Time::delay(0.040);
414 yDebug() << "Creating Transform frame3 -> frame2 at time" << std::to_string(yarp::os::Time::now());
415 CHECK(itf->setTransform("frame3", "frame2", m2));
416 yDebug() << "Created at time" << std::to_string(yarp::os::Time::now());
417 yarp::os::Time::delay(0.040);
418 yDebug() << "Creating Transform frame3 -> frame1 at time" << std::to_string(yarp::os::Time::now());
419 CHECK_FALSE(itf->setTransform("frame3", "frame1", m1));
420 yDebug() << "Intentionally skipped at time" << std::to_string(yarp::os::Time::now());
421 // itf->setTransform duplicate transform successfully skipped
422
423 yarp::sig::Matrix mt1;
424 yarp::sig::Matrix mt2;
425 yarp::sig::Matrix mt3;
426 itf->getTransform("frame2", "frame1", mt1);
427 itf->getTransform("frame3", "frame2", mt2);
428 itf->getTransform("frame3", "frame1", mt3);
429 CHECK(isEqual(mt1, m1, precision));
430 CHECK(isEqual(mt2, m2, precision));
431 CHECK(isEqual(mt3, (m1*m2), precision));
432 // itf->setTransform still working after duplicate transform
433 yInfo() << "Sub-test complete";
434 }
435
436 //test 12b
437 {
438 yInfo() << "Running Sub-test12b";
439 itf->clear();
440 std::string strs;
441 itf->allFramesAsString(strs); yInfo() << "Existing frames:" << strs;
442
443 CHECK(itf->setTransformStatic("frame2", "frame1", m1));
444 yarp::os::Time::delay(0.050);
445 itf->allFramesAsString(strs); yInfo() << "Existing frames:" << strs;
446
447 CHECK(itf->setTransformStatic("frame3", "frame2", m2));
448 yarp::os::Time::delay(0.050);
449 itf->allFramesAsString(strs); yInfo() << "Existing frames:" << strs;
450
451 CHECK_FALSE(itf->setTransformStatic("frame3", "frame1", m1));
452 //Here I checked false because itf->setTransformStatic of a duplicate transform must be skipped
453 itf->allFramesAsString(strs); yInfo() << "Existing frames:" << strs;
454
455 yarp::sig::Matrix mt1;
456 yarp::sig::Matrix mt2;
457 yarp::sig::Matrix mt3;
458 itf->getTransform("frame2", "frame1", mt1);
459 itf->getTransform("frame3", "frame2", mt2);
460 CHECK(isEqual(mt1, m1, precision));
461 CHECK(isEqual(mt2, m2, precision));
462
463 // checking that the last itf->setTransformStatic had no effect on the values
464 itf->getTransform("frame3", "frame1", mt3);
465 CHECK_FALSE(isEqual(mt3, m1, precision));
466 CHECK(isEqual(mt3, (m1*m2), precision));
467 yInfo() << "Sub-test complete";
468 }
469
470 //test 13
471 {
472 yInfo() << "Running Sub-test13";
473 itf->clear();
474 bool bcan = false;
475 bool canTransform = false;
476 bcan = itf->canTransform("not_existing_frame", "not_existing_frame", canTransform);
477 CHECK(bcan);
478 CHECK(canTransform);
479
480 CHECK(itf->setTransformStatic("frame2", "frame1", m1));
481 bcan = itf->canTransform("frame2", "frame2", canTransform);
482 CHECK(bcan);
483 CHECK(canTransform);
484 bcan = itf->canTransform("frame1", "frame1", canTransform);
485 CHECK(bcan);
486 CHECK(canTransform);
487
488 yarp::sig::Matrix mt1;
489 yarp::sig::Matrix eyemat(4, 4); eyemat.eye();
490 itf->getTransform("frame1", "frame1", mt1);
491 CHECK(isEqual(mt1, eyemat, precision));
492 itf->getTransform("frame2", "frame2", mt1);
493 CHECK(isEqual(mt1, eyemat, precision));
494 yInfo() << "Sub-test complete";
495 }
496 }
497
498
499 inline void exec_frameTransform_test_2(IFrameTransform* itf)
500 {
501 // Fake transform
502 yarp::sig::Matrix armToHand(4, 4);
503 yarp::sig::Matrix result(4, 4);
504 yarp::sig::Matrix handToFinger(4, 4);
505
506 armToHand.zero();
507 armToHand(0, 0) = 1; armToHand(0, 1) = 0; armToHand(0, 2) = 0; armToHand(0, 3) = 5;
508 armToHand(1, 0) = 0; armToHand(1, 1) = sqrt(2) / 2; armToHand(1, 2) = sqrt(2) / 2; armToHand(1, 3) = 4;
509 armToHand(2, 0) = 0; armToHand(2, 1) = -sqrt(2) / 2; armToHand(2, 2) = sqrt(2) / 2; armToHand(2, 3) = 3;
510 armToHand(3, 0) = 0; armToHand(3, 1) = 0; armToHand(3, 2) = 0; armToHand(3, 3) = 1;
511
512 handToFinger(0, 0) = 1; handToFinger(0, 1) = 0; handToFinger(0, 2) = 0; handToFinger(0, 3) = 5;
513 handToFinger(1, 0) = 0; handToFinger(1, 1) = sqrt(2) / 2; handToFinger(1, 2) = sqrt(2) / 2; handToFinger(1, 3) = 4;
514 handToFinger(2, 0) = 0; handToFinger(2, 1) = -sqrt(2) / 2; handToFinger(2, 2) = sqrt(2) / 2; handToFinger(2, 3) = 3;
515 handToFinger(3, 0) = 0; handToFinger(3, 1) = 0; handToFinger(3, 2) = 0; handToFinger(3, 3) = 1;
516
517 result = armToHand * handToFinger;
518
519 // test setTransformStatic
520 // static transforms cannot be set if they already exists
521 CHECK(itf->setTransformStatic("/hand", "/arm", armToHand));
522 CHECK(!itf->setTransformStatic("/hand", "/arm", handToFinger));
523
524 // static transforms cannot be set if an indirect connection already exists
525 CHECK(itf->setTransformStatic("/a", "/b", handToFinger));
526 CHECK(itf->setTransformStatic("/b", "/c", handToFinger));
527 CHECK(!itf->setTransformStatic("/a", "/c", handToFinger));
528 CHECK(itf->deleteTransform("/a", "/b"));
529 CHECK(itf->deleteTransform("/b", "/c"));
530
531 // test setTransform
532 CHECK(itf->setTransform("/finger", "/hand", handToFinger));
533 CHECK(!itf->setTransform("/finger", "/arm", result));
534
535 // test getTransform
536 CHECK(itf->clear());
537 CHECK(itf->setTransform("/finger", "/arm", result));
538 yarp::sig::Matrix resultFromTransform(4, 4);
539 CHECK(itf->getTransform("/finger", "/arm", resultFromTransform));
540 for (int i = 0; i < 4; i++) {
541 for (int j = 0; j < 4; j++) {
542 CHECK((resultFromTransform(i, j) - result(i, j)) <= pow(10, -14));
543 }
544 }
545
546 // test canTransform
547 bool canTransform = false;
548 CHECK(itf->canTransform("/finger", "/arm", canTransform));
549 CHECK(canTransform);
550 CHECK(itf->canTransform("/finger", "/head", canTransform));
551 CHECK(!canTransform);
552
553 // test frameExists
554 bool exists = false;
555 CHECK(itf->frameExists("/finger", exists));
556 CHECK(exists);
557 CHECK(itf->frameExists("/head", exists));
558 CHECK(!exists);
559
560 // test getAllFrameIds
561 CHECK(itf->clear());
562 CHECK(itf->setTransformStatic("/hand", "/arm", armToHand));
563 CHECK(itf->setTransformStatic("/finger", "/hand", handToFinger));
564 std::vector<std::string> frameIdsReturned;
565 std::vector<std::string> correctFrameIdsReturned{
566 "/arm",
567 "/hand",
568 "/finger"
569 };
570 CHECK(itf->getAllFrameIds(frameIdsReturned));
571 CHECK(frameIdsReturned.size() == correctFrameIdsReturned.size());
572 CHECK(frameIdsReturned == correctFrameIdsReturned);
573 std::vector<std::string> wrongFrameIdsReturned1{
574 "/hand",
575 "/arm",
576 "/finger"
577 };
578 CHECK(!(frameIdsReturned == wrongFrameIdsReturned1));
579
580 // test getParent
581 std::string parent;
582 CHECK(itf->getParent("/finger", parent));
583 CHECK(parent == "/hand");
584
585 // test transformPoint
586 yarp::sig::Vector startingPoint{ 0.0,0.0,0.0 };
587 yarp::sig::Vector wrongStartingPoint{ 0.0,0.0,0.0,0.0 };
588 yarp::sig::Vector endPoint{ 0.0,0.0,0.0,0.0 };
589 CHECK(itf->transformPoint("/finger", "/arm", startingPoint, endPoint));
590 CHECK(!itf->transformPoint("/finger", "/arm", wrongStartingPoint, endPoint));
591 CHECK(!itf->transformPoint("/finger", "/head", startingPoint, endPoint));
592 startingPoint.push_back(1);
593 yarp::sig::Vector correctEndPoint = result * startingPoint;
594 correctEndPoint.pop_back();
595 CHECK(endPoint.size() == correctEndPoint.size());
596 for (int i = 0; i < 3; i++) {
597 CHECK((endPoint[i] - correctEndPoint[i]) <= pow(10, -14));
598 }
599
600 // test transformPose
601 yarp::sig::Vector startingPose{ 0,0,0,0,0,0 };
602 yarp::sig::Vector wrongStartingPose{ 0,0,0,0,0,0,0 };
603 yarp::sig::Vector endPose;
604 yarp::math::FrameTransform transform;
605 transform.transFromVec(startingPose[0], startingPose[1], startingPose[2]);
606 transform.rotFromRPY(startingPose[3], startingPose[4], startingPose[5]);
607 transform.fromMatrix(result * transform.toMatrix());
608 CHECK(itf->transformPose("/finger", "/arm", startingPose, endPose));
609 CHECK((transform.translation.tX - endPose[0]) <= pow(10, -14));
610 CHECK((transform.translation.tY - endPose[1]) <= pow(10, -14));
611 CHECK((transform.translation.tZ - endPose[2]) <= pow(10, -14));
612 CHECK((transform.translation.tZ - endPose[2]) <= pow(10, -14));
613 CHECK((transform.getRPYRot()[0] - endPose[3]) <= pow(10, -14));
614 CHECK((transform.getRPYRot()[1] - endPose[4]) <= pow(10, -14));
615 CHECK((transform.getRPYRot()[2] - endPose[5]) <= pow(10, -14));
616 CHECK(!itf->transformPose("/finger", "/arm", wrongStartingPose, endPose));
617 CHECK(!itf->transformPose("/finger", "/head", startingPose, endPose));
618
619 // test transformQuaternion
620 yarp::math::Quaternion startingPoseQuaternion = transform.rotation;
621 yarp::math::Quaternion endPoseQuaternion;
622 CHECK(itf->transformQuaternion("/finger", "/arm", startingPoseQuaternion, endPoseQuaternion));
623 yarp::math::Quaternion correctedEndPoseQuaternion;
624 yarp::math::FrameTransform transformQuaternion;
625 itf->getTransform("/finger", "/arm", result);
626 transformQuaternion.rotation = startingPoseQuaternion;
627 correctedEndPoseQuaternion.fromRotationMatrix(result * transformQuaternion.toMatrix());
628 CHECK((endPoseQuaternion.x() - correctedEndPoseQuaternion.x()) <= pow(10, -14));
629 CHECK((endPoseQuaternion.y() - correctedEndPoseQuaternion.y()) <= pow(10, -14));
630 CHECK((endPoseQuaternion.z() - correctedEndPoseQuaternion.z()) <= pow(10, -14));
631 CHECK((endPoseQuaternion.w() - correctedEndPoseQuaternion.w()) <= pow(10, -14));
632 CHECK(!itf->transformQuaternion("/finger", "/head", startingPoseQuaternion, endPoseQuaternion));
633
634 // test clear
635 CHECK(itf->clear());
636 std::this_thread::sleep_for(std::chrono::milliseconds(250));
637 CHECK(!itf->getTransform("/hand", "/arm", result));
638 CHECK(!itf->getTransform("/finger", "/hand", result));
639 CHECK(!itf->getTransform("/finger", "/arm", result));
640 }
641}
642
643#endif
M_PI
#define M_PI
Definition FakeLocalizer.cpp:30
toString
std::string toString(const T &value)
convert an arbitrary type to string.
Definition FakeMotionControl.cpp:127
yInfo
#define yInfo(...)
Definition Log.h:319
yDebug
#define yDebug(...)
Definition Log.h:275
Matrix.h
contains the definition of a Matrix type
Vector.h
contains the definition of a Vector type
yarp::dev::IFrameTransform
Transform Interface.
Definition IFrameTransform.h:29
yarp::math::FrameTransform::transFromVec
void transFromVec(double X, double Y, double Z)
Definition FrameTransform.cpp:51
yarp::math::FrameTransform::toMatrix
yarp::sig::Matrix toMatrix() const
Definition FrameTransform.cpp:77
yarp::math::FrameTransform::getRPYRot
yarp::sig::Vector getRPYRot() const
Definition FrameTransform.cpp:68
yarp::math::FrameTransform::rotFromRPY
void rotFromRPY(double R, double P, double Y)
Definition FrameTransform.cpp:56
yarp::math::FrameTransform::fromMatrix
bool fromMatrix(const yarp::sig::Matrix &mat)
Definition FrameTransform.cpp:88
yarp::math::FrameTransform::translation
struct yarp::math::FrameTransform::Translation_t translation
yarp::os::BufferedPort
A mini-server for performing network communication in the background.
Definition BufferedPort.h:60
yarp::sig::Matrix
A class for a Matrix.
Definition Matrix.h:39
yarp::dev::tests::isEqual
bool isEqual(const yarp::sig::Vector &v1, const yarp::sig::Vector &v2, double precision)
Definition IFrameTransformStorageTest.h:34
yarp::dev::tests::exec_frameTransform_test_2
void exec_frameTransform_test_2(IFrameTransform *itf)
Definition IFrameTransformTest.h:499
yarp::dev::tests::exec_frameTransform_test_1
void exec_frameTransform_test_1(IFrameTransform *itf)
Definition IFrameTransformTest.h:89
yarp::dev
For streams capable of holding different kinds of content, check what they actually have.
Definition BatteryData.cpp:13
yarp::math::SE3inv
yarp::sig::Matrix SE3inv(const yarp::sig::Matrix &H)
Returns the inverse of a 4 by 4 rototranslational matrix (defined in Math.h).
Definition math.cpp:912
yarp::math::rpy2dcm
yarp::sig::Matrix rpy2dcm(const yarp::sig::Vector &rpy)
Converts roll-pitch-yaw angles in the corresponding dcm (direction cosine matrix) rotation matrix (de...
Definition math.cpp:847
yarp::math::dcm2rpy
yarp::sig::Vector dcm2rpy(const yarp::sig::Matrix &R)
Converts a dcm (direction cosine matrix) rotation matrix to roll-pitch-yaw angles (defined in Math....
Definition math.cpp:808
yarp::os::Time::now
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition Time.cpp:121
yarp::os::Time::delay
void delay(double seconds)
Wait for a certain number of seconds.
Definition Time.cpp:111
yarp::os
An interface to the operating system, including Port based communication.
Definition AbstractCarrier.h:13