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
14using namespace armarx;
15
16BOOST_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
47bool
52
53BOOST_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
100BOOST_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
128BOOST_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}
is_critical
bool is_critical(exceptions::local::NoInterpolationPossibleException const &ex)
Definition SplineInterpolationTest.cpp:48
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(basicTest)
Definition SplineInterpolationTest.cpp:16
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