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