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