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 
17 using namespace armarx;
18 using namespace VirtualRobot;
19 using namespace std;
20 
21 struct PosePkg
22 {
23 public:
24  Vector3BasePtr pos;
25  QuaternionBasePtr ori;
26  PoseBasePtr pose;
27 };
28 
29 PosePkg
30 createPosePkg(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 
40 double
41 roundTo(double d, int accuracy)
42 {
43  return floor(d * pow10(accuracy)) / pow10(accuracy);
44 }
45 
46 bool
47 equalPoseBase(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 
88 bool
89 equalTrajectory(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 
115 bool
116 equalUserWaypoint(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 
157 bool
158 equalTransition(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 
199 bool
200 equalDesignerTrajectory(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 
254 PoseBasePtr
255 poseToLocal(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 
262 BOOST_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 
296 BOOST_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 
342 BOOST_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 
495 BOOST_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 
588 BOOST_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 
678 BOOST_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 }
armarx::UserWaypointPtr
std::shared_ptr< UserWaypoint > UserWaypointPtr
Definition: UserWaypoint.h:142
armarx::CMakePackageFinder::packageFound
bool packageFound() const
Returns whether or not this package was found with cmake.
Definition: CMakePackageFinder.cpp:511
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
VirtualRobot
Definition: FramedPose.h:42
armarx::CMakePackageFinder
The CMakePackageFinder class provides an interface to the CMake Package finder capabilities.
Definition: CMakePackageFinder.h:52
roundTo
double roundTo(double d, int accuracy)
Definition: DesignerTrajectoryManagerTest.cpp:41
PosePkg::pose
PoseBasePtr pose
Definition: DesignerTrajectoryHolderTest.cpp:20
equalPoseBase
bool equalPoseBase(PoseBasePtr p1, PoseBasePtr p2)
Definition: DesignerTrajectoryManagerTest.cpp:47
armarx::eSplineInterpolation
@ eSplineInterpolation
Definition: InterpolationType.h:35
armarx::VariantType::Quaternion
const VariantTypeId Quaternion
Definition: Pose.h:39
equalTransition
bool equalTransition(armarx::TransitionPtr t1, armarx::TransitionPtr t2)
Definition: DesignerTrajectoryManagerTest.cpp:158
armarx::Environment
Definition: Environment.h:9
equalDesignerTrajectory
bool equalDesignerTrajectory(DesignerTrajectoryPtr dt1, DesignerTrajectoryPtr dt2)
Definition: DesignerTrajectoryManagerTest.cpp:200
IceInternal::Handle< Trajectory >
equalUserWaypoint
bool equalUserWaypoint(armarx::UserWaypointPtr u1, armarx::UserWaypointPtr u2)
Definition: DesignerTrajectoryManagerTest.cpp:116
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(testEqualityOfPoseBases)
Definition: DesignerTrajectoryManagerTest.cpp:262
armarx::EnvironmentPtr
std::shared_ptr< Environment > EnvironmentPtr
Definition: Environment.h:29
PosePkg
The PosePkg struct - Testing utility.
Definition: DesignerTrajectoryHolderTest.cpp:15
armarx::DesignerTrajectoryManager
The DesignerTrajectoryManager class enables easy interaction with the model.
Definition: DesignerTrajectoryManager.h:57
armarx::InterpolationType
InterpolationType
The InterpolationType enum lists all available interpolation types eLinearInterpolation: represents l...
Definition: InterpolationType.h:32
armarx::CMakePackageFinder::getDataDir
std::string getDataDir() const
Definition: CMakePackageFinder.h:194
armarx::NJointTaskSpaceDMPControllerMode::CartesianSelection
CartesianSelection
Definition: ControllerInterface.ice:34
armarx::TransitionPtr
std::shared_ptr< Transition > TransitionPtr
Definition: Transition.h:143
armarx::DesignerTrajectoryManagerPtr
std::shared_ptr< armarx::DesignerTrajectoryManager > DesignerTrajectoryManagerPtr
Typedefinitions.
Definition: DesignerTrajectoryManager.h:402
armarx::KinematicSolver::getInstance
static std::shared_ptr< KinematicSolver > getInstance(VirtualRobot::ScenePtr scenario, VirtualRobot::RobotPtr robot)
SINGLETON-FEATURES///////////////////////////////////////////////////////////////////////////////////...
Definition: KinematicSolver.cpp:105
CMakePackageFinder.h
armarx::Pose
The Pose class.
Definition: Pose.h:242
armarx::DesignerTrajectoryPtr
std::shared_ptr< DesignerTrajectory > DesignerTrajectoryPtr
Definition: DesignerTrajectory.h:163
std
Definition: Application.h:66
createPosePkg
PosePkg createPosePkg(float x, float y, float z, float qw, float qx, float qy, float qz)
Definition: DesignerTrajectoryManagerTest.cpp:30
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:43
LogSender.h
armarx::KinematicSolverPtr
std::shared_ptr< KinematicSolver > KinematicSolverPtr
Definition: KinematicSolver.h:287
armarx::navigation::core::Position
Eigen::Vector3f Position
Definition: basic_types.h:36
armarx::eLinearInterpolation
@ eLinearInterpolation
Definition: InterpolationType.h:34
PosePkg::pos
Vector3BasePtr pos
Definition: DesignerTrajectoryHolderTest.cpp:18
equalTrajectory
bool equalTrajectory(armarx::TrajectoryPtr t1, armarx::TrajectoryPtr t2)
Definition: DesignerTrajectoryManagerTest.cpp:89
PosePkg::ori
QuaternionBasePtr ori
Definition: DesignerTrajectoryHolderTest.cpp:19
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
dt
constexpr T dt
Definition: UnscentedKalmanFilterTest.cpp:45
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:19
poseToLocal
PoseBasePtr poseToLocal(PoseBasePtr pose, RobotPtr robot, std::string &rnsName)
Definition: DesignerTrajectoryManagerTest.cpp:255