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