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
18using namespace armarx;
19
20BOOST_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
165bool
170
171bool
176
177BOOST_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
263BOOST_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
301BOOST_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
342BOOST_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}
is_critical
bool is_critical(exceptions::local::NoInterpolationPossibleException const &ex)
Definition InterpolationSegmentFactoryTest.cpp:166
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(basicTest)
Definition InterpolationSegmentFactoryTest.cpp:20
is_critical2
bool is_critical2(exceptions::local::InterpolationNotDefinedException const &ex)
Definition InterpolationSegmentFactoryTest.cpp:172
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::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::OrientationConversion::toQuaternion
static QuaternionBasePtr toQuaternion(const double roll, const double pitch, const double yaw)
Definition OrientationConversion.cpp:58
armarx::Pose
The Pose class.
Definition Pose.h:243
armarx::Vector3
The Vector3 class.
Definition Pose.h:113
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition ArmarXTimeserver.cpp:28
armarx::AbstractInterpolationPtr
std::shared_ptr< AbstractInterpolation > AbstractInterpolationPtr
Definition AbstractInterpolation.h:76