InterpolationSegmentFactoryTest.cpp
Go to the documentation of this file.
1 #define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::InterpolationSegmentFactory
2 #define ARMARX_BOOST_TEST
3 
4 #include <RobotComponents/Test.h>
5 
6 #include "../InterpolationType.h"
7 #include "../InterpolationSegmentFactory.h"
8 #include "../exceptions/NoInterpolationPossibleException.h"
9 #include "../exceptions/InterpolationNotDefinedException.h"
10 
11 #include <boost/test/unit_test.hpp>
12 
13 #include "../../Interpolation/LinearInterpolation.h"
14 #include "../../Util/OrientationConversion.h"
15 #include <RobotAPI/libraries/core/Pose.h>
16 
17 using namespace armarx;
18 BOOST_AUTO_TEST_CASE(basicTest)
19 {
20  Vector3BasePtr pos1 = new Vector3(2, 4, 6);
21  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(0, 0, 0);
22  PoseBasePtr pose1 = new Pose(pos1, ori1);
23 
24  Vector3BasePtr pos2 = new Vector3(0, -2, -8);
25  QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(1, 1, 1);
26  PoseBasePtr pose2 = new Pose(pos2, ori2);
27 
28  Vector3BasePtr pos3 = new Vector3(5, 8, 7);
29  QuaternionBasePtr ori3 = OrientationConversion::toQuaternion(2, 0, -1);
30  PoseBasePtr pose3 = new Pose(pos3, ori3);
31 
32  Vector3BasePtr pos4 = new Vector3(4, 23, 1);
33  QuaternionBasePtr ori4 = OrientationConversion::toQuaternion(0, 0, 0);
34  PoseBasePtr pose4 = new Pose(pos4, ori4);
35 
36  Vector3BasePtr pos5 = new Vector3(7, 12, 4);
37  QuaternionBasePtr ori5 = OrientationConversion::toQuaternion(1, 1, 1);
38  PoseBasePtr pose5 = new Pose(pos5, ori5);
39 
40  Vector3BasePtr pos6 = new Vector3(7, 44, -3);
41  QuaternionBasePtr ori6 = OrientationConversion::toQuaternion(2, 0, -1);
42  PoseBasePtr pose6 = new Pose(pos6, ori6);
43 
44  Vector3BasePtr pos7 = new Vector3(5, 4, -8);
45  QuaternionBasePtr ori7 = OrientationConversion::toQuaternion(1, 1, 1);
46  PoseBasePtr pose7 = new Pose(pos7, ori7);
47 
48  Vector3BasePtr pos8 = new Vector3(5, -4, -8);
49  QuaternionBasePtr ori8 = OrientationConversion::toQuaternion(2, 0, -1);
50  PoseBasePtr pose8 = new Pose(pos8, ori8);
51 
52  Vector3BasePtr pos9 = new Vector3(0, 0, 4);
53  QuaternionBasePtr ori9 = OrientationConversion::toQuaternion(0, 0, 0);
54  PoseBasePtr pose9 = new Pose(pos9, ori9);
55 
56  Vector3BasePtr pos10 = new Vector3(0, 1, 2);
57  QuaternionBasePtr ori10 = OrientationConversion::toQuaternion(0, 0, 0);
58  PoseBasePtr pose10 = new Pose(pos10, ori10);
59 
60  Vector3BasePtr pos11 = new Vector3(0, 0, 0);
61  QuaternionBasePtr ori11 = OrientationConversion::toQuaternion(0, 0, 0);
62  PoseBasePtr pose11 = new Pose(pos11, ori11);
63 
64  std::vector<PoseBasePtr> cp = {pose1, pose2, pose3, pose4, pose5, pose6, pose7, pose8, pose9, pose10, pose11};
65 
66  std::vector<InterpolationType> interpolations = {InterpolationType::eSplineInterpolation,
67  InterpolationType::eSplineInterpolation,
68  InterpolationType::eLinearInterpolation,
69  InterpolationType::eLinearInterpolation,
70  InterpolationType::eLinearInterpolation,
71  InterpolationType::eLinearInterpolation,
72  InterpolationType::eLinearInterpolation,
73  InterpolationType::eSplineInterpolation,
74  InterpolationType::eSplineInterpolation,
75  InterpolationType::eSplineInterpolation
76  };
77 
78  std::vector<AbstractInterpolationPtr> ip = InterpolationSegmentFactory::produceInterpolationSegments(cp, interpolations);
79 
80  AbstractInterpolationPtr ips1 = ip[0];
81  AbstractInterpolationPtr ips2 = ip[1];
82  AbstractInterpolationPtr ips3 = ip[2];
83  AbstractInterpolationPtr ips4 = ip[3];
84  AbstractInterpolationPtr ips5 = ip[4];
85  AbstractInterpolationPtr ips6 = ip[5];
86  AbstractInterpolationPtr ips7 = ip[6];
87  AbstractInterpolationPtr ips8 = ip[7];
88  AbstractInterpolationPtr ips9 = ip[8];
89  AbstractInterpolationPtr ips10 = ip[9];
90 
91  BOOST_CHECK_EQUAL(ips1->getPoseAt(0)->position->x, 2);
92  BOOST_CHECK_EQUAL(ips1->getPoseAt(0)->position->y, 4);
93  BOOST_CHECK_EQUAL(ips1->getPoseAt(0)->position->z, 6);
94  BOOST_CHECK_EQUAL(ips1->getPoseAt(1)->position->x, 0);
95  BOOST_CHECK_EQUAL(ips1->getPoseAt(1)->position->y, -2);
96  BOOST_CHECK_EQUAL(ips1->getPoseAt(1)->position->z, -8);
97 
98  BOOST_CHECK_EQUAL(ips2->getPoseAt(0)->position->x, 0);
99  BOOST_CHECK_EQUAL(ips2->getPoseAt(0)->position->y, -2);
100  BOOST_CHECK_EQUAL(ips2->getPoseAt(0)->position->z, -8);
101  BOOST_CHECK_EQUAL(ips2->getPoseAt(1)->position->x, 5);
102  BOOST_CHECK_EQUAL(ips2->getPoseAt(1)->position->y, 8);
103  BOOST_CHECK_EQUAL(ips2->getPoseAt(1)->position->z, 7);
104 
105  BOOST_CHECK_EQUAL(ips3->getPoseAt(0)->position->x, 5);
106  BOOST_CHECK_EQUAL(ips3->getPoseAt(0)->position->y, 8);
107  BOOST_CHECK_EQUAL(ips3->getPoseAt(0)->position->z, 7);
108  BOOST_CHECK_EQUAL(ips3->getPoseAt(1)->position->x, 4);
109  BOOST_CHECK_EQUAL(ips3->getPoseAt(1)->position->y, 23);
110  BOOST_CHECK_EQUAL(ips3->getPoseAt(1)->position->z, 1);
111 
112  BOOST_CHECK_EQUAL(ips4->getPoseAt(0)->position->x, 4);
113  BOOST_CHECK_EQUAL(ips4->getPoseAt(0)->position->y, 23);
114  BOOST_CHECK_EQUAL(ips4->getPoseAt(0)->position->z, 1);
115  BOOST_CHECK_EQUAL(ips4->getPoseAt(1)->position->x, 7);
116  BOOST_CHECK_EQUAL(ips4->getPoseAt(1)->position->y, 12);
117  BOOST_CHECK_EQUAL(ips4->getPoseAt(1)->position->z, 4);
118 
119  BOOST_CHECK_EQUAL(ips5->getPoseAt(0)->position->x, 7);
120  BOOST_CHECK_EQUAL(ips5->getPoseAt(0)->position->y, 12);
121  BOOST_CHECK_EQUAL(ips5->getPoseAt(0)->position->z, 4);
122  BOOST_CHECK_EQUAL(ips5->getPoseAt(1)->position->x, 7);
123  BOOST_CHECK_EQUAL(ips5->getPoseAt(1)->position->y, 44);
124  BOOST_CHECK_EQUAL(ips5->getPoseAt(1)->position->z, -3);
125 
126  BOOST_CHECK_EQUAL(ips6->getPoseAt(0)->position->x, 7);
127  BOOST_CHECK_EQUAL(ips6->getPoseAt(0)->position->y, 44);
128  BOOST_CHECK_EQUAL(ips6->getPoseAt(0)->position->z, -3);
129  BOOST_CHECK_EQUAL(ips6->getPoseAt(1)->position->x, 5);
130  BOOST_CHECK_EQUAL(ips6->getPoseAt(1)->position->y, 4);
131  BOOST_CHECK_EQUAL(ips6->getPoseAt(1)->position->z, -8);
132 
133  BOOST_CHECK_EQUAL(ips7->getPoseAt(0)->position->x, 5);
134  BOOST_CHECK_EQUAL(ips7->getPoseAt(0)->position->y, 4);
135  BOOST_CHECK_EQUAL(ips7->getPoseAt(0)->position->z, -8);
136  BOOST_CHECK_EQUAL(ips7->getPoseAt(1)->position->x, 5);
137  BOOST_CHECK_EQUAL(ips7->getPoseAt(1)->position->y, -4);
138  BOOST_CHECK_EQUAL(ips7->getPoseAt(1)->position->z, -8);
139 
140  BOOST_CHECK_EQUAL(ips8->getPoseAt(0)->position->x, 5);
141  BOOST_CHECK_EQUAL(ips8->getPoseAt(0)->position->y, -4);
142  BOOST_CHECK_EQUAL(ips8->getPoseAt(0)->position->z, -8);
143  BOOST_CHECK_EQUAL(ips8->getPoseAt(1)->position->x, 0);
144  BOOST_CHECK_EQUAL(ips8->getPoseAt(1)->position->y, 0);
145  BOOST_CHECK_EQUAL(ips8->getPoseAt(1)->position->z, 4);
146 
147  BOOST_CHECK_EQUAL(ips9->getPoseAt(0)->position->x, 0);
148  BOOST_CHECK_EQUAL(ips9->getPoseAt(0)->position->y, 0);
149  BOOST_CHECK_EQUAL(ips9->getPoseAt(0)->position->z, 4);
150  BOOST_CHECK_EQUAL(ips9->getPoseAt(1)->position->x, 0);
151  BOOST_CHECK_EQUAL(ips9->getPoseAt(1)->position->y, 1);
152  BOOST_CHECK_EQUAL(ips9->getPoseAt(1)->position->z, 2);
153 
154  BOOST_CHECK_EQUAL(ips10->getPoseAt(0)->position->x, 0);
155  BOOST_CHECK_EQUAL(ips10->getPoseAt(0)->position->y, 1);
156  BOOST_CHECK_EQUAL(ips10->getPoseAt(0)->position->z, 2);
157  BOOST_CHECK_EQUAL(ips10->getPoseAt(1)->position->x, 0);
158  BOOST_CHECK_EQUAL(ips10->getPoseAt(1)->position->y, 0);
159  BOOST_CHECK_EQUAL(ips10->getPoseAt(1)->position->z, 0);
160 
161 
162 }
163 
164 bool is_critical(exceptions::local::NoInterpolationPossibleException const& ex)
165 {
166  return true;
167 }
168 
169 bool is_critical2(exceptions::local::InterpolationNotDefinedException const& ex)
170 {
171  return true;
172 }
173 
174 
175 BOOST_AUTO_TEST_CASE(wrongSplineTest)
176 {
177 
178  Vector3BasePtr pos1 = new Vector3(2, 4, 6);
179  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(0, 0, 0);
180  PoseBasePtr pose1 = new Pose(pos1, ori1);
181 
182  Vector3BasePtr pos2 = new Vector3(0, -2, -8);
183  QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(1, 1, 1);
184  PoseBasePtr pose2 = new Pose(pos2, ori2);
185 
186  Vector3BasePtr pos3 = new Vector3(5, 8, 7);
187  QuaternionBasePtr ori3 = OrientationConversion::toQuaternion(2, 0, -1);
188  PoseBasePtr pose3 = new Pose(pos3, ori3);
189 
190  Vector3BasePtr pos4 = new Vector3(4, 23, 1);
191  QuaternionBasePtr ori4 = OrientationConversion::toQuaternion(0, 0, 0);
192  PoseBasePtr pose4 = new Pose(pos4, ori4);
193 
194  Vector3BasePtr pos5 = new Vector3(7, 12, 4);
195  QuaternionBasePtr ori5 = OrientationConversion::toQuaternion(1, 1, 1);
196  PoseBasePtr pose5 = new Pose(pos5, ori5);
197 
198  Vector3BasePtr pos6 = new Vector3(7, 44, -3);
199  QuaternionBasePtr ori6 = OrientationConversion::toQuaternion(2, 0, -1);
200  PoseBasePtr pose6 = new Pose(pos6, ori6);
201 
202  Vector3BasePtr pos7 = new Vector3(5, 4, -8);
203  QuaternionBasePtr ori7 = OrientationConversion::toQuaternion(1, 1, 1);
204  PoseBasePtr pose7 = new Pose(pos7, ori7);
205 
206  Vector3BasePtr pos8 = new Vector3(5, -4, -8);
207  QuaternionBasePtr ori8 = OrientationConversion::toQuaternion(2, 0, -1);
208  PoseBasePtr pose8 = new Pose(pos8, ori8);
209 
210  Vector3BasePtr pos9 = new Vector3(0, 0, 4);
211  QuaternionBasePtr ori9 = OrientationConversion::toQuaternion(0, 0, 0);
212  PoseBasePtr pose9 = new Pose(pos9, ori9);
213 
214  Vector3BasePtr pos10 = new Vector3(0, 1, 2);
215  QuaternionBasePtr ori10 = OrientationConversion::toQuaternion(0, 0, 0);
216  PoseBasePtr pose10 = new Pose(pos10, ori10);
217 
218  Vector3BasePtr pos11 = new Vector3(0, 0, 0);
219  QuaternionBasePtr ori11 = OrientationConversion::toQuaternion(0, 0, 0);
220  PoseBasePtr pose11 = new Pose(pos11, ori11);
221 
222  std::vector<PoseBasePtr> cp = {pose1, pose2, pose3, pose4, pose5, pose6, pose7, pose8, pose9, pose10, pose11};
223 
224  std::vector<InterpolationType> interpolations = {InterpolationType::eSplineInterpolation,
225  InterpolationType::eLinearInterpolation,
226  InterpolationType::eLinearInterpolation,
227  InterpolationType::eLinearInterpolation,
228  InterpolationType::eLinearInterpolation,
229  InterpolationType::eLinearInterpolation,
230  InterpolationType::eLinearInterpolation,
231  InterpolationType::eSplineInterpolation,
232  InterpolationType::eSplineInterpolation,
233  InterpolationType::eSplineInterpolation
234  };
235 
236 
237  interpolations = {InterpolationType::eSplineInterpolation,
238  InterpolationType::eSplineInterpolation,
239  InterpolationType::eLinearInterpolation,
240  InterpolationType::eLinearInterpolation,
241  InterpolationType::eSplineInterpolation,
242  InterpolationType::eLinearInterpolation,
243  InterpolationType::eLinearInterpolation,
244  InterpolationType::eSplineInterpolation,
245  InterpolationType::eSplineInterpolation,
246  InterpolationType::eSplineInterpolation
247  };
248 
249  interpolations = {InterpolationType::eSplineInterpolation,
250  InterpolationType::eLinearInterpolation,
251  InterpolationType::eLinearInterpolation,
252  InterpolationType::eLinearInterpolation,
253  InterpolationType::eLinearInterpolation,
254  InterpolationType::eLinearInterpolation,
255  InterpolationType::eLinearInterpolation,
256  InterpolationType::eLinearInterpolation,
257  InterpolationType::eLinearInterpolation,
258  InterpolationType::eSplineInterpolation
259  };
260  //If no exception is thrown here InterpolationSegmentFactory automatically replaces wrong spline Interpolations with LinearInterpolations
261 }
262 
263 BOOST_AUTO_TEST_CASE(basicDominanceTest)
264 {
265  Vector3BasePtr pos1 = new Vector3(2, 4, 6);
266  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(0, 0, 0);
267  PoseBasePtr pose1 = new Pose(pos1, ori1);
268 
269  std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
270  for (int i = 0; i < 10; i++)
271  {
272  poses.push_back(pose1);
273  }
274  std::vector<VirtualRobot::IKSolver::CartesianSelection> selections = std::vector<VirtualRobot::IKSolver::CartesianSelection>();
275  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::All);
276  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Position);
277  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Orientation);
278  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::All);
279  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Position);
280  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Position);
281  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::All);
282  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::All);
283  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Orientation);
284  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::All);
285  InterpolationSegmentFactory::optimizeControlPoints(poses, selections);
286 
287 
288  BOOST_CHECK_EQUAL(selections[0], VirtualRobot::IKSolver::CartesianSelection::All);
289  BOOST_CHECK_EQUAL(selections[1], VirtualRobot::IKSolver::CartesianSelection::All);
290  BOOST_CHECK_EQUAL(selections[2], VirtualRobot::IKSolver::CartesianSelection::All);
291  BOOST_CHECK_EQUAL(selections[3], VirtualRobot::IKSolver::CartesianSelection::All);
292  BOOST_CHECK_EQUAL(selections[4], VirtualRobot::IKSolver::CartesianSelection::All);
293  BOOST_CHECK_EQUAL(selections[5], VirtualRobot::IKSolver::CartesianSelection::All);
294  BOOST_CHECK_EQUAL(selections[6], VirtualRobot::IKSolver::CartesianSelection::All);
295  BOOST_CHECK_EQUAL(selections[7], VirtualRobot::IKSolver::CartesianSelection::All);
296  BOOST_CHECK_EQUAL(selections[8], VirtualRobot::IKSolver::CartesianSelection::All);
297  BOOST_CHECK_EQUAL(selections[9], VirtualRobot::IKSolver::CartesianSelection::All);
298 
299 
300 }
301 
302 BOOST_AUTO_TEST_CASE(advancedDominanceTest)
303 {
304  Vector3BasePtr pos1 = new Vector3(2, 4, 6);
305  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(0, 0, 0);
306  PoseBasePtr pose1 = new Pose(pos1, ori1);
307 
308  std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
309  for (int i = 0; i < 11; i++)
310  {
311  poses.push_back(pose1);
312  }
313  std::vector<VirtualRobot::IKSolver::CartesianSelection> selections = std::vector<VirtualRobot::IKSolver::CartesianSelection>();
314  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::All);
315  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Orientation);
316  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::All);
317  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::X);
318  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Y);
319  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Position);
320  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Position);
321  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Y);
322  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Position);
323  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Z);
324  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Z);
325 
326  InterpolationSegmentFactory::optimizeControlPoints(poses, selections);
327 
328 
329  BOOST_CHECK_EQUAL(selections[0], VirtualRobot::IKSolver::CartesianSelection::All);
330  BOOST_CHECK_EQUAL(selections[1], VirtualRobot::IKSolver::CartesianSelection::All);
331  BOOST_CHECK_EQUAL(selections[2], VirtualRobot::IKSolver::CartesianSelection::All);
332  BOOST_CHECK_EQUAL(selections[3], VirtualRobot::IKSolver::CartesianSelection::Position);
333  BOOST_CHECK_EQUAL(selections[4], VirtualRobot::IKSolver::CartesianSelection::Position);
334  BOOST_CHECK_EQUAL(selections[5], VirtualRobot::IKSolver::CartesianSelection::Position);
335  BOOST_CHECK_EQUAL(selections[6], VirtualRobot::IKSolver::CartesianSelection::Position);
336  BOOST_CHECK_EQUAL(selections[7], VirtualRobot::IKSolver::CartesianSelection::Position);
337  BOOST_CHECK_EQUAL(selections[8], VirtualRobot::IKSolver::CartesianSelection::Position);
338  BOOST_CHECK_EQUAL(selections[9], VirtualRobot::IKSolver::CartesianSelection::Z);
339  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Z);
340 }
341 
342 BOOST_AUTO_TEST_CASE(reverseOrderDominanceTest)
343 {
344  Vector3BasePtr pos1 = new Vector3(2, 4, 6);
345  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(0, 0, 0);
346  PoseBasePtr pose1 = new Pose(pos1, ori1);
347 
348  std::vector<PoseBasePtr> poses = std::vector<PoseBasePtr>();
349  for (int i = 0; i < 20; i++)
350  {
351  poses.push_back(pose1);
352  }
353  std::vector<VirtualRobot::IKSolver::CartesianSelection> selections = std::vector<VirtualRobot::IKSolver::CartesianSelection>();
354  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::X);
355  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Orientation);
356  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Orientation);
357  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Position);
358  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::X);
359  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Orientation);
360  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::X);
361  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Y);
362  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Position);
363  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::X);
364  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Y);
365  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Z);
366  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Z);
367  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Position);
368  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Position);
369  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Y);
370  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Z);
371  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Y);
372  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::X);
373  selections.push_back(VirtualRobot::IKSolver::CartesianSelection::X);
374  InterpolationSegmentFactory::optimizeControlPoints(poses, selections);
375 
376 
377  BOOST_CHECK_EQUAL(selections[0], VirtualRobot::IKSolver::CartesianSelection::All);
378  BOOST_CHECK_EQUAL(selections[1], VirtualRobot::IKSolver::CartesianSelection::All);
379  BOOST_CHECK_EQUAL(selections[2], VirtualRobot::IKSolver::CartesianSelection::All);
380  BOOST_CHECK_EQUAL(selections[3], VirtualRobot::IKSolver::CartesianSelection::All);
381  BOOST_CHECK_EQUAL(selections[4], VirtualRobot::IKSolver::CartesianSelection::All);
382  BOOST_CHECK_EQUAL(selections[5], VirtualRobot::IKSolver::CartesianSelection::All);
383  BOOST_CHECK_EQUAL(selections[6], VirtualRobot::IKSolver::CartesianSelection::All);
384  BOOST_CHECK_EQUAL(selections[7], VirtualRobot::IKSolver::CartesianSelection::All);
385  BOOST_CHECK_EQUAL(selections[8], VirtualRobot::IKSolver::CartesianSelection::All);
386  BOOST_CHECK_EQUAL(selections[9], VirtualRobot::IKSolver::CartesianSelection::Position);
387  BOOST_CHECK_EQUAL(selections[10], VirtualRobot::IKSolver::CartesianSelection::Position);
388  BOOST_CHECK_EQUAL(selections[11], VirtualRobot::IKSolver::CartesianSelection::Position);
389  BOOST_CHECK_EQUAL(selections[12], VirtualRobot::IKSolver::CartesianSelection::Position);
390  BOOST_CHECK_EQUAL(selections[13], VirtualRobot::IKSolver::CartesianSelection::Position);
391  BOOST_CHECK_EQUAL(selections[14], VirtualRobot::IKSolver::CartesianSelection::Position);
392  BOOST_CHECK_EQUAL(selections[15], VirtualRobot::IKSolver::CartesianSelection::Position);
393  BOOST_CHECK_EQUAL(selections[16], VirtualRobot::IKSolver::CartesianSelection::Position);
394  BOOST_CHECK_EQUAL(selections[17], VirtualRobot::IKSolver::CartesianSelection::Position);
395  BOOST_CHECK_EQUAL(selections[18], VirtualRobot::IKSolver::CartesianSelection::Position);
396  BOOST_CHECK_EQUAL(selections[19], VirtualRobot::IKSolver::CartesianSelection::X);
397 
398 
399 }
400 
401 
402 
403 
404 
405 
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(basicTest)
Definition: InterpolationSegmentFactoryTest.cpp:18
is_critical
bool is_critical(exceptions::local::NoInterpolationPossibleException const &ex)
Definition: InterpolationSegmentFactoryTest.cpp:164
GfxTL::Orientation
ScalarT Orientation(const VectorXD< 2, ScalarT > &p1, const VectorXD< 2, ScalarT > &p2, const VectorXD< 2, ScalarT > &c)
Definition: Orientation.h:9
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
Pose.h
armarx::exceptions::local::NoInterpolationPossibleException
Definition: NoInterpolationPossibleException.h:40
armarx::eSplineInterpolation
@ eSplineInterpolation
Definition: InterpolationType.h:35
is_critical2
bool is_critical2(exceptions::local::InterpolationNotDefinedException const &ex)
Definition: InterpolationSegmentFactoryTest.cpp:169
armarx::AbstractInterpolationPtr
std::shared_ptr< AbstractInterpolation > AbstractInterpolationPtr
Definition: AbstractInterpolation.h:77
armarx::exceptions::local::InterpolationNotDefinedException
Definition: InterpolationNotDefinedException.h:42
armarx::OrientationConversion::toQuaternion
static QuaternionBasePtr toQuaternion(const double roll, const double pitch, const double yaw)
Definition: OrientationConversion.cpp:57
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:41
armarx::InterpolationSegmentFactory::optimizeControlPoints
static void optimizeControlPoints(std::vector< PoseBasePtr > &controlPoints, std::vector< VirtualRobot::IKSolver::CartesianSelection > &selections)
optimizeControlPoints changes the cartian selections and control points so that the IKSolving produce...
Definition: InterpolationSegmentFactory.cpp:191
armarx::InterpolationSegmentFactory::produceInterpolationSegments
static std::vector< AbstractInterpolationPtr > produceInterpolationSegments(std::vector< PoseBasePtr > controlPoints, std::vector< InterpolationType > interpolations)
produceInterpolationSegments constructs a vector of AbstractInterpolation the concrete Interpolationt...
Definition: InterpolationSegmentFactory.cpp:36
armarx::navigation::core::Position
Eigen::Vector3f Position
Definition: basic_types.h:36
armarx::eLinearInterpolation
@ eLinearInterpolation
Definition: InterpolationType.h:34
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28