testEigen3d.cpp
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 #include "Eigen3d.h"
3 
4 using namespace hrp;
5 
6 #if __cplusplus >= 201103L
7 using std::isnan;
8 #endif
9 
10 TEST(Eigen3d, omegaFromRot)
11 {
12  Matrix33 m;
13  Vector3 v;
14 
15  m << 1.0, 0, 0,
16  0, 1.0, 0,
17  0, 0, 1.0;
18  v = omegaFromRot(m);
19  EXPECT_EQ(v[0], 0);
20  EXPECT_EQ(v[1], 0);
21  EXPECT_EQ(v[2], 0);
22 
23  m << 1.0, 0, 0,
24  0, 1.0, 0,
25  0, 0, 1.0+2.0e-12;
26  v = omegaFromRot(m);
27  EXPECT_EQ(v[0], 0);
28  EXPECT_EQ(v[1], 0);
29  EXPECT_EQ(v[2], 0);
30 
31  m << 1.0, 0, 0,
32  0, 1.0, 0,
33  0, 0, 1.0+2.0e-6;
34  v = omegaFromRot(m);
35  EXPECT_EQ(v[0], 0);
36  EXPECT_EQ(v[1], 0);
37  EXPECT_EQ(v[2], 0);
38 
39  m = rotFromRpy(0, 0, 0);
40  v = omegaFromRot(m);
41  EXPECT_EQ(v[0], 0);
42  EXPECT_EQ(v[1], 0);
43  EXPECT_EQ(v[2], 0);
44 
45  for (int i = 1; i <= 16; i++) {
46  for (int j = 1; j <= 16; j++) {
47  for (int k = 1; k <= 16; k++) {
48  Vector3 rpy(pow(0.1, i), pow(0.1, j), pow(0.1, k));
49  m = rotFromRpy(rpy);
50  v = omegaFromRot(m);
51  EXPECT_FALSE(isnan(v[0]));
52  EXPECT_FALSE(isnan(v[1]));
53  EXPECT_FALSE(isnan(v[2]));
54  }
55  }
56  }
57 
58  for (int i = 0; i <= 180; i++) {
59  for (int j = 0; j <= 180; j++) {
60  for (int k = 0; k <= 180; k++) {
61  Vector3 rpy(i*M_PI/180, j*M_PI/180, k*M_PI/180);
62  m = rotFromRpy(rpy);
63  v = omegaFromRot(m);
64  EXPECT_FALSE(isnan(v[0]));
65  EXPECT_FALSE(isnan(v[1]));
66  EXPECT_FALSE(isnan(v[2]));
67  }
68  }
69  }
70 }
png_uint_32 i
Definition: png.h:2735
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
Matrix33 rotFromRpy(const Vector3 &rpy)
Definition: Eigen3d.h:33
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12
TEST(Eigen3d, omegaFromRot)
Definition: testEigen3d.cpp:10
def j(str, encoding="cp932")
#define M_PI
HRP_UTIL_EXPORT Vector3 omegaFromRot(const Matrix33 &r)
Definition: Eigen3d.cpp:63


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat May 8 2021 02:42:41