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 
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 BOOST_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 
145 BOOST_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 }
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::SplineInterpolationPtr
std::shared_ptr< SplineInterpolation > SplineInterpolationPtr
Definition: SplineInterpolation.h:72
FramedPose.h
armarx::AbstractInterpolationPtr
std::shared_ptr< AbstractInterpolation > AbstractInterpolationPtr
Definition: AbstractInterpolation.h:76
armarx::OrientationConversion::toQuaternion
static QuaternionBasePtr toQuaternion(const double roll, const double pitch, const double yaw)
Definition: OrientationConversion.cpp:58
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:43
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
armarx::SplineInterpolation
The SplineInterpolation class represents a linear Interpolation between a series of control points Sp...
Definition: SplineInterpolation.h:34