00001 #include <gtest/gtest.h> 00002 #include "Eigen3d.h" 00003 00004 using namespace hrp; 00005 00006 #if __cplusplus >= 201103L 00007 using std::isnan; 00008 #endif 00009 00010 TEST(Eigen3d, omegaFromRot) 00011 { 00012 Matrix33 m; 00013 Vector3 v; 00014 00015 m << 1.0, 0, 0, 00016 0, 1.0, 0, 00017 0, 0, 1.0; 00018 v = omegaFromRot(m); 00019 EXPECT_EQ(v[0], 0); 00020 EXPECT_EQ(v[1], 0); 00021 EXPECT_EQ(v[2], 0); 00022 00023 m << 1.0, 0, 0, 00024 0, 1.0, 0, 00025 0, 0, 1.0+2.0e-12; 00026 v = omegaFromRot(m); 00027 EXPECT_EQ(v[0], 0); 00028 EXPECT_EQ(v[1], 0); 00029 EXPECT_EQ(v[2], 0); 00030 00031 m << 1.0, 0, 0, 00032 0, 1.0, 0, 00033 0, 0, 1.0+2.0e-6; 00034 v = omegaFromRot(m); 00035 EXPECT_EQ(v[0], 0); 00036 EXPECT_EQ(v[1], 0); 00037 EXPECT_EQ(v[2], 0); 00038 00039 m = rotFromRpy(0, 0, 0); 00040 v = omegaFromRot(m); 00041 EXPECT_EQ(v[0], 0); 00042 EXPECT_EQ(v[1], 0); 00043 EXPECT_EQ(v[2], 0); 00044 00045 for (int i = 1; i <= 16; i++) { 00046 for (int j = 1; j <= 16; j++) { 00047 for (int k = 1; k <= 16; k++) { 00048 Vector3 rpy(pow(0.1, i), pow(0.1, j), pow(0.1, k)); 00049 m = rotFromRpy(rpy); 00050 v = omegaFromRot(m); 00051 EXPECT_FALSE(isnan(v[0])); 00052 EXPECT_FALSE(isnan(v[1])); 00053 EXPECT_FALSE(isnan(v[2])); 00054 } 00055 } 00056 } 00057 00058 for (int i = 0; i <= 180; i++) { 00059 for (int j = 0; j <= 180; j++) { 00060 for (int k = 0; k <= 180; k++) { 00061 Vector3 rpy(i*M_PI/180, j*M_PI/180, k*M_PI/180); 00062 m = rotFromRpy(rpy); 00063 v = omegaFromRot(m); 00064 EXPECT_FALSE(isnan(v[0])); 00065 EXPECT_FALSE(isnan(v[1])); 00066 EXPECT_FALSE(isnan(v[2])); 00067 } 00068 } 00069 } 00070 }