DesignerTrajectoryManagerTest.cpp
Go to the documentation of this file.
1#define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::DesignerTrajectoryManager
2#define ARMARX_BOOST_TEST
3
4
5#include "../Manager/DesignerTrajectoryManager.h"
6
7#include <VirtualRobot/XML/RobotIO.h>
8
9#include <ArmarXCore/core/logging/LogSender.h>
10#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
11
12#include <RobotComponents/Test.h>
13
14#include "../Interpolation/LinearInterpolation.h"
15#include "../Util/OrientationConversion.h"
16
17using namespace armarx;
18using namespace VirtualRobot;
19using namespace std;
20
21struct PosePkg
22{
23public:
24 Vector3BasePtr pos;
25 QuaternionBasePtr ori;
26 PoseBasePtr pose;
27};
28
29PosePkg
30createPosePkg(float x, float y, float z, float qw, float qx, float qy, float qz)
31{
32 PosePkg posePkg;
33 posePkg.pos = Vector3BasePtr(new Vector3(x, y, z));
34 posePkg.ori = QuaternionBasePtr(new Quaternion(qw, qx, qy, qz));
35 PoseBasePtr tmp = PoseBasePtr(new Pose(posePkg.pos, posePkg.ori));
36 posePkg.pose = tmp;
37 return posePkg;
38}
39
40double
41roundTo(double d, int accuracy)
42{
43 return floor(d * pow10(accuracy)) / pow10(accuracy);
44}
45
46bool
47equalPoseBase(PoseBasePtr p1, PoseBasePtr p2)
48{
49 int accuracy = 5;
50
51 // check position
52 if (roundTo(p1->position->x, accuracy) != roundTo(p2->position->x, accuracy))
53 {
54 return false;
55 }
56 if (roundTo(p1->position->y, accuracy) != roundTo(p2->position->y, accuracy))
57 {
58 return false;
59 }
60 if (roundTo(p1->position->z, accuracy) != roundTo(p2->position->z, accuracy))
61 {
62 return false;
63 }
64
65 // check orientation
66 if (roundTo(p1->orientation->qw, accuracy) != roundTo(p2->orientation->qw, accuracy))
67 {
68 return false;
69 }
70
71 if (roundTo(p1->orientation->qx, accuracy) != roundTo(p2->orientation->qx, accuracy))
72 {
73 return false;
74 }
75
76 if (roundTo(p1->orientation->qy, accuracy) != roundTo(p2->orientation->qy, accuracy))
77 {
78 return false;
79 }
80 if (roundTo(p1->orientation->qz, accuracy) != roundTo(p2->orientation->qz, accuracy))
81 {
82 return false;
83 }
84
85 return true;
86}
87
88bool
89equalTrajectory(armarx::TrajectoryPtr t1, armarx::TrajectoryPtr t2)
90{
91 // check dimension
92 if (t1->dim() != t2->dim())
93 {
94 return false;
95 }
96
97 // check data (joint angles)
98 for (unsigned int dim = 0; dim < t1->dim(); dim++)
99 {
100 if (t1->getDimensionData(dim) != t2->getDimensionData(dim))
101 {
102 return false;
103 }
104 }
105
106 // check timestamps
107 if (t1->getTimestamps() != t2->getTimestamps())
108 {
109 return false;
110 }
111
112 return true;
113}
114
115bool
116equalUserWaypoint(armarx::UserWaypointPtr u1, armarx::UserWaypointPtr u2)
117{
118 // check poses
119 if (equalPoseBase(u1->getPose(), u2->getPose()) == false)
120 {
121 return false;
122 }
123
124 // check joint angles
125 if (u1->getJointAngles() != u2->getJointAngles())
126 {
127 return false;
128 }
129
130 // check ik selection
131 if (u1->getIKSelection() != u2->getIKSelection())
132 {
133 return false;
134 }
135
136 // check is time optimal breakpoint
137 if (u1->getIsTimeOptimalBreakpoint() != u2->getIsTimeOptimalBreakpoint())
138 {
139 return false;
140 }
141
142 // check time optimal timestamp
143 if (u1->getTimeOptimalTimestamp() != u2->getTimeOptimalTimestamp())
144 {
145 return false;
146 }
147
148 // check user timestamp
149 if (u1->getUserTimestamp() != u2->getUserTimestamp())
150 {
151 return false;
152 }
153
154 return true;
155}
156
157bool
158equalTransition(armarx::TransitionPtr t1, armarx::TransitionPtr t2)
159{
160 // check start user waypoints
161 if (!equalUserWaypoint(t1->getStart(), t2->getStart()))
162 {
163 return false;
164 }
165
166 // check end user waypoints
167 if (!equalUserWaypoint(t1->getEnd(), t2->getEnd()))
168 {
169 return false;
170 }
171
172 // check time optimal duration
173 if (t1->getTimeOptimalDuration() != t2->getTimeOptimalDuration())
174 {
175 return false;
176 }
177
178 // check user duration
179 if (t1->getUserDuration() != t2->getUserDuration())
180 {
181 return false;
182 }
183
184 // check interpolation type
185 if (t1->getInterpolationType() != t2->getInterpolationType())
186 {
187 return false;
188 }
189
190 // check trajectory
191 if (!equalTrajectory(t1->getTrajectory(), t2->getTrajectory()))
192 {
193 return false;
194 }
195
196 return true;
197}
198
199bool
200equalDesignerTrajectory(DesignerTrajectoryPtr dt1, DesignerTrajectoryPtr dt2)
201{
202 // check inter breakpoint trajectories
203 std::vector<armarx::TrajectoryPtr> dt1_interBPT = dt1->getInterBreakpointTrajectories();
204 std::vector<armarx::TrajectoryPtr> dt2_interBPT = dt2->getInterBreakpointTrajectories();
205 if (dt1_interBPT.size() != dt2_interBPT.size())
206 {
207 return false;
208 }
209
210 for (unsigned int i = 0; i < dt1_interBPT.size(); i++)
211 {
212 if (!equalTrajectory(dt1_interBPT[i], dt2_interBPT[i]))
213 {
214 return false;
215 }
216 }
217
218 // check user waypoints
219 std::vector<UserWaypointPtr> dt1_uwp = dt1->getAllUserWaypoints();
220 std::vector<UserWaypointPtr> dt2_uwp = dt2->getAllUserWaypoints();
221 if (dt1_uwp.size() != dt2_uwp.size())
222 {
223 return false;
224 }
225
226 for (unsigned int i = 0; i < dt1_uwp.size(); i++)
227 {
228 if (!equalUserWaypoint(dt1_uwp[i], dt2_uwp[i]))
229 {
230 return false;
231 }
232 }
233
234 // check transitions
235 // the two designer trajectories have the same amount of transitions because
236 // they have the same amount of user waypoints (has been checked above)
237 for (unsigned int i = 0; i < dt1_uwp.size() - 1; i++)
238 {
239 if (!equalTransition(dt1->getTransition(i), dt2->getTransition(i)))
240 {
241 return false;
242 }
243 }
244
245 // check rns
246 if (dt1->getRns()->getName() != dt2->getRns()->getName())
247 {
248 return false;
249 }
250
251 return true;
252}
253
254PoseBasePtr
255poseToLocal(PoseBasePtr pose, RobotPtr robot, std::string& rnsName)
256{
257 return PoseBasePtr(
258 new Pose(robot->getRobotNodeSet(rnsName)->getKinematicRoot()->toLocalCoordinateSystem(
259 (new Pose(pose->position, pose->orientation))->toEigen())));
260}
261
262BOOST_AUTO_TEST_CASE(testEqualityOfPoseBases)
263{
264 PosePkg pp4 = createPosePkg(-316.302246093750,
265 777.949218750,
266 1194.246459960938,
267 0.5907033681869507,
268 -0.5503066778182983,
269 0.4992305040359497,
270 0.3146440684795380);
271 PosePkg pp2 = createPosePkg(-182.23925781250,
272 580.074218750,
273 1034.98925781250,
274 0.4338901340961456,
275 -0.4268467724323273,
276 0.5642792582511902,
277 0.5577904582023621); //reachable
278
279 BOOST_CHECK(equalPoseBase(pp2.pose, pp2.pose));
280 BOOST_CHECK(equalPoseBase(pp4.pose, pp4.pose));
281 BOOST_CHECK(!equalPoseBase(pp2.pose, pp4.pose));
282 BOOST_CHECK(!equalPoseBase(pp4.pose, pp2.pose));
283
284 // pose with minimal difference to pp4
285 PosePkg pp4_1 = createPosePkg(-316.302246093,
286 777.9492187,
287 1194.246459960746,
288 0.5907046681869507,
289 -0.55030667789183983,
290 0.4992304564358497,
291 0.3146440488984465480);
292
293 BOOST_CHECK(equalPoseBase(pp4.pose, pp4_1.pose));
294}
295
296BOOST_AUTO_TEST_CASE(wrongUsage)
297{
298 std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
299 CMakePackageFinder finder("RobotAPI");
300 RobotPtr robot;
301 if (finder.packageFound())
302 {
303 robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
304 }
305 else
306 {
307 robot = RobotIO::loadRobot(
308 "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
309 }
310
311
312 // Get RobotNodeSet: LeftArm
313 VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSets()[10];
314 //(0): Shoulder 1 L, (1): Shoulder 2 L, (2): Upperarm L, (3): Elbow L, (4): Underarm L
315 //(5): Wrist 1 L, (6): Wrist 2 L
316
317 // Create Pose Packages
318 PosePkg pp1_r = createPosePkg(4111, 7280, 1067, 0.6987, 0.1106, 0.7, -0.0969);
319
320 vector<double> pp1_r_ja = {0.0540, 0.0398, 0.3851, 0.1253, -0.2440, 0.1100, -0.0342};
321
322 IKSolver::CartesianSelection ikSelection = IKSolver::CartesianSelection::All;
323 InterpolationType interpolationType = InterpolationType::eLinearInterpolation;
324 EnvironmentPtr environment = std::shared_ptr<Environment>(new Environment());
325 environment->setRobot(robot);
326 DesignerTrajectoryManagerPtr m = std::shared_ptr<DesignerTrajectoryManager>(
327 new DesignerTrajectoryManager(rns->getName(), environment));
328 //m.initializeDesignerTrajectory(pp1_r_ja);
329
330 /////////////////////////////////////////////////////////////////////////////////////
331 //Not initialized Test
332 BOOST_CHECK_THROW(m->addWaypoint(pp1_r.pose), NotInitializedException);
333 BOOST_CHECK_THROW(m->insertWaypoint(0, pp1_r.pose), NotInitializedException);
334 BOOST_CHECK_THROW(m->editWaypointPoseBase(0, pp1_r.pose), NotInitializedException);
335 BOOST_CHECK_THROW(m->editWaypointIKSelection(0, ikSelection), NotInitializedException);
336 BOOST_CHECK_THROW(m->deleteWaypoint(0), NotInitializedException);
337 BOOST_CHECK_THROW(m->setTransitionInterpolation(0, interpolationType), NotInitializedException);
338 BOOST_CHECK_THROW(m->setWaypointAsBreakpoint(0, true), NotInitializedException);
339 BOOST_CHECK_THROW(m->setTransitionUserDuration(0, 10.0), NotInitializedException);
340}
341
342BOOST_AUTO_TEST_CASE(basicTest)
343{
344 // Get Robot
345 std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
346 CMakePackageFinder finder("RobotAPI");
347 RobotPtr robot;
348 if (finder.packageFound())
349 {
350 robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
351 }
352 else
353 {
354 robot = RobotIO::loadRobot(
355 "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
356 }
357
358 KinematicSolverPtr kc = KinematicSolver::getInstance(NULL, robot);
359
360 // Get RobotNodeSet: LeftArm
361 VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSets()[10];
362
363 // Create DesignerTrajectoryManager
364 EnvironmentPtr environment = std::make_shared<Environment>();
365 environment->setRobot(robot);
366 DesignerTrajectoryManagerPtr dtm =
367 std::make_shared<DesignerTrajectoryManager>(rns->getName(), environment);
368
369 // Create Poses
370 std::vector<double> p1ja = {
371 0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
372
373 PosePkg pp4 = createPosePkg(-316.302246093750,
374 777.949218750,
375 1194.246459960938,
376 0.5907033681869507,
377 -0.5503066778182983,
378 0.4992305040359497,
379 0.3146440684795380);
380 PosePkg pp2 = createPosePkg(-182.23925781250,
381 580.074218750,
382 1034.98925781250,
383 0.4338901340961456,
384 -0.4268467724323273,
385 0.5642792582511902,
386 0.5577904582023621); //reachable
387 PosePkg pp3 = createPosePkg(-226.792480468750,
388 580.723144531250,
389 1186.157348632812,
390 0.4336481690406799,
391 -0.4273631870746613,
392 0.5638203620910645,
393 0.5580471754074097); //reachable
394 PosePkg pp5 = createPosePkg(-348.304718,
395 580.476440,
396 712.264465,
397 0.4453609585762024,
398 -0.4538218379020691,
399 0.5429607033729553,
400 0.5485371351242065); //reachable
401
402 std::string rnsName = rns->getName();
403 PoseBasePtr p1 = poseToLocal(kc->doForwardKinematic(rns, p1ja), robot, rnsName);
404 PoseBasePtr p2 = poseToLocal(pp2.pose, robot, rnsName);
405 PoseBasePtr p3 = poseToLocal(pp3.pose, robot, rnsName);
406 PoseBasePtr p4 = poseToLocal(pp4.pose, robot, rnsName);
407 //PoseBasePtr p5nr;
408 //PoseBasePtr p6col;
409
410 ////////////////////////////////////////////////////////////////////////////////////////////////
411 // Initialize DesignerTrajectoryManager
412 dtm->initializeDesignerTrajectory(p1ja);
413
414 // test result
415 DesignerTrajectoryPtr dt = dtm->getDesignerTrajectory();
416 BOOST_CHECK_EQUAL(p1ja, dt->getUserWaypoint(0)->getJointAngles());
417 BOOST_CHECK(equalPoseBase(p1, dt->getUserWaypoint(0)->getPose()));
418
419 ////////////////////////////////////////////////////////////////////////////////////////////////
420 // Add/Insert Waypoint
421 dtm->addWaypoint(p2);
422 dtm->insertWaypoint(1, p3);
423 dtm->insertWaypoint(0, p4);
424
425 // p4, p1, p3, p2
426 dt = dtm->getDesignerTrajectory();
427 BOOST_CHECK(equalPoseBase(dt->getUserWaypoint(0)->getPose(), p4));
428 BOOST_CHECK(equalPoseBase(dt->getUserWaypoint(1)->getPose(), p1));
429 BOOST_CHECK(equalPoseBase(dt->getUserWaypoint(2)->getPose(), p3));
430 BOOST_CHECK(equalPoseBase(dt->getUserWaypoint(3)->getPose(), p2));
431
432 BOOST_CHECK(dt->getUserWaypoint(0)->getTimeOptimalTimestamp() == 0);
433
434
435 BOOST_CHECK(dt->getUserWaypoint(1)->getTimeOptimalTimestamp() != 0);
436 BOOST_CHECK(dt->getUserWaypoint(2)->getTimeOptimalTimestamp() != 0);
437 BOOST_CHECK(dt->getUserWaypoint(3)->getTimeOptimalTimestamp() != 0);
438
439
440 BOOST_CHECK(dt->getUserWaypoint(0)->getJointAngles().size() != 0);
441 BOOST_CHECK(dt->getUserWaypoint(1)->getJointAngles().size() != 0);
442 BOOST_CHECK(dt->getUserWaypoint(2)->getJointAngles().size() != 0);
443 BOOST_CHECK(dt->getUserWaypoint(3)->getJointAngles().size() != 0);
444
445
446 BOOST_CHECK_EQUAL(dt->getTransition(0)->getTimeOptimalDuration(),
447 dt->getTransition(0)->getEnd()->getTimeOptimalTimestamp() -
448 dt->getTransition(0)->getStart()->getTimeOptimalTimestamp());
449
450 BOOST_CHECK_EQUAL(dt->getTransition(1)->getTimeOptimalDuration(),
451 dt->getTransition(1)->getEnd()->getTimeOptimalTimestamp() -
452 dt->getTransition(1)->getStart()->getTimeOptimalTimestamp());
453
454 BOOST_CHECK_EQUAL(dt->getTransition(2)->getTimeOptimalDuration(),
455 dt->getTransition(2)->getEnd()->getTimeOptimalTimestamp() -
456 dt->getTransition(2)->getStart()->getTimeOptimalTimestamp());
457
458 BOOST_CHECK(dt->getTimeOptimalTrajectory()->size() > 1);
459
460 //////////////////////////////////////////////////////////////////////////////////////
461 dtm->setTransitionUserDuration(0, 5.0);
462 dt = dtm->getDesignerTrajectory();
463 BOOST_CHECK(dt->getUserWaypoint(0)->getUserTimestamp() == 0);
464 BOOST_CHECK(dt->getUserWaypoint(1)->getUserTimestamp() == 5);
465 BOOST_CHECK(dt->getUserWaypoint(2)->getUserTimestamp() > 5);
466 BOOST_CHECK(dt->getUserWaypoint(3)->getUserTimestamp() > 5);
467
468 dtm->setTransitionUserDuration(1, 10.0);
469 dt = dtm->getDesignerTrajectory();
470
471 BOOST_CHECK(dt->getUserWaypoint(0)->getUserTimestamp() == 0.0);
472 BOOST_CHECK(dt->getUserWaypoint(1)->getUserTimestamp() == 5.0);
473 BOOST_CHECK(dt->getUserWaypoint(2)->getUserTimestamp() == 15.0);
474 BOOST_CHECK(dt->getUserWaypoint(3)->getUserTimestamp() > 15.0);
475
476 dtm->setTransitionUserDuration(2, 15.0);
477 dt = dtm->getDesignerTrajectory();
478
479 BOOST_CHECK(dt->getUserWaypoint(0)->getUserTimestamp() == 0.0);
480 BOOST_CHECK(dt->getUserWaypoint(1)->getUserTimestamp() == 5.0);
481 BOOST_CHECK(dt->getUserWaypoint(2)->getUserTimestamp() == 15.0);
482 BOOST_CHECK(dt->getUserWaypoint(3)->getUserTimestamp() == 30.0);
483
484
485 ////////////////////////////////////////////////////////////////////////////////////////////////
486 // Check if isInitialized is resetted
487 dtm->deleteWaypoint(0);
488 dtm->deleteWaypoint(0);
489 dtm->deleteWaypoint(0);
490 dtm->deleteWaypoint(0);
491
492 BOOST_CHECK(dtm->getIsInitialized() == false);
493}
494
495BOOST_AUTO_TEST_CASE(undoRedoTest1)
496{
497 // Get Robot
498 std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
499 CMakePackageFinder finder("RobotAPI");
500 RobotPtr robot;
501 if (finder.packageFound())
502 {
503 robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
504 }
505 else
506 {
507 robot = RobotIO::loadRobot(
508 "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
509 }
510
511 KinematicSolverPtr kc = KinematicSolver::getInstance(NULL, robot);
512
513 // Get RobotNodeSet: LeftArm
514 VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSets()[10];
515
516 // Create DesignerTrajectoryManager
517 EnvironmentPtr environment = std::make_shared<Environment>();
518 environment->setRobot(robot);
519 DesignerTrajectoryManagerPtr dtm =
520 std::make_shared<DesignerTrajectoryManager>(rns->getName(), environment);
521
522 // Create Poses
523 std::vector<double> p1ja = {
524 0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
525
526 PosePkg pp4 = createPosePkg(-316.302246093750,
527 777.949218750,
528 1194.246459960938,
529 0.5907033681869507,
530 -0.5503066778182983,
531 0.4992305040359497,
532 0.3146440684795380);
533 PosePkg pp2 = createPosePkg(-182.23925781250,
534 580.074218750,
535 1034.98925781250,
536 0.4338901340961456,
537 -0.4268467724323273,
538 0.5642792582511902,
539 0.5577904582023621); //reachable
540 PosePkg pp3 = createPosePkg(-226.792480468750,
541 580.723144531250,
542 1186.157348632812,
543 0.4336481690406799,
544 -0.4273631870746613,
545 0.5638203620910645,
546 0.5580471754074097); //reachable
547 PosePkg pp5 = createPosePkg(-348.304718,
548 580.476440,
549 712.264465,
550 0.4453609585762024,
551 -0.4538218379020691,
552 0.5429607033729553,
553 0.5485371351242065); //reachable
554
555 std::string rnsName = rns->getName();
556 PoseBasePtr p1 = poseToLocal(kc->doForwardKinematic(rns, p1ja), robot, rnsName);
557 PoseBasePtr p2 = poseToLocal(pp2.pose, robot, rnsName);
558 PoseBasePtr p3 = poseToLocal(pp3.pose, robot, rnsName);
559 PoseBasePtr p4 = poseToLocal(pp4.pose, robot, rnsName);
560 //PoseBasePtr p5nr;
561 //PoseBasePtr p6col;
562
563 // Initialize DesignerTrajectoryManager
564 dtm->initializeDesignerTrajectory(p1ja);
565
566 ////////////////////////////////////////////////////////////////////////////////////////////////
567 // Undo
568
569 dtm->addWaypoint(p2);
570 dtm->addWaypoint(p3);
571 dtm->undo();
572 dtm->redo();
573 dtm->undo();
574 dtm->addWaypoint(p4);
575 dtm->undo();
576 dtm->redo();
577 dtm->editWaypointIKSelection(2, VirtualRobot::IKSolver::CartesianSelection::Position);
578 dtm->undo();
579
580
581 //BOOST_CHECK(equalDesignerTrajectory(dtm->getDesignerTrajectory(), dtptr1));
582 //dtm->redo();
583 //BOOST_CHECK(equalDesignerTrajectory(dtm->getDesignerTrajectory(), dtptr2));
584 //dtm->redo();
585 //BOOST_CHECK(equalDesignerTrajectory(dtm->getDesignerTrajectory(), dtptr2));
586}
587
588BOOST_AUTO_TEST_CASE(undoRedoTest2)
589{
590 // Get Robot
591 std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
592 CMakePackageFinder finder("RobotAPI");
593 RobotPtr robot;
594 if (finder.packageFound())
595 {
596 robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
597 }
598 else
599 {
600 robot = RobotIO::loadRobot(
601 "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
602 }
603
604 KinematicSolverPtr kc = KinematicSolver::getInstance(NULL, robot);
605
606 // Get RobotNodeSet: LeftArm
607 VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSets()[10];
608
609 // Create DesignerTrajectoryManager
610 EnvironmentPtr environment = std::make_shared<Environment>();
611 environment->setRobot(robot);
612 DesignerTrajectoryManagerPtr dtm =
613 std::make_shared<DesignerTrajectoryManager>(rns->getName(), environment);
614
615 // Create Poses
616 std::vector<double> p1ja = {
617 0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
618
619 PosePkg pp4 = createPosePkg(-316.302246093750,
620 777.949218750,
621 1194.246459960938,
622 0.5907033681869507,
623 -0.5503066778182983,
624 0.4992305040359497,
625 0.3146440684795380);
626 PosePkg pp2 = createPosePkg(-182.23925781250,
627 580.074218750,
628 1034.98925781250,
629 0.4338901340961456,
630 -0.4268467724323273,
631 0.5642792582511902,
632 0.5577904582023621); //reachable
633 PosePkg pp3 = createPosePkg(-226.792480468750,
634 580.723144531250,
635 1186.157348632812,
636 0.4336481690406799,
637 -0.4273631870746613,
638 0.5638203620910645,
639 0.5580471754074097); //reachable
640 PosePkg pp5 = createPosePkg(-348.304718,
641 580.476440,
642 712.264465,
643 0.4453609585762024,
644 -0.4538218379020691,
645 0.5429607033729553,
646 0.5485371351242065); //reachable
647
648 PoseBasePtr p1 = kc->doForwardKinematic(rns, p1ja);
649 std::string rnsName = rns->getName();
650 PoseBasePtr p2 = poseToLocal(pp2.pose, robot, rnsName);
651 PoseBasePtr p3 = poseToLocal(pp3.pose, robot, rnsName);
652 PoseBasePtr p4 = poseToLocal(pp4.pose, robot, rnsName);
653 //PoseBasePtr p5nr;
654 //PoseBasePtr p6col;
655
656
657 // Initialize DesignerTrajectoryManager
658 dtm->initializeDesignerTrajectory(p1ja);
659
660 ////////////////////////////////////////////////////////////////////////////////////////////////
661 // Undo
662
663 dtm->addWaypoint(p2);
664 DesignerTrajectoryPtr dt1 = dtm->getDesignerTrajectory();
665
666 dtm->addWaypoint(p3);
667 DesignerTrajectoryPtr dt2 = dtm->getDesignerTrajectory();
668 dtm->undo();
669
670 BOOST_CHECK(equalDesignerTrajectory(dtm->getDesignerTrajectory(), dt1));
671
672 dtm->redo();
673 BOOST_CHECK(equalDesignerTrajectory(dtm->getDesignerTrajectory(), dt2));
674 dtm->redo();
675 BOOST_CHECK(equalDesignerTrajectory(dtm->getDesignerTrajectory(), dt2));
676}
677
678BOOST_AUTO_TEST_CASE(breakpointTest)
679{
680 // Get Robot
681 std::string robotFile = "/RobotAPI/robots/Armar3/ArmarIII.xml";
682 CMakePackageFinder finder("RobotAPI");
683 RobotPtr robot;
684 if (finder.packageFound())
685 {
686 robot = VirtualRobot::RobotIO::loadRobot(finder.getDataDir() + robotFile);
687 }
688 else
689 {
690 robot = RobotIO::loadRobot(
691 "../../../../../data/RobotTrajectoryDesigner/Resources/ArmarIII.xml");
692 }
693
694 KinematicSolverPtr kc = KinematicSolver::getInstance(NULL, robot);
695
696 // Get RobotNodeSet: LeftArm
697 VirtualRobot::RobotNodeSetPtr rns = robot->getRobotNodeSets()[10];
698
699 // Create DesignerTrajectoryManager
700 EnvironmentPtr environment = std::make_shared<Environment>();
701 environment->setRobot(robot);
702 DesignerTrajectoryManagerPtr dtm =
703 std::make_shared<DesignerTrajectoryManager>(rns->getName(), environment);
704
705 // Create Poses
706 std::vector<double> p1ja = {
707 0.464789, 0.357171, -0.303694, -0.067161, -0.445988, -0.204292, -0.070153};
708
709 PosePkg pp4 = createPosePkg(-316.302246093750,
710 777.949218750,
711 1194.246459960938,
712 0.5907033681869507,
713 -0.5503066778182983,
714 0.4992305040359497,
715 0.3146440684795380);
716 PosePkg pp2 = createPosePkg(-182.23925781250,
717 580.074218750,
718 1034.98925781250,
719 0.4338901340961456,
720 -0.4268467724323273,
721 0.5642792582511902,
722 0.5577904582023621); //reachable
723 PosePkg pp3 = createPosePkg(-226.792480468750,
724 580.723144531250,
725 1186.157348632812,
726 0.4336481690406799,
727 -0.4273631870746613,
728 0.5638203620910645,
729 0.5580471754074097); //reachable
730 PosePkg pp5 = createPosePkg(-348.304718,
731 580.476440,
732 712.264465,
733 0.4453609585762024,
734 -0.4538218379020691,
735 0.5429607033729553,
736 0.5485371351242065); //reachable
737
738 std::string rnsName = rns->getName();
739 PoseBasePtr p1 = poseToLocal(kc->doForwardKinematic(rns, p1ja), robot, rnsName);
740 PoseBasePtr p2 = poseToLocal(pp2.pose, robot, rnsName);
741 PoseBasePtr p3 = poseToLocal(pp3.pose, robot, rnsName);
742 PoseBasePtr p4 = poseToLocal(pp4.pose, robot, rnsName);
743
744 ////////////////////////////////////////////////////////////////////////////////////////////////
745 ////////////////////////////////////////////////////////////////////////////////////////////////
746 ////////////////////////////////////////////////////////////////////////////////////////////////
747
748 dtm->initializeDesignerTrajectory(p1ja);
749 dtm->addWaypoint(p2);
750 dtm->addWaypoint(p3);
751
752 dtm->setTransitionInterpolation(0, InterpolationType::eSplineInterpolation);
753 dtm->setTransitionInterpolation(1, InterpolationType::eSplineInterpolation);
754
755 dtm->setWaypointAsBreakpoint(2, true);
756
757 BOOST_CHECK_NO_THROW(dtm->addWaypoint(p4));
758}
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(testEqualityOfPoseBases)
Definition DesignerTrajectoryManagerTest.cpp:262
equalTransition
bool equalTransition(armarx::TransitionPtr t1, armarx::TransitionPtr t2)
Definition DesignerTrajectoryManagerTest.cpp:158
roundTo
double roundTo(double d, int accuracy)
Definition DesignerTrajectoryManagerTest.cpp:41
equalUserWaypoint
bool equalUserWaypoint(armarx::UserWaypointPtr u1, armarx::UserWaypointPtr u2)
Definition DesignerTrajectoryManagerTest.cpp:116
createPosePkg
PosePkg createPosePkg(float x, float y, float z, float qw, float qx, float qy, float qz)
Definition DesignerTrajectoryManagerTest.cpp:30
equalTrajectory
bool equalTrajectory(armarx::TrajectoryPtr t1, armarx::TrajectoryPtr t2)
Definition DesignerTrajectoryManagerTest.cpp:89
equalDesignerTrajectory
bool equalDesignerTrajectory(DesignerTrajectoryPtr dt1, DesignerTrajectoryPtr dt2)
Definition DesignerTrajectoryManagerTest.cpp:200
poseToLocal
PoseBasePtr poseToLocal(PoseBasePtr pose, RobotPtr robot, std::string &rnsName)
Definition DesignerTrajectoryManagerTest.cpp:255
equalPoseBase
bool equalPoseBase(PoseBasePtr p1, PoseBasePtr p2)
Definition DesignerTrajectoryManagerTest.cpp:47
dt
constexpr T dt
Definition UnscentedKalmanFilterTest.cpp:45
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::DesignerTrajectoryManager
The DesignerTrajectoryManager class enables easy interaction with the model.
Definition DesignerTrajectoryManager.h:58
armarx::KinematicSolver::getInstance
static std::shared_ptr< KinematicSolver > getInstance(VirtualRobot::ScenePtr scenario, VirtualRobot::RobotPtr robot)
SINGLETON-FEATURES///////////////////////////////////////////////////////////////////////////////////...
Definition KinematicSolver.cpp:105
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::TrajectoryPtr
IceInternal::Handle< Trajectory > TrajectoryPtr
Definition Trajectory.h:52
armarx::DesignerTrajectoryManagerPtr
std::shared_ptr< armarx::DesignerTrajectoryManager > DesignerTrajectoryManagerPtr
Typedefinitions.
Definition DesignerTrajectoryManager.h:402
armarx::EnvironmentPtr
std::shared_ptr< Environment > EnvironmentPtr
Definition Environment.h:29
armarx::InterpolationType
InterpolationType
The InterpolationType enum lists all available interpolation types eLinearInterpolation: represents l...
Definition InterpolationType.h:33
armarx::TransitionPtr
std::shared_ptr< Transition > TransitionPtr
Definition Transition.h:143
armarx::DesignerTrajectoryPtr
std::shared_ptr< DesignerTrajectory > DesignerTrajectoryPtr
Definition DesignerTrajectory.h:163
armarx::UserWaypointPtr
std::shared_ptr< UserWaypoint > UserWaypointPtr
Definition UserWaypoint.h:142
PosePkg
The PosePkg struct - Testing utility.
Definition DesignerTrajectoryHolderTest.cpp:16
PosePkg::ori
QuaternionBasePtr ori
Definition DesignerTrajectoryHolderTest.cpp:19