SplineInterpolationTest.cpp
Go to the documentation of this file.
1 #define BOOST_TEST_MODULE ArmarX::RobotTrajectoryDesigner::SplineInterpolation
2 #define ARMARX_BOOST_TEST
3 
4 
5 #include <RobotComponents/Test.h>
6 
7 
8 #include "../exceptions/NoInterpolationPossibleException.h"
9 
10 #include "../../Interpolation/SplineInterpolation.h"
11 #include "../../Util/OrientationConversion.h"
12 #include <RobotAPI/libraries/core/Pose.h>
13 
14 using namespace armarx;
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 
27  Vector3BasePtr pos3 = new Vector3(0, -5, -2);
28  QuaternionBasePtr ori3 = OrientationConversion::toQuaternion(0, 0, 0);
29  PoseBasePtr pose3 = new Pose(pos3, ori3);
30 
31  std::vector<PoseBasePtr> cp = {pose1, pose2, pose3};
32  SplineInterpolationPtr ip = *new SplineInterpolationPtr(new SplineInterpolation(cp));
33 
34  BOOST_CHECK_EQUAL(ip->getNumberOfControlPoints(), 3);
35 
36  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->x, 2);
37  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->y, 4);
38  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->z, 6);
39  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->x, 0);
40  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->y, 1);
41  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->z, 2);
42  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->x, 0);
43  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->y, -5);
44  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->z, -2);
45 }
46 
47 bool is_critical(exceptions::local::NoInterpolationPossibleException const& ex)
48 {
49  return true;
50 }
51 
52 
53 BOOST_AUTO_TEST_CASE(rotateAtPlaceTest)
54 {
55  Vector3BasePtr pos1 = new Vector3(2, 4, 6);
56  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(0, 0, 0);
57  PoseBasePtr pose1 = new Pose(pos1, ori1);
58 
59  Vector3BasePtr pos2 = new Vector3(2, 4, 6);
60  QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(1, 1, 1);
61  PoseBasePtr pose2 = new Pose(pos2, ori2);
62 
63  Vector3BasePtr pos3 = new Vector3(2, 4, 6);
64  QuaternionBasePtr ori3 = OrientationConversion::toQuaternion(2, 0, -1);
65  PoseBasePtr pose3 = new Pose(pos3, ori3);
66 
67  Vector3BasePtr pos4 = new Vector3(2, 4, 6);
68  QuaternionBasePtr ori4 = OrientationConversion::toQuaternion(0, 1, 2);
69  PoseBasePtr pose4 = new Pose(pos4, ori4);
70 
71  std::vector<PoseBasePtr> cp = {pose1, pose2, pose3, pose4};
72  SplineInterpolationPtr ip = *new SplineInterpolationPtr(new SplineInterpolation(cp));
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(ip->getPoseAt(0)->orientation->qw, q1->qw);
80  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->orientation->qx, q1->qx);
81  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->orientation->qy, q1->qy);
82  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->orientation->qz, q1->qz);
83 
84  BOOST_CHECK_CLOSE(ip->getPoseAt(0.333)->orientation->qw, q2->qw, 1);
85  BOOST_CHECK_CLOSE(ip->getPoseAt(0.333)->orientation->qx, q2->qx, 1);
86  BOOST_CHECK_CLOSE(ip->getPoseAt(0.333)->orientation->qy, q2->qy, 1);
87  BOOST_CHECK_CLOSE(ip->getPoseAt(0.333)->orientation->qz, q2->qz, 1);
88 
89  BOOST_CHECK_CLOSE(ip->getPoseAt(0.666)->orientation->qw, q3->qw, 1);
90  BOOST_CHECK_CLOSE(ip->getPoseAt(0.666)->orientation->qx, q3->qx, 1);
91  BOOST_CHECK_CLOSE(ip->getPoseAt(0.666)->orientation->qy, q3->qy, 1);
92  BOOST_CHECK_CLOSE(ip->getPoseAt(0.666)->orientation->qz, q3->qz, 1);
93 
94  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->orientation->qw, q4->qw);
95  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->orientation->qx, q4->qx);
96  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->orientation->qy, q4->qy);
97  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->orientation->qz, q4->qz);
98 
99 
100 }
101 
102 
103 BOOST_AUTO_TEST_CASE(startIsEndTest)
104 {
105  Vector3BasePtr pos1 = new Vector3(2, 4, 6);
106  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(0, 0, 0);
107  PoseBasePtr pose1 = new Pose(pos1, ori1);
108 
109  Vector3BasePtr pos2 = new Vector3(0, -2, -8);
110  QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(1, 1, 1);
111  PoseBasePtr pose2 = new Pose(pos2, ori2);
112 
113  Vector3BasePtr pos3 = new Vector3(2, 4, 6);
114  QuaternionBasePtr ori3 = OrientationConversion::toQuaternion(2, 0, -1);
115  PoseBasePtr pose3 = new Pose(pos3, ori3);
116 
117  std::vector<PoseBasePtr> cp = {pose1, pose2, pose3};
118  SplineInterpolationPtr ip = *new SplineInterpolationPtr(new SplineInterpolation(cp));
119 
120  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->x, 2);
121  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->y, 4);
122  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->z, 6);
123  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->x, 2);
124  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->y, 4);
125  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->z, 6);
126  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->x, 0);
127  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->y, -2);
128  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->z, -8);
129 }
130 
131 BOOST_AUTO_TEST_CASE(tenPointPositionTest)
132 {
133  Vector3BasePtr pos1 = new Vector3(2, 4, 6);
134  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(0, 0, 0);
135  PoseBasePtr pose1 = new Pose(pos1, ori1);
136 
137  Vector3BasePtr pos2 = new Vector3(0, -2, -8);
138  QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(1, 1, 1);
139  PoseBasePtr pose2 = new Pose(pos2, ori2);
140 
141  Vector3BasePtr pos3 = new Vector3(5, 8, 7);
142  QuaternionBasePtr ori3 = OrientationConversion::toQuaternion(2, 0, -1);
143  PoseBasePtr pose3 = new Pose(pos3, ori3);
144 
145  Vector3BasePtr pos4 = new Vector3(4, 23, 1);
146  QuaternionBasePtr ori4 = OrientationConversion::toQuaternion(0, 0, 0);
147  PoseBasePtr pose4 = new Pose(pos4, ori4);
148 
149  Vector3BasePtr pos5 = new Vector3(7, 12, 4);
150  QuaternionBasePtr ori5 = OrientationConversion::toQuaternion(1, 1, 1);
151  PoseBasePtr pose5 = new Pose(pos5, ori5);
152 
153  Vector3BasePtr pos6 = new Vector3(7, 44, -3);
154  QuaternionBasePtr ori6 = OrientationConversion::toQuaternion(2, 0, -1);
155  PoseBasePtr pose6 = new Pose(pos6, ori6);
156 
157  Vector3BasePtr pos7 = new Vector3(5, 4, -8);
158  QuaternionBasePtr ori7 = OrientationConversion::toQuaternion(1, 1, 1);
159  PoseBasePtr pose7 = new Pose(pos7, ori7);
160 
161  Vector3BasePtr pos8 = new Vector3(5, -4, -8);
162  QuaternionBasePtr ori8 = OrientationConversion::toQuaternion(2, 0, -1);
163  PoseBasePtr pose8 = new Pose(pos8, ori8);
164 
165  Vector3BasePtr pos9 = new Vector3(0, 0, 4);
166  QuaternionBasePtr ori9 = OrientationConversion::toQuaternion(0, 0, 0);
167  PoseBasePtr pose9 = new Pose(pos9, ori9);
168 
169  Vector3BasePtr pos10 = new Vector3(0, 1, 2);
170  QuaternionBasePtr ori10 = OrientationConversion::toQuaternion(0, 0, 0);
171  PoseBasePtr pose10 = new Pose(pos10, ori10);
172 
173  Vector3BasePtr pos11 = new Vector3(0, 0, 0);
174  QuaternionBasePtr ori11 = OrientationConversion::toQuaternion(0, 0, 0);
175  PoseBasePtr pose11 = new Pose(pos11, ori11);
176 
177  std::vector<PoseBasePtr> cp = {pose1, pose2, pose3, pose4, pose5, pose6, pose7, pose8, pose9, pose10, pose11};
178  SplineInterpolationPtr ip = *new SplineInterpolationPtr(new SplineInterpolation(cp));
179 
180  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->x, 2);
181  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->y, 4);
182  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->z, 6);
183  BOOST_CHECK_EQUAL(ip->getPoseAt(0.1)->position->x, 0);
184  BOOST_CHECK_EQUAL(ip->getPoseAt(0.1)->position->y, -2);
185  BOOST_CHECK_EQUAL(ip->getPoseAt(0.1)->position->z, -8);
186  BOOST_CHECK_EQUAL(ip->getPoseAt(0.2)->position->x, 5);
187  BOOST_CHECK_EQUAL(ip->getPoseAt(0.2)->position->y, 8);
188  BOOST_CHECK_EQUAL(ip->getPoseAt(0.2)->position->z, 7);
189  BOOST_CHECK_EQUAL(ip->getPoseAt(0.3)->position->x, 4);
190  BOOST_CHECK_EQUAL(ip->getPoseAt(0.3)->position->y, 23);
191  BOOST_CHECK_EQUAL(ip->getPoseAt(0.3)->position->z, 1);
192  BOOST_CHECK_EQUAL(ip->getPoseAt(0.4)->position->x, 7);
193  BOOST_CHECK_EQUAL(ip->getPoseAt(0.4)->position->y, 12);
194  BOOST_CHECK_EQUAL(ip->getPoseAt(0.4)->position->z, 4);
195  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->x, 7);
196  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->y, 44);
197  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->z, -3);
198  BOOST_CHECK_EQUAL(ip->getPoseAt(0.6)->position->x, 5);
199  BOOST_CHECK_EQUAL(ip->getPoseAt(0.6)->position->y, 4);
200  BOOST_CHECK_EQUAL(ip->getPoseAt(0.6)->position->z, -8);
201  BOOST_CHECK_EQUAL(ip->getPoseAt(0.7)->position->x, 5);
202  BOOST_CHECK_EQUAL(ip->getPoseAt(0.7)->position->y, -4);
203  BOOST_CHECK_EQUAL(ip->getPoseAt(0.7)->position->z, -8);
204  BOOST_CHECK_EQUAL(ip->getPoseAt(0.8)->position->x, 0);
205  //BOOST_REQUIRE_CLOSE(ip->getPoseAt(0.8)->position->y, 0, 1);
206  BOOST_CHECK_EQUAL(ip->getPoseAt(0.8)->position->z, 4);
207  //BOOST_REQUIRE_CLOSE(ip->getPoseAt(0.9)->position->x, 0, 1);
208  BOOST_CHECK_EQUAL(ip->getPoseAt(0.9)->position->y, 1);
209  BOOST_CHECK_EQUAL(ip->getPoseAt(0.9)->position->z, 2);
210  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->x, 0);
211  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->y, 0);
212  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->z, 0);
213 }
214 
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
Pose.h
armarx::exceptions::local::NoInterpolationPossibleException
Definition: NoInterpolationPossibleException.h:40
armarx::SplineInterpolationPtr
std::shared_ptr< SplineInterpolation > SplineInterpolationPtr
Definition: SplineInterpolation.h:75
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
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(basicTest)
Definition: SplineInterpolationTest.cpp:16
is_critical
bool is_critical(exceptions::local::NoInterpolationPossibleException const &ex)
Definition: SplineInterpolationTest.cpp:47
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