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 <RobotComponents/Test.h>
5 
6 #include "../../Interpolation/LinearInterpolation.h"
7 #include "../../Util/OrientationConversion.h"
8 #include "../../Interpolation/SplineInterpolation.h"
9 
10 #include <RobotAPI/libraries/core/FramedPose.h>
11 #include <RobotAPI/libraries/core/Pose.h>
12 
13 using namespace armarx;
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 
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 
50 BOOST_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 
111 }
112 
113 
114 BOOST_AUTO_TEST_CASE(startIsEndTest)
115 {
116  Vector3BasePtr pos1 = new Vector3(2, 4, 6);
117  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(0, 0, 0);
118  PoseBasePtr pose1 = new Pose(pos1, ori1);
119 
120  Vector3BasePtr pos2 = new Vector3(0, -2, -8);
121  QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(1, 1, 1);
122  PoseBasePtr pose2 = new Pose(pos2, ori2);
123 
124  Vector3BasePtr pos3 = new Vector3(2, 4, 6);
125  QuaternionBasePtr ori3 = OrientationConversion::toQuaternion(2, 0, -1);
126  PoseBasePtr pose3 = new Pose(pos3, ori3);
127 
128  std::vector<PoseBasePtr> cp = {pose1, pose2, pose3};
129  SplineInterpolationPtr ip = *new SplineInterpolationPtr(new SplineInterpolation(cp));
130  AbstractInterpolationPtr ips1 = ip->getInterPolationSegment(0);
131  AbstractInterpolationPtr ips2 = ip->getInterPolationSegment(1);
132 
133 
134  BOOST_CHECK_EQUAL(ips1->getPoseAt(0)->position->x, 2);
135  BOOST_CHECK_EQUAL(ips1->getPoseAt(0)->position->y, 4);
136  BOOST_CHECK_EQUAL(ips1->getPoseAt(0)->position->z, 6);
137  BOOST_CHECK_EQUAL(ips1->getPoseAt(1)->position->x, 0);
138  BOOST_CHECK_EQUAL(ips1->getPoseAt(1)->position->y, -2);
139  BOOST_CHECK_EQUAL(ips1->getPoseAt(1)->position->z, -8);
140  BOOST_CHECK_EQUAL(ips2->getPoseAt(0)->position->x, 0);
141  BOOST_CHECK_EQUAL(ips2->getPoseAt(0)->position->y, -2);
142  BOOST_CHECK_EQUAL(ips2->getPoseAt(0)->position->z, -8);
143  BOOST_CHECK_EQUAL(ips2->getPoseAt(1)->position->x, 2);
144  BOOST_CHECK_EQUAL(ips2->getPoseAt(1)->position->y, 4);
145  BOOST_CHECK_EQUAL(ips2->getPoseAt(1)->position->z, 6);
146 
147 
148 }
149 
150 
151 BOOST_AUTO_TEST_CASE(tenPointPositionTest)
152 {
153  Vector3BasePtr pos1 = new Vector3(2, 4, 6);
154  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(0, 0, 0);
155  PoseBasePtr pose1 = new Pose(pos1, ori1);
156 
157  Vector3BasePtr pos2 = new Vector3(0, -2, -8);
158  QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(1, 1, 1);
159  PoseBasePtr pose2 = new Pose(pos2, ori2);
160 
161  Vector3BasePtr pos3 = new Vector3(5, 8, 7);
162  QuaternionBasePtr ori3 = OrientationConversion::toQuaternion(2, 0, -1);
163  PoseBasePtr pose3 = new Pose(pos3, ori3);
164 
165  Vector3BasePtr pos4 = new Vector3(4, 23, 1);
166  QuaternionBasePtr ori4 = OrientationConversion::toQuaternion(0, 0, 0);
167  PoseBasePtr pose4 = new Pose(pos4, ori4);
168 
169  Vector3BasePtr pos5 = new Vector3(7, 12, 4);
170  QuaternionBasePtr ori5 = OrientationConversion::toQuaternion(1, 1, 1);
171  PoseBasePtr pose5 = new Pose(pos5, ori5);
172 
173  Vector3BasePtr pos6 = new Vector3(7, 44, -3);
174  QuaternionBasePtr ori6 = OrientationConversion::toQuaternion(2, 0, -1);
175  PoseBasePtr pose6 = new Pose(pos6, ori6);
176 
177  Vector3BasePtr pos7 = new Vector3(5, 4, -8);
178  QuaternionBasePtr ori7 = OrientationConversion::toQuaternion(1, 1, 1);
179  PoseBasePtr pose7 = new Pose(pos7, ori7);
180 
181  Vector3BasePtr pos8 = new Vector3(5, -4, -8);
182  QuaternionBasePtr ori8 = OrientationConversion::toQuaternion(2, 0, -1);
183  PoseBasePtr pose8 = new Pose(pos8, ori8);
184 
185  Vector3BasePtr pos9 = new Vector3(0, 0, 4);
186  QuaternionBasePtr ori9 = OrientationConversion::toQuaternion(0, 0, 0);
187  PoseBasePtr pose9 = new Pose(pos9, ori9);
188 
189  Vector3BasePtr pos10 = new Vector3(0, 1, 2);
190  QuaternionBasePtr ori10 = OrientationConversion::toQuaternion(0, 0, 0);
191  PoseBasePtr pose10 = new Pose(pos10, ori10);
192 
193  Vector3BasePtr pos11 = new Vector3(0, 0, 0);
194  QuaternionBasePtr ori11 = OrientationConversion::toQuaternion(0, 0, 0);
195  PoseBasePtr pose11 = new Pose(pos11, ori11);
196 
197  std::vector<PoseBasePtr> cp = {pose1, pose2, pose3, pose4, pose5, pose6, pose7, pose8, pose9, pose10, pose11};
198  SplineInterpolationPtr ip = *new SplineInterpolationPtr(new SplineInterpolation(cp));
199  AbstractInterpolationPtr ips1 = ip->getInterPolationSegment(0);
200  AbstractInterpolationPtr ips2 = ip->getInterPolationSegment(1);
201  AbstractInterpolationPtr ips3 = ip->getInterPolationSegment(2);
202  AbstractInterpolationPtr ips4 = ip->getInterPolationSegment(3);
203  AbstractInterpolationPtr ips5 = ip->getInterPolationSegment(4);
204  AbstractInterpolationPtr ips6 = ip->getInterPolationSegment(5);
205  AbstractInterpolationPtr ips7 = ip->getInterPolationSegment(6);
206  AbstractInterpolationPtr ips8 = ip->getInterPolationSegment(7);
207  AbstractInterpolationPtr ips9 = ip->getInterPolationSegment(8);
208  AbstractInterpolationPtr ips10 = ip->getInterPolationSegment(9);
209 
210  BOOST_CHECK_EQUAL(ips1->getPoseAt(0)->position->x, 2);
211  BOOST_CHECK_EQUAL(ips1->getPoseAt(0)->position->y, 4);
212  BOOST_CHECK_EQUAL(ips1->getPoseAt(0)->position->z, 6);
213  BOOST_CHECK_EQUAL(ips1->getPoseAt(1)->position->x, 0);
214  BOOST_CHECK_EQUAL(ips1->getPoseAt(1)->position->y, -2);
215  BOOST_CHECK_EQUAL(ips1->getPoseAt(1)->position->z, -8);
216 
217  BOOST_CHECK_EQUAL(ips2->getPoseAt(0)->position->x, 0);
218  BOOST_CHECK_EQUAL(ips2->getPoseAt(0)->position->y, -2);
219  BOOST_CHECK_EQUAL(ips2->getPoseAt(0)->position->z, -8);
220  BOOST_CHECK_EQUAL(ips2->getPoseAt(1)->position->x, 5);
221  BOOST_CHECK_EQUAL(ips2->getPoseAt(1)->position->y, 8);
222  BOOST_CHECK_EQUAL(ips2->getPoseAt(1)->position->z, 7);
223 
224  BOOST_CHECK_EQUAL(ips3->getPoseAt(0)->position->x, 5);
225  BOOST_CHECK_EQUAL(ips3->getPoseAt(0)->position->y, 8);
226  BOOST_CHECK_EQUAL(ips3->getPoseAt(0)->position->z, 7);
227  BOOST_CHECK_EQUAL(ips3->getPoseAt(1)->position->x, 4);
228  BOOST_CHECK_EQUAL(ips3->getPoseAt(1)->position->y, 23);
229  BOOST_CHECK_EQUAL(ips3->getPoseAt(1)->position->z, 1);
230 
231  BOOST_CHECK_EQUAL(ips4->getPoseAt(0)->position->x, 4);
232  BOOST_CHECK_EQUAL(ips4->getPoseAt(0)->position->y, 23);
233  BOOST_CHECK_EQUAL(ips4->getPoseAt(0)->position->z, 1);
234  BOOST_CHECK_EQUAL(ips4->getPoseAt(1)->position->x, 7);
235  BOOST_CHECK_EQUAL(ips4->getPoseAt(1)->position->y, 12);
236  BOOST_CHECK_EQUAL(ips4->getPoseAt(1)->position->z, 4);
237 
238  BOOST_CHECK_EQUAL(ips5->getPoseAt(0)->position->x, 7);
239  BOOST_CHECK_EQUAL(ips5->getPoseAt(0)->position->y, 12);
240  BOOST_CHECK_EQUAL(ips5->getPoseAt(0)->position->z, 4);
241  BOOST_CHECK_EQUAL(ips5->getPoseAt(1)->position->x, 7);
242  BOOST_CHECK_EQUAL(ips5->getPoseAt(1)->position->y, 44);
243  BOOST_CHECK_EQUAL(ips5->getPoseAt(1)->position->z, -3);
244 
245  BOOST_CHECK_EQUAL(ips6->getPoseAt(0)->position->x, 7);
246  BOOST_CHECK_EQUAL(ips6->getPoseAt(0)->position->y, 44);
247  BOOST_CHECK_EQUAL(ips6->getPoseAt(0)->position->z, -3);
248  BOOST_CHECK_EQUAL(ips6->getPoseAt(1)->position->x, 5);
249  BOOST_CHECK_EQUAL(ips6->getPoseAt(1)->position->y, 4);
250  BOOST_CHECK_EQUAL(ips6->getPoseAt(1)->position->z, -8);
251 
252  BOOST_CHECK_EQUAL(ips7->getPoseAt(0)->position->x, 5);
253  BOOST_CHECK_EQUAL(ips7->getPoseAt(0)->position->y, 4);
254  BOOST_CHECK_EQUAL(ips7->getPoseAt(0)->position->z, -8);
255  BOOST_CHECK_EQUAL(ips7->getPoseAt(1)->position->x, 5);
256  BOOST_CHECK_EQUAL(ips7->getPoseAt(1)->position->y, -4);
257  BOOST_CHECK_EQUAL(ips7->getPoseAt(1)->position->z, -8);
258 
259  BOOST_CHECK_EQUAL(ips8->getPoseAt(0)->position->x, 5);
260  BOOST_CHECK_EQUAL(ips8->getPoseAt(0)->position->y, -4);
261  BOOST_CHECK_EQUAL(ips8->getPoseAt(0)->position->z, -8);
262  BOOST_CHECK_EQUAL(ips8->getPoseAt(1)->position->x, 0);
263  //BOOST_CHECK_EQUAL(ips8->getPoseAt(1)->position->y, 0);
264  BOOST_CHECK_EQUAL(ips8->getPoseAt(1)->position->z, 4);
265 
266  BOOST_CHECK_EQUAL(ips9->getPoseAt(0)->position->x, 0);
267  //BOOST_CHECK_EQUAL(ips9->getPoseAt(0)->position->y, 0);
268  BOOST_CHECK_EQUAL(ips9->getPoseAt(0)->position->z, 4);
269  //BOOST_CHECK_EQUAL(ips9->getPoseAt(1)->position->x, 0);
270  BOOST_CHECK_EQUAL(ips9->getPoseAt(1)->position->y, 1);
271  BOOST_CHECK_EQUAL(ips9->getPoseAt(1)->position->z, 2);
272 
273  //BOOST_CHECK_EQUAL(ips10->getPoseAt(0)->position->x, 0);
274  BOOST_CHECK_EQUAL(ips10->getPoseAt(0)->position->y, 1);
275  BOOST_CHECK_EQUAL(ips10->getPoseAt(0)->position->z, 2);
276  BOOST_CHECK_EQUAL(ips10->getPoseAt(1)->position->x, 0);
277  BOOST_CHECK_EQUAL(ips10->getPoseAt(1)->position->y, 0);
278  BOOST_CHECK_EQUAL(ips10->getPoseAt(1)->position->z, 0);
279 }
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
Pose.h
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(basicTest)
Definition: SplineInterpolationSegmentTest.cpp:15
armarx::AbstractInterpolationPtr
std::shared_ptr< AbstractInterpolation > AbstractInterpolationPtr
Definition: AbstractInterpolation.h:77
armarx::SplineInterpolationPtr
std::shared_ptr< SplineInterpolation > SplineInterpolationPtr
Definition: SplineInterpolation.h:75
FramedPose.h
armarx::OrientationConversion::toQuaternion
static QuaternionBasePtr toQuaternion(const double roll, const double pitch, const double yaw)
Definition: OrientationConversion.cpp:57
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:41
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::SplineInterpolation
The SplineInterpolation class represents a linear Interpolation between a series of control points Sp...
Definition: SplineInterpolation.h:36