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"
13 using namespace armarx;
14 using namespace std;
15 
16 BOOST_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 
44 BOOST_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 
75 BOOST_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 
107 BOOST_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 
154 BOOST_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 
188 BOOST_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 }
armarx::LinearInterpolationPtr
std::shared_ptr< LinearInterpolation > LinearInterpolationPtr
Definition: LinearInterpolation.h:56
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
Pose.h
FramedPose.h
armarx::OrientationConversion::toQuaternion
static QuaternionBasePtr toQuaternion(const double roll, const double pitch, const double yaw)
Definition: OrientationConversion.cpp:58
armarx::LinearInterpolation
The LinearInterpolation class represents a linear Interpolation between a series of control points Li...
Definition: LinearInterpolation.h:35
std
Definition: Application.h:66
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:43
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(basicTest)
Definition: LinearInterpolationTest.cpp:16
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27