UnitTestUtilsTests.cpp
Go to the documentation of this file.
1 
2 #include <gtest/gtest.h>
3 #include "Fixtures.h"
4 #include "UnitTestUtils.hpp"
5 
6 class UnitTestUtilsTests : public testing::Test
7 {
8  public:
9  void SetUp()
10  {
11  }
12 
13  void TearDown()
14  {
15  }
16 };
17 
18 TEST_F(UnitTestUtilsTests, testCheckSpatialMatrixEpsilonClose)
19 {
20  RobotDynamics::Math::SpatialMatrix m1, m2;
21 
22  m1.setZero();
23  m2.setZero();
24 
25  m1(5, 5) = 0.7;
26  m2(5, 5) = 0.701;
27 
28  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(m1, m2, 0.0011));
29  EXPECT_FALSE(unit_test_utils::checkSpatialMatrixEpsilonClose(m1, m2, 0.0009));
30 
31  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(m2, m1, 0.0011));
32  EXPECT_FALSE(unit_test_utils::checkSpatialMatrixEpsilonClose(m2, m1, 0.0009));
33 
34  m1.setZero();
35  m2.setZero();
36 
37  m1(0, 0) = 0.701;
38  m2(0, 0) = 0.7;
39 
40  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(m1, m2, 0.0011));
41  EXPECT_FALSE(unit_test_utils::checkSpatialMatrixEpsilonClose(m1, m2, 0.0009));
42 
43  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(m2, m1, 0.0011));
44  EXPECT_FALSE(unit_test_utils::checkSpatialMatrixEpsilonClose(m2, m1, 0.0009));
45 }
46 
47 TEST_F(UnitTestUtilsTests, testCheckSpatialVectorsEpsilonClose)
48 {
49  RobotDynamics::Math::SpatialVector m1, m2;
50 
51  m1.setZero();
52  m2.setZero();
53 
54  m1(5) = 0.7;
55  m2(5) = 0.701;
56 
57  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(m1, m2, 0.0011));
58  EXPECT_FALSE(unit_test_utils::checkSpatialVectorsEpsilonClose(m1, m2, 0.0009));
59 
60  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(m2, m1, 0.0011));
61  EXPECT_FALSE(unit_test_utils::checkSpatialVectorsEpsilonClose(m2, m1, 0.0009));
62 
63  m1.setZero();
64  m2.setZero();
65 
66  m1(0) = 0.701;
67  m2(0) = 0.7;
68 
69  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(m1, m2, 0.0011));
70  EXPECT_FALSE(unit_test_utils::checkSpatialVectorsEpsilonClose(m1, m2, 0.0009));
71 
72  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(m2, m1, 0.0011));
73  EXPECT_FALSE(unit_test_utils::checkSpatialVectorsEpsilonClose(m2, m1, 0.0009));
74 }
75 
76 TEST_F(UnitTestUtilsTests, testMatrixNdEpsilonClose)
77 {
78  RobotDynamics::Math::MatrixNd m1(2, 4), m2(2, 4);
79 
80  m1.setZero();
81  m2.setZero();
82 
83  m1(1, 3) = 0.7;
84  m2(1, 3) = 0.701;
85 
86  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(m1, m2, 0.0011));
87  EXPECT_FALSE(unit_test_utils::checkMatrixNdEpsilonClose(m1, m2, 0.0009));
88 
89  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(m2, m1, 0.0011));
90  EXPECT_FALSE(unit_test_utils::checkMatrixNdEpsilonClose(m2, m1, 0.0009));
91 
92  m1.setZero();
93  m2.setZero();
94 
95  m1(0, 0) = 0.701;
96  m2(0, 0) = 0.7;
97 
98  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(m1, m2, 0.0011));
99  EXPECT_FALSE(unit_test_utils::checkMatrixNdEpsilonClose(m1, m2, 0.0009));
100 
101  EXPECT_TRUE(unit_test_utils::checkMatrixNdEpsilonClose(m2, m1, 0.0011));
102  EXPECT_FALSE(unit_test_utils::checkMatrixNdEpsilonClose(m2, m1, 0.0009));
103 
104  RobotDynamics::Math::MatrixNd m3(4, 2);
105  m3.setZero();
106 
107  try
108  {
109  unit_test_utils::checkMatrixNdEpsilonClose(m1, m3, 0.0011);
110  FAIL();
111  }
112  catch (RobotDynamics::RdlException& e)
113  {
114  EXPECT_STREQ(e.what(), "Cannot compare MatrixNd's of different sizes!");
115  }
116 }
117 
118 TEST_F(UnitTestUtilsTests, testCheckMatrix3dEpsilonClose)
119 {
120  RobotDynamics::Math::Matrix3d m1, m2;
121 
122  m1.setZero();
123  m2.setZero();
124 
125  m1(2, 2) = 0.7;
126  m2(2, 2) = 0.701;
127 
128  EXPECT_TRUE(unit_test_utils::checkMatrix3dEpsilonClose(m1, m2, 0.0011));
129  EXPECT_FALSE(unit_test_utils::checkMatrix3dEpsilonClose(m1, m2, 0.0009));
130 
131  EXPECT_TRUE(unit_test_utils::checkMatrix3dEpsilonClose(m2, m1, 0.0011));
132  EXPECT_FALSE(unit_test_utils::checkMatrix3dEpsilonClose(m2, m1, 0.0009));
133 
134  m1.setZero();
135  m2.setZero();
136 
137  m1(0, 0) = 0.701;
138  m2(0, 0) = 0.7;
139 
140  EXPECT_TRUE(unit_test_utils::checkMatrix3dEpsilonClose(m1, m2, 0.0011));
141  EXPECT_FALSE(unit_test_utils::checkMatrix3dEpsilonClose(m1, m2, 0.0009));
142 
143  EXPECT_TRUE(unit_test_utils::checkMatrix3dEpsilonClose(m2, m1, 0.0011));
144  EXPECT_FALSE(unit_test_utils::checkMatrix3dEpsilonClose(m2, m1, 0.0009));
145 }
146 
147 TEST_F(UnitTestUtilsTests, testCheckMatrix3dEq)
148 {
149  RobotDynamics::Math::Matrix3d m1, m2;
150 
151  m1.setZero();
152  m2.setZero();
153 
154  m1(2, 2) = 0.7;
155  m2(2, 2) = 0.7;
156 
157  EXPECT_TRUE(unit_test_utils::checkMatrix3dEq(m1, m2));
158 
159  m2(2, 2) = 0.7000001;
160  EXPECT_FALSE(unit_test_utils::checkMatrix3dEq(m2, m1));
161 
162  m1.setZero();
163  m2.setZero();
164 
165  m1(0, 0) = 0.7;
166  m2(0, 0) = 0.7;
167 
168  EXPECT_TRUE(unit_test_utils::checkMatrix3dEq(m1, m2));
169 
170  m2(0, 0) = 0.70000001;
171  EXPECT_FALSE(unit_test_utils::checkMatrix3dEq(m2, m1));
172 }
173 
174 TEST_F(UnitTestUtilsTests, testCheckMatrixNdEq)
175 {
176  RobotDynamics::Math::MatrixNd m1(3, 3), m2(3, 3);
177 
178  m1.setZero();
179  m2.setZero();
180 
181  m1(2, 2) = 0.7;
182  m2(2, 2) = 0.7;
183 
184  EXPECT_TRUE(unit_test_utils::checkMatrixNdEq(m1, m2));
185 
186  m2(2, 2) = 0.7000001;
187  EXPECT_FALSE(unit_test_utils::checkMatrixNdEq(m2, m1));
188 
189  m1.setZero();
190  m2.setZero();
191 
192  m1(0, 0) = 0.7;
193  m2(0, 0) = 0.7;
194 
195  EXPECT_TRUE(unit_test_utils::checkMatrixNdEq(m1, m2));
196 
197  m2(0, 0) = 0.70000001;
198  EXPECT_FALSE(unit_test_utils::checkMatrixNdEq(m2, m1));
199 
200  RobotDynamics::Math::MatrixNd m3(3, 4);
201 
202  try
203  {
204  EXPECT_FALSE(unit_test_utils::checkMatrixNdEq(m2, m3));
205  FAIL();
206  }
207  catch (RobotDynamics::RdlException& e)
208  {
209  EXPECT_STREQ(e.what(), "Cannot compare MatrixNd's of different sizes!");
210  }
211 }
212 
213 TEST_F(UnitTestUtilsTests, testCheckSpatialVectorsEq)
214 {
215  RobotDynamics::Math::SpatialVector m1, m2;
216 
217  m1.setZero();
218  m2.setZero();
219 
220  m1(2) = 0.7;
221  m2(2) = 0.7;
222 
223  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEq(m1, m2));
224 
225  m2(2) = 0.7000001;
226  EXPECT_FALSE(unit_test_utils::checkSpatialVectorsEq(m2, m1));
227 
228  m1.setZero();
229  m2.setZero();
230 
231  m1(0) = 0.7;
232  m2(0) = 0.7;
233 
234  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEq(m1, m2));
235 
236  m2(0) = 0.70000001;
237  EXPECT_FALSE(unit_test_utils::checkSpatialVectorsEq(m2, m1));
238 }
239 
240 TEST_F(UnitTestUtilsTests, testCheckVector3dEq)
241 {
242  RobotDynamics::Math::Vector3d m1, m2;
243 
244  m1.setZero();
245  m2.setZero();
246 
247  m1(2) = 0.7;
248  m2(2) = 0.7;
249 
250  EXPECT_TRUE(unit_test_utils::checkVector3dEq(m1, m2));
251 
252  m2(2) = 0.7000001;
253  EXPECT_FALSE(unit_test_utils::checkVector3dEq(m2, m1));
254 
255  m1.setZero();
256  m2.setZero();
257 
258  m1(0) = 0.7;
259  m2(0) = 0.7;
260 
261  EXPECT_TRUE(unit_test_utils::checkVector3dEq(m1, m2));
262 
263  m2(0) = 0.70000001;
264  EXPECT_FALSE(unit_test_utils::checkVector3dEq(m2, m1));
265 }
266 
267 TEST_F(UnitTestUtilsTests, testCheckVector4dEq)
268 {
269  RobotDynamics::Math::Vector4d m1, m2;
270 
271  m1.setZero();
272  m2.setZero();
273 
274  m1(2) = 0.7;
275  m2(2) = 0.7;
276 
277  EXPECT_TRUE(unit_test_utils::checkVector4dEq(m1, m2));
278 
279  m2(2) = 0.7000001;
280  EXPECT_FALSE(unit_test_utils::checkVector4dEq(m2, m1));
281 
282  m1.setZero();
283  m2.setZero();
284 
285  m1(0) = 0.7;
286  m2(0) = 0.7;
287 
288  EXPECT_TRUE(unit_test_utils::checkVector4dEq(m1, m2));
289 
290  m2(0) = 0.70000001;
291  EXPECT_FALSE(unit_test_utils::checkVector4dEq(m2, m1));
292 }
293 
294 TEST_F(UnitTestUtilsTests, testCheckVectorNdEq)
295 {
296  RobotDynamics::Math::VectorNd m1(8), m2(8);
297 
298  m1.setZero();
299  m2.setZero();
300 
301  m1(7) = 0.7;
302  m2(7) = 0.7;
303 
304  EXPECT_TRUE(unit_test_utils::checkVectorNdEq(m1, m2));
305 
306  m2(2) = 0.7000001;
307  EXPECT_FALSE(unit_test_utils::checkVectorNdEq(m2, m1));
308 
309  m1.setZero();
310  m2.setZero();
311 
312  m1(0) = 0.7;
313  m2(0) = 0.7;
314 
315  EXPECT_TRUE(unit_test_utils::checkVectorNdEq(m1, m2));
316 
317  m2(0) = 0.70000001;
318  EXPECT_FALSE(unit_test_utils::checkVectorNdEq(m2, m1));
319 
320  RobotDynamics::Math::VectorNd m3(4);
321 
322  try
323  {
324  unit_test_utils::checkVectorNdEq(m2, m3);
325  FAIL();
326  }
327  catch (RobotDynamics::RdlException& e)
328  {
329  EXPECT_STREQ(e.what(), "Cannot compare vectors because they are not the same length!");
330  }
331 }
332 
333 TEST_F(UnitTestUtilsTests, testCheckVector3dEpsilonClose)
334 {
335  RobotDynamics::Math::Vector3d m1, m2;
336 
337  m1.setZero();
338  m2.setZero();
339 
340  m1(2) = 0.7;
341  m2(2) = 0.701;
342 
343  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(m1, m2, 0.0011));
344  EXPECT_FALSE(unit_test_utils::checkVector3dEpsilonClose(m1, m2, 0.0009));
345 
346  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(m2, m1, 0.0011));
347  EXPECT_FALSE(unit_test_utils::checkVector3dEpsilonClose(m2, m1, 0.0009));
348 
349  m1.setZero();
350  m2.setZero();
351 
352  m1(0) = 0.701;
353  m2(0) = 0.7;
354 
355  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(m1, m2, 0.0011));
356  EXPECT_FALSE(unit_test_utils::checkVector3dEpsilonClose(m1, m2, 0.0009));
357 
358  EXPECT_TRUE(unit_test_utils::checkVector3dEpsilonClose(m2, m1, 0.0011));
359  EXPECT_FALSE(unit_test_utils::checkVector3dEpsilonClose(m2, m1, 0.0009));
360 }
361 
362 TEST_F(UnitTestUtilsTests, testCheckVector4dEpsilonClose)
363 {
364  RobotDynamics::Math::Vector4d m1, m2;
365 
366  m1.setZero();
367  m2.setZero();
368 
369  m1(2) = 0.7;
370  m2(2) = 0.701;
371 
372  EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(m1, m2, 0.0011));
373  EXPECT_FALSE(unit_test_utils::checkVector4dEpsilonClose(m1, m2, 0.0009));
374 
375  EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(m2, m1, 0.0011));
376  EXPECT_FALSE(unit_test_utils::checkVector4dEpsilonClose(m2, m1, 0.0009));
377 
378  m1.setZero();
379  m2.setZero();
380 
381  m1(0) = 0.701;
382  m2(0) = 0.7;
383 
384  EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(m1, m2, 0.0011));
385  EXPECT_FALSE(unit_test_utils::checkVector4dEpsilonClose(m1, m2, 0.0009));
386 
387  EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(m2, m1, 0.0011));
388  EXPECT_FALSE(unit_test_utils::checkVector4dEpsilonClose(m2, m1, 0.0009));
389 }
390 
391 TEST_F(UnitTestUtilsTests, testCheckVectorNdEpsilonClose)
392 {
393  RobotDynamics::Math::VectorNd m1(8), m2(8);
394 
395  m1.setZero();
396  m2.setZero();
397 
398  m1(7) = 0.7;
399  m2(7) = 0.701;
400 
401  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(m1, m2, 0.0011));
402  EXPECT_FALSE(unit_test_utils::checkVectorNdEpsilonClose(m1, m2, 0.0009));
403 
404  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(m2, m1, 0.0011));
405  EXPECT_FALSE(unit_test_utils::checkVectorNdEpsilonClose(m2, m1, 0.0009));
406 
407  m1.setZero();
408  m2.setZero();
409 
410  m1(0) = 0.701;
411  m2(0) = 0.7;
412 
413  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(m1, m2, 0.0011));
414  EXPECT_FALSE(unit_test_utils::checkVectorNdEpsilonClose(m1, m2, 0.0009));
415 
416  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(m2, m1, 0.0011));
417  EXPECT_FALSE(unit_test_utils::checkVectorNdEpsilonClose(m2, m1, 0.0009));
418 
419  RobotDynamics::Math::VectorNd m3(2);
420 
421  try
422  {
423  unit_test_utils::checkVectorNdEpsilonClose(m1, m3, 0.0011);
424  FAIL();
425  }
426  catch (RobotDynamics::RdlException& e)
427  {
428  EXPECT_STREQ(e.what(), "Cannot compare vectors because they are not the same length!");
429  }
430 }
431 
432 TEST_F(FixedBase3DoFPlanar, integration)
433 {
434  randomizeStates();
435  double h = 0.001;
436  Math::VectorNd x_euler = Math::VectorNd::Zero(model->q_size + model->qdot_size);
437  Math::VectorNd x_rk4 = Math::VectorNd::Zero(model->q_size + model->qdot_size);
438  unit_test_utils::integrateEuler(*model, Q, QDot, x_euler, Tau, h);
439  unit_test_utils::integrateRk4(*model, Q, QDot, x_rk4, Tau, h);
440 
441  EXPECT_TRUE(unit_test_utils::checkVectorNdEpsilonClose(x_euler, x_rk4, 1.e-3));
442 }
443 
444 int main(int argc, char** argv)
445 {
446  ::testing::InitGoogleTest(&argc, argv);
447  return RUN_ALL_TESTS();
448 }
unit_test_utils::checkVectorNdEpsilonClose
static bool checkVectorNdEpsilonClose(const RobotDynamics::Math::VectorNd &v1, const RobotDynamics::Math::VectorNd &v2, const double epsilon)
Definition: UnitTestUtils.hpp:288
Tau
VectorNd Tau
Definition: RdlTwoLegModelTests.cpp:32
QDot
VectorNd QDot
Definition: RdlTwoLegModelTests.cpp:30
Q
VectorNd Q
Definition: RdlTwoLegModelTests.cpp:29
unit_test_utils::checkVector4dEpsilonClose
static bool checkVector4dEpsilonClose(const RobotDynamics::Math::Vector4d &v1, const RobotDynamics::Math::Vector4d &v2, const double epsilon)
Definition: UnitTestUtils.hpp:275
unit_test_utils::checkMatrix3dEq
static bool checkMatrix3dEq(const RobotDynamics::Math::Matrix3d &t1, const RobotDynamics::Math::Matrix3d &t2)
Definition: UnitTestUtils.hpp:196
unit_test_utils::checkVector3dEq
static bool checkVector3dEq(const RobotDynamics::Math::Vector3d &v1, const RobotDynamics::Math::Vector3d &v2)
Definition: UnitTestUtils.hpp:212
RobotDynamics::RdlException::what
virtual const char * what() const
Definition: RdlExceptions.hpp:31
main
int main(int argc, char **argv)
Definition: UnitTestUtilsTests.cpp:444
RobotDynamics::Math::VectorNd
Eigen::VectorXd VectorNd
Definition: rdl_eigenmath.h:25
unit_test_utils::checkMatrixNdEpsilonClose
static bool checkMatrixNdEpsilonClose(const RobotDynamics::Math::MatrixNd &t1, const RobotDynamics::Math::MatrixNd &t2, const double epsilon)
Definition: UnitTestUtils.hpp:154
unit_test_utils::integrateRk4
void integrateRk4(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::VectorNd &x, const Math::VectorNd &tau, double h, Math::SpatialForceV *f_ext=nullptr)
Using a Runge-Kutta 4th order algorithm, integrate the system dynamics one step.
Definition: UnitTestUtils.hpp:471
unit_test_utils::checkSpatialMatrixEpsilonClose
static bool checkSpatialMatrixEpsilonClose(const RobotDynamics::Math::SpatialMatrix &t1, const RobotDynamics::Math::SpatialMatrix &t2, const double epsilon)
Definition: UnitTestUtils.hpp:122
unit_test_utils::checkSpatialVectorsEpsilonClose
static bool checkSpatialVectorsEpsilonClose(const RobotDynamics::Math::SpatialVector &v1, const RobotDynamics::Math::SpatialVector &v2, const double epsilon)
Definition: UnitTestUtils.hpp:306
unit_test_utils::checkVector4dEq
static bool checkVector4dEq(const RobotDynamics::Math::Vector4d &v1, const RobotDynamics::Math::Vector4d &v2)
Definition: UnitTestUtils.hpp:225
RobotDynamics::Math::MatrixNd
Eigen::MatrixXd MatrixNd
Definition: rdl_eigenmath.h:26
unit_test_utils::integrateEuler
void integrateEuler(Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::VectorNd &x, const Math::VectorNd &tau, double h, Math::SpatialForceV *f_ext=nullptr)
Using simple Euler integration, integrate the system dynamics one step.
Definition: UnitTestUtils.hpp:395
unit_test_utils::checkSpatialVectorsEq
static bool checkSpatialVectorsEq(const RobotDynamics::Math::SpatialVector &v1, const RobotDynamics::Math::SpatialVector &v2)
Definition: UnitTestUtils.hpp:319
RobotDynamics::RdlException
A custom exception.
Definition: RdlExceptions.hpp:20
unit_test_utils::checkVector3dEpsilonClose
static bool checkVector3dEpsilonClose(const RobotDynamics::Math::Vector3d &v1, const RobotDynamics::Math::Vector3d &v2, const double epsilon)
Definition: UnitTestUtils.hpp:256
TEST_F
TEST_F(UnitTestUtilsTests, testCheckSpatialMatrixEpsilonClose)
Definition: UnitTestUtilsTests.cpp:18
unit_test_utils::checkMatrixNdEq
static bool checkMatrixNdEq(const RobotDynamics::Math::MatrixNd &t1, const RobotDynamics::Math::MatrixNd &t2)
Definition: UnitTestUtils.hpp:175
unit_test_utils::checkVectorNdEq
static bool checkVectorNdEq(const RobotDynamics::Math::VectorNd &v1, const RobotDynamics::Math::VectorNd &v2)
Definition: UnitTestUtils.hpp:238
unit_test_utils::checkMatrix3dEpsilonClose
static bool checkMatrix3dEpsilonClose(const RobotDynamics::Math::Matrix3d &t1, const RobotDynamics::Math::Matrix3d &t2, const double epsilon)
Definition: UnitTestUtils.hpp:138

rdl_dynamics
Author(s):
autogenerated on Tue Apr 20 2021 02:25:28