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