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 "../../Interpolation/SplineInterpolation.h"
6 
7 #include <RobotAPI/libraries/core/Pose.h>
8 
9 #include <RobotComponents/Test.h>
10 
11 #include "../../Util/OrientationConversion.h"
12 #include "../exceptions/NoInterpolationPossibleException.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
48 is_critical(exceptions::local::NoInterpolationPossibleException const& ex)
49 {
50  return true;
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 BOOST_AUTO_TEST_CASE(startIsEndTest)
101 {
102  Vector3BasePtr pos1 = new Vector3(2, 4, 6);
103  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(0, 0, 0);
104  PoseBasePtr pose1 = new Pose(pos1, ori1);
105 
106  Vector3BasePtr pos2 = new Vector3(0, -2, -8);
107  QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(1, 1, 1);
108  PoseBasePtr pose2 = new Pose(pos2, ori2);
109 
110  Vector3BasePtr pos3 = new Vector3(2, 4, 6);
111  QuaternionBasePtr ori3 = OrientationConversion::toQuaternion(2, 0, -1);
112  PoseBasePtr pose3 = new Pose(pos3, ori3);
113 
114  std::vector<PoseBasePtr> cp = {pose1, pose2, pose3};
115  SplineInterpolationPtr ip = *new SplineInterpolationPtr(new SplineInterpolation(cp));
116 
117  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->x, 2);
118  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->y, 4);
119  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->z, 6);
120  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->x, 2);
121  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->y, 4);
122  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->z, 6);
123  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->x, 0);
124  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->y, -2);
125  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->z, -8);
126 }
127 
128 BOOST_AUTO_TEST_CASE(tenPointPositionTest)
129 {
130  Vector3BasePtr pos1 = new Vector3(2, 4, 6);
131  QuaternionBasePtr ori1 = OrientationConversion::toQuaternion(0, 0, 0);
132  PoseBasePtr pose1 = new Pose(pos1, ori1);
133 
134  Vector3BasePtr pos2 = new Vector3(0, -2, -8);
135  QuaternionBasePtr ori2 = OrientationConversion::toQuaternion(1, 1, 1);
136  PoseBasePtr pose2 = new Pose(pos2, ori2);
137 
138  Vector3BasePtr pos3 = new Vector3(5, 8, 7);
139  QuaternionBasePtr ori3 = OrientationConversion::toQuaternion(2, 0, -1);
140  PoseBasePtr pose3 = new Pose(pos3, ori3);
141 
142  Vector3BasePtr pos4 = new Vector3(4, 23, 1);
143  QuaternionBasePtr ori4 = OrientationConversion::toQuaternion(0, 0, 0);
144  PoseBasePtr pose4 = new Pose(pos4, ori4);
145 
146  Vector3BasePtr pos5 = new Vector3(7, 12, 4);
147  QuaternionBasePtr ori5 = OrientationConversion::toQuaternion(1, 1, 1);
148  PoseBasePtr pose5 = new Pose(pos5, ori5);
149 
150  Vector3BasePtr pos6 = new Vector3(7, 44, -3);
151  QuaternionBasePtr ori6 = OrientationConversion::toQuaternion(2, 0, -1);
152  PoseBasePtr pose6 = new Pose(pos6, ori6);
153 
154  Vector3BasePtr pos7 = new Vector3(5, 4, -8);
155  QuaternionBasePtr ori7 = OrientationConversion::toQuaternion(1, 1, 1);
156  PoseBasePtr pose7 = new Pose(pos7, ori7);
157 
158  Vector3BasePtr pos8 = new Vector3(5, -4, -8);
159  QuaternionBasePtr ori8 = OrientationConversion::toQuaternion(2, 0, -1);
160  PoseBasePtr pose8 = new Pose(pos8, ori8);
161 
162  Vector3BasePtr pos9 = new Vector3(0, 0, 4);
163  QuaternionBasePtr ori9 = OrientationConversion::toQuaternion(0, 0, 0);
164  PoseBasePtr pose9 = new Pose(pos9, ori9);
165 
166  Vector3BasePtr pos10 = new Vector3(0, 1, 2);
167  QuaternionBasePtr ori10 = OrientationConversion::toQuaternion(0, 0, 0);
168  PoseBasePtr pose10 = new Pose(pos10, ori10);
169 
170  Vector3BasePtr pos11 = new Vector3(0, 0, 0);
171  QuaternionBasePtr ori11 = OrientationConversion::toQuaternion(0, 0, 0);
172  PoseBasePtr pose11 = new Pose(pos11, ori11);
173 
174  std::vector<PoseBasePtr> cp = {
175  pose1, pose2, pose3, pose4, pose5, pose6, pose7, pose8, pose9, pose10, pose11};
176  SplineInterpolationPtr ip = *new SplineInterpolationPtr(new SplineInterpolation(cp));
177 
178  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->x, 2);
179  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->y, 4);
180  BOOST_CHECK_EQUAL(ip->getPoseAt(0)->position->z, 6);
181  BOOST_CHECK_EQUAL(ip->getPoseAt(0.1)->position->x, 0);
182  BOOST_CHECK_EQUAL(ip->getPoseAt(0.1)->position->y, -2);
183  BOOST_CHECK_EQUAL(ip->getPoseAt(0.1)->position->z, -8);
184  BOOST_CHECK_EQUAL(ip->getPoseAt(0.2)->position->x, 5);
185  BOOST_CHECK_EQUAL(ip->getPoseAt(0.2)->position->y, 8);
186  BOOST_CHECK_EQUAL(ip->getPoseAt(0.2)->position->z, 7);
187  BOOST_CHECK_EQUAL(ip->getPoseAt(0.3)->position->x, 4);
188  BOOST_CHECK_EQUAL(ip->getPoseAt(0.3)->position->y, 23);
189  BOOST_CHECK_EQUAL(ip->getPoseAt(0.3)->position->z, 1);
190  BOOST_CHECK_EQUAL(ip->getPoseAt(0.4)->position->x, 7);
191  BOOST_CHECK_EQUAL(ip->getPoseAt(0.4)->position->y, 12);
192  BOOST_CHECK_EQUAL(ip->getPoseAt(0.4)->position->z, 4);
193  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->x, 7);
194  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->y, 44);
195  BOOST_CHECK_EQUAL(ip->getPoseAt(0.5)->position->z, -3);
196  BOOST_CHECK_EQUAL(ip->getPoseAt(0.6)->position->x, 5);
197  BOOST_CHECK_EQUAL(ip->getPoseAt(0.6)->position->y, 4);
198  BOOST_CHECK_EQUAL(ip->getPoseAt(0.6)->position->z, -8);
199  BOOST_CHECK_EQUAL(ip->getPoseAt(0.7)->position->x, 5);
200  BOOST_CHECK_EQUAL(ip->getPoseAt(0.7)->position->y, -4);
201  BOOST_CHECK_EQUAL(ip->getPoseAt(0.7)->position->z, -8);
202  BOOST_CHECK_EQUAL(ip->getPoseAt(0.8)->position->x, 0);
203  //BOOST_REQUIRE_CLOSE(ip->getPoseAt(0.8)->position->y, 0, 1);
204  BOOST_CHECK_EQUAL(ip->getPoseAt(0.8)->position->z, 4);
205  //BOOST_REQUIRE_CLOSE(ip->getPoseAt(0.9)->position->x, 0, 1);
206  BOOST_CHECK_EQUAL(ip->getPoseAt(0.9)->position->y, 1);
207  BOOST_CHECK_EQUAL(ip->getPoseAt(0.9)->position->z, 2);
208  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->x, 0);
209  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->y, 0);
210  BOOST_CHECK_EQUAL(ip->getPoseAt(1)->position->z, 0);
211 }
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
Pose.h
armarx::exceptions::local::NoInterpolationPossibleException
Definition: NoInterpolationPossibleException.h:39
armarx::SplineInterpolationPtr
std::shared_ptr< SplineInterpolation > SplineInterpolationPtr
Definition: SplineInterpolation.h:72
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
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:48
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