KinematicSolverTest.cpp
Go to the documentation of this file.
1#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::KinematicSolver
2#define ARMARX_BOOST_TEST
3
4
5#include "../KinematicSolver.h"
6
7#include <VirtualRobot/XML/RobotIO.h>
8
9#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
10
11#include <RobotAPI/libraries/core/FramedPose.h>
12#include <RobotAPI/libraries/core/Pose.h>
13
14#include <RobotComponents/Test.h>
15
16#include "../Interpolation/InterpolationSegmentFactory.h"
17#include "../Interpolation/LinearInterpolation.h"
18#include "../Util/OrientationConversion.h"
19
20using namespace armarx;
21
22using namespace std;
23
24using namespace VirtualRobot;
25
26//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
27/// \brief The PosePkg struct - Testing utility
28///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
29struct PosePkg
30{
31public:
32 Vector3BasePtr pos;
33 QuaternionBasePtr ori;
34 PoseBasePtr pose;
35};
36
37PosePkg
38createPosePkg(float x, float y, float z, float qw, float qx, float qy, float qz)
39{
40 PosePkg posePkg;
41 posePkg.pos = Vector3BasePtr(new Vector3(x, y, z));
42 posePkg.ori = QuaternionBasePtr(new Quaternion(qw, qx, qy, qz));
43 PoseBasePtr tmp = PoseBasePtr(new Pose(posePkg.pos, posePkg.ori));
44 posePkg.pose = tmp;
45 return posePkg;
46}
47
48bool
49isEqualRounded(PoseBasePtr current, PoseBasePtr other, double delta)
50{
51 if (current->orientation->qw - other->orientation->qw > delta &&
52 other->orientation->qw - current->orientation->qw > delta)
53 {
54 return false;
55 }
56 if (current->orientation->qx - other->orientation->qx > delta &&
57 other->orientation->qx - current->orientation->qx > delta)
58 {
59 return false;
60 }
61 if (current->orientation->qy - other->orientation->qy > delta &&
62 other->orientation->qy - current->orientation->qy > delta)
63 {
64 return false;
65 }
66 if (current->orientation->qz - other->orientation->qz > delta &&
67 other->orientation->qz - current->orientation->qz > delta)
68 {
69 return false;
70 }
71 if (current->position->x - other->position->x > delta &&
72 other->position->x - current->position->x > delta)
73 {
74 return false;
75 }
76 if (current->position->y - other->position->y > delta &&
77 other->position->y - current->position->y > delta)
78 {
79 return false;
80 }
81 if (current->position->z - other->position->z > delta &&
82 other->position->z - current->position->z > delta)
83 {
84 return false;
85 }
86 return true;
87}
88
89/////////////////////////////////////////////////////////////////////////////////////////////////////////////
90/// Tests basic functionality of Kineamtic Solver
91/////////////////////////////////////////////////////////////////////////////////////////////////////////////
92BOOST_AUTO_TEST_CASE(pleaseWorkTest)
93{
94 RobotPtr robot;
95 std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
96 CMakePackageFinder finder("RobotAPI");
97
98 if (finder.packageFound())
99 {
100 robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
101 }
102 KinematicSolverPtr ks1 = KinematicSolver::getInstance(NULL, robot);
103 vector<double> v = {2, 3, 4, 5, 0, 1, 2, 3, 3, 4};
104 ks1->doForwardKinematic(robot->getRobotNodeSet("TorsoRightArm"), v);
105}
106
107/////////////////////////////////////////////////////////////////////////////////////////////////////////////
108/// Tests whether the KinematicSolver is really a singleton so there is no Problem instantiating the instance
109/////////////////////////////////////////////////////////////////////////////////////////////////////////////
110BOOST_AUTO_TEST_CASE(singletonTest)
111{
112 RobotPtr robot;
113 std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
114 CMakePackageFinder finder("RobotAPI");
115
116 if (finder.packageFound())
117 {
118 robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
119 }
120 std::cout << robot->getRobotNodeSet("TorsoRightArm")->getAllRobotNodes().size();
121 KinematicSolverPtr ks1 = KinematicSolver::getInstance(NULL, robot);
122 ks1->doForwardKinematic(robot->getRobotNodeSet("TorsoRightArm"),
123 {2, 3, 4, 5, 0, 1, 2, 3, 3, 4});
124 KinematicSolverPtr ks2 = KinematicSolver::getInstance(NULL, NULL);
125 ks2->doForwardKinematic(robot->getRobotNodeSet("TorsoRightArm"),
126 {2, 3, 4, 5, 0, 1, 2, 3, 3, 4});
127 KinematicSolverPtr ks3 = KinematicSolver::getInstance(NULL, robot);
128 ks2->doForwardKinematic(robot->getRobotNodeSet("TorsoRightArm"),
129 {2, 3, 4, 5, 0, 1, 2, 3, 3, 4});
130 KinematicSolver::reset();
131 //shouldnt crash after reset
132 ks1->doForwardKinematic(robot->getRobotNodeSet("TorsoRightArm"),
133 {2, 3, 4, 5, 0, 1, 2, 3, 3, 4});
134}
135
136/////////////////////////////////////////////////////////////////////////////////////////////////////////////
137/// Tests whether the calculated Joint Angles are really there to reach the desired Pose and not a totally different
138/// Pose; furhermore tests whether doForwardKinematic is really the inverse function of the IKSolving functions
139/////////////////////////////////////////////////////////////////////////////////////////////////////////////
140BOOST_AUTO_TEST_CASE(backAndForthTest)
141{
142 RobotPtr robot;
143 std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
144 CMakePackageFinder finder("RobotAPI");
145
146 if (finder.packageFound())
147 {
148 robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
149 }
150 KinematicSolverPtr ks1 = KinematicSolver::getInstance(NULL, robot);
151 PoseBasePtr pose = PoseBasePtr(
152 new Pose(new Vector3(-300, 750, 1250), OrientationConversion::toQuaternion(0, 0, 0)));
153 PoseBasePtr result =
154 ks1->doForwardKinematic(robot->getRobotNodeSet("TorsoLeftArm"),
155 ks1->solveIK(robot->getRobotNodeSet("TorsoLeftArm"),
156 pose,
157 IKSolver::CartesianSelection::Position,
158 50));
159 BOOST_CHECK_CLOSE(pose->position->x, result->position->x, 1);
160 BOOST_CHECK_CLOSE(pose->position->y, result->position->y, 1);
161 BOOST_CHECK_CLOSE(pose->position->z, result->position->z, 1);
162
163 /*
164 //ks1->solveRecursiveIK(robot->getRobotNodeSet("TorsoLeftArm"), robot->getRobotNodeSet("TorsoLeftArm")->getJointValues() , pose, IKSolver::CartesianSelection::Position);
165 //result = ks1->doForwardKinematic(robot->getRobotNodeSet("TorsoLeftArm"),);
166 BOOST_CHECK_CLOSE(pose->position->x, result->position->x, 1);
167 BOOST_CHECK_CLOSE(pose->position->y, result->position->y, 1);
168 BOOST_CHECK_CLOSE(pose->position->z, result->position->z, 1);
169
170 //result = ks1->doForwardKinematic(robot->getRobotNodeSet("TorsoLeftArm"), ks1->solveRecursiveIK(robot->getRobotNodeSet("TorsoLeftArm"), robot->getRobotNodeSet("TorsoLeftArm")->getJointValues() , pose, IKSolver::CartesianSelection::Position));
171 BOOST_CHECK_EQUAL(pose->position->x, result->position->x);
172 BOOST_CHECK_EQUAL(pose->position->y, result->position->y);
173 BOOST_CHECK_EQUAL(pose->position->z, result->position->z);
174
175 //result = ks1->doForwardKinematic(robot->getRobotNodeSet("TorsoLeftArm"), ks1->solveRecursiveIK(robot->getRobotNodeSet("TorsoLeftArm"), robot->getRobotNodeSet("TorsoLeftArm")->getJointValues() , pose, IKSolver::CartesianSelection::Position));
176 BOOST_CHECK_EQUAL(pose->position->x, result->position->x);
177 BOOST_CHECK_EQUAL(pose->position->y, result->position->y);
178 BOOST_CHECK_EQUAL(pose->position->z, result->position->z);*/
179}
180
181/////////////////////////////////////////////////////////////////////////////////////////////////////////////
182/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for linear
183/// interpolation.
184/////////////////////////////////////////////////////////////////////////////////////////////////////////////
185BOOST_AUTO_TEST_CASE(recursiveIKLinear)
186{
187 RobotPtr robot;
188 std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
189 CMakePackageFinder finder("RobotAPI");
190
191 if (finder.packageFound())
192 {
193 robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
194 }
195 KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
196 Vector3BasePtr pos1 = new Vector3(-100, 700, 1000);
197 QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(1, 1, 1);
198 PoseBasePtr pose1 = new Pose(pos1, ori1);
199
200 Vector3BasePtr pos2 = new Vector3(0, 600, 1400);
201 QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(0, 0, 0);
202 PoseBasePtr pose2 = new Pose(pos2, ori2);
203
204
205 Vector3BasePtr pos3 = new Vector3(-250, 700, 1350);
206 QuaternionBasePtr ori3 = OrientationConversion::toQuaternion(0, 0, 0);
207 PoseBasePtr pose3 = new Pose(pos3, ori3);
208
209 std::vector<PoseBasePtr> cp = {pose1, pose2, pose3};
210 LinearInterpolationPtr ip = *new LinearInterpolationPtr(new LinearInterpolation(cp));
211
212
213 std::vector<PoseBasePtr> myPoints = std::vector<PoseBasePtr>();
214 std::vector<IKSolver::CartesianSelection> mySelection =
215 std::vector<IKSolver::CartesianSelection>();
216 for (double d = 0; d < 1; d = d + 0.01f)
217 {
218 myPoints.push_back(ip->getPoseAt(d));
219 mySelection.push_back(IKSolver::CartesianSelection::All);
220 }
221 std::vector<std::vector<double>> result =
222 ks->solveRecursiveIK(robot->getRobotNodeSet("TorsoLeftArm"),
223 ks->solveIK(robot->getRobotNodeSet("TorsoLeftArm"),
224 pose1,
225 IKSolver::CartesianSelection::Position,
226 50),
227 myPoints,
228 mySelection);
229 /*for (vector<double> tempResult : result)
230 {
231 for (double angle : tempResult)
232 {
233 //std::cout << std::to_string(angle) + ",";
234 }
235 //std::cout << "----------------------------------\n";*/
236}
237
238/////////////////////////////////////////////////////////////////////////////////////////////////////////////
239/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for spline
240/// interpolation.
241/////////////////////////////////////////////////////////////////////////////////////////////////////////////
242/*REACHABLE START
243
244{JOINTANGLES
245{
246 "Shoulder 1 L": 0.464789,
247 "Shoulder 2 L": 0.357171,
248 "Upperarm L": -0.303694,
249 "Elbow L": -0.067161,
250 "Underarm L": -0.445988,
251 "Wrist 1 L": -0.204292,
252 "Wrist 2 L": -0.382231
253}
254}
255{GLOBALPOSE ,
256{
257{
258 "agent" : "Armar3_1",
259 "frame" : "Armar3_Base",
260 "qw" : 0.4438760876655579,
261 "qx" : -0.4535938203334808,
262 "qy" : 0.5431225895881653,
263 "qz" : 0.5497677922248840,
264 "type" : "::armarx::FramedPoseBase",
265 "x" : -493.14355468750,
266 "y" : 415.3535156250,
267 "z" : 1071.592285156250
268}
269
270}
271
272}
273----------------------------------------------------------------------------------------
274REACHABLE MIDDLE
275
276{
277 "Shoulder 1 L": -0.147836,
278 "Shoulder 2 L": -0.132739,
279 "Upperarm L": 0.418313,
280 "Elbow L": -0.248675,
281 "Underarm L": 0.039376,
282 "Wrist 1 L": 0.385483,
283 "Wrist 2 L": -0.070153
284}
285
286{
287 "qw" : 0.4338901340961456,
288 "qx" : -0.4268467724323273,
289 "qy" : 0.5642792582511902,
290 "qz" : 0.5577904582023621,
291 "x" : -182.23925781250,
292 "y" : 580.074218750,
293 "z" : 1034.98925781250
294}
295---------------------------------------------------------------------------------------------
296REACHABLE END
297{
298 "Shoulder 1 L": -0.196959,
299 "Shoulder 2 L": -0.096939,
300 "Upperarm L": 0.202604,
301 "Elbow L": 0.307059,
302 "Underarm L": 0.128317,
303 "Wrist 1 L": 0.105766,
304 "Wrist 2 L": 0.513981
305}
306
307{
308 "qw" : 0.4336481690406799,
309 "qx" : -0.4273631870746613,
310 "qy" : 0.5638203620910645,
311 "qz" : 0.5580471754074097,
312 "x" : -226.792480468750,
313 "y" : 580.723144531250,
314 "z" : 1186.157348632812
315}---------------------------------------------------------------------------------------------
316*/
317BOOST_AUTO_TEST_CASE(recursiveIKThreeCP)
318{
319 std::vector<double> ja = {
320 0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
321 PosePkg p1 = createPosePkg(-348.304718,
322 580.476440,
323 712.264465,
324 0.4453609585762024,
325 -0.4538218379020691,
326 0.5429607033729553,
327 0.5485371351242065); //reachable
328 PosePkg p2 = createPosePkg(-182.23925781250,
329 580.074218750,
330 1034.98925781250,
331 0.4338901340961456,
332 -0.4268467724323273,
333 0.5642792582511902,
334 0.5577904582023621); //reachable
335 PosePkg p3 = createPosePkg(-226.792480468750,
336 580.723144531250,
337 1186.157348632812,
338 0.4336481690406799,
339 -0.4273631870746613,
340 0.5638203620910645,
341 0.5580471754074097); //reachable
342
343 std::vector<AbstractInterpolationPtr> ip =
344 InterpolationSegmentFactory::produceLinearInterpolationSegments(
345 {p1.pose, p2.pose, p3.pose});
346 std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
347 std::vector<IKSolver::CartesianSelection> mySelection =
348 std::vector<IKSolver::CartesianSelection>();
349 for (AbstractInterpolationPtr current : ip)
350 {
351 for (int i = 0; i < 100; i++)
352 {
353 poses.push_back(current->getPoseAt(i / 100.0));
354 std::cout << std::to_string(current->getPoseAt(i / 100.0)->position->x) + "\n";
355 mySelection.push_back(IKSolver::CartesianSelection::Position);
356 }
357 }
358
359 RobotPtr robot;
360 std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
361 CMakePackageFinder finder("RobotAPI");
362
363 if (finder.packageFound())
364 {
365 robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
366 }
367 KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
368 //Look whther first Pose is reachable
369 PoseBasePtr result2 = ks->doForwardKinematicRelative(robot->getRobotNodeSet("LeftArm"), ja);
370 std::vector<std::vector<double>> result =
371 ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
372 for (vector<double> tempResult : result)
373 {
374 for (double angle : tempResult)
375 {
376 std::cout << std::to_string(angle) + ",";
377 }
378 //std::cout << "----------------------------------\n";
379 }
380}
381
382/////////////////////////////////////////////////////////////////////////////////////////////////////////////
383/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for spline
384/// interpolation.
385/////////////////////////////////////////////////////////////////////////////////////////////////////////////
386/*
387{
388 "Shoulder 1 L": -0.818259,
389 "Shoulder 2 L": 0.428277,
390 "Upperarm L": 0.136299,
391 "Elbow L": -0.608422,
392 "Underarm L": 1.064518,
393 "Wrist 1 L": -0.227969,
394 "Wrist 2 L": -0.289433
395
396{
397 "agent" : "Armar3_2",
398 "frame" : "Armar3_Base",
399 "qw" : -0.03198593109846115,
400 "qx" : 0.01920612715184689,
401 "qy" : 0.7076730132102966,
402 "qz" : 0.7055543065071106,
403 "type" : "::armarx::FramedPoseBase",
404 "x" : -502.756347656250,
405 "y" : 702.6647949218750,
406 "z" : 1286.8144531250
407}
408-----------------------------------------------------------------
409{
410 "Shoulder 1 L": -0.656211,
411 "Shoulder 2 L": 0.329582,
412 "Upperarm L": 0.476974,
413 "Elbow L": 0.139211,
414 "Underarm L": 2.697165,
415 "Wrist 1 L": -0.446896,
416 "Wrist 2 L": -0.089351
417}
418
419{
420 "agent" : "Armar3_2",
421 "frame" : "Armar3_Base",
422 "qw" : 0.6405168175697327,
423 "qx" : -0.3970025181770325,
424 "qy" : -0.3363495767116547,
425 "qz" : -0.5647976398468018,
426 "type" : "::armarx::FramedPoseBase",
427 "x" : -290.5917968750,
428 "y" : 636.383300781250,
429 "z" : 1363.315063476562
430}
431-----------------------------------------------------------------
432{
433 "Shoulder 1 L": -0.963948,
434 "Shoulder 2 L": -0.103554,
435 "Upperarm L": -0.255582,
436 "Elbow L": -0.983395,
437 "Underarm L": -0.254656,
438 "Wrist 1 L": -0.458602,
439 "Wrist 2 L": 0.268590
440}
441
442{
443 "agent" : "Armar3_2",
444 "frame" : "Armar3_Base",
445 "qw" : 0.5907033681869507,
446 "qx" : -0.5503066778182983,
447 "qy" : 0.4992305040359497,
448 "qz" : 0.3146440684795380,
449 "type" : "::armarx::FramedPoseBase",
450 "x" : -316.302246093750,
451 "y" : 777.949218750,
452 "z" : 1194.246459960938
453}
454*/
455BOOST_AUTO_TEST_CASE(recursiveIKThreeCPWithRotation)
456{
457 std::vector<double> ja = {
458 -0.818259, 0.428277, 0.136299, -0.608422, 1.064518, -0.227969, -0.289433};
459 PosePkg p1 = createPosePkg(-337.719116,
460 927.767395,
461 907.007935,
462 -0.03198593109846115,
463 0.01920612715184689,
464 0.7076730132102966,
465 0.7055543065071106);
466 PosePkg p2 = createPosePkg(-290.5917968750,
467 636.383300781250,
468 1363.315063476562,
469 0.6405168175697327,
470 -0.3970025181770325,
471 -0.3363495767116547,
472 -0.5647976398468018);
473 PosePkg p3 = createPosePkg(-316.302246093750,
474 777.949218750,
475 1194.246459960938,
476 0.5907033681869507,
477 -0.5503066778182983,
478 0.4992305040359497,
479 0.3146440684795380);
480
481 std::vector<AbstractInterpolationPtr> ip =
482 InterpolationSegmentFactory::produceLinearInterpolationSegments(
483 {p1.pose, p2.pose, p3.pose});
484 std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
485 std::vector<IKSolver::CartesianSelection> mySelection =
486 std::vector<IKSolver::CartesianSelection>();
487 for (AbstractInterpolationPtr current : ip)
488 {
489 for (int i = 0; i < 50; i++)
490 {
491 poses.push_back(current->getPoseAt(i / 50.0));
492 mySelection.push_back(IKSolver::CartesianSelection::Position);
493 }
494 }
495
496 RobotPtr robot;
497 std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
498 CMakePackageFinder finder("RobotAPI");
499
500 if (finder.packageFound())
501 {
502 robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
503 }
504 KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
505 //Look whther first Pose is reachable
506 PoseBasePtr result2 = ks->doForwardKinematicRelative(robot->getRobotNodeSet("LeftArm"), ja);
507 //std::cout << "XXXXXXXXX" + std::to_string(result2->position->x) + "," + std::to_string(result2->position->y) + "," + std::to_string(result2->position->z) + "\n";
508 std::vector<std::vector<double>> result =
509 ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
510 for (vector<double> tempResult : result)
511 {
512 for (double angle : tempResult)
513 {
514 std::cout << std::to_string(angle) + ",";
515 }
516 std::cout << "----------------------------------\n";
517 }
518}
519
520/////////////////////////////////////////////////////////////////////////////////////////////////////////////
521/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for spline
522/// interpolation.
523/////////////////////////////////////////////////////////////////////////////////////////////////////////////
524/*REACHABLE START
525
526UNREACHABLE START
527
528{
529 "Shoulder 1 L": -0.248207,
530 "Shoulder 2 L": -0.235603,
531 "Upperarm L": 0.130206,
532 "Elbow L": -1.000947,
533 "Underarm L": 0.057843,
534 "Wrist 1 L": 0.197757,
535 "Wrist 2 L": -0.663139
536}
537
538{
539 "agent" : "Armar3_1",
540 "frame" : "Armar3_Base",
541 "qw" : 0.4207711517810822,
542 "qx" : -0.4548574388027191,
543 "qy" : 0.5738259553909302,
544 "qz" : 0.5355183482170105,
545 "type" : "::armarx::FramedPoseBase",
546 "x" : -234.163574218750,
547 "y" : 562.9924316406250,
548 "z" : 902.9812011718750
549}
550-----------------------------------------------------------------
551{
552 "Shoulder 1 L": -0.763830,
553 "Shoulder 2 L": -0.244323,
554 "Upperarm L": 0.619645,
555 "Elbow L": -0.281197,
556 "Underarm L": 0.040509,
557 "Wrist 1 L": 0.444159,
558 "Wrist 2 L": 0.536926
559}
560
561{
562 "agent" : "Armar3_1",
563 "frame" : "Armar3_Base",
564 "qw" : 0.4365486800670624,
565 "qx" : -0.4239800870418549,
566 "qy" : 0.5666229724884033,
567 "qz" : 0.5555219650268555,
568 "type" : "::armarx::FramedPoseBase",
569 "x" : -95.164550781250,
570 "y" : 705.6569824218750,
571 "z" : 1245.026000976562
572}---------------------------------------------------------------------------------------------
573*/
574BOOST_AUTO_TEST_CASE(unreachableStart)
575{
576 std::vector<double> ja = {
577 0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
578 PosePkg p1 = createPosePkg(-348.304718,
579 580.476440,
580 712.264465,
581 0.4453609585762024,
582 -0.4538218379020691,
583 0.5429607033729553,
584 0.5485371351242065); //unreachable
585 PosePkg p2 = createPosePkg(-182.23925781250,
586 580.074218750,
587 1034.98925781250,
588 0.4338901340961456,
589 -0.4268467724323273,
590 0.5642792582511902,
591 0.5577904582023621); //reachable
592
593 std::vector<AbstractInterpolationPtr> ip =
594 InterpolationSegmentFactory::produceLinearInterpolationSegments({p1.pose, p2.pose});
595 std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
596 std::vector<IKSolver::CartesianSelection> mySelection =
597 std::vector<IKSolver::CartesianSelection>();
598 for (AbstractInterpolationPtr current : ip)
599 {
600 for (int i = 0; i < 50; i++)
601 {
602 poses.push_back(current->getPoseAt(i / 50.0));
603 //std::cout << std::to_string(current->getPoseAt(i / 50.0)->position->x) + "\n";
604 mySelection.push_back(IKSolver::CartesianSelection::Position);
605 }
606 }
607
608 RobotPtr robot;
609 std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
610 CMakePackageFinder finder("RobotAPI");
611
612 if (finder.packageFound())
613 {
614 robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
615 }
616 KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
617 //Look whther first Pose is reachable
618 PoseBasePtr result2 = ks->doForwardKinematicRelative(robot->getRobotNodeSet("LeftArm"), ja);
619 std::vector<std::vector<double>> result =
620 ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
621 for (vector<double> tempResult : result)
622 {
623 for (double angle : tempResult)
624 {
625 std::cout << std::to_string(angle) + ",";
626 }
627 std::cout << "----------------------------------\n";
628 }
629}
630
631/////////////////////////////////////////////////////////////////////////////////////////////////////////////
632/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for spline
633/// interpolation.
634/////////////////////////////////////////////////////////////////////////////////////////////////////////////
635/*
636
637UNREACHABLE END
638
639{
640 "Shoulder 1 L": -0.763830,
641 "Shoulder 2 L": -0.244323,
642 "Upperarm L": 0.619645,
643 "Elbow L": -0.281197,
644 "Underarm L": 0.040509,
645 "Wrist 1 L": 0.444159,
646 "Wrist 2 L": 0.536926
647}
648
649{
650 "agent" : "Armar3_1",
651 "frame" : "Armar3_Base",
652 "qw" : 0.4365486800670624,
653 "qx" : -0.4239800870418549,
654 "qy" : 0.5666229724884033,
655 "qz" : 0.5555219650268555,
656 "type" : "::armarx::FramedPoseBase",
657 "x" : -95.164550781250,
658 "y" : 705.6569824218750,
659 "z" : 1245.026000976562
660}
661-----------------------------------------------------------------
662{
663 "Shoulder 1 L": -1.132457,
664 "Shoulder 2 L": -0.244346,
665 "Upperarm L": 1.040296,
666 "Elbow L": -0.892947,
667 "Underarm L": -0.627532,
668 "Wrist 1 L": 0.523122,
669 "Wrist 2 L": 0.100948
670}
671
672{
673 "agent" : "Armar3_1",
674 "frame" : "Armar3_Base",
675 "qw" : 0.4369190335273743,
676 "qx" : -0.4234801232814789,
677 "qy" : 0.5673816800117493,
678 "qz" : 0.5548373460769653,
679 "type" : "::armarx::FramedPoseBase",
680 "x" : -93.015136718750,
681 "y" : 784.5754394531250,
682 "z" : 1246.448608398438
683}---------------------------------------------------------------------------------------------
684*/
685BOOST_AUTO_TEST_CASE(UnreachableEnd)
686{
687 std::vector<double> ja = {
688 -0.763830, -0.244323, 0.619645, -0.281197, 0.040509, 0.444159, 0.536926};
689 PosePkg p1 = createPosePkg(4.825809,
690 788.516541,
691 1083.528442,
692 0.4453609585762024,
693 -0.4538218379020691,
694 0.5429607033729553,
695 0.5485371351242065); //reachable
696 PosePkg p2 = createPosePkg(-93.015136718750,
697 784.5754394531250,
698 500.448608398438,
699 0.4338901340961456,
700 -0.4268467724323273,
701 0.5642792582511902,
702 0.5577904582023621); //unreachable
703
704 std::vector<AbstractInterpolationPtr> ip =
705 InterpolationSegmentFactory::produceLinearInterpolationSegments({p1.pose, p2.pose});
706 std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
707 std::vector<IKSolver::CartesianSelection> mySelection =
708 std::vector<IKSolver::CartesianSelection>();
709 for (AbstractInterpolationPtr current : ip)
710 {
711 for (int i = 0; i < 50; i++)
712 {
713 poses.push_back(current->getPoseAt(i / 50.0));
714 std::cout << std::to_string(current->getPoseAt(i / 50.0)->position->x) + "\n";
715 mySelection.push_back(IKSolver::CartesianSelection::Position);
716 }
717 }
718
719 RobotPtr robot;
720 std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
721 CMakePackageFinder finder("RobotAPI");
722
723 if (finder.packageFound())
724 {
725 robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
726 }
727 KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
728 //Look whther first Pose is reachable
729 PoseBasePtr result2 = ks->doForwardKinematicRelative(robot->getRobotNodeSet("LeftArm"), ja);
730 //std::cout << "XXXXXXXXX" + std::to_string(result2->position->x) + "," + std::to_string(result2->position->y) + "," + std::to_string(result2->position->z) + "\n";
731 //BOOST_CHECK_THROW(ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection), armarx::LocalException);
732}
733
734/////////////////////////////////////////////////////////////////////////////////////////////////////////////
735/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for spline
736/// interpolation.
737/////////////////////////////////////////////////////////////////////////////////////////////////////////////
738/*REACHABLE START
739
740ENREACHABLE MID
741
742{
743 "agent" : "Armar3_1",
744 "frame" : "Armar3_Base",
745 "qw" : 0.4367099106311798,
746 "qx" : -0.4243824779987335,
747 "qy" : 0.5662068724632263,
748 "qz" : 0.5555122494697571,
749 "type" : "::armarx::FramedPoseBase",
750 "x" : -112.750,
751 "y" : 696.7272949218750,
752 "z" : 1181.076660156250
753}
754
755{
756 "Shoulder 1 L": -0.663384,
757 "Shoulder 2 L": -0.188166,
758 "Upperarm L": 0.658974,
759 "Elbow L": -0.346670,
760 "Underarm L": -0.050623,
761 "Wrist 1 L": 0.490788,
762 "Wrist 2 L": 0.335159
763}
764-----------------------------------------------------------------
765{
766 "Shoulder 1 L": -1.346342,
767 "Shoulder 2 L": -0.244346,
768 "Upperarm L": 0.968929,
769 "Elbow L": -0.641009,
770 "Underarm L": -0.288792,
771 "Wrist 1 L": 0.454785,
772 "Wrist 2 L": 0.657779
773}
774
775{
776 "agent" : "Armar3_1",
777 "frame" : "Armar3_Base",
778 "qw" : 0.3848522007465363,
779 "qx" : -0.3703520894050598,
780 "qy" : 0.6027946472167969,
781 "qz" : 0.5927618741989136,
782 "type" : "::armarx::FramedPoseBase",
783 "x" : -64.90527343750,
784 "y" : 766.2385253906250,
785 "z" : 1396.328857421875
786}
787-----------------------------------------------------------------
788{
789 "Shoulder 1 L": -1.531028,
790 "Shoulder 2 L": -0.213389,
791 "Upperarm L": 1.010730,
792 "Elbow L": -0.683772,
793 "Underarm L": 0.458575,
794 "Wrist 1 L": -0.436870,
795 "Wrist 2 L": 0.699134
796}
797
798{
799 "agent" : "Armar3_1",
800 "frame" : "Armar3_Base",
801 "qw" : -0.0135999433696270,
802 "qx" : 0.03095184452831745,
803 "qy" : 0.7067553400993347,
804 "qz" : 0.7066496610641479,
805 "type" : "::armarx::FramedPoseBase",
806 "x" : -79.7832031250,
807 "y" : 766.7092285156250,
808 "z" : 1471.756713867188
809}---------------------------------------------------------------------------------------------
810*/
811/////////////////////////////////////////////////////////////////////////////////////////////////////////////
812/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for spline
813/// interpolation.
814/////////////////////////////////////////////////////////////////////////////////////////////////////////////
815BOOST_AUTO_TEST_CASE(unreachableMid)
816{
817 std::vector<double> ja = {
818 -0.763830, -0.244323, 0.619645, -0.281197, 0.040509, 0.444159, 0.536926};
819 PosePkg p1 = createPosePkg(4.825809,
820 788.516541,
821 1083.528442,
822 0.4453609585762024,
823 -0.4538218379020691,
824 0.5429607033729553,
825 0.5485371351242065); //reachable
826 PosePkg p2 = createPosePkg(-93.015136718750,
827 784.5754394531250,
828 500.448608398438,
829 0.4338901340961456,
830 -0.4268467724323273,
831 0.5642792582511902,
832 0.5577904582023621); //unreachable
833 PosePkg p3 = createPosePkg(4.825809,
834 788.516541,
835 1083.528442,
836 0.4453609585762024,
837 -0.4538218379020691,
838 0.5429607033729553,
839 0.5485371351242065); //reachable
840
841
842 std::vector<AbstractInterpolationPtr> ip =
843 InterpolationSegmentFactory::produceLinearInterpolationSegments(
844 {p1.pose, p2.pose, p3.pose});
845 std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
846 std::vector<IKSolver::CartesianSelection> mySelection =
847 std::vector<IKSolver::CartesianSelection>();
848 for (AbstractInterpolationPtr current : ip)
849 {
850 for (int i = 0; i < 50; i++)
851 {
852 poses.push_back(current->getPoseAt(i / 50.0));
853 //std::cout << std::to_string(current->getPoseAt(i / 50.0)->position->x) + "\n";
854 mySelection.push_back(IKSolver::CartesianSelection::Position);
855 }
856 }
857 RobotPtr robot;
858 std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
859 CMakePackageFinder finder("RobotAPI");
860
861 if (finder.packageFound())
862 {
863 robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
864 }
865 KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
866 //Look whther first Pose is reachable
867 PoseBasePtr result2 = ks->doForwardKinematicRelative(robot->getRobotNodeSet("LeftArm"), ja);
868 //std::cout << "XXXXXXXXX" + std::to_string(result2->position->x) + "," + std::to_string(result2->position->y) + "," + std::to_string(result2->position->z) + "\n";
869 //BOOST_CHECK_THROW(ks->solveRecursiveIKRelative(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection), armarx::LocalException);
870}
871
872/*
873//COLLSIONSTART
874{
875GlobalPose
876-0.022014 -0.469058 0.882893 56.5798
877-0.0480052 0.882584 0.467697 233.545
878 -0.998605 -0.0320876 -0.0419464 992.508
879 0 0 0 1
880}
881{
8820.315176
883-0.0275324
8841.00421
8850.0182586
8860.127499
887-0.0255035
888-0.299747
889}
890-----------------------------------------------
891{
892GlobalPose
893-0.567009 -0.7504 0.339706 -95.4907
894 0.148646 0.312423 0.938241 761.478
895 -0.810188 0.582488 -0.0656025 1208.03
896 0 0 0 1
897}
898{
899-0.890715
900-0.244346
9010.487422
902-0.723896
9030.232289
904-0.0929337
9050.130688
906*/
907/////////////////////////////////////////////////////////////////////////////////////////////////////////////
908/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for spline
909/// interpolation.
910/////////////////////////////////////////////////////////////////////////////////////////////////////////////
911BOOST_AUTO_TEST_CASE(collisionStart)
912{
913 std::vector<double> ja = {
914 0.315176, -0.0275324, 1.00421, 0.0182586, 0.127499, -0.0255035, -0.299747};
915 PosePkg p1 = createPosePkg(56.5798,
916 233.545,
917 992.508,
918 0.4453609585762024,
919 -0.4538218379020691,
920 0.5429607033729553,
921 0.5485371351242065); //Colliding Pose
922 PosePkg p2 = createPosePkg(-95.4907,
923 761.478,
924 1208.03,
925 0.4338901340961456,
926 -0.4268467724323273,
927 0.5642792582511902,
928 0.5577904582023621); //Not Colliding Pose
929
930
931 std::vector<AbstractInterpolationPtr> ip =
932 InterpolationSegmentFactory::produceLinearInterpolationSegments({p1.pose, p2.pose});
933 std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
934 std::vector<IKSolver::CartesianSelection> mySelection =
935 std::vector<IKSolver::CartesianSelection>();
936 for (AbstractInterpolationPtr current : ip)
937 {
938 for (int i = 0; i < 50; i++)
939 {
940 poses.push_back(current->getPoseAt(i / 50.0));
941 //std::cout << std::to_string(current->getPoseAt(i / 50.0)->position->x) + "\n";
942 mySelection.push_back(IKSolver::CartesianSelection::Position);
943 }
944 }
945
946 RobotPtr robot;
947 std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
948 CMakePackageFinder finder("RobotAPI");
949
950 if (finder.packageFound())
951 {
952 robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
953 }
954 KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
955 //Look whther first Pose is reachable
956 PoseBasePtr result2 = ks->doForwardKinematic(robot->getRobotNodeSet("LeftArm"), ja);
957 std::cout << "XXXXXXXXX" + std::to_string(result2->position->x) + "," +
958 std::to_string(result2->position->y) + "," +
959 std::to_string(result2->position->z) + "\n";
960 std::vector<std::vector<double>> result =
961 ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
962 /*for (vector<double> tempResult : result)
963 {
964 for (double angle : tempResult)
965 {
966 //std::cout << std::to_string(angle) + ",";
967 }
968 std::cout << "----------------------------------\n";
969 }*/
970}
971
972/////////////////////////////////////////////////////////////////////////////////////////////////////////////
973/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for spline
974/// interpolation.
975/////////////////////////////////////////////////////////////////////////////////////////////////////////////
976/*
977 * // START
978 Global Pose
979-0.471909 -0.163874 0.866284 3.02088
980 0.39698 0.837837 0.374748 389.723
981-0.787216 0.520744 -0.330328 938.033
982 0 0 0 1
983JointAngles
984-0.0475329
985-0.0275045
9860.790436
987-0.205607
9880.561071
989-0.322912
990-0.256206
991
992//END
993
994Global Pose
995-0.222048 0.721908 0.655395 -167.276
9960.00806859 0.673513 -0.739131 -187.905
997-0.975002 -0.158835 -0.155377 999.493
998 0 0 0 1
999jointAngles
10000.733038
10010.535259
10021.22173
10030.0361974
10040.991169
1005-0.506145
1006-0.663225
1007 *
1008 *
1009}*/
1010BOOST_AUTO_TEST_CASE(CollisonMid)
1011{
1012 std::vector<double> ja = {
1013 -0.0475329, -0.0275045, 0.790436, -0.205607, 0.561071, -0.322912, -0.256206};
1014 PosePkg p1 = createPosePkg(3.02088,
1015 389.723,
1016 938.033,
1017 0.4453609585762024,
1018 -0.4538218379020691,
1019 0.5429607033729553,
1020 0.5485371351242065); //no collision here
1021 //in between there is a collision with robot body
1022 PosePkg p2 = createPosePkg(-167.276,
1023 -187.905,
1024 999.493,
1025 0.4338901340961456,
1026 -0.4268467724323273,
1027 0.5642792582511902,
1028 0.5577904582023621); //no one here either
1029
1030 std::vector<AbstractInterpolationPtr> ip =
1031 InterpolationSegmentFactory::produceLinearInterpolationSegments({p1.pose, p2.pose});
1032 std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
1033 std::vector<IKSolver::CartesianSelection> mySelection =
1034 std::vector<IKSolver::CartesianSelection>();
1035 for (AbstractInterpolationPtr current : ip)
1036 {
1037 for (int i = 0; i < 50; i++)
1038 {
1039 poses.push_back(current->getPoseAt(i / 50.0));
1040 //std::cout << std::to_string(current->getPoseAt(i / 50.0)->position->x) + "\n";
1041 mySelection.push_back(IKSolver::CartesianSelection::Position);
1042 }
1043 }
1044
1045 RobotPtr robot;
1046 std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
1047 CMakePackageFinder finder("RobotAPI");
1048
1049 if (finder.packageFound())
1050 {
1051 robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
1052 }
1053 KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
1054 //Look whther first Pose is reachable
1055 PoseBasePtr result2 = ks->doForwardKinematicRelative(robot->getRobotNodeSet("LeftArm"), ja);
1056 std::vector<std::vector<double>> result =
1057 ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
1058 /*for (vector<double> tempResult : result)
1059 {
1060 for (double angle : tempResult)
1061 {
1062 //std::cout << std::to_string(angle) + ",";
1063 }
1064 std::cout << "----------------------------------\n";
1065 }*/
1066}
1067
1068/////////////////////////////////////////////////////////////////////////////////////////////////////////////
1069/// No Movement
1070/////////////////////////////////////////////////////////////////////////////////////////////////////////////
1071
1072
1073/////////////////////////////////////////////////////////////////////////////////////////////////////////////
1074/// Tests recursive IK method in "realistic setting - e.g. for calclating the Pose necessarry for spline
1075/// interpolation.
1076/////////////////////////////////////////////////////////////////////////////////////////////////////////////
1077BOOST_AUTO_TEST_CASE(NoMovement)
1078{
1079 std::cout << "NO MOVEMENT START";
1080 std::vector<double> ja = {
1081 -0.0475329, -0.0275045, 0.790436, -0.205607, 0.561071, -0.322912, -0.256206};
1082 PosePkg p1 = createPosePkg(3.02088,
1083 389.723,
1084 938.033,
1085 0.4453609585762024,
1086 -0.4538218379020691,
1087 0.5429607033729553,
1088 0.5485371351242065); //no collision here
1089 PosePkg p2 = createPosePkg(3.02088,
1090 389.723,
1091 938.033,
1092 0.4453609585762024,
1093 -0.4538218379020691,
1094 0.5429607033729553,
1095 0.5485371351242065); //no collision here
1096
1097 std::vector<PoseBasePtr> cp = {p1.pose, p2.pose};
1098 LinearInterpolationPtr ip = *new LinearInterpolationPtr(new LinearInterpolation(cp));
1099 std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
1100 std::vector<IKSolver::CartesianSelection> mySelection =
1101 std::vector<IKSolver::CartesianSelection>();
1102 for (int i = 0; i < 50; i++)
1103 {
1104 poses.push_back(ip->getPoseAt(i / 50.0));
1105 std::cout << std::to_string(ip->getPoseAt(i / 50.0)->position->x) + "\n";
1106 mySelection.push_back(IKSolver::CartesianSelection::Position);
1107 }
1108
1109
1110 RobotPtr robot;
1111 std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
1112 CMakePackageFinder finder("RobotAPI");
1113
1114 if (finder.packageFound())
1115 {
1116 robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
1117 }
1118 KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
1119 //Look whther first Pose is reachable
1120 std::vector<std::vector<double>> result =
1121 ks->solveRecursiveIK(robot->getRobotNodeSet("LeftArm"), ja, poses, mySelection);
1122 for (vector<double> tempResult : result)
1123 {
1124 for (double angle : tempResult)
1125 {
1126 std::cout << std::to_string(angle) + ",";
1127 }
1128 std::cout << "----------------------------------\n";
1129 }
1130 std::cout << "NO MOVEMENT END";
1131}
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(pleaseWorkTest)
Tests basic functionality of Kineamtic Solver.
Definition KinematicSolverTest.cpp:92
isEqualRounded
bool isEqualRounded(PoseBasePtr current, PoseBasePtr other, double delta)
Definition KinematicSolverTest.cpp:49
createPosePkg
PosePkg createPosePkg(float x, float y, float z, float qw, float qx, float qy, float qz)
Definition KinematicSolverTest.cpp:38
armarx::CMakePackageFinder
The CMakePackageFinder class provides an interface to the CMake Package finder capabilities.
Definition CMakePackageFinder.h:53
armarx::CMakePackageFinder::packageFound
bool packageFound() const
Returns whether or not this package was found with cmake.
Definition CMakePackageFinder.cpp:511
armarx::InterpolationSegmentFactory::produceLinearInterpolationSegments
static std::vector< AbstractInterpolationPtr > produceLinearInterpolationSegments(std::vector< PoseBasePtr > controlPoints)
produceInterpolationSegments constructs a vector of LinearInterpolations
Definition InterpolationSegmentFactory.cpp:161
armarx::KinematicSolver::getInstance
static std::shared_ptr< KinematicSolver > getInstance(VirtualRobot::ScenePtr scenario, VirtualRobot::RobotPtr robot)
SINGLETON-FEATURES///////////////////////////////////////////////////////////////////////////////////...
Definition KinematicSolver.cpp:105
armarx::KinematicSolver::reset
static void reset()
reset Destroys the current instance of KinematicSolver that a new one can be created via call of getI...
Definition KinematicSolver.cpp:117
armarx::LinearInterpolation
The LinearInterpolation class represents a linear Interpolation between a series of control points Li...
Definition LinearInterpolation.h:36
armarx::OrientationConversion::toQuaternion
static QuaternionBasePtr toQuaternion(const double roll, const double pitch, const double yaw)
Definition OrientationConversion.cpp:58
armarx::Pose
The Pose class.
Definition Pose.h:243
armarx::Quaternion
The Quaternion class.
Definition Pose.h:174
armarx::Vector3
The Vector3 class.
Definition Pose.h:113
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition Bus.h:19
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition ArmarXTimeserver.cpp:28
armarx::KinematicSolverPtr
std::shared_ptr< KinematicSolver > KinematicSolverPtr
Definition KinematicSolver.h:287
armarx::LinearInterpolationPtr
std::shared_ptr< LinearInterpolation > LinearInterpolationPtr
Definition LinearInterpolation.h:56
armarx::AbstractInterpolationPtr
std::shared_ptr< AbstractInterpolation > AbstractInterpolationPtr
Definition AbstractInterpolation.h:76
angle
double angle(const Point &a, const Point &b, const Point &c)
Definition point.hpp:109
PosePkg
The PosePkg struct - Testing utility.
Definition DesignerTrajectoryHolderTest.cpp:16
PosePkg::ori
QuaternionBasePtr ori
Definition DesignerTrajectoryHolderTest.cpp:19