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