LinearInterpolationTest.cpp
Go to the documentation of this file.
1 #define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::LinearInterpolation
2 #define ARMARX_BOOST_TEST
3 
4 
5 #include <RobotComponents/Test.h>
6 
7 #include "../../Interpolation/LinearInterpolation.h"
8 #include "../../Util/OrientationConversion.h"
9 #include <RobotAPI/libraries/core/Pose.h>
10 #include <RobotAPI/libraries/core/FramedPose.h>
11 using namespace armarx;
12 using namespace std;
13 
14 
15 BOOST_AUTO_TEST_CASE(basicTest)
16 {
17  Vector3BasePtr pos1 = new Vector3(2, 4, 6);
18  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(1, 1, 1);
19  PoseBasePtr pose1 = new Pose(pos1, ori1);
20 
21  Vector3BasePtr pos2 = new Vector3(0, 1, 2);
22  QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(0, 0, 0);
23  PoseBasePtr pose2 = new Pose(pos2, ori2);
24 
25  std::vector<PoseBasePtr> cp = *new std::vector<PoseBasePtr>();
26  cp.push_back(pose1);
27  cp.push_back(pose2);
28  LinearInterpolationPtr ip = *new LinearInterpolationPtr(new LinearInterpolation(cp));
29 
30  BOOST_CHECK_EQUAL(ip->getNumberOfControlPoints(), 2);
31 
32  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->x, 2);
33  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->y, 4);
34  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->z, 6);
35  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->x, 0);
36  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->y, 1);
37  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->z, 2);
38  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->x, 1);
39  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->y, 2.5);
40  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->z, 4);
41 }
42 
43 BOOST_AUTO_TEST_CASE(threePointPositionTest)
44 {
45  Vector3BasePtr pos1 = new Vector3(2, 4, 6);
46  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(1, 1, 1);
47  PoseBasePtr pose1 = new Pose(pos1, ori1);
48 
49  Vector3BasePtr pos2 = new Vector3(0, 1, 2);
50  QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(0, 0, 0);
51  PoseBasePtr pose2 = new Pose(pos2, ori2);
52 
53 
54  Vector3BasePtr pos3 = new Vector3(0, -5, -2);
55  QuaternionBasePtr ori3 = OrientationConversion::toQuaternion(0, 0, 0);
56  PoseBasePtr pose3 = new Pose(pos3, ori3);
57 
58  std::vector<PoseBasePtr> cp = {pose1, pose2, pose3};
59  LinearInterpolationPtr ip = *new LinearInterpolationPtr(new LinearInterpolation(cp));
60 
61  BOOST_CHECK_EQUAL(ip->getNumberOfControlPoints(), 3);
62 
63  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->x, 2);
64  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->y, 4);
65  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->z, 6);
66  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->x, 0);
67  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->y, 1);
68  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->z, 2);
69  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->x, 0);
70  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->y, -5);
71  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->z, -2);
72 }
73 
74 BOOST_AUTO_TEST_CASE(samePointTwiceTest)
75 {
76  Vector3BasePtr pos1 = new Vector3(2, 4, 6);
77  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(0, 0, 0);
78  PoseBasePtr pose1 = new Pose(pos1, ori1);
79 
80  Vector3BasePtr pos2 = new Vector3(2, 4, 6);
81  QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(0, 0, 0);
82  PoseBasePtr pose2 = new Pose(pos2, ori2);
83 
84  std::vector<PoseBasePtr> cp = {pose1, pose2};
85  LinearInterpolationPtr ip = *new LinearInterpolationPtr(new LinearInterpolation(cp));
86 
87  BOOST_CHECK_EQUAL(ip->getNumberOfControlPoints(), 2);
88 
89  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->x, 2);
90  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->y, 4);
91  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->z, 6);
92  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->x, 2);
93  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->y, 4);
94  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->z, 6);
95  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->x, 2);
96  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->y, 4);
97  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->z, 6);
98  BOOST_CHECK_EQUAL(ip->getPoseAt(0.154645)->position->x, 2);
99  BOOST_CHECK_EQUAL(ip->getPoseAt(0.154645)->position->y, 4);
100  BOOST_CHECK_EQUAL(ip->getPoseAt(0.154645)->position->z, 6);
101  BOOST_CHECK_EQUAL(ip->getPoseAt(0.8997)->position->x, 2);
102  BOOST_CHECK_EQUAL(ip->getPoseAt(0.8997)->position->y, 4);
103  BOOST_CHECK_EQUAL(ip->getPoseAt(0.8997)->position->z, 6);
104 
105 }
106 
107 
108 BOOST_AUTO_TEST_CASE(rotateAtPlaceTest)
109 {
110  Vector3BasePtr pos1 = new Vector3(2, 4, 6);
111  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(0, 0, 0);
112  PoseBasePtr pose1 = new Pose(pos1, ori1);
113 
114  Vector3BasePtr pos2 = new Vector3(2, 4, 6);
115  QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(1, 1, 1);
116  PoseBasePtr pose2 = new Pose(pos2, ori2);
117 
118  Vector3BasePtr pos3 = new Vector3(2, 4, 6);
119  QuaternionBasePtr ori3 = OrientationConversion::toQuaternion(2, 0, -1);
120  PoseBasePtr pose3 = new Pose(pos3, ori3);
121 
122  Vector3BasePtr pos4 = new Vector3(2, 4, 6);
123  QuaternionBasePtr ori4 = OrientationConversion::toQuaternion(0, 1, 2);
124  PoseBasePtr pose4 = new Pose(pos4, ori4);
125 
126  std::vector<PoseBasePtr> cp = {pose1, pose2, pose3, pose4};
127  LinearInterpolationPtr ip = *new LinearInterpolationPtr(new LinearInterpolation(cp));
128 
129  QuaternionBasePtr q1 = OrientationConversion::toQuaternion(0, 0, 0);
130  QuaternionBasePtr q2 = OrientationConversion::toQuaternion(1, 1, 1);
131  QuaternionBasePtr q3 = OrientationConversion::toQuaternion(2, 0, -1);
132  QuaternionBasePtr q4 = OrientationConversion::toQuaternion(0, 1, 2);
133 
134  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->orientation->qw, q1->qw);
135  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->orientation->qx, q1->qx);
136  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->orientation->qy, q1->qy);
137  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->orientation->qz, q1->qz);
138 
139  BOOST_CHECK_CLOSE(ip->getPoseAt(0.333)->orientation->qw, q2->qw, 1);
140  BOOST_CHECK_CLOSE(ip->getPoseAt(0.333)->orientation->qx, q2->qx, 1);
141  BOOST_CHECK_CLOSE(ip->getPoseAt(0.333)->orientation->qy, q2->qy, 1);
142  BOOST_CHECK_CLOSE(ip->getPoseAt(0.333)->orientation->qz, q2->qz, 1);
143 
144  BOOST_CHECK_CLOSE(ip->getPoseAt(0.666)->orientation->qw, q3->qw, 1);
145  BOOST_CHECK_CLOSE(ip->getPoseAt(0.666)->orientation->qx, q3->qx, 1);
146  BOOST_CHECK_CLOSE(ip->getPoseAt(0.666)->orientation->qy, q3->qy, 1);
147  BOOST_CHECK_CLOSE(ip->getPoseAt(0.666)->orientation->qz, q3->qz, 1);
148 
149  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->orientation->qw, q4->qw);
150  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->orientation->qx, q4->qx);
151  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->orientation->qy, q4->qy);
152  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->orientation->qz, q4->qz);
153 
154 
155 }
156 
157 BOOST_AUTO_TEST_CASE(startIsEndTest)
158 {
159  Vector3BasePtr pos1 = new Vector3(2, 4, 6);
160  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(0, 0, 0);
161  PoseBasePtr pose1 = new Pose(pos1, ori1);
162 
163  Vector3BasePtr pos2 = new Vector3(0, -2, -8);
164  QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(1, 1, 1);
165  PoseBasePtr pose2 = new Pose(pos2, ori2);
166 
167  Vector3BasePtr pos3 = new Vector3(2, 4, 6);
168  QuaternionBasePtr ori3 = OrientationConversion::toQuaternion(2, 0, -1);
169  PoseBasePtr pose3 = new Pose(pos3, ori3);
170 
171  std::vector<PoseBasePtr> cp = {pose1, pose2, pose3};
172  LinearInterpolationPtr ip = *new LinearInterpolationPtr(new LinearInterpolation(cp));
173 
174  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->x, 2);
175  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->y, 4);
176  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->z, 6);
177  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->x, 2);
178  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->y, 4);
179  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->z, 6);
180  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->x, 0);
181  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->y, -2);
182  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->z, -8);
183  BOOST_CHECK_EQUAL(ip->getPoseAt(0.25)->position->x, 1);
184  BOOST_CHECK_EQUAL(ip->getPoseAt(0.25)->position->y, 1);
185  BOOST_CHECK_EQUAL(ip->getPoseAt(0.25)->position->z, -1);
186  BOOST_CHECK_EQUAL(ip->getPoseAt(0.75)->position->x, 1);
187  BOOST_CHECK_EQUAL(ip->getPoseAt(0.75)->position->y, 1);
188  BOOST_CHECK_EQUAL(ip->getPoseAt(0.75)->position->z, -1);
189 
190 
191 }
192 
193 BOOST_AUTO_TEST_CASE(tenPointPositionTest)
194 {
195  Vector3BasePtr pos1 = new Vector3(2, 4, 6);
196  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(0, 0, 0);
197  PoseBasePtr pose1 = new Pose(pos1, ori1);
198 
199  Vector3BasePtr pos2 = new Vector3(0, -2, -8);
200  QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(1, 1, 1);
201  PoseBasePtr pose2 = new Pose(pos2, ori2);
202 
203  Vector3BasePtr pos3 = new Vector3(5, 8, 7);
204  QuaternionBasePtr ori3 = OrientationConversion::toQuaternion(2, 0, -1);
205  PoseBasePtr pose3 = new Pose(pos3, ori3);
206 
207  Vector3BasePtr pos4 = new Vector3(4, 23, 1);
208  QuaternionBasePtr ori4 = OrientationConversion::toQuaternion(0, 0, 0);
209  PoseBasePtr pose4 = new Pose(pos4, ori4);
210 
211  Vector3BasePtr pos5 = new Vector3(7, 12, 4);
212  QuaternionBasePtr ori5 = OrientationConversion::toQuaternion(1, 1, 1);
213  PoseBasePtr pose5 = new Pose(pos5, ori5);
214 
215  Vector3BasePtr pos6 = new Vector3(7, 44, -3);
216  QuaternionBasePtr ori6 = OrientationConversion::toQuaternion(2, 0, -1);
217  PoseBasePtr pose6 = new Pose(pos6, ori6);
218 
219  Vector3BasePtr pos7 = new Vector3(5, 4, -8);
220  QuaternionBasePtr ori7 = OrientationConversion::toQuaternion(1, 1, 1);
221  PoseBasePtr pose7 = new Pose(pos7, ori7);
222 
223  Vector3BasePtr pos8 = new Vector3(5, -4, -8);
224  QuaternionBasePtr ori8 = OrientationConversion::toQuaternion(2, 0, -1);
225  PoseBasePtr pose8 = new Pose(pos8, ori8);
226 
227  Vector3BasePtr pos9 = new Vector3(0, 0, 4);
228  QuaternionBasePtr ori9 = OrientationConversion::toQuaternion(0, 0, 0);
229  PoseBasePtr pose9 = new Pose(pos9, ori9);
230 
231  Vector3BasePtr pos10 = new Vector3(0, 1, 2);
232  QuaternionBasePtr ori10 = OrientationConversion::toQuaternion(0, 0, 0);
233  PoseBasePtr pose10 = new Pose(pos10, ori10);
234 
235  Vector3BasePtr pos11 = new Vector3(0, 0, 0);
236  QuaternionBasePtr ori11 = OrientationConversion::toQuaternion(0, 0, 0);
237  PoseBasePtr pose11 = new Pose(pos11, ori11);
238 
239  /*
240  * T
241  * O
242  * D
243  * O
244  * :
245  *
246  * N
247  * U
248  * L
249  * L
250  * P
251  * O
252  * I
253  * N
254  * T
255  * E
256  * R
257  */
258  std::vector<PoseBasePtr> cp = {pose1, pose2, pose3, pose4, pose5, pose6, pose7, pose8, pose9, pose10, pose11};
259  LinearInterpolationPtr ip = *new LinearInterpolationPtr(new LinearInterpolation(cp));
260 
261  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->x, 2);
262  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->y, 4);
263  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->z, 6);
264  BOOST_CHECK_EQUAL(ip->getPoseAt(0.1)->position->x, 0);
265  BOOST_CHECK_EQUAL(ip->getPoseAt(0.1)->position->y, -2);
266  BOOST_CHECK_EQUAL(ip->getPoseAt(0.1)->position->z, -8);
267  BOOST_CHECK_EQUAL(ip->getPoseAt(0.2)->position->x, 5);
268  BOOST_CHECK_EQUAL(ip->getPoseAt(0.2)->position->y, 8);
269  BOOST_CHECK_EQUAL(ip->getPoseAt(0.2)->position->z, 7);
270  BOOST_CHECK_EQUAL(ip->getPoseAt(0.3)->position->x, 4);
271  BOOST_CHECK_EQUAL(ip->getPoseAt(0.3)->position->y, 23);
272  BOOST_CHECK_EQUAL(ip->getPoseAt(0.3)->position->z, 1);
273  BOOST_CHECK_EQUAL(ip->getPoseAt(0.4)->position->x, 7);
274  BOOST_CHECK_EQUAL(ip->getPoseAt(0.4)->position->y, 12);
275  BOOST_CHECK_EQUAL(ip->getPoseAt(0.4)->position->z, 4);
276  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->x, 7);
277  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->y, 44);
278  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->z, -3);
279  BOOST_CHECK_EQUAL(ip->getPoseAt(0.6)->position->x, 5);
280  BOOST_CHECK_EQUAL(ip->getPoseAt(0.6)->position->y, 4);
281  BOOST_CHECK_EQUAL(ip->getPoseAt(0.6)->position->z, -8);
282  BOOST_CHECK_EQUAL(ip->getPoseAt(0.7)->position->x, 5);
283  BOOST_CHECK_EQUAL(ip->getPoseAt(0.7)->position->y, -4);
284  BOOST_CHECK_EQUAL(ip->getPoseAt(0.7)->position->z, -8);
285  BOOST_CHECK_EQUAL(ip->getPoseAt(0.8)->position->x, 0);
286  BOOST_CHECK_EQUAL(ip->getPoseAt(0.8)->position->y, 0);
287  BOOST_CHECK_EQUAL(ip->getPoseAt(0.8)->position->z, 4);
288  BOOST_CHECK_EQUAL(ip->getPoseAt(0.9)->position->x, 0);
289  BOOST_CHECK_EQUAL(ip->getPoseAt(0.9)->position->y, 1);
290  BOOST_CHECK_EQUAL(ip->getPoseAt(0.9)->position->z, 2);
291  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->x, 0);
292  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->y, 0);
293  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->z, 0);
294 
295 
296 
297 }
298 
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
Pose.h
armarx::LinearInterpolationPtr
std::shared_ptr< LinearInterpolation > LinearInterpolationPtr
Definition: LinearInterpolation.h:57
FramedPose.h
armarx::OrientationConversion::toQuaternion
static QuaternionBasePtr toQuaternion(const double roll, const double pitch, const double yaw)
Definition: OrientationConversion.cpp:57
armarx::LinearInterpolation
The LinearInterpolation class represents a linear Interpolation between a series of control points Li...
Definition: LinearInterpolation.h:36
std
Definition: Application.h:66
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:41
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(basicTest)
Definition: LinearInterpolationTest.cpp:15
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28